mirror of https://github.com/ArduPilot/ardupilot
Tracker: moved accel cal update to vehicle code
This commit is contained in:
parent
5dcfa95444
commit
39b4825442
|
@ -52,7 +52,6 @@ const AP_Scheduler::Task Tracker::scheduler_tasks[] = {
|
|||
SCHED_TASK(one_second_loop, 1, 3900),
|
||||
SCHED_TASK_CLASS(Compass, &tracker.compass, cal_update, 50, 100),
|
||||
SCHED_TASK(stats_update, 1, 200),
|
||||
SCHED_TASK(accel_cal_update, 10, 100)
|
||||
};
|
||||
|
||||
void Tracker::get_scheduler_tasks(const AP_Scheduler::Task *&tasks,
|
||||
|
|
|
@ -194,7 +194,6 @@ private:
|
|||
void update_ahrs();
|
||||
void compass_save();
|
||||
void update_compass(void);
|
||||
void accel_cal_update(void);
|
||||
void update_GPS(void);
|
||||
void handle_battery_failsafe(const char* type_str, const int8_t action);
|
||||
|
||||
|
|
|
@ -25,20 +25,6 @@ void Tracker::compass_save() {
|
|||
}
|
||||
}
|
||||
|
||||
/*
|
||||
Accel calibration
|
||||
*/
|
||||
void Tracker::accel_cal_update() {
|
||||
if (hal.util->get_soft_armed()) {
|
||||
return;
|
||||
}
|
||||
ins.acal_update();
|
||||
float trim_roll, trim_pitch;
|
||||
if (ins.get_new_trim(trim_roll, trim_pitch)) {
|
||||
ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0));
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
read the GPS
|
||||
*/
|
||||
|
|
Loading…
Reference in New Issue