From 385ee7d26ded59c92d8aa1216cb76260bbfdf2ac Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 16 Mar 2024 18:29:55 +1100 Subject: [PATCH] GCS_MAVLink: cope with AHRS not being available --- libraries/GCS_MAVLink/GCS.h | 3 ++ libraries/GCS_MAVLink/GCS_Common.cpp | 46 ++++++++++++++++++---------- libraries/GCS_MAVLink/ap_message.h | 10 ++++-- 3 files changed, 40 insertions(+), 19 deletions(-) diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index efd5fbbd1ac3a..bd26264667b56 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -25,6 +25,7 @@ #include #include #include +#include #include #include "ap_message.h" @@ -710,7 +711,9 @@ class GCS_MAVLINK virtual float vfr_hud_climbrate() const; virtual float vfr_hud_airspeed() const; virtual int16_t vfr_hud_throttle() const { return 0; } +#if AP_AHRS_ENABLED virtual float vfr_hud_alt() const; +#endif #if HAL_HIGH_LATENCY2_ENABLED virtual int16_t high_latency_target_altitude() const { return 0; } diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index c655e464b8068..cf5074011055e 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -557,10 +557,10 @@ void GCS_MAVLINK::send_proximity() } #endif // HAL_PROXIMITY_ENABLED +#if AP_AHRS_ENABLED // report AHRS2 state void GCS_MAVLINK::send_ahrs2() { -#if AP_AHRS_ENABLED const AP_AHRS &ahrs = AP::ahrs(); Vector3f euler; Location loc {}; @@ -575,8 +575,8 @@ void GCS_MAVLINK::send_ahrs2() loc.lat, loc.lng); } -#endif } +#endif // AP_AHRS_ENABLED MissionItemProtocol *GCS::get_prot_for_mission_type(const MAV_MISSION_TYPE mission_type) const { @@ -979,9 +979,6 @@ ap_message GCS_MAVLINK::mavlink_id_to_ap_message_id(const uint32_t mavlink_id) c ap_message msg_id; } map[] { { MAVLINK_MSG_ID_HEARTBEAT, MSG_HEARTBEAT}, - { MAVLINK_MSG_ID_ATTITUDE, MSG_ATTITUDE}, - { MAVLINK_MSG_ID_ATTITUDE_QUATERNION, MSG_ATTITUDE_QUATERNION}, - { MAVLINK_MSG_ID_GLOBAL_POSITION_INT, MSG_LOCATION}, { MAVLINK_MSG_ID_HOME_POSITION, MSG_HOME}, { MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN, MSG_ORIGIN}, { MAVLINK_MSG_ID_SYS_STATUS, MSG_SYS_STATUS}, @@ -992,7 +989,6 @@ ap_message GCS_MAVLINK::mavlink_id_to_ap_message_id(const uint32_t mavlink_id) c { MAVLINK_MSG_ID_MEMINFO, MSG_MEMINFO}, { MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT, MSG_NAV_CONTROLLER_OUTPUT}, { MAVLINK_MSG_ID_MISSION_CURRENT, MSG_CURRENT_WAYPOINT}, - { MAVLINK_MSG_ID_VFR_HUD, MSG_VFR_HUD}, { MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, MSG_SERVO_OUTPUT_RAW}, { MAVLINK_MSG_ID_RC_CHANNELS, MSG_RC_CHANNELS}, { MAVLINK_MSG_ID_RC_CHANNELS_RAW, MSG_RC_CHANNELS_RAW}, @@ -1017,12 +1013,19 @@ ap_message GCS_MAVLINK::mavlink_id_to_ap_message_id(const uint32_t mavlink_id) c #if AP_FENCE_ENABLED { MAVLINK_MSG_ID_FENCE_STATUS, MSG_FENCE_STATUS}, #endif - { MAVLINK_MSG_ID_AHRS, MSG_AHRS}, #if AP_SIM_ENABLED { MAVLINK_MSG_ID_SIMSTATE, MSG_SIMSTATE}, { MAVLINK_MSG_ID_SIM_STATE, MSG_SIM_STATE}, #endif +#if AP_AHRS_ENABLED { MAVLINK_MSG_ID_AHRS2, MSG_AHRS2}, + { MAVLINK_MSG_ID_AHRS, MSG_AHRS}, + { MAVLINK_MSG_ID_ATTITUDE, MSG_ATTITUDE}, + { MAVLINK_MSG_ID_ATTITUDE_QUATERNION, MSG_ATTITUDE_QUATERNION}, + { MAVLINK_MSG_ID_GLOBAL_POSITION_INT, MSG_LOCATION}, + { MAVLINK_MSG_ID_LOCAL_POSITION_NED, MSG_LOCAL_POSITION}, + { MAVLINK_MSG_ID_VFR_HUD, MSG_VFR_HUD}, +#endif { MAVLINK_MSG_ID_HWSTATUS, MSG_HWSTATUS}, { MAVLINK_MSG_ID_WIND, MSG_WIND}, #if AP_RANGEFINDER_ENABLED @@ -1055,7 +1058,6 @@ ap_message GCS_MAVLINK::mavlink_id_to_ap_message_id(const uint32_t mavlink_id) c { MAVLINK_MSG_ID_MAG_CAL_REPORT, MSG_MAG_CAL_REPORT}, #endif { MAVLINK_MSG_ID_EKF_STATUS_REPORT, MSG_EKF_STATUS_REPORT}, - { MAVLINK_MSG_ID_LOCAL_POSITION_NED, MSG_LOCAL_POSITION}, { MAVLINK_MSG_ID_PID_TUNING, MSG_PID_TUNING}, { MAVLINK_MSG_ID_VIBRATION, MSG_VIBRATION}, #if AP_RPM_ENABLED @@ -2226,9 +2228,9 @@ void GCS_MAVLINK::send_scaled_pressure3() send_scaled_pressure_instance(2, mavlink_msg_scaled_pressure3_send); } +#if AP_AHRS_ENABLED void GCS_MAVLINK::send_ahrs() { -#if AP_AHRS_ENABLED const AP_AHRS &ahrs = AP::ahrs(); const Vector3f &omega_I = ahrs.get_gyro_drift(); mavlink_msg_ahrs_send( @@ -2240,8 +2242,8 @@ void GCS_MAVLINK::send_ahrs() 0, ahrs.get_error_rp(), ahrs.get_error_yaw()); -#endif } +#endif // AP_AHRS_ENABLED /* send a statustext text string to specific MAVLink bitmask @@ -2689,10 +2691,12 @@ void GCS_MAVLINK::send_opticalflow() const Vector2f &flowRate = optflow->flowRate(); const Vector2f &bodyRate = optflow->bodyRate(); - float hagl; + float hagl = 0; +#if AP_AHRS_ENABLED if (!AP::ahrs().get_hagl(hagl)) { hagl = 0; } +#endif // populate and send message mavlink_msg_optical_flow_send( @@ -2776,12 +2780,12 @@ void GCS_MAVLINK::send_autopilot_version() const } +#if AP_AHRS_ENABLED /* send LOCAL_POSITION_NED message */ void GCS_MAVLINK::send_local_position() const { -#if AP_AHRS_ENABLED const AP_AHRS &ahrs = AP::ahrs(); Vector3f local_position, velocity; @@ -2800,8 +2804,8 @@ void GCS_MAVLINK::send_local_position() const velocity.x, velocity.y, velocity.z); -#endif } +#endif /* send VIBRATION message @@ -3177,6 +3181,7 @@ float GCS_MAVLINK::vfr_hud_climbrate() const return 0; } +#if AP_AHRS_ENABLED float GCS_MAVLINK::vfr_hud_alt() const { return global_position_current_loc.alt * 0.01f; // cm -> m @@ -3184,7 +3189,6 @@ float GCS_MAVLINK::vfr_hud_alt() const void GCS_MAVLINK::send_vfr_hud() { -#if AP_AHRS_ENABLED AP_AHRS &ahrs = AP::ahrs(); // return values ignored; we send stale data @@ -3198,8 +3202,8 @@ void GCS_MAVLINK::send_vfr_hud() abs(vfr_hud_throttle()), vfr_hud_alt(), vfr_hud_climbrate()); -#endif } +#endif // AP_AHRS_ENABLED /* handle a MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN command @@ -5862,6 +5866,7 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id) switch(id) { +#if AP_AHRS_ENABLED case MSG_ATTITUDE: CHECK_PAYLOAD_SIZE(ATTITUDE); send_attitude(); @@ -5871,6 +5876,7 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id) CHECK_PAYLOAD_SIZE(ATTITUDE_QUATERNION); send_attitude_quaternion(); break; +#endif case MSG_NEXT_PARAM: CHECK_PAYLOAD_SIZE(PARAM_VALUE); @@ -5888,12 +5894,12 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id) send_hwstatus(); break; +#if AP_AHRS_ENABLED case MSG_LOCATION: CHECK_PAYLOAD_SIZE(GLOBAL_POSITION_INT); send_global_position_int(); break; -#if AP_AHRS_ENABLED case MSG_HOME: CHECK_PAYLOAD_SIZE(HOME_POSITION); send_home_position(); @@ -6053,10 +6059,12 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id) break; #endif #endif // AP_GPS_ENABLED +#if AP_AHRS_ENABLED case MSG_LOCAL_POSITION: CHECK_PAYLOAD_SIZE(LOCAL_POSITION_NED); send_local_position(); break; +#endif #if HAL_MOUNT_ENABLED case MSG_GIMBAL_DEVICE_ATTITUDE_STATUS: @@ -6180,10 +6188,12 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id) send_sys_status(); break; +#if AP_AHRS_ENABLED case MSG_AHRS2: CHECK_PAYLOAD_SIZE(AHRS2); send_ahrs2(); break; +#endif case MSG_PID_TUNING: CHECK_PAYLOAD_SIZE(PID_TUNING); @@ -6195,20 +6205,24 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id) send_nav_controller_output(); break; +#if AP_AHRS_ENABLED case MSG_AHRS: CHECK_PAYLOAD_SIZE(AHRS); send_ahrs(); break; +#endif case MSG_EXTENDED_SYS_STATE: CHECK_PAYLOAD_SIZE(EXTENDED_SYS_STATE); send_extended_sys_state(); break; +#if AP_AHRS_ENABLED case MSG_VFR_HUD: CHECK_PAYLOAD_SIZE(VFR_HUD); send_vfr_hud(); break; +#endif case MSG_VIBRATION: CHECK_PAYLOAD_SIZE(VIBRATION); diff --git a/libraries/GCS_MAVLink/ap_message.h b/libraries/GCS_MAVLink/ap_message.h index 973c48d51bad1..42724b12a2548 100644 --- a/libraries/GCS_MAVLink/ap_message.h +++ b/libraries/GCS_MAVLink/ap_message.h @@ -6,17 +6,23 @@ #pragma once +#include + enum ap_message : uint8_t { MSG_HEARTBEAT, +#if AP_AHRS_ENABLED + MSG_AHRS, + MSG_AHRS2, MSG_ATTITUDE, MSG_ATTITUDE_QUATERNION, MSG_LOCATION, + MSG_VFR_HUD, +#endif MSG_SYS_STATUS, MSG_POWER_STATUS, MSG_MEMINFO, MSG_NAV_CONTROLLER_OUTPUT, MSG_CURRENT_WAYPOINT, - MSG_VFR_HUD, MSG_SERVO_OUTPUT_RAW, MSG_RC_CHANNELS, MSG_RC_CHANNELS_RAW, @@ -38,10 +44,8 @@ enum ap_message : uint8_t { MSG_NEXT_MISSION_REQUEST_FENCE, MSG_NEXT_PARAM, MSG_FENCE_STATUS, - MSG_AHRS, MSG_SIMSTATE, MSG_SIM_STATE, - MSG_AHRS2, MSG_HWSTATUS, MSG_WIND, MSG_RANGEFINDER,