diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 33be088a08..76504f3301 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -727,7 +727,6 @@ private: // GCS_Mavlink.cpp void gcs_send_heartbeat(void); - void send_sys_status(mavlink_channel_t chan); void send_nav_controller_output(mavlink_channel_t chan); void send_rpm(mavlink_channel_t chan); diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 6ac16e873b..e77e486a97 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -118,30 +118,15 @@ void GCS_MAVLINK_Copter::send_position_target_global_int() 0.0f); // yaw_rate } -NOINLINE void Copter::send_sys_status(mavlink_channel_t chan) +void GCS_MAVLINK_Copter::get_sensor_status_flags(uint32_t &present, + uint32_t &enabled, + uint32_t &health) { - int16_t battery_current = -1; - int8_t battery_remaining = -1; + copter.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 = copter.control_sensors_present; + enabled = copter.control_sensors_enabled; + health = copter.control_sensors_health; } void NOINLINE Copter::send_nav_controller_output(mavlink_channel_t chan) @@ -269,16 +254,6 @@ bool GCS_MAVLINK_Copter::try_send_message(enum ap_message 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); - copter.send_sys_status(chan); - break; - case MSG_NAV_CONTROLLER_OUTPUT: CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT); copter.send_nav_controller_output(chan); diff --git a/ArduCopter/GCS_Mavlink.h b/ArduCopter/GCS_Mavlink.h index 27e58de2dc..51f4e72182 100644 --- a/ArduCopter/GCS_Mavlink.h +++ b/ArduCopter/GCS_Mavlink.h @@ -48,6 +48,8 @@ private: bool vehicle_initialised() const override; + void get_sensor_status_flags(uint32_t &present, uint32_t &enabled, uint32_t &health); + void packetReceived(const mavlink_status_t &status, mavlink_message_t &msg) override;