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

Rover: move sending of sys_status message up

Browse Source
This commit is contained in:
Peter Barker 2019-01-23 12:36:59 +11:00 committed by Andrew Tridgell
parent 120f0822ae
commit 7ea223eac6
3 changed files with 9 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
APMrover2/GCS_Mavlink.cpp
View File

@ -75,30 +75,15 @@ MAV_STATE GCS_MAVLINK_Rover::system_status() const
return MAV_STATE_ACTIVE; return MAV_STATE_ACTIVE;
} }
void Rover::send_sys_status(mavlink_channel_t chan) void GCS_MAVLINK_Rover::get_sensor_status_flags(uint32_t &present,
uint32_t &enabled,
uint32_t &health)
{ {
int16_t battery_current = -1; rover.update_sensor_status_flags();
int8_t battery_remaining = -1;
if (battery.has_current() && battery.healthy()) { present = rover.control_sensors_present;
battery_remaining = battery.capacity_remaining_pct(); enabled = rover.control_sensors_enabled;
battery_current = battery.current_amps() * 100; health = rover.control_sensors_health;
}
update_sensor_status_flags();
mavlink_msg_sys_status_send(
chan,
control_sensors_present,
control_sensors_enabled,
control_sensors_health,
static_cast<uint16_t>(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 Rover::send_nav_controller_output(mavlink_channel_t chan) void Rover::send_nav_controller_output(mavlink_channel_t chan)
@ -328,16 +313,6 @@ bool GCS_MAVLINK_Rover::try_send_message(enum ap_message id)
switch (id) { switch (id) {
case MSG_SYS_STATUS:
// send extended status only once vehicle has been initialised
// to avoid unnecessary errors being reported to user
if (!vehicle_initialised()) {
return true;
}
CHECK_PAYLOAD_SIZE(SYS_STATUS);
rover.send_sys_status(chan);
break;
case MSG_NAV_CONTROLLER_OUTPUT: case MSG_NAV_CONTROLLER_OUTPUT:
CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT); CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT);
rover.send_nav_controller_output(chan); rover.send_nav_controller_output(chan);

2
APMrover2/GCS_Mavlink.h
View File

@ -31,6 +31,8 @@ protected:
bool vehicle_initialised() const override; bool vehicle_initialised() const override;
void get_sensor_status_flags(uint32_t &present, uint32_t &enabled, uint32_t &health);
private: private:
void handleMessage(mavlink_message_t * msg) override; void handleMessage(mavlink_message_t * msg) override;

1
APMrover2/Rover.h
View File

@ -438,7 +438,6 @@ private:
void fence_check(); void fence_check();
// GCS_Mavlink.cpp // GCS_Mavlink.cpp
void send_sys_status(mavlink_channel_t chan);
void send_nav_controller_output(mavlink_channel_t chan); void send_nav_controller_output(mavlink_channel_t chan);
void send_servo_out(mavlink_channel_t chan); void send_servo_out(mavlink_channel_t chan);
void send_pid_tuning(mavlink_channel_t chan); void send_pid_tuning(mavlink_channel_t chan);