AP_DDS: pre-arm check service
[ardupilot.git] / ArduCopter / baro_ground_effect.cpp
blob3333fb36e84d33db2fabdece6e323116277d93e5
1 #include "Copter.h"
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);
11 return;
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();
22 vel_target.z = 0.0f;
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();
28 vel.z = 0.0f;
29 xy_speed_cms = vel.length();
32 // takeoff logic
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;
55 // landing logic
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);
82 return;
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());