diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index 60056b6fcc7a5b74f34aa2ce8f58c2899ac7a550..2cede18da01164586f91d573ecf60903c02c5bfd 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -1083,7 +1083,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) if (packet.param1 == 1 || packet.param2 == 1 || packet.param3 == 1) { - startup_IMU_ground(); + startup_IMU_ground(true); } if (packet.param4 == 1) { trim_radio(); @@ -1188,7 +1188,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) case MAV_ACTION_CALIBRATE_ACC: case MAV_ACTION_CALIBRATE_PRESSURE: case MAV_ACTION_REBOOT: // this is a rough interpretation - startup_IMU_ground(); + startup_IMU_ground(true); result=1; break; diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index f508e6c38d54184aa315cfcf4b4442848b9b34b5..5519f0cd2253bfc3f01ebccacc811037c59d1d5d 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -49,6 +49,7 @@ public: k_param_num_resets, k_param_log_last_filenumber, // *** Deprecated - remove with next eeprom number change k_param_reset_switch_chan, + k_param_manual_level, // 110: Telemetry control @@ -300,6 +301,7 @@ public: AP_Int16 log_bitmask; AP_Int16 log_last_filenumber; // *** Deprecated - remove with next eeprom number change AP_Int8 reset_switch_chan; + AP_Int8 manual_level; AP_Int16 airspeed_cruise; AP_Int16 min_gndspeed; AP_Int16 pitch_trim; diff --git a/ArduPlane/Parameters.pde b/ArduPlane/Parameters.pde index 4b0bda9ae017503a2b418688196d93788510997a..3619095c056f38a6ae182a93e0e74a9c1e8072cf 100644 --- a/ArduPlane/Parameters.pde +++ b/ArduPlane/Parameters.pde @@ -23,6 +23,7 @@ static const AP_Param::Info var_info[] PROGMEM = { GSCALAR(kff_rudder_mix, "KFF_RDDRMIX"), GSCALAR(kff_pitch_to_throttle, "KFF_PTCH2THR"), GSCALAR(kff_throttle_to_pitch, "KFF_THR2PTCH"), + GSCALAR(manual_level, "MANUAL_LEVEL"), GSCALAR(crosstrack_gain, "XTRK_GAIN_SC"), GSCALAR(crosstrack_entry_angle, "XTRK_ANGLE_CD"), diff --git a/ArduPlane/system.pde b/ArduPlane/system.pde index 0dc50c54d9908608d1974b06464eceea69308ae6..72584598fa8edad71e59fae4dfc8bef4a281ee4a 100644 --- a/ArduPlane/system.pde +++ b/ArduPlane/system.pde @@ -318,7 +318,7 @@ static void startup_ground(void) //IMU ground start //------------------------ // - startup_IMU_ground(); + startup_IMU_ground(false); // read the radio to set trims // --------------------------- @@ -441,7 +441,7 @@ static void check_short_failsafe() } -static void startup_IMU_ground(void) +static void startup_IMU_ground(bool force_accel_level) { #if HIL_MODE != HIL_MODE_ATTITUDE gcs_send_text_P(SEVERITY_MEDIUM, PSTR("Warming up ADC...")); @@ -454,7 +454,12 @@ static void startup_IMU_ground(void) mavlink_delay(1000); imu.init(IMU::COLD_START, mavlink_delay, flash_leds, &timer_scheduler); - imu.init_accel(mavlink_delay, flash_leds); + if (force_accel_level || g.manual_level == 0) { + // when MANUAL_LEVEL is set to 1 we don't do accelerometer + // levelling on each boot, and instead rely on the user to do + // it once via the ground station + imu.init_accel(mavlink_delay, flash_leds); + } ahrs.set_centripetal(1); ahrs.reset();