You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
108 lines
2.5 KiB
108 lines
2.5 KiB
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- |
|
|
|
#include "Tracker.h" |
|
|
|
void Tracker::init_barometer(void) |
|
{ |
|
gcs_send_text(MAV_SEVERITY_INFO, "Calibrating barometer"); |
|
barometer.calibrate(); |
|
gcs_send_text(MAV_SEVERITY_INFO, "Barometer calibration complete"); |
|
} |
|
|
|
// read the barometer and return the updated altitude in meters |
|
void Tracker::update_barometer(void) |
|
{ |
|
barometer.update(); |
|
} |
|
|
|
|
|
/* |
|
update INS and attitude |
|
*/ |
|
void Tracker::update_ahrs() |
|
{ |
|
ahrs.update(); |
|
} |
|
|
|
|
|
/* |
|
read and update compass |
|
*/ |
|
void Tracker::update_compass(void) |
|
{ |
|
if (g.compass_enabled && compass.read()) { |
|
ahrs.set_compass(&compass); |
|
compass.learn_offsets(); |
|
} else { |
|
ahrs.set_compass(NULL); |
|
} |
|
} |
|
|
|
/* |
|
if the compass is enabled then try to accumulate a reading |
|
*/ |
|
void Tracker::compass_accumulate(void) |
|
{ |
|
if (g.compass_enabled) { |
|
compass.accumulate(); |
|
} |
|
} |
|
|
|
/* |
|
try to accumulate a baro reading |
|
*/ |
|
void Tracker::barometer_accumulate(void) |
|
{ |
|
barometer.accumulate(); |
|
} |
|
|
|
/* |
|
calibrate compass |
|
*/ |
|
void Tracker::compass_cal_update() { |
|
if (!hal.util->get_soft_armed()) { |
|
compass.compass_cal_update(); |
|
} |
|
} |
|
|
|
/* |
|
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) { |
|
ground_start_count = 5; |
|
|
|
} else { |
|
// Now have an initial GPS position |
|
// use it as the HOME position in future startups |
|
current_loc = gps.location(); |
|
set_home(current_loc); |
|
|
|
// set system clock for log timestamps |
|
hal.util->set_system_clock(gps.time_epoch_usec()); |
|
|
|
if (g.compass_enabled) { |
|
// Set compass declination automatically |
|
compass.set_initial_location(gps.location().lat, gps.location().lng); |
|
} |
|
ground_start_count = 0; |
|
} |
|
} |
|
} |
|
} |
|
|
|
|