diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index 813bb26974..ce5baf070f 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/ArduSub/GCS_Mavlink.cpp @@ -863,13 +863,6 @@ void GCS_MAVLINK_Sub::handle_change_alt_request(AP_Mission::Mission_Command &cmd 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; @@ -878,40 +871,6 @@ MAV_RESULT GCS_MAVLINK_Sub::_handle_command_preflight_calibration(const mavlink_ 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); diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index 4507f58e71..da122d8cd8 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -633,7 +633,6 @@ private: bool terrain_use(); void init_ardupilot(); void startup_INS_ground(); - bool calibrate_gyros(); bool position_ok(); bool ekf_position_ok(); bool optflow_position_ok(); diff --git a/ArduSub/system.cpp b/ArduSub/system.cpp index 1d9333b61e..e119abe78c 100644 --- a/ArduSub/system.cpp +++ b/ArduSub/system.cpp @@ -230,20 +230,6 @@ void Sub::startup_INS_ground() } // calibrate gyros - returns true if successfully calibrated -bool Sub::calibrate_gyros() -{ - // gyro offset calibration - sub.ins.init_gyro(); - - // reset ahrs gyro bias - if (sub.ins.gyro_calibrated_ok_all()) { - sub.ahrs.reset_gyro_drift(); - return true; - } - - return false; -} - // position_ok - returns true if the horizontal absolute position is ok and home position is set bool Sub::position_ok() {