15 float y_in, y_in3, y_out;
24 y_in3 = y_in*y_in*y_in;
40 #if FRAME_CONFIG != HELI_FRAME 42 if (!
motors->armed() ||
ap.land_complete) {
57 float throttle =
motors->get_throttle();
62 motors->update_throttle_hover(0.01
f);
80 if (thr_mid <= 0.0
f) {
81 thr_mid =
motors->get_throttle_hover();
95 if (throttle_control < mid_stick) {
97 throttle_in = ((float)throttle_control)*0.5f/(float)mid_stick;
98 }
else if(throttle_control > mid_stick) {
100 throttle_in = 0.5f + ((float)(throttle_control-mid_stick)) * 0.5
f / (
float)(1000-mid_stick);
108 float throttle_out = throttle_in*(1.0f-expo) + expo*throttle_in*throttle_in*throttle_in;
121 #if TOY_MODE_ENABLED == ENABLED 129 float desired_rate = 0.0f;
141 if (throttle_control < deadband_bottom) {
143 desired_rate =
get_pilot_speed_dn() * (throttle_control-deadband_bottom) / deadband_bottom;
144 }
else if (throttle_control > deadband_top) {
146 desired_rate =
g.
pilot_speed_up * (throttle_control-deadband_top) / (1000.0
f-deadband_top);
158 return MAX(0,
motors->get_throttle_hover()/2.0f);
165 #if RANGEFINDER_ENABLED == ENABLED 166 static uint32_t last_call_ms = 0;
167 float distance_error;
168 float velocity_correction;
180 if ((target_rate<0 && !motors->limit.throttle_lower) || (target_rate>0 && !
motors->limit.throttle_upper)) {
216 return (target_rate + velocity_correction);
218 return (
float)target_rate;
225 #if AC_AVOID_ENABLED == ENABLED
void rotate_body_frame_to_NE(float &x, float &y)
AC_AttitudeControl_t * attitude_control
void set_throttle_takeoff()
float get_altitude() const
int16_t constrain_int16(const int16_t amt, const int16_t low, const int16_t high)
#define RANGEFINDER_TIMEOUT_MS
float get_pilot_desired_throttle(int16_t throttle_control, float thr_mid=0.0f)
void update_throttle_hover()
struct Copter::@0 rangefinder_state
#define RANGEFINDER_GLITCH_NUM_SAMPLES
AP_InertialNav_NavEKF inertial_nav
#define THR_SURFACE_TRACKING_VELZ_MAX
void set_integrator(float i)
int16_t get_throttle_mid(void)
bool is_zero(const T fVal1)
float get_avoidance_adjusted_climbrate(float target_rate)
void set_accel_throttle_I_from_pilot_throttle()
float get_accel_z() const
AP_Int16 throttle_deadzone
float get_pilot_desired_climb_rate(float throttle_control)
void throttle_adjust(float &throttle_control)
float target_rangefinder_alt
const Vector3f & get_desired_velocity()
float constrain_float(const float amt, const float low, const float high)
#define ROLL_PITCH_YAW_INPUT_MAX
AP_Float rangefinder_gain
struct Copter::@2 failsafe
AC_PosControl * pos_control
virtual bool has_manual_throttle() const =0
void adjust_velocity_z(float kP, float accel_cmss, float &climb_rate_cms, float dt)
uint16_t get_pilot_speed_dn()
float get_non_takeoff_throttle()
#define RANGEFINDER_GLITCH_ALT_CM
AC_PID & get_accel_z_pid()
control_mode_t control_mode
float get_pilot_desired_yaw_rate(int16_t stick_angle)
float get_surface_tracking_climb_rate(int16_t target_rate, float current_alt_target, float dt)