Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-08 08:53:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: allow non-blocking calibrations while armed

Browse Source
This commit is contained in:
Andrew Tridgell 2016-05-24 15:52:28 +10:00
parent 0f50f483b8
commit 6e4c0dd3d7
1 changed files with 40 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

44
ArduPlane/GCS_Mavlink.cpp
View File

@ -1333,12 +1333,16 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break; break;
case MAV_CMD_PREFLIGHT_CALIBRATION: case MAV_CMD_PREFLIGHT_CALIBRATION:
if(hal.util->get_soft_armed()) {
result = MAV_RESULT_FAILED;
break;
}
plane.in_calibration = true; plane.in_calibration = true;
if (is_equal(packet.param1,1.0f)) { if (is_equal(packet.param1,1.0f)) {
/*
gyro calibration
*/
if (hal.util->get_soft_armed()) {
send_text(MAV_SEVERITY_WARNING, "No calibration while armed");
result = MAV_RESULT_FAILED;
break;
}
plane.ins.init_gyro(); plane.ins.init_gyro();
if (plane.ins.gyro_calibrated_ok_all()) { if (plane.ins.gyro_calibrated_ok_all()) {
plane.ahrs.reset_gyro_drift(); plane.ahrs.reset_gyro_drift();
@ -1347,15 +1351,39 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
result = MAV_RESULT_FAILED; result = MAV_RESULT_FAILED;
} }
} else if (is_equal(packet.param3,1.0f)) { } else if (is_equal(packet.param3,1.0f)) {
/*
baro and airspeed calibration
*/
if (hal.util->get_soft_armed() && plane.is_flying()) {
send_text(MAV_SEVERITY_WARNING, "No calibration while flying");
result = MAV_RESULT_FAILED;
break;
}
plane.init_barometer(false); plane.init_barometer(false);
if (plane.airspeed.enabled()) { if (plane.airspeed.enabled()) {
plane.zero_airspeed(false); plane.zero_airspeed(false);
} }
result = MAV_RESULT_ACCEPTED; result = MAV_RESULT_ACCEPTED;
} else if (is_equal(packet.param4,1.0f)) { } else if (is_equal(packet.param4,1.0f)) {
/*
radio trim
*/
if (hal.util->get_soft_armed()) {
send_text(MAV_SEVERITY_WARNING, "No calibration while armed");
result = MAV_RESULT_FAILED;
break;
}
plane.trim_radio(); plane.trim_radio();
result = MAV_RESULT_ACCEPTED; result = MAV_RESULT_ACCEPTED;
} else if (is_equal(packet.param5,1.0f)) { } else if (is_equal(packet.param5,1.0f)) {
/*
accel calibration
*/
if (hal.util->get_soft_armed()) {
send_text(MAV_SEVERITY_WARNING, "No calibration while armed");
result = MAV_RESULT_FAILED;
break;
}
result = MAV_RESULT_ACCEPTED; result = MAV_RESULT_ACCEPTED;
// start with gyro calibration // start with gyro calibration
plane.ins.init_gyro(); plane.ins.init_gyro();
@ -1369,6 +1397,14 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
plane.ins.get_acal()->start(this); plane.ins.get_acal()->start(this);
} else if (is_equal(packet.param5,2.0f)) { } else if (is_equal(packet.param5,2.0f)) {
/*
ahrs trim
*/
if (hal.util->get_soft_armed()) {
send_text(MAV_SEVERITY_WARNING, "No calibration while armed");
result = MAV_RESULT_FAILED;
break;
}
// start with gyro calibration // start with gyro calibration
plane.ins.init_gyro(); plane.ins.init_gyro();
// accel trim // accel trim