in MavLinkCom/src/impl/MavLinkVehicleImpl.cpp [151:483]
void MavLinkVehicleImpl::handleMessage(std::shared_ptr<MavLinkConnection> connection, const MavLinkMessage& msg)
{
MavLinkNodeImpl::handleMessage(connection, msg);
//status messages should usually be only sent by actual PX4. However if someone else is sending it to, we should listen it.
//in future it would be good to have ability to add system IDs we are interested in
//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);
int submode = (custom >> 8);
bool isOffboard = (mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD);
if (isOffboard) {
vehicle_state_.controls.offboard = isOffboard;
Utils::log("MavLinkVehicle: confirmed offboard mode\n");
}
else if (vehicle_state_.controls.offboard != isOffboard) {
vehicle_state_.controls.offboard = isOffboard;
Utils::log("MavLinkVehicle: is no longer in offboard mode\n");
}
if (!isOffboard && (requested_mode_ != custom) && (custom != previous_mode_)) {
if (control_request_sent_) {
// user may have changed modes on us! So we need to honor that and not
// try and take it back.
vehicle_state_.controls.offboard = false;
control_requested_ = false;
control_request_sent_ = false;
Utils::log(Utils::stringf("MavLinkVehicle: detected mode change (mode=%d, submode=%d), will stop trying to do offboard control\n",
mode,
submode));
}
}
previous_mode_ = custom;
}
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 MavLinkControlSystemState::kMessageId: { // CONTROL_SYSTEM_STATE:
MavLinkControlSystemState cnt;
cnt.decode(msg);
std::lock_guard<std::mutex> guard(state_mutex_);
state_version_++;
updateReadStats(msg);
vehicle_state_.local_est.acc.x = cnt.x_acc;
vehicle_state_.local_est.acc.y = cnt.x_acc;
vehicle_state_.local_est.acc.z = cnt.x_acc;
//Utils::logMessage("Received attitude, acc=[%2.2f %2.2f %2.2f]", att.pitch, att.roll, att.yaw);
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.lin_vel.x = value.vx;
vehicle_state_.local_est.lin_vel.y = value.vy;
vehicle_state_.local_est.lin_vel.z = value.vz;
vehicle_state_.local_est.updated_on = value.time_boot_ms;
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.x = pos.vx;
vehicle_state_.global_est.vel.y = pos.vy;
vehicle_state_.global_est.vel.z = 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
MavLinkVfrHud vfrhud;
vfrhud.decode(msg);
std::lock_guard<std::mutex> guard(state_mutex_);
state_version_++;
updateReadStats(msg);
vehicle_state_.vfrhud.true_airspeed = vfrhud.airspeed;
vehicle_state_.vfrhud.groundspeed = vfrhud.groundspeed;
vehicle_state_.vfrhud.altitude = vfrhud.alt;
vehicle_state_.vfrhud.climb_rate = vfrhud.climb;
vehicle_state_.vfrhud.heading = vfrhud.heading;
vehicle_state_.vfrhud.throttle = vfrhud.throttle;
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::log(Utils::stringf("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 MavLinkCommandAck::kMessageId: {
// This one is tricky, we can't do sendCommandAndWaitForAck in this case because it takes too long
// but we do want to know when we get the ack. So this is async ACK processing!
MavLinkCommandAck ack;
ack.decode(msg);
if (ack.command == MavCmdNavGuidedEnable::kCommandId) {
MAV_RESULT ackResult = static_cast<MAV_RESULT>(ack.result);
if (ackResult == MAV_RESULT::MAV_RESULT_TEMPORARILY_REJECTED) {
Utils::log("### command MavCmdNavGuidedEnable result: MAV_RESULT_TEMPORARILY_REJECTED");
}
else if (ackResult == MAV_RESULT::MAV_RESULT_UNSUPPORTED) {
Utils::log("### command MavCmdNavGuidedEnable result: MAV_RESULT_UNSUPPORTED");
vehicle_state_.controls.offboard = false;
}
else if (ackResult == MAV_RESULT::MAV_RESULT_FAILED) {
Utils::log("### command MavCmdNavGuidedEnable result: MAV_RESULT_FAILED");
vehicle_state_.controls.offboard = false;
}
else if (ackResult == MAV_RESULT::MAV_RESULT_ACCEPTED) {
Utils::log("### command MavCmdNavGuidedEnableresult: MAV_RESULT_ACCEPTED");
vehicle_state_.controls.offboard = true;
}
}
break;
}
case MavLinkAttPosMocap::kMessageId: {
MavLinkAttPosMocap mocap;
mocap.decode(msg);
std::lock_guard<std::mutex> guard(state_mutex_);
state_version_++;
updateReadStats(msg);
vehicle_state_.mocap.pose.pos.x = mocap.x;
vehicle_state_.mocap.pose.pos.y = mocap.y;
vehicle_state_.mocap.pose.pos.z = mocap.z;
Utils::copy(mocap.q, vehicle_state_.mocap.pose.q);
vehicle_state_.mocap.updated_on = mocap.time_usec;
break;
}
default:
break;
}
}