diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 652a3dc3b1..43b3798e5a 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -849,20 +849,6 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_preflight_calibration(const mavlink return ret; } -MAV_RESULT GCS_MAVLINK_Plane::_handle_command_preflight_calibration_baro() -{ - /* - baro and airspeed calibration - */ - MAV_RESULT ret = GCS_MAVLINK::_handle_command_preflight_calibration_baro(); - if (ret == MAV_RESULT_ACCEPTED) { - if (plane.airspeed.enabled()) { - plane.zero_airspeed(false); - } - } - return ret; -} - MAV_RESULT GCS_MAVLINK_Plane::_handle_command_preflight_calibration(const mavlink_command_long_t &packet) { if (is_equal(packet.param4,1.0f)) { diff --git a/ArduPlane/GCS_Mavlink.h b/ArduPlane/GCS_Mavlink.h index 6277491d07..2f05769bf5 100644 --- a/ArduPlane/GCS_Mavlink.h +++ b/ArduPlane/GCS_Mavlink.h @@ -33,7 +33,6 @@ protected: bool set_mode(uint8_t mode) override; - MAV_RESULT _handle_command_preflight_calibration_baro() 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; diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 988b05e91a..5e2c51447d 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -929,7 +929,6 @@ private: void init_rangefinder(void); void read_rangefinder(void); void read_airspeed(void); - void zero_airspeed(bool in_startup); void read_receiver_rssi(void); void rpm_update(void); void button_update(void); diff --git a/ArduPlane/sensors.cpp b/ArduPlane/sensors.cpp index 0d519de436..4ba806c15b 100644 --- a/ArduPlane/sensors.cpp +++ b/ArduPlane/sensors.cpp @@ -96,15 +96,6 @@ void Plane::read_airspeed(void) } } -void Plane::zero_airspeed(bool in_startup) -{ - airspeed.calibrate(in_startup); - read_airspeed(); - // update barometric calibration with new airspeed supplied temperature - barometer.update_calibration(); - gcs().send_text(MAV_SEVERITY_INFO,"Airspeed calibration started"); -} - // read the receiver RSSI as an 8 bit number for MAVLink // RC_CHANNELS_SCALED message void Plane::read_receiver_rssi(void) diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index 08ca2e392a..1597d4302a 100644 --- a/ArduPlane/system.cpp +++ b/ArduPlane/system.cpp @@ -592,7 +592,7 @@ void Plane::startup_INS_ground(void) if (airspeed.enabled()) { // initialize airspeed sensor // -------------------------- - zero_airspeed(true); + airspeed.calibrate(true); } else { gcs().send_text(MAV_SEVERITY_WARNING,"No airspeed"); }