mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
Tracker: move automatic declination setting into AP_Compass itself
This commit is contained in:
parent
9d54b4820f
commit
2428487383
@ -97,9 +97,6 @@ void Tracker::one_second_loop()
|
|||||||
// updated armed/disarmed status LEDs
|
// updated armed/disarmed status LEDs
|
||||||
update_armed_disarmed();
|
update_armed_disarmed();
|
||||||
|
|
||||||
// init compass location for declination
|
|
||||||
init_compass_location();
|
|
||||||
|
|
||||||
if (!ahrs.home_is_set()) {
|
if (!ahrs.home_is_set()) {
|
||||||
// set home to current location
|
// set home to current location
|
||||||
Location temp_loc;
|
Location temp_loc;
|
||||||
|
@ -248,7 +248,6 @@ private:
|
|||||||
// sensors.cpp
|
// sensors.cpp
|
||||||
void update_ahrs();
|
void update_ahrs();
|
||||||
void compass_save();
|
void compass_save();
|
||||||
void init_compass_location();
|
|
||||||
void update_compass(void);
|
void update_compass(void);
|
||||||
void accel_cal_update(void);
|
void accel_cal_update(void);
|
||||||
void update_GPS(void);
|
void update_GPS(void);
|
||||||
|
@ -8,21 +8,6 @@ void Tracker::update_ahrs()
|
|||||||
ahrs.update();
|
ahrs.update();
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
|
||||||
initialise compass's location used for declination
|
|
||||||
*/
|
|
||||||
void Tracker::init_compass_location(void)
|
|
||||||
{
|
|
||||||
// update initial location used for declination
|
|
||||||
if (!compass_init_location) {
|
|
||||||
Location loc;
|
|
||||||
if (ahrs.get_position(loc)) {
|
|
||||||
compass.set_initial_location(loc.lat, loc.lng);
|
|
||||||
compass_init_location = true;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
/*
|
/*
|
||||||
read and update compass
|
read and update compass
|
||||||
*/
|
*/
|
||||||
@ -85,11 +70,6 @@ void Tracker::update_GPS(void)
|
|||||||
if (!set_home(current_loc)) {
|
if (!set_home(current_loc)) {
|
||||||
// silently ignored
|
// silently ignored
|
||||||
}
|
}
|
||||||
|
|
||||||
if (AP::compass().enabled()) {
|
|
||||||
// Set compass declination automatically
|
|
||||||
compass.set_initial_location(gps.location().lat, gps.location().lng);
|
|
||||||
}
|
|
||||||
ground_start_count = 0;
|
ground_start_count = 0;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
Loading…
Reference in New Issue
Block a user