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

Plane: factor out preflight calibration function

This commit is contained in:
Peter Barker 2018-03-17 20:26:03 +11:00 committed by Francisco Ferreira
parent f6b7ca75e9
commit 91c2ece7a2
2 changed files with 86 additions and 94 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

177
ArduPlane/GCS_Mavlink.cpp
View File

@ -839,6 +839,89 @@ void GCS_MAVLINK_Plane::handle_change_alt_request(AP_Mission::Mission_Command &c
plane.reset_offset_altitude(); plane.reset_offset_altitude();
} }
MAV_RESULT GCS_MAVLINK_Plane::handle_command_preflight_calibration(const mavlink_command_long_t &packet)
{
plane.in_calibration = true;
MAV_RESULT ret = GCS_MAVLINK::handle_command_preflight_calibration(packet);
plane.in_calibration = false;
return ret;
}
MAV_RESULT GCS_MAVLINK_Plane::_handle_command_preflight_calibration(const mavlink_command_long_t &packet)
{
if (is_equal(packet.param1,1.0f)) {
/*
gyro calibration
*/
plane.ins.init_gyro();
if (!plane.ins.gyro_calibrated_ok_all()) {
return MAV_RESULT_FAILED;
}
plane.ahrs.reset_gyro_drift();
return MAV_RESULT_ACCEPTED;
}
if (is_equal(packet.param3,1.0f)) {
/*
baro and airspeed calibration
*/
plane.init_barometer(false);
if (plane.airspeed.enabled()) {
plane.zero_airspeed(false);
}
return MAV_RESULT_ACCEPTED;
}
if (is_equal(packet.param4,1.0f)) {
/*
radio trim
*/
plane.trim_radio();
return MAV_RESULT_ACCEPTED;
}
if (is_equal(packet.param5,1.0f)) {
/*
accel calibration
*/
// start with gyro calibration
plane.ins.init_gyro();
// reset ahrs gyro bias
if (!plane.ins.gyro_calibrated_ok_all()) {
return MAV_RESULT_FAILED;
}
plane.ahrs.reset_gyro_drift();
plane.ins.acal_init();
plane.ins.get_acal()->start(this);
return MAV_RESULT_ACCEPTED;
}
if (is_equal(packet.param5,2.0f)) {
/*
ahrs trim
*/
// start with gyro calibration
plane.ins.init_gyro();
// accel trim
float trim_roll, trim_pitch;
if(!plane.ins.calibrate_trim(trim_roll, trim_pitch)) {
return MAV_RESULT_FAILED;
}
// reset ahrs's trim to suggested values from calibration routine
plane.ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0));
return MAV_RESULT_ACCEPTED;
}
if (is_equal(packet.param5,4.0f)) {
// simple accel calibration
return plane.ins.simple_accel_cal(plane.ahrs);
}
return GCS_MAVLINK::_handle_command_preflight_calibration(packet);
}
void GCS_MAVLINK_Plane::packetReceived(const mavlink_status_t &status, void GCS_MAVLINK_Plane::packetReceived(const mavlink_status_t &status,
mavlink_message_t &msg) mavlink_message_t &msg)
{ {
@ -1076,100 +1159,6 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg)
result = MAV_RESULT_ACCEPTED; result = MAV_RESULT_ACCEPTED;
break; break;
case MAV_CMD_PREFLIGHT_CALIBRATION:
plane.in_calibration = true;
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();
if (plane.ins.gyro_calibrated_ok_all()) {
plane.ahrs.reset_gyro_drift();
result = MAV_RESULT_ACCEPTED;
} else {
result = MAV_RESULT_FAILED;
}
} 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);
if (plane.airspeed.enabled()) {
plane.zero_airspeed(false);
}
result = MAV_RESULT_ACCEPTED;
} 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();
result = MAV_RESULT_ACCEPTED;
} 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;
// start with gyro calibration
plane.ins.init_gyro();
// reset ahrs gyro bias
if (plane.ins.gyro_calibrated_ok_all()) {
plane.ahrs.reset_gyro_drift();
} else {
result = MAV_RESULT_FAILED;
}
plane.ins.acal_init();
plane.ins.get_acal()->start(this);
} 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
plane.ins.init_gyro();
// accel trim
float trim_roll, trim_pitch;
if(plane.ins.calibrate_trim(trim_roll, trim_pitch)) {
// reset ahrs's trim to suggested values from calibration routine
plane.ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0));
result = MAV_RESULT_ACCEPTED;
} else {
result = MAV_RESULT_FAILED;
}
} else if (is_equal(packet.param5,4.0f)) {
// simple accel calibration
result = plane.ins.simple_accel_cal(plane.ahrs);
}
else {
send_text(MAV_SEVERITY_WARNING, "Unsupported preflight calibration");
}
plane.in_calibration = false;
break;
case MAV_CMD_COMPONENT_ARM_DISARM: case MAV_CMD_COMPONENT_ARM_DISARM:
if (is_equal(packet.param1,1.0f)) { if (is_equal(packet.param1,1.0f)) {
// run pre_arm_checks and arm_checks and display failures // run pre_arm_checks and arm_checks and display failures

3
ArduPlane/GCS_Mavlink.h
View File

@ -33,6 +33,9 @@ protected:
bool set_mode(uint8_t mode) override; bool set_mode(uint8_t mode) override;
MAV_RESULT handle_command_preflight_calibration(const mavlink_command_long_t &packet) override;
MAV_RESULT _handle_command_preflight_calibration(const mavlink_command_long_t &packet) override;
private: private:
void handleMessage(mavlink_message_t * msg) override; void handleMessage(mavlink_message_t * msg) override;