ardupilot/AntennaTracker/sensors.cpp

69 lines
1.7 KiB
C++

#include "Tracker.h"
/*
update INS and attitude
*/
void Tracker::update_ahrs()
{
ahrs.update();
}
/*
read and update compass
*/
void Tracker::update_compass(void)
{
compass.read();
}
// Save compass offsets
void Tracker::compass_save() {
if (AP::compass().available() &&
compass.get_learn_type() >= Compass::LEARN_INTERNAL &&
!hal.util->get_soft_armed()) {
compass.save_offsets();
}
}
/*
read the GPS
*/
void Tracker::update_GPS(void)
{
gps.update();
static uint32_t last_gps_msg_ms;
static uint8_t ground_start_count = 5;
if (gps.last_message_time_ms() != last_gps_msg_ms &&
gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
last_gps_msg_ms = gps.last_message_time_ms();
if (ground_start_count > 1) {
ground_start_count--;
} else if (ground_start_count == 1) {
// We countdown N number of good GPS fixes
// so that the altitude is more accurate
// -------------------------------------
if (current_loc.lat == 0 && current_loc.lng == 0) {
ground_start_count = 5;
} else {
// Now have an initial GPS position
// use it as the HOME position in future startups
current_loc = gps.location();
if (!set_home(current_loc)) {
// silently ignored
}
ground_start_count = 0;
}
}
}
}
void Tracker::handle_battery_failsafe(const char* type_str, const int8_t action)
{
// NOP
// useful failsafes in the future would include actually recalling the vehicle
// that is tracked before the tracker loses power to continue tracking it
}