Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

GCS_MAVLink: cope with AHRS not being available #26537

Merged
merged 1 commit into from
Mar 18, 2024
+40 −19
Merged

GCS_MAVLink: cope with AHRS not being available #26537

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 3 additions & 0 deletions libraries/GCS_MAVLink/GCS.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,6 +25,7 @@
#include <AP_SerialManager/AP_SerialManager.h>
#include <AP_RangeFinder/AP_RangeFinder_config.h>
#include <AP_Winch/AP_Winch_config.h>
#include <AP_AHRS/AP_AHRS_config.h>
#include <AP_Arming/AP_Arming_config.h>

#include "ap_message.h"
Expand Down Expand Up @@ -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; }
Expand Down
46 changes: 30 additions & 16 deletions libraries/GCS_MAVLink/GCS_Common.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 {};
Expand All @@ -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
{
Expand Down Expand Up @@ -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},
Expand All @@ -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},
Expand All @@ -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
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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(
Expand All @@ -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
Expand Down Expand Up @@ -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(
Expand Down Expand Up @@ -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;
Expand All @@ -2800,8 +2804,8 @@ void GCS_MAVLINK::send_local_position() const
velocity.x,
velocity.y,
velocity.z);
#endif
}
#endif

/*
send VIBRATION message
Expand Down Expand Up @@ -3177,14 +3181,14 @@ 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
}

void GCS_MAVLINK::send_vfr_hud()
{
#if AP_AHRS_ENABLED
AP_AHRS &ahrs = AP::ahrs();

// return values ignored; we send stale data
Expand All @@ -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
Expand Down Expand Up @@ -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();
Expand All @@ -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);
Expand All @@ -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();
Expand Down Expand Up @@ -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:
Expand Down Expand Up @@ -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);
Expand All @@ -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);
Expand Down
10 changes: 7 additions & 3 deletions libraries/GCS_MAVLink/ap_message.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,17 +6,23 @@

#pragma once

#include <AP_AHRS/AP_AHRS_config.h>

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,
Expand All @@ -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,
Expand Down
Loading