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

Rover: create a preflight calibration function 

Some functionality has moved up.
This commit is contained in:
Peter Barker 2018-03-17 19:19:30 +11:00 committed by Francisco Ferreira
parent 09d4e36970
commit 762b31b147
2 changed files with 42 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

92
APMrover2/GCS_Mavlink.cpp
View File

@ -639,6 +639,46 @@ void GCS_MAVLINK_Rover::handle_change_alt_request(AP_Mission::Mission_Command &c
// nothing to do
}
MAV_RESULT GCS_MAVLINK_Rover::_handle_command_preflight_calibration(const mavlink_command_long_t &packet)
{
if (is_equal(packet.param4, 1.0f)) {
rover.trim_radio();
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);
}
void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
{
switch (msg->msgid) {
@ -794,58 +834,6 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
result = MAV_RESULT_ACCEPTED;
break;
case MAV_CMD_PREFLIGHT_CALIBRATION:
if (hal.util->get_soft_armed()) {
result = MAV_RESULT_FAILED;
break;
}
if (is_equal(packet.param1, 1.0f)) {
rover.ins.init_gyro();
if (rover.ins.gyro_calibrated_ok_all()) {
rover.ahrs.reset_gyro_drift();
result = MAV_RESULT_ACCEPTED;
} else {
result = MAV_RESULT_FAILED;
}
} else if (is_equal(packet.param3, 1.0f)) {
rover.init_barometer(false);
result = MAV_RESULT_ACCEPTED;
} else if (is_equal(packet.param4, 1.0f)) {
rover.trim_radio();
result = MAV_RESULT_ACCEPTED;
} else if (is_equal(packet.param5, 1.0f)) {
result = MAV_RESULT_ACCEPTED;
// start with gyro calibration
rover.ins.init_gyro();
// reset ahrs gyro bias
if (rover.ins.gyro_calibrated_ok_all()) {
rover.ahrs.reset_gyro_drift();
} else {
result = MAV_RESULT_FAILED;
}
rover.ins.acal_init();
rover.ins.get_acal()->start(this);
} else 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)) {
// reset ahrs's trim to suggested values from calibration routine
rover.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 = rover.ins.simple_accel_cal(rover.ahrs);
} else {
send_text(MAV_SEVERITY_WARNING, "Unsupported preflight calibration");
}
break;
case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
if (is_equal(packet.param1, 1.0f) || is_equal(packet.param1, 3.0f)) {
// when packet.param1 == 3 we reboot to hold in bootloader

2
APMrover2/GCS_Mavlink.h
View File

@ -30,6 +30,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;