mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-06 16:08:28 -04:00
0885d55905
Now works with 2 servo alt-azimuth mounted antenna tracking mount. Tested on Flymaple, with Eagle Tree antenna tracker.
68 lines
1.2 KiB
Plaintext
68 lines
1.2 KiB
Plaintext
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
|
|
|
static void init_barometer(void)
|
|
{
|
|
gcs_send_text_P(SEVERITY_LOW, PSTR("Calibrating barometer"));
|
|
barometer.calibrate();
|
|
gcs_send_text_P(SEVERITY_LOW, PSTR("barometer calibration complete"));
|
|
}
|
|
|
|
// read the barometer and return the updated altitude in meters
|
|
static void update_barometer(void)
|
|
{
|
|
barometer.read();
|
|
}
|
|
|
|
|
|
/*
|
|
update INS and attitude
|
|
*/
|
|
static void update_ahrs()
|
|
{
|
|
ahrs.update();
|
|
}
|
|
|
|
|
|
/*
|
|
read and update compass
|
|
*/
|
|
static void 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
|
|
*/
|
|
static void compass_accumulate(void)
|
|
{
|
|
if (g.compass_enabled) {
|
|
compass.accumulate();
|
|
}
|
|
}
|
|
|
|
/*
|
|
try to accumulate a baro reading
|
|
*/
|
|
static void barometer_accumulate(void)
|
|
{
|
|
barometer.accumulate();
|
|
}
|
|
|
|
|
|
/*
|
|
read the GPS
|
|
*/
|
|
static void update_GPS(void)
|
|
{
|
|
g_gps->update();
|
|
// REVISIT: add compass variation with compass.set_initial_location(g_gps->latitude, g_gps->longitude);
|
|
// when have a solid GPS fix. Also init_home();
|
|
}
|
|
|