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

GCS_MAVLink: move sending of sys_status message up

Browse Source
This commit is contained in:
Peter Barker 2019-01-23 12:36:45 +11:00 committed by Andrew Tridgell
parent df1632054e
commit 120f0822ae
4 changed files with 48 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/GCS_MAVLink/GCS.h
View File

@ -225,6 +225,7 @@ public:
static void send_collision_all(const AP_Avoidance::Obstacle &threat, MAV_COLLISION_ACTION behaviour);
void send_accelcal_vehicle_position(uint32_t position);
void send_scaled_imu(uint8_t instance, void (*send_fn)(mavlink_channel_t chan, uint32_t time_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag));
void send_sys_status();
// return a bitmap of active channels. Used by libraries to loop
// over active channels to send to all active channels
@ -295,6 +296,7 @@ protected:
virtual MAV_MODE base_mode() const = 0;
virtual uint32_t custom_mode() const = 0;
virtual MAV_STATE system_status() const = 0;
virtual void get_sensor_status_flags(uint32_t &present, uint32_t &enabled, uint32_t &health) = 0;
bool waypoint_receiving; // currently receiving
// the following two variables are only here because of Tracker

44
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -3696,6 +3696,45 @@ void GCS_MAVLINK::send_hwstatus()
0);
}
void GCS_MAVLINK::send_sys_status()
{
// send extended status only once vehicle has been initialised
// to avoid unnecessary errors being reported to user
if (!vehicle_initialised()) {
return;
}
int16_t battery_current = -1;
int8_t battery_remaining = -1;
const AP_BattMonitor &battery = AP::battery();
if (battery.has_current() && battery.healthy()) {
battery_remaining = battery.capacity_remaining_pct();
battery_current = battery.current_amps() * 100;
}
uint32_t control_sensors_present;
uint32_t control_sensors_enabled;
uint32_t control_sensors_health;
get_sensor_status_flags(control_sensors_present, control_sensors_enabled, control_sensors_health);
mavlink_msg_sys_status_send(
chan,
control_sensors_present,
control_sensors_enabled,
control_sensors_health,
static_cast<uint16_t>(AP::scheduler().load_average() * 1000),
battery.voltage() * 1000, // mV
battery_current, // in 10mA units
battery_remaining, // in %
0, // comm drops %,
0, // comm drops in pkts,
0, 0, 0, 0);
}
void GCS_MAVLINK::send_attitude() const
{
const AP_AHRS &ahrs = AP::ahrs();
@ -3955,6 +3994,11 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id)
send_simstate();
break;
case MSG_SYS_STATUS:
CHECK_PAYLOAD_SIZE(SYS_STATUS);
send_sys_status();
break;
case MSG_AHRS2:
CHECK_PAYLOAD_SIZE(AHRS2);
send_ahrs2();

1
libraries/GCS_MAVLink/GCS_Dummy.h
View File

@ -32,6 +32,7 @@ protected:
MAV_MODE base_mode() const override { return (MAV_MODE)MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; }
uint32_t custom_mode() const override { return 3; } // magic number
MAV_STATE system_status() const override { return MAV_STATE_CALIBRATING; }
void get_sensor_status_flags(uint32_t &present, uint32_t &enabled, uint32_t &health) override { present = 0; enabled = 0; health = 0; }
};

1
libraries/GCS_MAVLink/examples/routing/routing.cpp
View File

@ -38,6 +38,7 @@ protected:
MAV_MODE base_mode() const override { return (MAV_MODE)MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; }
uint32_t custom_mode() const override { return 3; } // magic number
MAV_STATE system_status() const override { return MAV_STATE_CALIBRATING; }
void get_sensor_status_flags(uint32_t &present, uint32_t &enabled, uint32_t &health) override { present = 0; enabled = 0; health = 0; }
private: