8 #define POSHOLD_SPEED_0 10 // speed below which it is always safe to switch to loiter 11 #define POSHOLD_BRAKE_TIME_ESTIMATE_MAX (600*4) // max number of cycles the brake will be applied before we switch to loiter 12 #define POSHOLD_BRAKE_TO_LOITER_TIMER (150*4) // Number of cycles to transition from brake mode to loiter mode. Must be lower than POSHOLD_LOITER_STAB_TIMER 13 #define POSHOLD_WIND_COMP_START_TIMER (150*4) // Number of cycles to start wind compensation update after loiter is engaged 14 #define POSHOLD_CONTROLLER_TO_PILOT_MIX_TIMER (50*4) // Set it from 100 to 200, the number of centiseconds loiter and manual commands are mixed to make a smooth transition. 15 #define POSHOLD_SMOOTH_RATE_FACTOR 0.0125f // filter applied to pilot's roll/pitch input as it returns to center. A lower number will cause the roll/pitch to return to zero more slowly if the brake_rate is also low. 16 #define POSHOLD_WIND_COMP_TIMER_10HZ 40 // counter value used to reduce wind compensation to 10hz 17 #define LOOP_RATE_FACTOR 4 // used to adapt PosHold params to loop_rate 18 #define TC_WIND_COMP 0.0025f // Time constant for poshold_update_wind_comp_estimate() 21 #define POSHOLD_STICK_RELEASE_SMOOTH_ANGLE 1800 // max angle required (in centi-degrees) after which the smooth stick release effect is applied 22 #define POSHOLD_WIND_COMP_ESTIMATE_SPEED_MAX 10 // wind compensation estimates will only run when velocity is at or below this speed in cm/s 76 if (!
copter.position_ok() && !ignore_checks) {
97 if (
ap.land_complete) {
123 float target_roll, target_pitch;
124 float target_yaw_rate = 0;
125 float target_climb_rate = 0;
126 float takeoff_climb_rate = 0.0f;
127 float brake_to_loiter_mix;
128 float controller_to_pilot_roll_mix;
129 float controller_to_pilot_pitch_mix;
130 float vel_fw, vel_right;
138 if (!
motors->armed() || !
ap.auto_armed || !
motors->get_interlock()) {
146 if (!
copter.failsafe.radio) {
161 #if FRAME_CONFIG == HELI_FRAME 179 if (
ap.land_complete_maybe) {
184 if (
ap.land_complete) {
186 if (target_climb_rate < 0.0
f) {
228 poshold.brake_angle_max_roll = 0;
230 poshold.braking_time_updated_roll =
false;
243 if (!
poshold.braking_time_updated_roll) {
250 poshold.braking_time_updated_roll =
true;
260 if (
poshold.brake_timeout_roll > 0) {
290 if (
poshold.controller_to_pilot_timer_roll > 0) {
291 poshold.controller_to_pilot_timer_roll--;
322 poshold.brake_angle_max_pitch = 0;
324 poshold.braking_time_updated_pitch =
false;
337 if (!
poshold.braking_time_updated_pitch) {
344 poshold.braking_time_updated_pitch =
true;
354 if (
poshold.brake_timeout_pitch > 0) {
384 if (
poshold.controller_to_pilot_timer_pitch > 0) {
385 poshold.controller_to_pilot_timer_pitch--;
427 if (
poshold.brake_to_loiter_timer > 0) {
428 poshold.brake_to_loiter_timer--;
514 float angle_max =
copter.aparm.angle_max;
522 if (
copter.rangefinder_alt_ok()) {
541 if ((lean_angle_filtered > 0 && lean_angle_raw < 0) || (lean_angle_filtered < 0 && lean_angle_raw > 0) || (fabsf(lean_angle_raw) >
POSHOLD_STICK_RELEASE_SMOOTH_ANGLE)) {
542 lean_angle_filtered = lean_angle_raw;
545 if (lean_angle_filtered > 0) {
550 lean_angle_filtered = MAX(lean_angle_filtered, lean_angle_raw);
553 lean_angle_filtered =
MIN(lean_angle_filtered, lean_angle_raw);
563 return (int16_t)((mix_ratio * first_control) + ((1.0
f-mix_ratio)*second_control));
575 if (brake_rate <= 0) {
581 lean_angle = -
poshold.brake_gain * velocity * (1.0f+500.0f/(velocity+60.0f));
583 lean_angle = -
poshold.brake_gain * velocity * (1.0f+500.0f/(-velocity+60.0f));
587 brake_angle =
constrain_int16((int16_t)lean_angle, brake_angle - brake_rate, brake_angle + brake_rate);
598 if (
poshold.wind_comp_start_timer > 0) {
599 poshold.wind_comp_start_timer--;
615 poshold.wind_comp_ef.x = accel_target.
x;
622 poshold.wind_comp_ef.y = accel_target.
y;
void zero_throttle_and_relax_ac()
int16_t brake_timeout_roll
#define POSHOLD_WIND_COMP_START_TIMER
virtual float get_velocity_z() const=0
float get_alt_target() const
int16_t controller_final_roll
#define POSHOLD_STICK_RELEASE_SMOOTH_ANGLE
int16_t constrain_int16(const int16_t amt, const int16_t low, const int16_t high)
virtual const Vector3f & get_position() const=0
#define POSHOLD_CONTROLLER_TO_PILOT_MIX_TIMER
const Vector3f & get_accel_target() const
float get_surface_tracking_climb_rate(int16_t target_rate, float current_alt_target, float dt)
AP_Int16 poshold_brake_angle_max
void takeoff_get_climb_rates(float &pilot_climb_rate, float &takeoff_climb_rate)
float get_avoidance_adjusted_climbrate(float target_rate)
int16_t controller_final_pitch
void poshold_roll_controller_to_pilot_override()
void poshold_get_wind_comp_lean_angles(int16_t &roll_angle, int16_t &pitch_angle)
void update_z_controller()
int16_t controller_to_pilot_timer_roll
uint16_t get_pilot_speed_dn(void)
int16_t brake_to_loiter_timer
void update_simple_mode(void)
AC_AttitudeControl_t *& attitude_control
void init_target(const Vector3f &position)
float get_pilot_desired_climb_rate(float throttle_control)
#define POSHOLD_WIND_COMP_ESTIMATE_SPEED_MAX
float get_pilot_desired_yaw_rate(int16_t stick_angle)
void soften_for_landing()
void poshold_pitch_controller_to_pilot_override()
AP_Float pilot_takeoff_alt
int16_t controller_to_pilot_timer_pitch
void add_takeoff_climb_rate(float climb_rate_cms, float dt)
int16_t brake_angle_max_pitch
void update(float ekfGndSpdLimit, float ekfNavVelGainScaler)
void takeoff_timer_start(float alt_cm)
int16_t get_control_in() const
void relax_alt_hold_controllers(float throttle_setting)
poshold_rp_mode pitch_mode
bool is_zero(const T fVal1)
#define POSHOLD_BRAKE_TIME_ESTIMATE_MAX
void set_desired_velocity_z(float vel_z_cms)
void set_speed_z(float speed_down, float speed_up)
uint16_t wind_comp_start_timer
void set_throttle_takeoff(void)
bool init(bool ignore_checks) override
void set_accel_z(float accel_cmss)
#define POSHOLD_BRAKE_TO_LOITER_TIMER
#define POSHOLD_WIND_COMP_TIMER_10HZ
AP_Int16 poshold_brake_rate
void get_pilot_desired_lean_angles(float &roll_out, float &pitch_out, float angle_max, float angle_limit) const
void poshold_update_wind_comp_estimate()
void poshold_update_pilot_lean_angle(float &lean_angle_filtered, float &lean_angle_raw)
DESIRED_THROTTLE_UNLIMITED
int16_t poshold_mix_controls(float mix_ratio, int16_t first_control, int16_t second_control)
virtual void set_alt_target_from_climb_rate_ff(float climb_rate_cms, float dt, bool force_descend)
uint8_t braking_time_updated_pitch
AC_PosControl *& pos_control
#define POSHOLD_SMOOTH_RATE_FACTOR
takeoff_state_t & takeoff_state
float constrain_float(const float amt, const float low, const float high)
int16_t brake_angle_max_roll
virtual float get_velocity_xy() const=0
int16_t brake_timeout_pitch
static struct @15 poshold
AP_HAL_MAIN_CALLBACKS & copter
void poshold_update_brake_angle_from_velocity(int16_t &brake_angle, float velocity)
float & ekfNavVelGainScaler
AP_InertialNav & inertial_nav
virtual const Vector3f & get_velocity() const=0
poshold_rp_mode roll_mode
RC_Channel *& channel_yaw
uint8_t braking_time_updated_roll
int32_t get_pitch() const
void set_land_complete(bool b)
RC_Channel *& channel_throttle
void set_alt_target_to_current_alt()