diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index f9de2f5cc5..beb1b5925e 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -153,30 +153,15 @@ void Plane::send_fence_status(mavlink_channel_t chan) #endif -void Plane::send_sys_status(mavlink_channel_t chan) +void GCS_MAVLINK_Plane::get_sensor_status_flags(uint32_t &present, + uint32_t &enabled, + uint32_t &health) { - int16_t battery_current = -1; - int8_t battery_remaining = -1; + plane.update_sensor_status_flags(); - if (battery.has_current() && battery.healthy()) { - battery_remaining = battery.capacity_remaining_pct(); - battery_current = battery.current_amps() * 100; - } - - update_sensor_status_flags(); - - 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); + present = plane.control_sensors_present; + enabled = plane.control_sensors_enabled; + health = plane.control_sensors_health; } void Plane::send_nav_controller_output(mavlink_channel_t chan) @@ -435,11 +420,6 @@ bool GCS_MAVLINK_Plane::try_send_message(enum ap_message id) switch (id) { - case MSG_SYS_STATUS: - CHECK_PAYLOAD_SIZE(SYS_STATUS); - plane.send_sys_status(chan); - break; - case MSG_NAV_CONTROLLER_OUTPUT: if (plane.control_mode != MANUAL) { CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT); diff --git a/ArduPlane/GCS_Mavlink.h b/ArduPlane/GCS_Mavlink.h index 58170e4d0c..c92bcbd92d 100644 --- a/ArduPlane/GCS_Mavlink.h +++ b/ArduPlane/GCS_Mavlink.h @@ -49,6 +49,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); uint8_t radio_in_rssi() const; diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 661e422ad2..26fa5f169a 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -799,7 +799,6 @@ private: void update_load_factor(void); void send_fence_status(mavlink_channel_t chan); void update_sensor_status_flags(void); - void send_sys_status(mavlink_channel_t chan); void send_nav_controller_output(mavlink_channel_t chan); void send_servo_out(mavlink_channel_t chan); void send_wind(mavlink_channel_t chan);