diff --git a/AntennaTracker/AntennaTracker.cpp b/AntennaTracker/AntennaTracker.cpp index c104bf5998..081dde4856 100644 --- a/AntennaTracker/AntennaTracker.cpp +++ b/AntennaTracker/AntennaTracker.cpp @@ -37,6 +37,7 @@ const AP_Scheduler::Task Tracker::scheduler_tasks[] = { SCHED_TASK(update_tracking, 50, 1000), SCHED_TASK(update_GPS, 10, 4000), SCHED_TASK(update_compass, 10, 1500), + SCHED_TASK(update_battery, 10, 1500), SCHED_TASK(update_barometer, 10, 1500), SCHED_TASK(gcs_update, 50, 1700), SCHED_TASK(gcs_data_stream_send, 50, 3000), diff --git a/AntennaTracker/GCS_Mavlink.cpp b/AntennaTracker/GCS_Mavlink.cpp index 828c8412c3..f9665babc4 100644 --- a/AntennaTracker/GCS_Mavlink.cpp +++ b/AntennaTracker/GCS_Mavlink.cpp @@ -78,6 +78,30 @@ void Tracker::send_attitude(mavlink_channel_t chan) } +void Tracker::send_extended_status1(mavlink_channel_t chan) +{ + int16_t battery_current = -1; + int8_t battery_remaining = -1; + + if (battery.has_current() && battery.healthy()) { + battery_remaining = battery.capacity_remaining_pct(); + battery_current = battery.current_amps() * 100; + } + + mavlink_msg_sys_status_send( + chan, + 0, + 0, + 0, + static_cast(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 Tracker::send_location(mavlink_channel_t chan) { uint32_t fix_time; @@ -201,6 +225,11 @@ bool GCS_MAVLINK_Tracker::try_send_message(enum ap_message id) tracker.send_simstate(chan); break; + case MSG_EXTENDED_STATUS1: + CHECK_PAYLOAD_SIZE(SYS_STATUS); + tracker.send_extended_status1(chan); + break; + default: return GCS_MAVLINK::try_send_message(id); } diff --git a/AntennaTracker/Tracker.h b/AntennaTracker/Tracker.h index ad1af70a2d..dd96f3cdf1 100644 --- a/AntennaTracker/Tracker.h +++ b/AntennaTracker/Tracker.h @@ -150,6 +150,9 @@ private: AP_BoardConfig_CAN BoardConfig_CAN = AP_BoardConfig_CAN::create(); #endif + // Battery Sensors + AP_BattMonitor battery = AP_BattMonitor::create(); + struct Location current_loc; enum ControlMode control_mode = INITIALISING; @@ -201,6 +204,7 @@ private: void ten_hz_logging_loop(); void send_heartbeat(mavlink_channel_t chan); void send_attitude(mavlink_channel_t chan); + void send_extended_status1(mavlink_channel_t chan); void send_location(mavlink_channel_t chan); void send_nav_controller_output(mavlink_channel_t chan); void send_simstate(mavlink_channel_t chan); @@ -222,6 +226,7 @@ private: void update_barometer(void); void update_ahrs(); void update_compass(void); + void update_battery(void); void compass_accumulate(void); void accel_cal_update(void); void barometer_accumulate(void); diff --git a/AntennaTracker/sensors.cpp b/AntennaTracker/sensors.cpp index 9a4854ccdc..bdcf357526 100644 --- a/AntennaTracker/sensors.cpp +++ b/AntennaTracker/sensors.cpp @@ -46,6 +46,11 @@ void Tracker::update_compass(void) } } +void Tracker::update_battery() +{ + battery.read(); +} + /* if the compass is enabled then try to accumulate a reading */