diff --git a/ArduCopter/APM_Config.h b/ArduCopter/APM_Config.h index f79e91ca80..4b5dddc7fb 100644 --- a/ArduCopter/APM_Config.h +++ b/ArduCopter/APM_Config.h @@ -39,6 +39,7 @@ // 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 PRECISION_LANDING ENABLED // enable precision landing using companion computer or IRLock sensor +//#define GNDEFFECT_COMPENSATION ENABLED // enable ground effect compensation for barometer (if propwash interferes with the barometer on the ground) // other settings //#define THROTTLE_IN_DEADBAND 100 // redefine size of throttle deadband in pwm (0 ~ 1000) diff --git a/ArduCopter/ArduCopter.cpp b/ArduCopter/ArduCopter.cpp index 68d72f7185..93100fe509 100644 --- a/ArduCopter/ArduCopter.cpp +++ b/ArduCopter/ArduCopter.cpp @@ -319,6 +319,10 @@ void Copter::throttle_loop() // update trad heli swash plate movement heli_update_landing_swash(); #endif + +#if GNDEFFECT_COMPENSATION == 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 4c0096eca6..0da7d8ad73 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -540,6 +540,16 @@ private: } heli_flags; #endif +#if GNDEFFECT_COMPENSATION == ENABLED + // ground effect detector + struct { + bool takeoff_expected; + bool touchdown_expected; + 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[]; static const struct LogStructure log_structure[]; @@ -842,6 +852,9 @@ 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/baro_ground_effect.cpp b/ArduCopter/baro_ground_effect.cpp new file mode 100644 index 0000000000..1349126ce2 --- /dev/null +++ b/ArduCopter/baro_ground_effect.cpp @@ -0,0 +1,72 @@ +/// -*- 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()) { + // disarmed - disable ground effect and return + gndeffect_state.takeoff_expected = false; + gndeffect_state.touchdown_expected = false; + ahrs.setTakeoffExpected(gndeffect_state.takeoff_expected); + ahrs.setTouchdownExpected(gndeffect_state.touchdown_expected); + return; + } + + // variable initialization + uint32_t tnow_ms = millis(); + float xy_des_speed_cms = 0.0f; + float xy_speed_cms = 0.0f; + float des_climb_rate_cms = pos_control.get_desired_velocity().z; + + if(pos_control.is_active_xy()) { + Vector3f vel_target = pos_control.get_vel_target(); + vel_target.z = 0.0f; + xy_des_speed_cms = vel_target.length(); + } + + if(position_ok() || optflow_position_ok()) { + Vector3f vel = inertial_nav.get_velocity(); + vel.z = 0.0f; + xy_speed_cms = vel.length(); + } + + // takeoff logic + + // if we are armed and haven't yet taken off + if (motors.armed() && ap.land_complete && !gndeffect_state.takeoff_expected) { + gndeffect_state.takeoff_expected = true; + } + + // if we aren't taking off yet, reset the takeoff timer, altitude and complete flag + bool throttle_up = mode_has_manual_throttle(control_mode) && g.rc_3.control_in > 0; + if (!throttle_up && ap.land_complete) { + gndeffect_state.takeoff_time_ms = tnow_ms; + gndeffect_state.takeoff_alt_cm = inertial_nav.get_altitude(); + } + + // if we are in takeoff_expected and we meet the conditions for having taken off + // end the takeoff_expected state + if (gndeffect_state.takeoff_expected && (tnow_ms-gndeffect_state.takeoff_time_ms > 5000 || inertial_nav.get_altitude()-gndeffect_state.takeoff_alt_cm > 50.0f)) { + gndeffect_state.takeoff_expected = false; + } + + // landing logic + Vector3f angle_target_rad = attitude_control.get_att_target_euler_cd() * radians(0.01f); + bool small_angle_request = cosf(angle_target_rad.x)*cosf(angle_target_rad.y) > cosf(radians(7.5f)); + bool xy_speed_low = (position_ok() || optflow_position_ok()) && xy_speed_cms <= 125.0f; + bool xy_speed_demand_low = pos_control.is_active_xy() && xy_des_speed_cms <= 125.0f; + bool slow_horizontal = xy_speed_demand_low || (xy_speed_low && !pos_control.is_active_xy()) || (control_mode == ALT_HOLD && small_angle_request); + + bool descent_demanded = pos_control.is_active_z() && des_climb_rate_cms < 0.0f; + bool slow_descent_demanded = descent_demanded && des_climb_rate_cms >= -100.0f; + bool z_speed_low = abs(inertial_nav.get_velocity_z()) <= 60.0f; + bool slow_descent = (slow_descent_demanded || (z_speed_low && descent_demanded)); + + gndeffect_state.touchdown_expected = slow_horizontal && slow_descent; + + // Prepare the EKF for ground effect if either takeoff or touchdown is expected. + 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 6c2eacec97..9d76f8c36d 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -213,6 +213,10 @@ # 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 + // possible values for FS_GCS parameter #define FS_GCS_DISABLED 0 #define FS_GCS_ENABLED_ALWAYS_RTL 1