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

Sub: factor out preflight calibration function

Browse Source
This commit is contained in:
Peter Barker 2018-03-17 22:35:01 +11:00 committed by Francisco Ferreira
parent 91c2ece7a2
commit fe293abf37
2 changed files with 63 additions and 57 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

118
ArduSub/GCS_Mavlink.cpp
View File

@ -861,6 +861,67 @@ void GCS_MAVLINK_Sub::handle_change_alt_request(AP_Mission::Mission_Command &cmd
// To-Do: update target altitude for loiter or waypoint controller depending upon nav mode
}
MAV_RESULT GCS_MAVLINK_Sub::_handle_command_preflight_calibration(const mavlink_command_long_t &packet)
{
if (is_equal(packet.param1,1.0f)) {
if (!sub.calibrate_gyros()) {
return MAV_RESULT_FAILED;
}
return MAV_RESULT_ACCEPTED;
}
if (is_equal(packet.param3,1.0f)) {
if (!sub.sensor_health.depth || sub.motors.armed()) {
return MAV_RESULT_FAILED;
}
sub.init_barometer(true);
return MAV_RESULT_ACCEPTED;
}
if (is_equal(packet.param4,1.0f)) {
return MAV_RESULT_UNSUPPORTED;
}
if (is_equal(packet.param5,1.0f)) {
// 3d accel calibration
if (!sub.calibrate_gyros()) {
return MAV_RESULT_FAILED;
}
sub.ins.acal_init();
sub.ins.get_acal()->start(this);
return MAV_RESULT_ACCEPTED;
}
if (is_equal(packet.param5,2.0f)) {
// calibrate gyros
if (!sub.calibrate_gyros()) {
return MAV_RESULT_FAILED;
}
// accel trim
float trim_roll, trim_pitch;
if (!sub.ins.calibrate_trim(trim_roll, trim_pitch)) {
return MAV_RESULT_FAILED;
}
// reset ahrs's trim to suggested values from calibration routine
sub.ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0));
return MAV_RESULT_ACCEPTED;
}
if (is_equal(packet.param5,4.0f)) {
// simple accel calibration
return sub.ins.simple_accel_cal(sub.ahrs);
}
if (is_equal(packet.param6,1.0f)) {
// compassmot calibration
//result = sub.mavlink_compassmot(chan);
gcs().send_text(MAV_SEVERITY_INFO, "#CompassMot calibration not supported");
return MAV_RESULT_UNSUPPORTED;
}
return GCS_MAVLINK::_handle_command_preflight_calibration(packet);
}
void GCS_MAVLINK_Sub::handleMessage(mavlink_message_t* msg)
{
MAV_RESULT result = MAV_RESULT_FAILED; // assume failure. Each messages id is responsible for return ACK or NAK if required
@ -1124,63 +1185,6 @@ void GCS_MAVLINK_Sub::handleMessage(mavlink_message_t* msg)
}
break;
case MAV_CMD_PREFLIGHT_CALIBRATION:
// exit immediately if armed
if (sub.motors.armed()) {
result = MAV_RESULT_FAILED;
break;
}
if (is_equal(packet.param1,1.0f)) {
if (sub.calibrate_gyros()) {
result = MAV_RESULT_ACCEPTED;
} else {
result = MAV_RESULT_FAILED;
}
} else if (is_equal(packet.param3,1.0f)) {
if (!sub.sensor_health.depth || sub.motors.armed()) {
result = MAV_RESULT_FAILED;
} else {
sub.init_barometer(true);
result = MAV_RESULT_ACCEPTED;
}
} else if (is_equal(packet.param4,1.0f)) {
result = MAV_RESULT_UNSUPPORTED;
} else if (is_equal(packet.param5,1.0f)) {
// 3d accel calibration
result = MAV_RESULT_ACCEPTED;
if (!sub.calibrate_gyros()) {
result = MAV_RESULT_FAILED;
break;
}
sub.ins.acal_init();
sub.ins.get_acal()->start(this);
} else if (is_equal(packet.param5,2.0f)) {
// calibrate gyros
if (!sub.calibrate_gyros()) {
result = MAV_RESULT_FAILED;
break;
}
// accel trim
float trim_roll, trim_pitch;
if (sub.ins.calibrate_trim(trim_roll, trim_pitch)) {
// reset ahrs's trim to suggested values from calibration routine
sub.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 = sub.ins.simple_accel_cal(sub.ahrs);
} else if (is_equal(packet.param6,1.0f)) {
// compassmot calibration
//result = sub.mavlink_compassmot(chan);
gcs().send_text(MAV_SEVERITY_INFO, "#CompassMot calibration not supported");
result = MAV_RESULT_UNSUPPORTED;
}
break;
case MAV_CMD_COMPONENT_ARM_DISARM:
if (is_equal(packet.param1,1.0f)) {
// attempt to arm and return success or failure

2
ArduSub/GCS_Mavlink.h
View File

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