Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Tracker: add support for SYS_STATUS sending, battery

This commit is contained in:
Peter Barker 2017-11-27 12:51:11 +11:00 committed by Randy Mackay
parent cab972a429
commit 33feeb1c11
4 changed files with 40 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
AntennaTracker/AntennaTracker.cpp
View File

@ -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),

29
AntennaTracker/GCS_Mavlink.cpp
View File

@ -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<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 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);
}

5
AntennaTracker/Tracker.h
View File

@ -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);

5
AntennaTracker/sensors.cpp
View File

@ -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
*/