4 #define LAND_CHECK_ANGLE_ERROR_DEG 30.0f // maximum angle error to be considered landing 5 #define LAND_CHECK_LARGE_ANGLE_CD 1500.0f // maximum angle target to be considered landing 6 #define LAND_CHECK_ACCEL_MOVING 3.0f // maximum acceleration after subtracting gravity 23 #if PARACHUTE == ENABLED 46 }
else if (
ap.land_complete) {
47 #if FRAME_CONFIG == HELI_FRAME 58 #if FRAME_CONFIG == HELI_FRAME 60 bool motor_at_lower_limit =
motors->limit.throttle_lower;
75 if (motor_at_lower_limit && accel_stationary && descent_rate_low && rangefinder_check) {
95 if(
ap.land_complete == b )
105 ap.land_complete =
b;
107 #if STATS_ENABLED == ENABLED 118 if (
ap.land_complete &&
motors->armed() && disarm_on_land_configured && mode_disarms_on_land) {
127 if (
ap.land_complete_maybe == b)
133 ap.land_complete_maybe =
b;
141 #if FRAME_CONFIG != HELI_FRAME 143 if(!
motors->armed() ||
ap.land_complete) {
174 if ( large_angle_request || large_angle_error || accel_moving || descent_not_demanded) {
float norm(const T first, const U second, const Params... parameters)
void update_land_detector()
float get_velocity_z() const
bool rangefinder_alt_ok()
const Vector3f & get_accel_ef_blended() const override
AC_AttitudeControl_t * attitude_control
#define LAND_CHECK_ANGLE_ERROR_DEG
void set_land_complete_maybe(bool b)
#define LAND_DETECTOR_ACCEL_MAX
LowPassFilterVector3f land_accel_ef_filter
AP_Int16 throttle_behavior
uint16_t get_loop_rate_hz(void)
void update_land_and_crash_detectors()
#define LAND_DETECTOR_MAYBE_TRIGGER_SEC
struct _USB_OTG_GOTGCTL_TypeDef::@51 b
float get_loop_period_s()
void update_throttle_thr_mix()
struct Copter::@0 rangefinder_state
#define THR_BEHAVE_DISARM_ON_LAND_DETECT
AP_InertialNav_NavEKF inertial_nav
void set_likely_flying(bool b)
static uint32_t land_detector_count
#define LAND_CHECK_LARGE_ANGLE_CD
RC_Channel * channel_throttle
int16_t get_control_in() const
void Log_Write_Event(uint8_t id)
void set_land_complete(bool b)
const Vector3f & get() const
#define DATA_LAND_COMPLETE_MAYBE
const Vector3f & get_desired_velocity()
AC_PosControl * pos_control
void init_disarm_motors()
virtual bool has_manual_throttle() const =0
float get_non_takeoff_throttle()
#define LAND_RANGEFINDER_MIN_ALT_CM
#define LAND_DETECTOR_TRIGGER_SEC
virtual bool allows_arming(bool from_gcs) const =0
#define LAND_CHECK_ACCEL_MOVING
#define DATA_LAND_COMPLETE
Vector3f apply(Vector3f sample, float dt)