Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

Tracker: calibrate gyros on accel cal and trim

Browse Source
This commit is contained in:
Randy Mackay 2015-09-21 12:10:19 +09:00 committed by Grant Morphett
parent 79b8dbb176
commit ecda56006c
1 changed files with 8 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
AntennaTracker/GCS_Mavlink.cpp
View File

@ -613,11 +613,19 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
} else if (is_equal(packet.param5,1.0f)) {
float trim_roll, trim_pitch;
AP_InertialSensor_UserInteract_MAVLink interact(this);
// start with gyro calibration
tracker.ins.init_gyro();
// reset ahrs gyro bias
if (tracker.ins.gyro_calibrated_ok_all()) {
tracker.ahrs.reset_gyro_drift();
}
if(tracker.ins.calibrate_accel(&interact, trim_roll, trim_pitch)) {
// reset ahrs's trim to suggested values from calibration routine
tracker.ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0));
}
} else if (is_equal(packet.param5,2.0f)) {
// start with gyro calibration
tracker.ins.init_gyro();
// accel trim
float trim_roll, trim_pitch;
if(tracker.ins.calibrate_trim(trim_roll, trim_pitch)) {