Line data    Source code

       1             : #include "Copter.h"
       2             : 
       3             : #if AP_GROUNDEFFECT_ENABLED
       4     3800955 : void Copter::update_ground_effect_detector(void)
       5             : {
       6     3800955 :     AP_GroundEffect &gndeff = g2.ground_effect;
       7             : 
       8             :     // throw mode never wants the takeoff expected EKF code
       9     3800955 :     gndeff.enable_takeoff_comp(flightmode->mode_number() != Mode::Number::THROW);
      10     3800955 :     gndeff.set_high_vibrations(vibration_check.high_vibes);
      11             : 
      12             :     // ALT_HOLD has manual attitude and no NE controller, so a near-level
      13             :     // attitude target stands in for "pilot is asking for slow horizontal"
      14     3800955 :     bool pilot_slow_horizontal = false;
      15     3800955 :     if (flightmode->mode_number() == Mode::Number::ALT_HOLD) {
      16      303777 :         const Vector3f angle_target_rad = attitude_control->get_att_target_euler_rad();
      17      303777 :         pilot_slow_horizontal = cosf(angle_target_rad.x) * cosf(angle_target_rad.y) > cosf(radians(7.5f));
      18             :     }
      19     3800955 :     gndeff.set_pilot_demanding_slow_horizontal(pilot_slow_horizontal);
      20             : 
      21     3800955 :     const bool throttle_up = flightmode->has_manual_throttle() && channel_throttle->get_control_in() > 0;
      22     3800955 :     gndeff.update(motors->armed(), ap.land_complete, throttle_up);
      23     3800955 : }
      24             : #endif  // AP_GROUNDEFFECT_ENABLED
      25             : 
      26             : // update ekf terrain height stable setting
      27             : // when set to true, this allows the EKF to stabilize the normally barometer based altitude using a rangefinder
      28             : // this is not related to terrain following
      29     3800955 : void Copter::update_ekf_terrain_height_stable()
      30             : {
      31             :     // set to false if no position estimate
      32     3800955 :     if (!position_ok() && !ekf_has_relative_position()) {
      33      872854 :         ahrs.set_terrain_hgt_stable(false);
      34      872854 :         return;
      35             :     }
      36             : 
      37             :     // consider terrain height stable if vehicle is taking off or landing
      38     2928101 :     ahrs.set_terrain_hgt_stable(flightmode->is_taking_off() || flightmode->is_landing());
      39             : }