Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-06 16:03:58 -04:00
Code Issues Packages Projects Releases Wiki Activity

Sub: move sending of sys_status message up

Browse Source
This commit is contained in:
Peter Barker 2019-01-23 14:22:18 +11:00 committed by Andrew Tridgell
parent 9a2ca025dd
commit fc19ce03b6
3 changed files with 13 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

46
ArduSub/GCS_Mavlink.cpp
View File

@ -84,12 +84,10 @@ MAV_STATE GCS_MAVLINK_Sub::system_status() const
return MAV_STATE_STANDBY;
}
NOINLINE void Sub::send_sys_status(mavlink_channel_t chan)
void Sub::get_sensor_status_flags(uint32_t &control_sensors_present,
uint32_t &control_sensors_enabled,
uint32_t &control_sensors_health)
{
uint32_t control_sensors_present;
uint32_t control_sensors_enabled;
uint32_t control_sensors_health;
// default sensors present
control_sensors_present = MAVLINK_SENSOR_PRESENT_DEFAULT;
@ -174,15 +172,6 @@ NOINLINE void Sub::send_sys_status(mavlink_channel_t chan)
control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_BATTERY;
}
int16_t battery_current = -1;
int8_t battery_remaining = -1;
if (battery.has_current() && battery.healthy()) {
// percent remaining is not necessarily accurate at the moment
//battery_remaining = battery.capacity_remaining_pct();
battery_current = battery.current_amps() * 100;
}
#if AP_TERRAIN_AVAILABLE && AC_TERRAIN
switch (terrain.status()) {
case AP_Terrain::TerrainStatusDisabled:
@ -215,20 +204,13 @@ NOINLINE void Sub::send_sys_status(mavlink_channel_t chan)
control_sensors_enabled &= ~(MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL);
control_sensors_health &= ~(MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL);
}
}
mavlink_msg_sys_status_send(
chan,
control_sensors_present,
control_sensors_enabled,
control_sensors_health,
(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 GCS_MAVLINK_Sub::get_sensor_status_flags(uint32_t &control_sensors_present,
uint32_t &control_sensors_enabled,
uint32_t &control_sensors_health)
{
return sub.get_sensor_status_flags(control_sensors_present, control_sensors_enabled, control_sensors_health);
}
void NOINLINE Sub::send_nav_controller_output(mavlink_channel_t chan)
@ -399,16 +381,6 @@ bool GCS_MAVLINK_Sub::try_send_message(enum ap_message id)
send_info();
break;
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);
sub.send_sys_status(chan);
break;
case MSG_NAV_CONTROLLER_OUTPUT:
CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT);
sub.send_nav_controller_output(chan);

1
ArduSub/GCS_Mavlink.h
View File

@ -46,6 +46,7 @@ private:
MAV_MODE base_mode() const override;
uint32_t custom_mode() const override;
MAV_STATE system_status() const override;
void get_sensor_status_flags(uint32_t &present, uint32_t &enabled, uint32_t &health);
int16_t vfr_hud_throttle() const override;

4
ArduSub/Sub.h
View File

@ -475,7 +475,9 @@ private:
void rotate_body_frame_to_NE(float &x, float &y);
void gcs_send_heartbeat(void);
void send_heartbeat(mavlink_channel_t chan);
void send_sys_status(mavlink_channel_t chan);
void get_sensor_status_flags(uint32_t &control_sensors_present,
uint32_t &control_sensors_enabled,
uint32_t &control_sensors_health);
void send_nav_controller_output(mavlink_channel_t chan);
#if RPM_ENABLED == ENABLED
void send_rpm(mavlink_channel_t chan);