You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
72 lines
3.1 KiB
72 lines
3.1 KiB
/// -*- 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.get_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
|
|
|