void Aircraft::processMessage(const mavlink_message_t& msg) { //handle msg switch (msg.msgid) { case MAVLINK_MSG_ID_HEARTBEAT: { MAV_TYPE source = (MAV_TYPE)mavlink_msg_heartbeat_get_type(&msg); if (source == MAV_TYPE_GCS) { if (!connected()) { m_mavType = source; } return; } m_mavSystem = msg.sysid; m_mavComponent = msg.compid; m_customMode = (CustomMode)mavlink_msg_heartbeat_get_custom_mode(&msg); m_armed = ((mavlink_msg_heartbeat_get_base_mode(&msg) & MAV_MODE_FLAG_SAFETY_ARMED) != 0); m_state = (MAV_STATE)mavlink_msg_heartbeat_get_system_status(&msg); if (!m_gotHeartbeat) { m_gotHeartbeat = 1; requestMavlinkStreams(); // Make modules request their stuff DistanceAlert.requestData(m_mavSystem, m_mavComponent); ParameterManager.requestData(m_mavSystem, m_mavComponent); } } break; case MAVLINK_MSG_ID_SYS_STATUS: { if (!m_gotHeartbeat) { return; } uint32_t enabledSensors = mavlink_msg_sys_status_get_onboard_control_sensors_enabled(&msg); uint32_t presentSensors = mavlink_msg_sys_status_get_onboard_control_sensors_present(&msg); m_sensorsPresence = presentSensors & enabledSensors; m_sensorsHealth = mavlink_msg_sys_status_get_onboard_control_sensors_health(&msg); } break; case MAVLINK_MSG_ID_GPS_RAW_INT: { if (!m_gotHeartbeat) { return; } m_fixType = mavlink_msg_gps_raw_int_get_fix_type(&msg); m_satellites = mavlink_msg_gps_raw_int_get_satellites_visible(&msg); m_hdop = mavlink_msg_gps_raw_int_get_eph(&msg); m_vdop = mavlink_msg_gps_raw_int_get_epv(&msg); } break; case MAVLINK_MSG_ID_VFR_HUD: { if (!m_gotHeartbeat) { return; } m_airSpeed = mavlink_msg_vfr_hud_get_airspeed(&msg); m_groundSpeed = mavlink_msg_vfr_hud_get_groundspeed(&msg); m_heading = mavlink_msg_vfr_hud_get_heading(&msg); // 0..360 deg, 0=north m_throttle = (uint8_t)mavlink_msg_vfr_hud_get_throttle(&msg); m_altitude = mavlink_msg_vfr_hud_get_alt(&msg); m_climbRate = mavlink_msg_vfr_hud_get_climb(&msg); } break; case MAVLINK_MSG_ID_STATUSTEXT: { if (!m_gotHeartbeat) { return; } memset(m_lastWarning, 0, WARNING_TEXT_TOTAL + 1); mavlink_msg_statustext_get_text(&msg, m_lastWarning); int length = strlen(m_lastWarning); for (int i = 0; i < length; i++) { m_lastWarning[i] = tolower(m_lastWarning[i]); } m_lastWarningTimestamp = millis(); } break; case MAVLINK_MSG_ID_EKF_STATUS_REPORT: { if (!m_gotHeartbeat) { return; } const float ekf_warn_level = 0.5; const float ekf_bad_level = 0.8; mavlink_ekf_status_report_t ekf_status; mavlink_msg_ekf_status_report_decode(&msg, &ekf_status); if ((ekf_status.velocity_variance >= ekf_bad_level) || (ekf_status.pos_horiz_variance >= ekf_bad_level) || (ekf_status.pos_vert_variance >= ekf_bad_level) || (ekf_status.compass_variance >= ekf_bad_level) || (ekf_status.terrain_alt_variance >= ekf_bad_level)) { m_ekfStatus = EKFStatus_BAD; } else if ((ekf_status.velocity_variance >= ekf_warn_level) || (ekf_status.pos_horiz_variance >= ekf_warn_level) || (ekf_status.pos_vert_variance >= ekf_warn_level) || (ekf_status.compass_variance >= ekf_warn_level) || (ekf_status.terrain_alt_variance >= ekf_warn_level)) { m_ekfStatus = EKFStatus_WARN; } else { m_ekfStatus = EKFStatus_OK; } } break; case MAVLINK_MSG_ID_MAG_CAL_PROGRESS: { m_magCalStatus = (MAG_CAL_STATUS)mavlink_msg_mag_cal_progress_get_cal_status(&msg); m_magCalPercentage = mavlink_msg_mag_cal_progress_get_completion_pct(&msg); /* if (m_magCalStatus == MAG_CAL_SUCCESS) { cancelCompassCalibration(); } */ } break; case MAVLINK_MSG_ID_MAG_CAL_REPORT: { m_magCalStatus = (MAG_CAL_STATUS)mavlink_msg_mag_cal_report_get_cal_status(&msg); } break; } if (m_gotHeartbeat) { ParameterManager.handleMessage(&msg); DistanceAlert.handleMessage(&msg); } }
void MAVLink_Message_Handler::handle_message(uint64_t timestamp, mavlink_message_t &msg) { // ::fprintf(stderr, "msg.msgid=%u\n", msg.msgid); switch(msg.msgid) { case MAVLINK_MSG_ID_AHRS2: { mavlink_ahrs2_t decoded; mavlink_msg_ahrs2_decode(&msg, &decoded); handle_decoded_message(timestamp, decoded); break; } case MAVLINK_MSG_ID_ATTITUDE: { mavlink_attitude_t decoded; mavlink_msg_attitude_decode(&msg, &decoded); handle_decoded_message(timestamp, decoded); break; } case MAVLINK_MSG_ID_EKF_STATUS_REPORT: { mavlink_ekf_status_report_t decoded; mavlink_msg_ekf_status_report_decode(&msg, &decoded); handle_decoded_message(timestamp, decoded); break; } case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: { mavlink_global_position_int_t decoded; mavlink_msg_global_position_int_decode(&msg, &decoded); handle_decoded_message(timestamp, decoded); break; } case MAVLINK_MSG_ID_GPS_RAW_INT: { mavlink_gps_raw_int_t decoded; mavlink_msg_gps_raw_int_decode(&msg, &decoded); handle_decoded_message(timestamp, decoded); break; } case MAVLINK_MSG_ID_HEARTBEAT: { mavlink_heartbeat_t decoded; mavlink_msg_heartbeat_decode(&msg, &decoded); handle_decoded_message(timestamp, decoded); break; } case MAVLINK_MSG_ID_MOUNT_STATUS: { mavlink_mount_status_t decoded; mavlink_msg_mount_status_decode(&msg, &decoded); handle_decoded_message(timestamp, decoded); break; } case MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT: { mavlink_nav_controller_output_t decoded; mavlink_msg_nav_controller_output_decode(&msg, &decoded); handle_decoded_message(timestamp, decoded); break; } case MAVLINK_MSG_ID_PARAM_VALUE: { mavlink_param_value_t decoded; mavlink_msg_param_value_decode(&msg, &decoded); handle_decoded_message(timestamp, decoded); break; } case MAVLINK_MSG_ID_REMOTE_LOG_DATA_BLOCK: { mavlink_remote_log_data_block_t decoded; mavlink_msg_remote_log_data_block_decode(&msg, &decoded); handle_decoded_message(timestamp, decoded); break; } case MAVLINK_MSG_ID_SCALED_PRESSURE: { mavlink_scaled_pressure_t decoded; mavlink_msg_scaled_pressure_decode(&msg, &decoded); handle_decoded_message(timestamp, decoded); break; } case MAVLINK_MSG_ID_SCALED_PRESSURE2: { mavlink_scaled_pressure2_t decoded; mavlink_msg_scaled_pressure2_decode(&msg, &decoded); handle_decoded_message(timestamp, decoded); break; } case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW: { mavlink_servo_output_raw_t decoded; mavlink_msg_servo_output_raw_decode(&msg, &decoded); handle_decoded_message(timestamp, decoded); break; } case MAVLINK_MSG_ID_STATUSTEXT: { mavlink_statustext_t decoded; mavlink_msg_statustext_decode(&msg, &decoded); handle_decoded_message(timestamp, decoded); break; } case MAVLINK_MSG_ID_SYS_STATUS: { mavlink_sys_status_t decoded; mavlink_msg_sys_status_decode(&msg, &decoded); handle_decoded_message(timestamp, decoded); break; } case MAVLINK_MSG_ID_SYSTEM_TIME: { mavlink_system_time_t decoded; mavlink_msg_system_time_decode(&msg, &decoded); handle_decoded_message(timestamp, decoded); break; } case MAVLINK_MSG_ID_VFR_HUD: { mavlink_vfr_hud_t decoded; mavlink_msg_vfr_hud_decode(&msg, &decoded); handle_decoded_message(timestamp, decoded); break; } } }