From 6da53ae3b2a01c59c81c55c52a346eebcd5939c9 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 17 Sep 2015 21:32:06 +0900 Subject: [PATCH] Plane: replace SKIP_GYRO_CAL with INS_GYR_CAL Also calibrate gyros with accel cal and set trim --- ArduPlane/GCS_Mavlink.cpp | 13 ++++++++----- ArduPlane/Parameters.cpp | 7 ------- ArduPlane/Parameters.h | 4 +--- ArduPlane/system.cpp | 16 ++++------------ ArduPlane/test.cpp | 6 ++---- 5 files changed, 15 insertions(+), 31 deletions(-) diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index e4ea43710e..fbde1d3fc5 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -237,7 +237,7 @@ void Plane::send_extended_status1(mavlink_channel_t chan) control_sensors_health |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW; } #endif - if (!ins.get_gyro_health_all() || (!g.skip_gyro_cal && !ins.gyro_calibrated_ok_all())) { + if (!ins.get_gyro_health_all() || !ins.gyro_calibrated_ok_all()) { control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_3D_GYRO; } if (!ins.get_accel_health_all()) { @@ -1219,10 +1219,11 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) } else if (is_equal(packet.param5,1.0f)) { float trim_roll, trim_pitch; AP_InertialSensor_UserInteract_MAVLink interact(this); - if (plane.g.skip_gyro_cal) { - // start with gyro calibration, otherwise if the user - // has SKIP_GYRO_CAL=1 they don't get to do it - plane.ins.init_gyro(); + // start with gyro calibration + plane.ins.init_gyro(); + // reset ahrs gyro bias + if (plane.ins.gyro_calibrated_ok_all()) { + plane.ahrs.reset_gyro_drift(); } if(plane.ins.calibrate_accel(&interact, trim_roll, trim_pitch)) { // reset ahrs's trim to suggested values from calibration routine @@ -1232,6 +1233,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) result = MAV_RESULT_FAILED; } } else if (is_equal(packet.param5,2.0f)) { + // start with gyro calibration + plane.ins.init_gyro(); // accel trim float trim_roll, trim_pitch; if(plane.ins.calibrate_trim(trim_roll, trim_pitch)) { diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index cec5897102..53d124d05d 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -128,13 +128,6 @@ const AP_Param::Info Plane::var_info[] PROGMEM = { // @User: Advanced GSCALAR(stick_mixing, "STICK_MIXING", STICK_MIXING_FBW), - // @Param: SKIP_GYRO_CAL - // @DisplayName: Skip gyro calibration - // @Description: When enabled this tells the APM to skip the normal gyroscope calibration at startup, and instead use the saved gyro calibration from the last flight. You should only enable this if you are careful to check that your aircraft has good attitude control before flying, as some boards may have significantly different gyro calibration between boots, especially if the temperature changes a lot. If gyro calibration is skipped then APM relies on using the gyro drift detection code to get the right gyro calibration in the few minutes after it boots. This option is mostly useful where the requirement to hold the plane still while it is booting is a significant problem. - // @Values: 0:Disabled,1:Enabled - // @User: Advanced - GSCALAR(skip_gyro_cal, "SKIP_GYRO_CAL", 0), - // @Param: AUTO_FBW_STEER // @DisplayName: Use FBWA steering in AUTO // @Description: When enabled this option gives FBWA navigation and steering in AUTO mode. This can be used to allow manual stabilised piloting with waypoint logic for triggering payloads. With this enabled the pilot has the same control over the plane as in FBWA mode, and the normal AUTO navigation is completely disabled. This option is not recommended for normal use. diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 52753d6531..2911b7d761 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -91,7 +91,7 @@ public: k_param_scheduler, k_param_relay, k_param_takeoff_throttle_delay, - k_param_skip_gyro_cal, + k_param_skip_gyro_cal, // unused k_param_auto_fbw_steer, k_param_waypoint_max_radius, k_param_ground_steer_alt, @@ -363,8 +363,6 @@ public: // attitude controller type. AP_Int8 att_controller; - // skip gyro calibration - AP_Int8 skip_gyro_cal; AP_Int8 auto_fbw_steer; // Estimation diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index 9d6e94b3f4..7a66416526 100644 --- a/ArduPlane/system.cpp +++ b/ArduPlane/system.cpp @@ -276,7 +276,7 @@ void Plane::startup_ground(void) // Makes the servos wiggle // step 1 = 1 wiggle // ----------------------- - if (!g.skip_gyro_cal) { + if (ins.gyro_calibration_timing() != AP_InertialSensor::GYRO_CAL_NEVER) { demo_servos(1); } @@ -300,7 +300,7 @@ void Plane::startup_ground(void) // Makes the servos wiggle - 3 times signals ready to fly // ----------------------- - if (!g.skip_gyro_cal) { + if (ins.gyro_calibration_timing() != AP_InertialSensor::GYRO_CAL_NEVER) { demo_servos(3); } @@ -566,15 +566,7 @@ void Plane::startup_INS_ground(void) } #endif - AP_InertialSensor::Start_style style; - if (g.skip_gyro_cal) { - style = AP_InertialSensor::WARM_START; - arming.set_skip_gyro_cal(true); - } else { - style = AP_InertialSensor::COLD_START; - } - - if (style == AP_InertialSensor::COLD_START) { + if (ins.gyro_calibration_timing() != AP_InertialSensor::GYRO_CAL_NEVER) { gcs_send_text_P(MAV_SEVERITY_ALERT, PSTR("Beginning INS calibration; do not move plane")); hal.scheduler->delay(100); } @@ -584,7 +576,7 @@ void Plane::startup_INS_ground(void) ahrs.set_vehicle_class(AHRS_VEHICLE_FIXED_WING); ahrs.set_wind_estimation(true); - ins.init(style, ins_sample_rate); + ins.init(ins_sample_rate); ahrs.reset(); // read Baro pressure at ground diff --git a/ArduPlane/test.cpp b/ArduPlane/test.cpp index 662ec5579c..6a81903f5d 100644 --- a/ArduPlane/test.cpp +++ b/ArduPlane/test.cpp @@ -373,8 +373,7 @@ int8_t Plane::test_ins(uint8_t argc, const Menu::arg *argv) ahrs.set_fly_forward(true); ahrs.set_wind_estimation(true); - ins.init(AP_InertialSensor::COLD_START, - ins_sample_rate); + ins.init(ins_sample_rate); ahrs.reset(); print_hit_enter(); @@ -435,8 +434,7 @@ int8_t Plane::test_mag(uint8_t argc, const Menu::arg *argv) ahrs.set_compass(&compass); // we need the AHRS initialised for this test - ins.init(AP_InertialSensor::COLD_START, - ins_sample_rate); + ins.init(ins_sample_rate); ahrs.reset(); uint16_t counter = 0;