Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-07 00:13:59 -04:00
Code Issues Packages Projects Releases Wiki Activity

GCS_MAVLink: cope with AHRS not being available

Browse Source
This commit is contained in:
Peter Barker 2024-03-16 18:29:55 +11:00 committed by Andrew Tridgell
parent 50fe9e915f
commit abd5d0e3e9
3 changed files with 40 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
libraries/GCS_MAVLink/GCS.h
View File

@ -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"
@ -710,7 +711,9 @@ protected:
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; }

46
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -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);

10
libraries/GCS_MAVLink/ap_message.h
View File

@ -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,
@ -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,