Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-21 16:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

Rover: Save compass offsets when disarmed and learning is on

Browse Source
This commit is contained in:
Raouf 2018-06-18 12:21:04 +09:00 committed by Randy Mackay
parent 2d5cbd2cf8
commit 9d51e87762
3 changed files with 11 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
APMrover2/APMrover2.cpp
View File

@ -79,6 +79,7 @@ const AP_Scheduler::Task Rover::scheduler_tasks[] = {
SCHED_TASK_CLASS(AP_Notify, &rover.notify, update, 50, 300),
SCHED_TASK(one_second_loop, 1, 1500),
SCHED_TASK(compass_cal_update, 50, 200),
SCHED_TASK(compass_save, 0.1, 200),
SCHED_TASK(accel_cal_update, 10, 200),
#if LOGGING_ENABLED == ENABLED
SCHED_TASK_CLASS(DataFlash_Class, &rover.DataFlash, periodic_tasks, 50, 300),
@ -273,18 +274,6 @@ void Rover::one_second_loop(void)
// cope with changes to mavlink system ID
mavlink_system.sysid = g.sysid_this_mav;
static uint8_t counter;
counter++;
// save compass offsets once a minute
if (counter >= 60) {
if (g.compass_enabled) {
compass.save_offsets();
}
counter = 0;
}
// update home position if not soft armed and gps position has
// changed. Update every 1s at most
if (!hal.util->get_soft_armed() &&

1
APMrover2/Rover.h
View File

@ -506,6 +506,7 @@ private:
void update_visual_odom();
void update_wheel_encoder();
void compass_cal_update(void);
void compass_save(void);
void accel_cal_update(void);
void read_rangefinders(void);
void init_proximity();

9
APMrover2/sensors.cpp
View File

@ -152,6 +152,15 @@ void Rover::compass_cal_update() {
}
}
// Save compass offsets
void Rover::compass_save() {
if (g.compass_enabled &&
compass.get_learn_type() >= Compass::LEARN_INTERNAL &&
!arming.is_armed()) {
compass.save_offsets();
}
}
// Accel calibration
void Rover::accel_cal_update() {