aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Rover: move common calibration functions up

Browse Source
This commit is contained in:
Peter Barker 2018-03-17 22:56:07 +11:00 committed by Francisco Ferreira
parent d878690d00
commit b407a4eed8
1 changed files with 0 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
APMrover2/GCS_Mavlink.cpp
View File

@ -646,36 +646,6 @@ MAV_RESULT GCS_MAVLINK_Rover::_handle_command_preflight_calibration(const mavlin
return MAV_RESULT_ACCEPTED;
}
if (is_equal(packet.param5, 1.0f)) {
// start with gyro calibration
rover.ins.init_gyro();
if (!rover.ins.gyro_calibrated_ok_all()) {
return MAV_RESULT_FAILED;
}
// reset ahrs gyro bias
rover.ahrs.reset_gyro_drift();
rover.ins.acal_init();
rover.ins.get_acal()->start(this);
return MAV_RESULT_ACCEPTED;
}
if (is_equal(packet.param5, 2.0f)) {
// start with gyro calibration
rover.ins.init_gyro();
// accel trim
float trim_roll, trim_pitch;
if (!rover.ins.calibrate_trim(trim_roll, trim_pitch)) {
return MAV_RESULT_FAILED;
}
// reset ahrs's trim to suggested values from calibration routine
rover.ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0));
return MAV_RESULT_ACCEPTED;
}
if (is_equal(packet.param5,4.0f)) {
// simple accel calibration
return rover.ins.simple_accel_cal(rover.ahrs);
}
return GCS_MAVLINK::_handle_command_preflight_calibration(packet);
}