Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Vehicle: moved accel cal update handling from vehicles

This commit is contained in:
Andrew Tridgell 2021-08-17 12:51:35 +10:00
parent fefade783b
commit fd86cb0591
2 changed files with 34 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
libraries/AP_Vehicle/AP_Vehicle.cpp
View File

@ -242,6 +242,7 @@ const AP_Scheduler::Task AP_Vehicle::scheduler_tasks[] = {
#if OSD_ENABLED
SCHED_TASK(publish_osd_info, 1, 10),
#endif
SCHED_TASK(accel_cal_update, 10, 100),
};
void AP_Vehicle::get_common_scheduler_tasks(const AP_Scheduler::Task*& tasks, uint8_t& num_tasks)
@ -429,6 +430,36 @@ void AP_Vehicle::get_osd_roll_pitch_rad(float &roll, float &pitch) const
#endif
#ifndef HAL_CAL_ALWAYS_REBOOT
// allow for forced reboot after accelcal
#define HAL_CAL_ALWAYS_REBOOT 0
#endif
/*
update accel cal
*/
void AP_Vehicle::accel_cal_update()
{
if (hal.util->get_soft_armed()) {
return;
}
ins.acal_update();
// check if new trim values, and set them
Vector3f trim_rad;
if (ins.get_new_trim(trim_rad)) {
ahrs.set_trim(trim_rad);
}
#if HAL_CAL_ALWAYS_REBOOT
if (ins.accel_cal_requires_reboot() &&
!hal.util->get_soft_armed()) {
hal.scheduler->delay(1000);
hal.scheduler->reboot(false);
}
#endif
}
AP_Vehicle *AP_Vehicle::_singleton = nullptr;
AP_Vehicle *AP_Vehicle::get_singleton()

3
libraries/AP_Vehicle/AP_Vehicle.h
View File

@ -358,6 +358,9 @@ protected:
void publish_osd_info();
#endif
// update accel calibration
void accel_cal_update();
ModeReason control_mode_reason = ModeReason::UNKNOWN;
private: