text: // Copyright (c) Microsoft Corporation. All rights reserved.
  // Licensed under the MIT License.
  
  #include "MavLinkVehicleImpl.hpp"
  #include "MavLinkConnection.hpp"
  #include "../serial_com/SerialPort.hpp"
  #include "../serial_com/UdpClientPort.hpp"
  #include <exception>
  #include <cstring>
  #include <thread>
  #include <boost/lexical_cast.hpp>
  using namespace common_utils;
  
  using namespace mavlinkcom_impl;
  #define PACKET_PAYLOAD 253	//hard coded in MavLink code - do not change
  
  void mavlink_euler_to_quaternion(float roll, float pitch, float yaw, float quaternion[4])
  {
  	float cosPhi_2 = cosf(roll / 2);
  	float sinPhi_2 = sinf(roll / 2);
  	float cosTheta_2 = cosf(pitch / 2);
  	float sinTheta_2 = sinf(pitch / 2);
  	float cosPsi_2 = cosf(yaw / 2);
  	float sinPsi_2 = sinf(yaw / 2);
  	quaternion[0] = (cosPhi_2 * cosTheta_2 * cosPsi_2 +
  		sinPhi_2 * sinTheta_2 * sinPsi_2);
  	quaternion[1] = (sinPhi_2 * cosTheta_2 * cosPsi_2 -
  		cosPhi_2 * sinTheta_2 * sinPsi_2);
  	quaternion[2] = (cosPhi_2 * sinTheta_2 * cosPsi_2 +
  		sinPhi_2 * cosTheta_2 * sinPsi_2);
  	quaternion[3] = (cosPhi_2 * cosTheta_2 * sinPsi_2 -
  		sinPhi_2 * sinTheta_2 * cosPsi_2);
  }
  
  enum PX4_CUSTOM_MAIN_MODE {
  	PX4_CUSTOM_MAIN_MODE_NONE = 0,
  	PX4_CUSTOM_MAIN_MODE_MANUAL = 1,
  	PX4_CUSTOM_MAIN_MODE_ALTCTL = 2,
  	PX4_CUSTOM_MAIN_MODE_POSCTL = 3,
  	PX4_CUSTOM_MAIN_MODE_AUTO = 4,
  	PX4_CUSTOM_MAIN_MODE_ACRO = 5,
  	PX4_CUSTOM_MAIN_MODE_OFFBOARD = 6,
  	PX4_CUSTOM_MAIN_MODE_STABILIZED = 7,
  	PX4_CUSTOM_MAIN_MODE_RATTITUDE = 8
  };
  
  enum PX4_CUSTOM_SUB_MODE_AUTO {
  	PX4_CUSTOM_SUB_MODE_AUTO_NONE = 0,
  	PX4_CUSTOM_SUB_MODE_AUTO_READY = 1,
  	PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF = 2,
  	PX4_CUSTOM_SUB_MODE_AUTO_LOITER = 3,
  	PX4_CUSTOM_SUB_MODE_AUTO_MISSION = 4,
  	PX4_CUSTOM_SUB_MODE_AUTO_RTL = 5,
  	PX4_CUSTOM_SUB_MODE_AUTO_LAND = 6,
  	PX4_CUSTOM_SUB_MODE_AUTO_RTGS = 7,
  	PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET = 8
  };
  
  // ------------------------------------------------------------------------------
  //   Defines
  // ------------------------------------------------------------------------------
  
  /**
  * Defines for mavlink_set_position_target_local_ned_t.type_mask
  *
  * Bitmask to indicate which dimensions should be ignored by the vehicle
  *
  * a value of 0b0000000000000000 or 0b0000001000000000 indicates that none of
  * the setpoint dimensions should be ignored.
  *
  * If bit 10 is set the floats afx afy afz should be interpreted as force
  * instead of acceleration.
  *
  * Mapping:
  * bit 1: x,
  * bit 2: y,
  * bit 3: z,
  * bit 4: vx,
  * bit 5: vy,
  * bit 6: vz,
  * bit 7: ax,
  * bit 8: ay,
  * bit 9: az,
  * bit 10: is force setpoint,
  * bit 11: yaw,
  * bit 12: yaw rate
  * remaining bits unused
  *
  * Combine bitmasks with bitwise &
  *
  * Example for position and yaw angle:
  * uint16_t type_mask =
  *     MAVLINK_MSG_SET_POSITION_TARGET_POSITION &
  *     MAVLINK_MSG_SET_POSITION_TARGET_YAW_ANGLE;
  */
  
  // bit number  876543210987654321
  #define MAVLINK_MSG_SET_POSITION_TARGET_IGNORE_POSITION     0x7			// if 0x7 bits are on the PX4 ignores position.
  #define MAVLINK_MSG_SET_POSITION_TARGET_ALT_HOLD			0x3			// if 0x4 bit can be used to do z altitude hold.
  #define MAVLINK_MSG_SET_POSITION_TARGET_IGNORE_VELOCITY     0x38		// if 0x38 bits are on on the PX4 ignores velocity.
  #define MAVLINK_MSG_SET_POSITION_TARGET_IGNORE_ACCELERATION 0x1C0		// if 0x1C0 bits are on the PX4 ignores acceleration.
  #define MAVLINK_MSG_SET_POSITION_TARGET_FORCE		        0x200		// if 0x200 is on the PX4 interprets acceleration as a force.
  #define MAVLINK_MSG_SET_POSITION_TARGET_IGNORE_YAW_ANGLE    0x400		// if 0x400 is on the PX4 ignores the yaw 
  #define MAVLINK_MSG_SET_POSITION_TARGET_IGNORE_YAW_RATE     0x800		// if 0x800 is on the PX4 ignores the yaw rate.
  #define MAVLINK_MSG_SET_POSITION_TARGET_TAKEOFF			    0x1000
  #define MAVLINK_MSG_SET_POSITION_TARGET_LAND			    0x2000
  #define MAVLINK_MSG_SET_POSITION_TARGET_LOITER			    0x3000
  #define MAVLINK_MSG_SET_POSITION_TARGET_IDLE			    0x4000
  
  MavLinkVehicleImpl::MavLinkVehicleImpl(int system_id, int component_id)
  	: MavLinkNodeImpl(system_id, component_id)
  {
  }
  
  MavLinkVehicleImpl::~MavLinkVehicleImpl()
  {
  }
  
  AsyncResult<MavLinkHomePosition> MavLinkVehicleImpl::waitForHomePosition()
  {
  	Utils::logMessage("Waiting for home position...");
  
  	std::shared_ptr<MavLinkConnection> con = ensureConnection();
  	AsyncResult<MavLinkHomePosition> result([=](int subscription) {
  		con->unsubscribe(subscription);
  	});
  
  	this->setMessageInterval(static_cast<int>(MavLinkMessageIds::MAVLINK_MSG_ID_HOME_POSITION), 1);
  
  	int subscription = con->subscribe([=](std::shared_ptr<MavLinkConnection> connection, const MavLinkMessage& m) {
          if (m.msgid == static_cast<uint8_t>(MavLinkMessageIds::MAVLINK_MSG_ID_HOME_POSITION)) {
  			MavLinkHomePosition pos;
  			pos.decode(m);
  			result.setResult(pos);
  		}
  	});
  
  	return result;
  }
  
  AsyncResult<bool> MavLinkVehicleImpl::allowFlightControlOverUsb()
  {
  	MavLinkParameter  p;
  	p.name = "CBRK_USB_CHK";
  	p.value = 197848;
  	return setParameter(p);
  }
  
  void MavLinkVehicleImpl::handleMessage(std::shared_ptr<MavLinkConnection> connection, const MavLinkMessage& msg)
  {
  	if (msg.sysid != getTargetSystemId())
  	{
  		// we only care about messages from our intended remote node. 
  		return;
  	}
  
  	switch (msg.msgid) {
      case MavLinkHeartbeat::kMessageId: { // MAVLINK_MSG_ID_HEARTBEAT:
  		heartbeat_throttle_ = false;
  
  		MavLinkHeartbeat heartbeat;
  		heartbeat.decode(msg);
  
  		vehicle_state_.mode = heartbeat.base_mode;
  
  		bool armed = (heartbeat.base_mode & static_cast<uint8_t>(MAV_MODE_FLAG::MAV_MODE_FLAG_SAFETY_ARMED)) != 0;
  		std::lock_guard<std::mutex> guard(state_mutex_);
  		if (vehicle_state_.controls.armed != armed) {
  			state_version_++;
  			vehicle_state_.controls.armed = armed;
  		}
  		if (heartbeat.autopilot == static_cast<uint8_t>(MAV_AUTOPILOT::MAV_AUTOPILOT_PX4)) {
  			int custom = (heartbeat.custom_mode >> 16);
  			int mode = (custom & 0xff);
  			bool isOffboard = (mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD);
  			if (vehicle_state_.controls.offboard != isOffboard) {
  				if (vehicle_state_.controls.offboard && offboard_control_mode_) {
  					// if we just lost offboard control, but user didn't call releaseControl, 
  					// so in this case request loiter so PX4 doesn't land the drone.
                      releaseControl();
  					loiter();
                      // but we keep this flag set to true so that subsequent offboard commands work.
                      offboard_control_mode_ = true;
  				}
  				vehicle_state_.controls.offboard = isOffboard;
  			}
  		}
  	}
  		break;
  
      case MavLinkAttitude::kMessageId: { // MAVLINK_MSG_ID_ATTITUDE:
  		MavLinkAttitude att;
  		att.decode(msg);
  
  		std::lock_guard<std::mutex> guard(state_mutex_);
  		state_version_++;
  		updateReadStats(msg);
          vehicle_state_.attitude.roll = att.roll;
          vehicle_state_.attitude.pitch = att.pitch;
          vehicle_state_.attitude.yaw = att.yaw;
          vehicle_state_.attitude.roll_rate = att.rollspeed;
          vehicle_state_.attitude.pitch_rate = att.pitchspeed;
          vehicle_state_.attitude.yaw_rate = att.yawspeed;
  		vehicle_state_.attitude.updated_on = att.time_boot_ms;
  		//Utils::logMessage("Received attitude, acc=[%2.2f %2.2f %2.2f]", att.pitch, att.roll, att.yaw);
  		break;
  	}
      case MavLinkGlobalPositionInt::kMessageId: { // MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
  		MavLinkGlobalPositionInt pos;
  		pos.decode(msg);
  		std::lock_guard<std::mutex> guard(state_mutex_);
  		state_version_++;
  		updateReadStats(msg);
  		vehicle_state_.global_est.pos.lat = static_cast<float>(pos.lat) / 1E7f;
  		vehicle_state_.global_est.pos.lon = static_cast<float>(pos.lon) / 1E7f;
  		vehicle_state_.global_est.pos.alt = static_cast<float>(pos.alt) / 1E3f;
  		vehicle_state_.global_est.vel.vx = pos.vx;
  		vehicle_state_.global_est.vel.vy = pos.vy;
  		vehicle_state_.global_est.vel.vz = pos.vz;
  		vehicle_state_.global_est.alt_ground = pos.relative_alt;
  		vehicle_state_.global_est.heading = static_cast<float>(pos.hdg) / 100;
  		vehicle_state_.global_est.updated_on = pos.time_boot_ms;
  		break;
  	}
      case MavLinkRcChannelsScaled::kMessageId: {
  		MavLinkRcChannelsScaled ch;
  		ch.decode(msg);
  		std::lock_guard<std::mutex> guard(state_mutex_);
  		state_version_++;
  		updateReadStats(msg);
  		int port = ch.port;
  		// we can store up to 16 channels in rc_channels_scaled.
  		if (port < 2) {
  			int offset = port * 8;
  			vehicle_state_.rc.rc_channels_scaled[0 + offset] = ch.chan1_scaled;
  			vehicle_state_.rc.rc_channels_scaled[1 + offset] = ch.chan2_scaled;
  			vehicle_state_.rc.rc_channels_scaled[2 + offset] = ch.chan3_scaled;
  			vehicle_state_.rc.rc_channels_scaled[3 + offset] = ch.chan4_scaled;
  			vehicle_state_.rc.rc_channels_scaled[4 + offset] = ch.chan5_scaled;
  			vehicle_state_.rc.rc_channels_scaled[5 + offset] = ch.chan6_scaled;
  			vehicle_state_.rc.rc_channels_scaled[6 + offset] = ch.chan7_scaled;
  			vehicle_state_.rc.rc_channels_scaled[7 + offset] = ch.chan8_scaled;
  		}
  
  		break;
  	}
      case MavLinkRcChannels::kMessageId: { // MAVLINK_MSG_ID_RC_CHANNELS:
  		MavLinkRcChannels ch;
  		ch.decode(msg);
  
  		vehicle_state_.rc.rc_channels_count = ch.chancount;
  		vehicle_state_.rc.rc_signal_strength = ch.rssi;
  		vehicle_state_.rc.updated_on = ch.time_boot_ms;
  
  		break;
  	}
      case MavLinkServoOutputRaw::kMessageId: { // MAVLINK_MSG_ID_SERVO_OUTPUT_RAW:
  		// The RAW values of the servo outputs
  		MavLinkServoOutputRaw servo;
  		servo.decode(msg);
  		std::lock_guard<std::mutex> guard(state_mutex_);
  		state_version_++;
  		updateReadStats(msg);
  		vehicle_state_.servo.servo_raw[0] = servo.servo1_raw; vehicle_state_.servo.servo_raw[1] = servo.servo2_raw; vehicle_state_.servo.servo_raw[2] = servo.servo3_raw; vehicle_state_.servo.servo_raw[3] = servo.servo4_raw;
  		vehicle_state_.servo.servo_raw[4] = servo.servo5_raw; vehicle_state_.servo.servo_raw[5] = servo.servo6_raw; vehicle_state_.servo.servo_raw[6] = servo.servo7_raw; vehicle_state_.servo.servo_raw[7] = servo.servo8_raw;
  		vehicle_state_.servo.servo_port = servo.port;
  		vehicle_state_.servo.updated_on = servo.time_usec;
  
  		break;
  	}
  
      case MavLinkVfrHud::kMessageId: { // MAVLINK_MSG_ID_VFR_HUD:
  		// Metrics typically displayed on a HUD for fixed wing aircraft
  		break;
  	}
      case MavLinkHighresImu::kMessageId: { // MAVLINK_MSG_ID_HIGHRES_IMU:
  		// The IMU readings in SI units in NED body frame
  		break;
  	}
      case MavLinkAltitude::kMessageId: { // MAVLINK_MSG_ID_ALTITUDE:
  		MavLinkAltitude altitude;
  		altitude.decode(msg);
  		std::lock_guard<std::mutex> guard(state_mutex_);
  		state_version_++;
  		updateReadStats(msg);
          vehicle_state_.altitude.altitude_amsl = altitude.altitude_amsl;
          vehicle_state_.altitude.altitude_local = altitude.altitude_local;
          vehicle_state_.altitude.altitude_monotonic = altitude.altitude_monotonic;
          vehicle_state_.altitude.bottom_clearance = altitude.bottom_clearance;
          vehicle_state_.altitude.altitude_relative = altitude.altitude_relative;
          vehicle_state_.altitude.altitude_terrain = altitude.altitude_terrain;
  		vehicle_state_.altitude.updated_on = altitude.time_usec;
  		break;
  	}
      case MavLinkSysStatus::kMessageId: {
  		//printSystemStatus(&msg);
  		break;
  	}
      case MavLinkHomePosition::kMessageId: { // MAVLINK_MSG_ID_HOME_POSITION:
  		MavLinkHomePosition home;
  		home.decode(msg);
  		std::lock_guard<std::mutex> guard(state_mutex_);
  		state_version_++;
  		updateReadStats(msg);
  		vehicle_state_.home.global_pos.lat = static_cast<float>(home.latitude) / 1E7f;
  		vehicle_state_.home.global_pos.lon = static_cast<float>(home.longitude) / 1E7f;
  		vehicle_state_.home.global_pos.alt = static_cast<float>(home.altitude) / 1E3f;
  		vehicle_state_.home.approach.x = home.approach_x;
  		vehicle_state_.home.approach.y = home.approach_y;
  		vehicle_state_.home.approach.z = home.approach_z;
  		vehicle_state_.home.local_pose.pos.x = home.x;
  		vehicle_state_.home.local_pose.pos.y = home.y;
  		vehicle_state_.home.local_pose.pos.z = home.z;
  		vehicle_state_.home.is_set = true;
  		Utils::copy(home.q, vehicle_state_.home.local_pose.q);
  		break;
  	}
      case MavLinkBatteryStatus::kMessageId: { // MAVLINK_MSG_ID_BATTERY_STATUS
  		// todo: use this to determine when we need to do emergency landing...
  		break;
  	}
      case MavLinkAttitudeTarget::kMessageId: { // MAVLINK_MSG_ID_ATTITUDE_TARGET
  		// Reports the current commanded attitude of the vehicle as specified by the autopilot
  		break;
  	}
      case MavLinkExtendedSysState::kMessageId: { // MAVLINK_MSG_ID_EXTENDED_SYS_STATE:
  		// Provides state for additional features
  		// The general system state
  		MavLinkExtendedSysState extstatus;
  		extstatus.decode(msg);
          bool landed = extstatus.landed_state == static_cast<int>(MAV_LANDED_STATE::MAV_LANDED_STATE_ON_GROUND);
  		std::lock_guard<std::mutex> guard(state_mutex_);
  		if (vehicle_state_.controls.landed != landed) {
  			state_version_++;
  			updateReadStats(msg);
  			vehicle_state_.controls.landed = landed;
  		}
  		break;
  	}
      case MavLinkActuatorControlTarget::kMessageId: { // MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGE
  		break;
  	}
      case MavLinkStatustext::kMessageId: { // MAVLINK_MSG_ID_STATUSTEXT:
  		MavLinkStatustext statustext;
  		statustext.decode(msg);
  		Utils::logMessage("Received status sev=%d, text, %s",
  			static_cast<int>(statustext.severity), statustext.text);
  		break;
  	}
      case MavLinkMessageInterval::kMessageId: { // MAVLINK_MSG_ID_MESSAGE_INTERVAL:
  		MavLinkMessageInterval msgInt;
  		msgInt.decode(msg);
  		Utils::logMessage("Received message interval for msg=%d, interval = %d",
  			static_cast<int>(msgInt.message_id), msgInt.interval_us);
  		break;
  	}
      case MavLinkNamedValueInt::kMessageId: { // MAVLINK_MSG_ID_NAMED_VALUE_INT
  		break;
  	}
      case MavLinkHilControls::kMessageId: { // MAVLINK_MSG_ID_HIL_CONTROLS:
  		MavLinkHilControls value;
  		value.decode(msg);
  		std::lock_guard<std::mutex> guard(state_mutex_);
  		state_version_++;
  		updateReadStats(msg);
  		vehicle_state_.controls.actuator_controls[0] = value.roll_ailerons;
  		vehicle_state_.controls.actuator_controls[1] = value.pitch_elevator;
  		vehicle_state_.controls.actuator_controls[2] = value.yaw_rudder;
  		vehicle_state_.controls.actuator_controls[3] = value.throttle;
          vehicle_state_.controls.actuator_controls[4] = value.aux1;
          vehicle_state_.controls.actuator_controls[5] = value.aux2;
          vehicle_state_.controls.actuator_controls[6] = value.aux3;
          vehicle_state_.controls.actuator_controls[7] = value.aux4;
          vehicle_state_.controls.actuator_mode = value.mode;
          vehicle_state_.controls.actuator_nav_mode = value.nav_mode;
  		vehicle_state_.controls.updated_on = value.time_usec;
  		break;
  	}
      case MavLinkLocalPositionNed::kMessageId: { // MAVLINK_MSG_ID_LOCAL_POSITION_NED:
  		MavLinkLocalPositionNed value;
  		value.decode(msg);
  		std::lock_guard<std::mutex> guard(state_mutex_);
  		state_version_++;
  		updateReadStats(msg);
          vehicle_state_.local_est.pos.x = value.x;
          vehicle_state_.local_est.pos.y = value.y;
          vehicle_state_.local_est.pos.z = value.z;
          vehicle_state_.local_est.vel.vx = value.vx;
          vehicle_state_.local_est.vel.vy = value.vy;
          vehicle_state_.local_est.vel.vz = value.vz;
  		vehicle_state_.local_est.updated_on = value.time_boot_ms;
  		break;
  	}
      default:
          break;
  	}
  }
  
  void MavLinkVehicleImpl::writeMessage(MavLinkMessageBase& msg, bool update_stats)
  {
  	sendMessage(msg);
  	if (update_stats) {
  		vehicle_state_.stats.last_write_msg_id = msg.msgid;
  		vehicle_state_.stats.last_write_msg_time = getTimeStamp();
  	}
  }
  
  AsyncResult<bool> MavLinkVehicleImpl::armDisarm(bool arm)
  {
  	MavCmdComponentArmDisarm cmd{};
  	cmd.p1ToArm = arm ? 1.0f : 0.0f;
  	offboard_control_mode_ = false;
  	return sendCommandAndWaitForAck(cmd);
  }
  
  AsyncResult<bool> MavLinkVehicleImpl::takeoff(float z, float pitch, float yaw)
  {
  	float targetAlt = vehicle_state_.home.global_pos.alt - z;
  	Utils::logMessage("Take off to %f", targetAlt);
  	MavCmdNavTakeoff cmd{};
  	cmd.MinimumPitch = pitch;
  	cmd.YawAngle = yaw;
  	cmd.Latitude = INFINITY;
  	cmd.Longitude = INFINITY;
  	cmd.Altitude = targetAlt;
  	offboard_control_mode_ = false;
  	
  	return sendCommandAndWaitForAck(cmd);
  }
  
  AsyncResult<bool> MavLinkVehicleImpl::land(float yaw, float lat, float lon, float altitude)
  {
  	MavCmdNavLand cmd{};
  	cmd.AbortAlt = 1;
  	cmd.DesiredYawAngle = yaw;
  	cmd.Latitude = lat;
  	cmd.Longitude = lon;
  	cmd.Altitude = altitude;
  	offboard_control_mode_ = false;
  	return sendCommandAndWaitForAck(cmd);
  }
  
  AsyncResult<bool> MavLinkVehicleImpl::returnToHome()
  {
  	MavCmdNavReturnToLaunch cmd{};
  	return sendCommandAndWaitForAck(cmd);
  }
  
  bool MavLinkVehicleImpl::getRcSwitch(int channel, float threshold)
  {
  	if (threshold < -1 || threshold > 1) {
  		throw std::runtime_error(Utils::stringf("RC channel threshold is out of range, expecting -1 < threshold < 1, but got %f", threshold));
  	}
  	// if threshold < 0 then the threshold is inverted.
  	if (channel > 0 && channel < 18) {
  		std::lock_guard<std::mutex> guard(state_mutex_);
  		int16_t position = vehicle_state_.rc.rc_channels_scaled[channel - 1];
  		// RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX.
  		// Convert it to a floating point number between -1 and 1.
  		float value = static_cast<float>(position) / 10000.0f; 
  		if (threshold < 0) {
  			return value < threshold;
  		}
  		else {
  			return value > threshold;
  		}
  	}
  	return false;
  }
  
  void MavLinkVehicleImpl::requestControl()
  {
      offboard_control_mode_ = true;
  
      // PX4 requires at least one offboard control message in order to set offboard_control_signal_lost 
      // to false before it will accept transition to offboard control mode.
      auto state = getVehicleState();
      auto pos = state.local_est.pos;
      moveToLocalPosition(pos.x, pos.y, pos.z, true, static_cast<float>(state.attitude.yaw * M_PI / 180));
  
      int retries = 100;
      while (retries-- > 0) {
          bool r = false;
          MavCmdNavGuidedEnable cmd{};
          cmd.OnOff = 1;
          if (!sendCommandAndWaitForAck(cmd).wait(1000, &r)) {
              // timeout?
          }
          else if (!r) {
              // REJECT_OFFBOARD, so try again.
              moveToLocalPosition(pos.x, pos.y, pos.z, true, static_cast<float>(state.attitude.yaw * M_PI / 180));
              std::this_thread::sleep_for(std::chrono::milliseconds(20));
          }
          else {
              // got it!
              return;
          }
      }
  
      // hmmm, if we got here, then offboard has been consistently rejected!
      throw std::runtime_error("offboard rejected");
  
  }
  
  void MavLinkVehicleImpl::requestControlNoCheck()
  {
      // this method is used to re-request control while move* methods are being called in case we lost it due 
      // to a timeout.
  	offboard_control_mode_ = true;
  
  	MavCmdNavGuidedEnable cmd{};
  	cmd.OnOff = 1;
  	sendCommand(cmd);
  }
  
  // return true if we still have offboard control (can lose this if user flips the switch).
  bool MavLinkVehicleImpl::hasOffboardControl()
  {
  	return offboard_control_mode_;
  }
  
  void MavLinkVehicleImpl::releaseControl()
  {
  	offboard_control_mode_ = false;
  	MavCmdNavGuidedEnable cmd{};
  	cmd.OnOff = 0;
  	sendCommand(cmd);
  }
  
  void MavLinkVehicleImpl::checkOffboard()
  {
  	if (!offboard_control_mode_)
  	{
  		throw std::runtime_error("You must call requestControl first.");
  	}
  	bool isOffboard = false;
  	{
  		std::lock_guard<std::mutex> guard(state_mutex_);
  		isOffboard = vehicle_state_.controls.offboard;
  	}
  	if (offboard_control_mode_)
  	{
  		// if user called requestControl, but drone timed out the offboard control
  		// then we should re-request it.
  		if (!isOffboard && !heartbeat_throttle_) {
  
  			heartbeat_throttle_ = true;
  			requestControlNoCheck();
  		}
  	}
  }
  
  uint32_t MavLinkVehicleImpl::getTimeStamp()
  {
  	return static_cast<uint32_t>(std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count());
  }
  
  void MavLinkVehicleImpl::offboardIdle() {
  
  	MavLinkSetPositionTargetGlobalInt msg;
  	msg.target_component = getTimeStamp();
  	msg.target_system = getTargetSystemId();
  	msg.target_component = getTargetComponentId();
  	msg.coordinate_frame = static_cast<uint8_t>(MAV_FRAME::MAV_FRAME_GLOBAL_INT);
  	msg.afx = msg.afy = msg.afz = msg.alt = 0;
  	msg.lat_int = msg.lon_int = 0;
  	msg.vx = msg.vy = msg.vz = 0;
  	msg.yaw = msg.yaw_rate = 0;
  
  	msg.type_mask = MAVLINK_MSG_SET_POSITION_TARGET_IGNORE_VELOCITY |
  		MAVLINK_MSG_SET_POSITION_TARGET_IGNORE_ACCELERATION |
  		MAVLINK_MSG_SET_POSITION_TARGET_IGNORE_POSITION |
  		MAVLINK_MSG_SET_POSITION_TARGET_IGNORE_YAW_ANGLE |
  		MAVLINK_MSG_SET_POSITION_TARGET_IGNORE_YAW_RATE |
  		MAVLINK_MSG_SET_POSITION_TARGET_IDLE;
  
  	writeMessage(msg);
  }
  
  void MavLinkVehicleImpl::moveToGlobalPosition(float lat, float lon, float alt, bool isYaw, float yawOrRate)
  {
  	checkOffboard();
  
  	MavLinkSetPositionTargetGlobalInt msg;
  	msg.target_component = getTimeStamp();	
  	msg.target_system = getTargetSystemId();
  	msg.target_component = getTargetComponentId();
  	msg.afx = msg.afy = msg.afz = 0;
  	msg.vx = msg.vy = msg.vz = 0;
  	msg.coordinate_frame = static_cast<uint8_t>(MAV_FRAME::MAV_FRAME_GLOBAL_INT);
  
  	msg.type_mask = MAVLINK_MSG_SET_POSITION_TARGET_IGNORE_VELOCITY |
  		MAVLINK_MSG_SET_POSITION_TARGET_IGNORE_ACCELERATION;
  
  	if (isYaw)
  	{
  		msg.type_mask |= MAVLINK_MSG_SET_POSITION_TARGET_IGNORE_YAW_RATE;
  		msg.yaw = yawOrRate;
  		msg.yaw_rate = 0;
  	}
  	else
  	{
  		msg.type_mask |= MAVLINK_MSG_SET_POSITION_TARGET_IGNORE_YAW_ANGLE;
  		msg.yaw = 0;
  		msg.yaw_rate = yawOrRate;
  	}
  
  
  	msg.lat_int = static_cast<int>(lat * 1E7);
  	msg.lon_int = static_cast<int>(lon * 1E7);
  	msg.alt = alt;
  
  	writeMessage(msg);
  }
  
  void MavLinkVehicleImpl::setStabilizedFlightMode()
  {
  	int mode = static_cast<int>(MAV_MODE_FLAG::MAV_MODE_FLAG_STABILIZE_ENABLED);
  	if ((vehicle_state_.mode & static_cast<int>(MAV_MODE_FLAG::MAV_MODE_FLAG_HIL_ENABLED)) != 0) {
  		mode |= static_cast<int>(MAV_MODE_FLAG::MAV_MODE_FLAG_HIL_ENABLED); // must preserve this flag.
  	}
  	if ((vehicle_state_.mode & static_cast<uint8_t>(MAV_MODE_FLAG::MAV_MODE_FLAG_SAFETY_ARMED)) != 0) {
  		mode |= static_cast<int>(MAV_MODE_FLAG::MAV_MODE_FLAG_SAFETY_ARMED); // must preserve this flag.
  	}
  	MavCmdDoSetMode cmd{};
  	cmd.Mode = static_cast<float>(mode);
  	sendCommand(cmd);
  }
  
  void MavLinkVehicleImpl::setHomePosition(float lat, float lon, float alt)
  {
  	MavCmdDoSetHome cmd{};
  	cmd.UseCurrent = 0;
  	cmd.Latitude = lat;
  	cmd.Longitude = lon;
  	cmd.Altitude = alt;
  	sendCommand(cmd);
  }
  void MavLinkVehicleImpl::setAutoMode()
  {
  	offboard_control_mode_ = false;
  	int mode = static_cast<int>(MAV_MODE_FLAG::MAV_MODE_FLAG_CUSTOM_MODE_ENABLED);
  	if ((vehicle_state_.mode & static_cast<int>(MAV_MODE_FLAG::MAV_MODE_FLAG_HIL_ENABLED)) != 0) {
  		mode |= static_cast<int>(MAV_MODE_FLAG::MAV_MODE_FLAG_HIL_ENABLED); // must preserve this flag.
  	}
  	if ((vehicle_state_.mode & static_cast<int>(MAV_MODE_FLAG::MAV_MODE_FLAG_SAFETY_ARMED)) != 0) {
  		mode |= static_cast<int>(MAV_MODE_FLAG::MAV_MODE_FLAG_SAFETY_ARMED); // must preserve this flag.
  	}
  	mode |= static_cast<int>(MAV_MODE_FLAG::MAV_MODE_FLAG_AUTO_ENABLED);
  	MavCmdDoSetMode cmd{};
  	cmd.Mode = static_cast<float>(mode);
  	cmd.CustomMode = PX4_CUSTOM_MAIN_MODE_AUTO;
  	cmd.CustomSubMode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION;
  	sendCommand(cmd);
  }
  
  
  AsyncResult<bool> MavLinkVehicleImpl::loiter()
  {
  	offboard_control_mode_ = false;
  	int mode = static_cast<int>(MAV_MODE_FLAG::MAV_MODE_FLAG_CUSTOM_MODE_ENABLED);
  	if ((vehicle_state_.mode & static_cast<int>(MAV_MODE_FLAG::MAV_MODE_FLAG_HIL_ENABLED)) != 0) {
  		mode |= static_cast<int>(MAV_MODE_FLAG::MAV_MODE_FLAG_HIL_ENABLED); // must preserve this flag.
  	}
  	if ((vehicle_state_.mode & static_cast<int>(MAV_MODE_FLAG::MAV_MODE_FLAG_SAFETY_ARMED)) != 0) {
  		mode |= static_cast<int>(MAV_MODE_FLAG::MAV_MODE_FLAG_SAFETY_ARMED); // must preserve this flag.
  	}
  	MavCmdDoSetMode cmd{};
  	cmd.Mode = static_cast<float>(mode);
  	cmd.CustomMode = PX4_CUSTOM_MAIN_MODE_AUTO;
  	cmd.CustomSubMode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
  	return sendCommandAndWaitForAck(cmd);
  }
  
  bool MavLinkVehicleImpl::isLocalControlSupported()
  {
  	MavLinkAutopilotVersion cap;
  	if (!getCapabilities().wait(5000, &cap)) {
  		throw std::runtime_error(Utils::stringf("Five second timeout waiting for response to mavlink command MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES"));
  	}
  	return (cap.capabilities & static_cast<int>(MAV_PROTOCOL_CAPABILITY::MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_LOCAL_NED)) != 0;
  }
  
  void MavLinkVehicleImpl::moveByLocalVelocity(float vx, float vy, float vz, bool isYaw, float yawOrRate)
  {
  	checkOffboard();
  
  	MavLinkSetPositionTargetLocalNed msg;
  	msg.time_boot_ms = getTimeStamp();
  	msg.target_system = getTargetSystemId();
  	msg.target_component = getTargetComponentId();
  	msg.afx = msg.afy = msg.afz = 0;
  	msg.x = msg.y = msg.z = 0;
  	msg.vx = vx; msg.vy = vy; msg.vz = vz;
  	msg.coordinate_frame = static_cast<uint8_t>(MAV_FRAME::MAV_FRAME_LOCAL_NED);
  
  	msg.type_mask = MAVLINK_MSG_SET_POSITION_TARGET_IGNORE_POSITION |
  		MAVLINK_MSG_SET_POSITION_TARGET_IGNORE_ACCELERATION;
  
      if (isYaw) {
  		msg.type_mask |= MAVLINK_MSG_SET_POSITION_TARGET_IGNORE_YAW_RATE;
  		msg.yaw = yawOrRate;
  		msg.yaw_rate = 0;
      } else {
  		msg.type_mask |= MAVLINK_MSG_SET_POSITION_TARGET_IGNORE_YAW_ANGLE;
  		msg.yaw = 0;
  		msg.yaw_rate = yawOrRate;
  	}
  
  	writeMessage(msg);
  
  }
  
  void MavLinkVehicleImpl::moveByLocalVelocityWithAltHold(float vx, float vy, float z, bool isYaw, float yawOrRate)
  {
  	if (!offboard_control_mode_)
  	{
  		throw std::runtime_error("You must call requestControl first.");
  	}
  
  	MavLinkSetPositionTargetLocalNed msg;
  	msg.time_boot_ms = getTimeStamp();
  	msg.target_system = getTargetSystemId();
  	msg.target_component = getTargetComponentId();
  	msg.afx = msg.afy = msg.afz = 0;
  	msg.x = msg.y = msg.z = z;
  	msg.vx = vx; msg.vy = vy; msg.vz = 0;
  	msg.coordinate_frame = static_cast<uint8_t>(MAV_FRAME::MAV_FRAME_LOCAL_NED);
  
  	msg.type_mask = MAVLINK_MSG_SET_POSITION_TARGET_ALT_HOLD |
  		MAVLINK_MSG_SET_POSITION_TARGET_IGNORE_ACCELERATION;
  
      if (isYaw) {
  		msg.type_mask |= MAVLINK_MSG_SET_POSITION_TARGET_IGNORE_YAW_RATE;
  		msg.yaw = yawOrRate;
  		msg.yaw_rate = 0;
  	}
  	else
  	{
  		msg.type_mask |= MAVLINK_MSG_SET_POSITION_TARGET_IGNORE_YAW_ANGLE;
  		msg.yaw = 0;
  		msg.yaw_rate = yawOrRate;
  	}
  
  	writeMessage(msg);
  }
  
  void MavLinkVehicleImpl::moveToLocalPosition(float x, float y, float z, bool isYaw, float yawOrRate)
  {
  	checkOffboard();
  
  	MavLinkSetPositionTargetLocalNed msg;
  	msg.time_boot_ms = getTimeStamp();
  	msg.target_system = getTargetSystemId();
  	msg.target_component = getTargetComponentId();
  	msg.afx = msg.afy = msg.afz = 0;
  	msg.vx = msg.vy = msg.vz = 0;
  	msg.x = x; msg.y = y; msg.z = z;
  	msg.coordinate_frame = static_cast<uint8_t>(MAV_FRAME::MAV_FRAME_LOCAL_NED);
  
  	msg.type_mask = MAVLINK_MSG_SET_POSITION_TARGET_IGNORE_VELOCITY |
  		MAVLINK_MSG_SET_POSITION_TARGET_IGNORE_ACCELERATION;
      if (isYaw) {
  		msg.type_mask |= MAVLINK_MSG_SET_POSITION_TARGET_IGNORE_YAW_RATE;
  		msg.yaw = yawOrRate;
  		msg.yaw_rate = 0;
  	}
  	else
  	{
  		msg.type_mask |= MAVLINK_MSG_SET_POSITION_TARGET_IGNORE_YAW_ANGLE;
  		msg.yaw = 0;
  		msg.yaw_rate = yawOrRate;
  	}
  
  	writeMessage(msg);
  }
  
  bool MavLinkVehicleImpl::isAttitudeControlSupported()
  {
  	MavLinkAutopilotVersion cap;
  	if (!getCapabilities().wait(5000, &cap)) {
  		throw std::runtime_error(Utils::stringf("Five second timeout waiting for response to mavlink command MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES"));
  	}
  	return (cap.capabilities & static_cast<int>(MAV_PROTOCOL_CAPABILITY::MAV_PROTOCOL_CAPABILITY_SET_ATTITUDE_TARGET)) != 0;
  }
  
  void MavLinkVehicleImpl::moveByAttitude(float roll, float pitch, float yaw, float rollRate, float pitchRate, float yawRate, float thrust)
  {
  	checkOffboard();
  
  	MavLinkSetAttitudeTarget msg;
  	msg.time_boot_ms = getTimeStamp();
  	msg.target_system = getTargetSystemId();
  	msg.target_component = getTargetComponentId();
  	
  	float radRoll = roll * static_cast<float>(M_PI) / 180.0f;
  	float radPitch = pitch * static_cast<float>(M_PI) / 180.0f;
  	float radYaw = yaw * static_cast<float>(M_PI) / 180.0f;
  	mavlink_euler_to_quaternion(radRoll, radPitch, radYaw, msg.q);
  
  	// thrust must be between -1 and 1.
  	thrust = static_cast<float>(fmax(-1.0f, fmin(1.0f, thrust)));
  	msg.thrust = thrust;	
  	msg.body_roll_rate = rollRate * static_cast<float>(M_PI) / 180.0f;
  	msg.body_pitch_rate = pitchRate * static_cast<float>(M_PI) / 180.0f;
  	msg.body_yaw_rate = yawRate * static_cast<float>(M_PI) / 180.0f;
  
  	const uint8_t kIgnoreBodyRollRateBit = 1; //  mask bit 1: body roll rate,
  	const uint8_t kIgnoreBodyPitchRateBit = 2; // bit 2: body pitch rate, 
  	const uint8_t kIgnoreBodyYawRateBit = 4; // bit 3: body yaw rate. 
  	const uint8_t kIgnoreBit4Reserved = 8; //  bit 4-bit 6: reserved
  	const uint8_t kIgnoreBit5Reserved = 0x10;
  	const uint8_t kIgnoreBit6Reserved = 0x20;
  	const uint8_t kIgnoreThrottleBit = 0x40; // bit 7: throttle, 
  	const uint8_t kIgnoreAttitudeBit = 0x80; // bit 8: attitude
  
  	msg.type_mask = 0;
  	if (rollRate == 0) {
  		msg.type_mask |= kIgnoreBodyRollRateBit;
  	}
  	if (pitchRate == 0) {
  		msg.type_mask |= kIgnoreBodyPitchRateBit;
  	}
  	if (yawRate == 0) {
  		msg.type_mask |= kIgnoreBodyYawRateBit;
  	}
  	writeMessage(msg);
  }
  
  void MavLinkVehicleImpl::resetCommandParams(MavLinkCommandLong& cmd)
  {
  	cmd.param1 = cmd.param2 = cmd.param3 = cmd.param4 = cmd.param5 = cmd.param6 = cmd.param7 = 0;
  }
  
  const VehicleState& MavLinkVehicleImpl::getVehicleState()
  {
  	std::lock_guard<std::mutex> guard(state_mutex_);
  	return vehicle_state_; // return a snapshot.
  }
  
  int MavLinkVehicleImpl::getVehicleStateVersion()
  {
  	std::lock_guard<std::mutex> guard(state_mutex_);
  	return state_version_; 
  }
  
  void MavLinkVehicleImpl::updateReadStats(const MavLinkMessage& msg)
  {
  	vehicle_state_.stats.last_read_msg_id = msg.msgid;
  	vehicle_state_.stats.last_read_msg_time = getTimeStamp();
  }
  