Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-22 08:38:36 -04:00
Code Issues Packages Projects Releases Wiki Activity

GCS_MAVLink: tidy sending of high-latency message

Browse Source
This commit is contained in:
Peter Barker 2021-07-08 12:10:54 +10:00 committed by Andrew Tridgell
parent abd5daccbb
commit 07aa00f2b4
2 changed files with 27 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/GCS_MAVLink/GCS.h
View File

@ -319,7 +319,7 @@ public:
int8_t battery_remaining_pct(const uint8_t instance) const;
#if HAL_HIGH_LATENCY2_ENABLED
void send_high_latency() const;
void send_high_latency2() const;
#endif // HAL_HIGH_LATENCY2_ENABLED
// lock a channel, preventing use by MAVLink

76
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -5225,7 +5225,7 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id)
case MSG_HIGH_LATENCY2:
#if HAL_HIGH_LATENCY2_ENABLED
CHECK_PAYLOAD_SIZE(HIGH_LATENCY2);
send_high_latency();
send_high_latency2();
#endif // HAL_HIGH_LATENCY2_ENABLED
break;
@ -5772,23 +5772,21 @@ GCS &gcs()
send HIGH_LATENCY2 message
*/
#if HAL_HIGH_LATENCY2_ENABLED
void GCS_MAVLINK::send_high_latency() const
void GCS_MAVLINK::send_high_latency2() const
{
AP_AHRS &ahrs = AP::ahrs();
struct Location global_position_current;
Location global_position_current;
UNUSED_RESULT(ahrs.get_position(global_position_current));
Location cur = AP::gps().location();
const AP_BattMonitor &battery = AP::battery();
float battery_current;
float battery_current = -1;
int8_t battery_remaining = -1;
if (battery.healthy() && battery.current_amps(battery_current)) {
battery_remaining = battery_remaining_pct(AP_BATT_PRIMARY_INSTANCE);
battery_current = constrain_float(battery_current * 100,-INT16_MAX,INT16_MAX);
} else {
battery_current = -1;
}
AP_Mission *mission = AP::mission();
@ -5800,52 +5798,30 @@ void GCS_MAVLINK::send_high_latency() const
uint32_t present;
uint32_t enabled;
uint32_t health;
uint16_t failure_flags = 0;
gcs().get_sensor_status_flags(present, enabled, health);
// Remap HL_FAILURE_FLAG from system status flags
if (!(health & MAV_SYS_STATUS_SENSOR_GPS))
{
failure_flags |= HL_FAILURE_FLAG_GPS;
}
if (!(health & MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE))
{
failure_flags |= HL_FAILURE_FLAG_DIFFERENTIAL_PRESSURE;
}
if (!(health & MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE))
{
failure_flags |= HL_FAILURE_FLAG_ABSOLUTE_PRESSURE;
}
if (!(health & MAV_SYS_STATUS_SENSOR_3D_ACCEL))
{
failure_flags |= HL_FAILURE_FLAG_3D_ACCEL;
}
if (!(health & MAV_SYS_STATUS_SENSOR_3D_GYRO))
{
failure_flags |= HL_FAILURE_FLAG_3D_GYRO;
}
if (!(health & MAV_SYS_STATUS_SENSOR_3D_MAG))
{
failure_flags |= HL_FAILURE_FLAG_3D_MAG;
}
if (!(health & MAV_SYS_STATUS_TERRAIN))
{
failure_flags |= HL_FAILURE_FLAG_TERRAIN;
}
if (!(health & MAV_SYS_STATUS_SENSOR_BATTERY))
{
failure_flags |= HL_FAILURE_FLAG_BATTERY;
}
if (!(health & MAV_SYS_STATUS_SENSOR_RC_RECEIVER))
{
failure_flags |= HL_FAILURE_FLAG_RC_RECEIVER;
}
if (!(health & MAV_SYS_STATUS_GEOFENCE))
{
failure_flags |= HL_FAILURE_FLAG_GEOFENCE;
}
if (!(health & MAV_SYS_STATUS_AHRS))
{
failure_flags |= HL_FAILURE_FLAG_ESTIMATOR;
static const struct PACKED status_map_t {
MAV_SYS_STATUS_SENSOR sensor;
HL_FAILURE_FLAG failure_flag;
} status_map[] {
{ MAV_SYS_STATUS_SENSOR_GPS, HL_FAILURE_FLAG_GPS },
{ MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE, HL_FAILURE_FLAG_DIFFERENTIAL_PRESSURE },
{ MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE, HL_FAILURE_FLAG_ABSOLUTE_PRESSURE },
{ MAV_SYS_STATUS_SENSOR_3D_ACCEL, HL_FAILURE_FLAG_3D_ACCEL },
{ MAV_SYS_STATUS_SENSOR_3D_GYRO, HL_FAILURE_FLAG_3D_GYRO },
{ MAV_SYS_STATUS_SENSOR_3D_MAG, HL_FAILURE_FLAG_3D_MAG },
{ MAV_SYS_STATUS_TERRAIN, HL_FAILURE_FLAG_TERRAIN },
{ MAV_SYS_STATUS_SENSOR_BATTERY, HL_FAILURE_FLAG_BATTERY },
{ MAV_SYS_STATUS_SENSOR_RC_RECEIVER, HL_FAILURE_FLAG_RC_RECEIVER },
{ MAV_SYS_STATUS_GEOFENCE, HL_FAILURE_FLAG_GEOFENCE },
{ MAV_SYS_STATUS_AHRS, HL_FAILURE_FLAG_ESTIMATOR },
};
uint16_t failure_flags = 0;
for (auto &map_entry : status_map) {
if ((health & map_entry.sensor) == 0) {
failure_flags |= map_entry.failure_flag;
}
}
//send_text(MAV_SEVERITY_INFO, "Yaw: %u", (((uint16_t)ahrs.yaw_sensor / 100) % 360));