3 void Copter::update_ground_effect_detector(void)
5 if(!g2
.gndeffect_comp_enabled
|| !motors
->armed()) {
6 // disarmed - disable ground effect and return
7 gndeffect_state
.takeoff_expected
= false;
8 gndeffect_state
.touchdown_expected
= false;
9 ahrs
.set_takeoff_expected(gndeffect_state
.takeoff_expected
);
10 ahrs
.set_touchdown_expected(gndeffect_state
.touchdown_expected
);
14 // variable initialization
15 uint32_t tnow_ms
= millis();
16 float xy_des_speed_cms
= 0.0f
;
17 float xy_speed_cms
= 0.0f
;
18 float des_climb_rate_cms
= pos_control
->get_vel_desired_cms().z
;
20 if (pos_control
->is_active_xy()) {
21 Vector3f vel_target
= pos_control
->get_vel_target_cms();
23 xy_des_speed_cms
= vel_target
.length();
26 if (position_ok() || ekf_has_relative_position()) {
27 Vector3f vel
= inertial_nav
.get_velocity_neu_cms();
29 xy_speed_cms
= vel
.length();
34 if (flightmode
->mode_number() == Mode::Number::THROW
) {
35 // throw mode never wants the takeoff expected EKF code
36 gndeffect_state
.takeoff_expected
= false;
37 } else if (motors
->armed() && ap
.land_complete
) {
38 // if we are armed and haven't yet taken off then we expect an imminent takeoff
39 gndeffect_state
.takeoff_expected
= true;
42 // if we aren't taking off yet, reset the takeoff timer, altitude and complete flag
43 const bool throttle_up
= flightmode
->has_manual_throttle() && channel_throttle
->get_control_in() > 0;
44 if (!throttle_up
&& ap
.land_complete
) {
45 gndeffect_state
.takeoff_time_ms
= tnow_ms
;
46 gndeffect_state
.takeoff_alt_cm
= inertial_nav
.get_position_z_up_cm();
49 // if we are in takeoff_expected and we meet the conditions for having taken off
50 // end the takeoff_expected state
51 if (gndeffect_state
.takeoff_expected
&& (tnow_ms
-gndeffect_state
.takeoff_time_ms
> 5000 || inertial_nav
.get_position_z_up_cm()-gndeffect_state
.takeoff_alt_cm
> 50.0f
)) {
52 gndeffect_state
.takeoff_expected
= false;
56 Vector3f angle_target_rad
= attitude_control
->get_att_target_euler_cd() * radians(0.01f
);
57 bool small_angle_request
= cosf(angle_target_rad
.x
)*cosf(angle_target_rad
.y
) > cosf(radians(7.5f
));
58 bool xy_speed_low
= (position_ok() || ekf_has_relative_position()) && xy_speed_cms
<= 125.0f
;
59 bool xy_speed_demand_low
= pos_control
->is_active_xy() && xy_des_speed_cms
<= 125.0f
;
60 bool slow_horizontal
= xy_speed_demand_low
|| (xy_speed_low
&& !pos_control
->is_active_xy()) || (flightmode
->mode_number() == Mode::Number::ALT_HOLD
&& small_angle_request
);
62 bool descent_demanded
= pos_control
->is_active_z() && des_climb_rate_cms
< 0.0f
;
63 bool slow_descent_demanded
= descent_demanded
&& des_climb_rate_cms
>= -100.0f
;
64 bool z_speed_low
= fabsf(inertial_nav
.get_velocity_z_up_cms()) <= 60.0f
;
65 bool slow_descent
= (slow_descent_demanded
|| (z_speed_low
&& descent_demanded
));
67 gndeffect_state
.touchdown_expected
= slow_horizontal
&& slow_descent
;
69 // Prepare the EKF for ground effect if either takeoff or touchdown is expected.
70 ahrs
.set_takeoff_expected(gndeffect_state
.takeoff_expected
);
71 ahrs
.set_touchdown_expected(gndeffect_state
.touchdown_expected
);
74 // update ekf terrain height stable setting
75 // when set to true, this allows the EKF to stabilize the normally barometer based altitude using a rangefinder
76 // this is not related to terrain following
77 void Copter::update_ekf_terrain_height_stable()
79 // set to false if no position estimate
80 if (!position_ok() && !ekf_has_relative_position()) {
81 ahrs
.set_terrain_hgt_stable(false);
85 // consider terrain height stable if vehicle is taking off or landing
86 ahrs
.set_terrain_hgt_stable(flightmode
->is_taking_off() || flightmode
->is_landing());