diff --git a/ArduCopter/APM_Config.h b/ArduCopter/APM_Config.h index 94f49f88d2..35506b4bb3 100644 --- a/ArduCopter/APM_Config.h +++ b/ArduCopter/APM_Config.h @@ -39,7 +39,6 @@ // features below are disabled by default on all boards //#define SPRAYER ENABLED // enable the crop sprayer feature (two ESC controlled pumps the speed of which depends upon the vehicle's horizontal velocity) -//#define GNDEFFECT_COMPENSATION ENABLED // enable ground effect compensation for barometer (if propwash interferes with the barometer on the ground) //#define CAL_ALWAYS_REBOOT // flight controller will reboot after compass or accelerometer calibration completes //#define DISALLOW_GCS_MODE_CHANGE_DURING_RC_FAILSAFE // disable mode changes from GCS during Radio failsafes. Avoids a race condition for vehicle like Solo in which the RC and telemetry travel along the same link diff --git a/ArduCopter/ArduCopter.cpp b/ArduCopter/ArduCopter.cpp index eb4f09f11b..162fbb9a23 100644 --- a/ArduCopter/ArduCopter.cpp +++ b/ArduCopter/ArduCopter.cpp @@ -315,9 +315,8 @@ void Copter::throttle_loop() heli_update_landing_swash(); #endif -#if GNDEFFECT_COMPENSATION == ENABLED + // compensate for ground effect (if enabled) update_ground_effect_detector(); -#endif // GNDEFFECT_COMPENSATION == ENABLED } // update_mount - update camera mount position diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index c796d27ec2..3b9a1ba1c1 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -592,7 +592,6 @@ private: int16_t hover_roll_trim_scalar_slew; #endif -#if GNDEFFECT_COMPENSATION == ENABLED // ground effect detector struct { bool takeoff_expected; @@ -600,7 +599,6 @@ private: uint32_t takeoff_time_ms; float takeoff_alt_cm; } gndeffect_state; -#endif // GNDEFFECT_COMPENSATION == ENABLED static const AP_Scheduler::Task scheduler_tasks[]; static const AP_Param::Info var_info[]; @@ -927,9 +925,7 @@ private: void update_land_and_crash_detectors(); void update_land_detector(); void update_throttle_thr_mix(); -#if GNDEFFECT_COMPENSATION == ENABLED void update_ground_effect_detector(void); -#endif // GNDEFFECT_COMPENSATION == ENABLED void landinggear_update(); void update_notify(); void motor_test_output(); diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 94c61d18aa..dbe4685e00 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -988,6 +988,13 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @User: Standard AP_GROUPINFO("THROW_TYPE", 4, ParametersG2, throw_type, ThrowType_Upward), + // @Param: GND_EFFECT_COMP + // @DisplayName: Ground Effect Compensation Enable/Disable + // @Description: Ground Effect Compensation Enable/Disable + // @Values: 0:Disabled,1:Enabled + // @User: Advanced + AP_GROUPINFO("GND_EFFECT_COMP", 5, ParametersG2, gndeffect_comp_enabled, 0), + AP_GROUPEND }; diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 7d1d048ed8..7e5bc0bc90 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -554,6 +554,9 @@ public: // Throw mode parameters AP_Int8 throw_nextmode; AP_Int8 throw_type; + + // ground effect compensation enable/disable + AP_Int8 gndeffect_comp_enabled; }; extern const AP_Param::Info var_info[]; diff --git a/ArduCopter/baro_ground_effect.cpp b/ArduCopter/baro_ground_effect.cpp index 8b9fe07906..4861c6fa6e 100644 --- a/ArduCopter/baro_ground_effect.cpp +++ b/ArduCopter/baro_ground_effect.cpp @@ -1,10 +1,10 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- #include "Copter.h" -#if GNDEFFECT_COMPENSATION == ENABLED + void Copter::update_ground_effect_detector(void) { - if(!motors.armed()) { + if(!g2.gndeffect_comp_enabled || !motors.armed()) { // disarmed - disable ground effect and return gndeffect_state.takeoff_expected = false; gndeffect_state.touchdown_expected = false; @@ -69,4 +69,3 @@ void Copter::update_ground_effect_detector(void) ahrs.setTakeoffExpected(gndeffect_state.takeoff_expected); ahrs.setTouchdownExpected(gndeffect_state.touchdown_expected); } -#endif // GNDEFFECT_COMPENSATION == ENABLED diff --git a/ArduCopter/config.h b/ArduCopter/config.h index d2ae4e5213..dc984f3c1a 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -190,10 +190,6 @@ # define FS_GCS_TIMEOUT_MS 5000 // gcs failsafe triggers after 5 seconds with no GCS heartbeat #endif -#ifndef GNDEFFECT_COMPENSATION - # define GNDEFFECT_COMPENSATION DISABLED -#endif - // Radio failsafe while using RC_override #ifndef FS_RADIO_RC_OVERRIDE_TIMEOUT_MS # define FS_RADIO_RC_OVERRIDE_TIMEOUT_MS 1000 // RC Radio failsafe triggers after 1 second while using RC_override from ground station