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;
}
}
}
