15 loiter_nav(
copter.loiter_nav),
16 pos_control(
copter.pos_control),
17 inertial_nav(
copter.inertial_nav),
19 attitude_control(
copter.attitude_control),
21 channel_roll(
copter.channel_roll),
22 channel_pitch(
copter.channel_pitch),
23 channel_throttle(
copter.channel_throttle),
24 channel_yaw(
copter.channel_yaw),
27 takeoff_state(
copter.takeoff_state),
28 ekfGndSpdLimit(
copter.ekfGndSpdLimit),
30 heli_flags(
copter.heli_flags),
32 ekfNavVelGainScaler(
copter.ekfNavVelGainScaler)
41 #if MODE_ACRO_ENABLED == ENABLED 55 #if MODE_AUTO_ENABLED == ENABLED 61 #if MODE_CIRCLE_ENABLED == ENABLED 67 #if MODE_LOITER_ENABLED == ENABLED 73 #if MODE_GUIDED_ENABLED == ENABLED 83 #if MODE_RTL_ENABLED == ENABLED 89 #if MODE_DRIFT_ENABLED == ENABLED 95 #if MODE_SPORT_ENABLED == ENABLED 105 #if AUTOTUNE_ENABLED == ENABLED 111 #if MODE_POSHOLD_ENABLED == ENABLED 117 #if MODE_BRAKE_ENABLED == ENABLED 123 #if MODE_THROW_ENABLED == ENABLED 129 #if ADSB_ENABLED == ENABLED 135 #if MODE_GUIDED_NOGPS_ENABLED == ENABLED 141 #if MODE_SMARTRTL_ENABLED == ENABLED 147 #if OPTFLOW == ENABLED 181 if (new_flightmode ==
nullptr) {
187 bool ignore_checks = !
motors->armed();
189 #if FRAME_CONFIG == HELI_FRAME 193 gcs().
send_text(MAV_SEVERITY_WARNING,
"Flight mode change failed");
199 if (!new_flightmode->
init(ignore_checks)) {
200 gcs().
send_text(MAV_SEVERITY_WARNING,
"Flight mode change failed");
214 #if ADSB_ENABLED == ENABLED 218 #if AC_FENCE == ENABLED 225 #if FRSKY_TELEM_ENABLED == ENABLED 228 #if DEVO_TELEM_ENABLED == ENABLED 232 #if CAMERA == ENABLED 257 #if AUTOTUNE_ENABLED == ENABLED 264 #if MODE_AUTO_ENABLED == ENABLED 271 #endif // MOUNT == ENABLED 284 #if MODE_SMARTRTL_ENABLED == ENABLED 291 #if FRAME_CONFIG == HELI_FRAME 295 motors->set_acro_tail(
false);
303 input_manager.set_stab_col_ramp(1.0);
304 }
else if (new_flightmode == &
mode_acro){
305 input_manager.set_stab_col_ramp(0.0);
341 float total_in =
norm(pitch_out, roll_out);
342 if (total_in > angle_limit) {
343 float ratio = angle_limit / total_in;
349 roll_out = (18000/
M_PI) * atanf(cosf(pitch_out*(
M_PI/18000))*tanf(roll_out*(
M_PI/18000)));
356 if (!
ap.land_complete) {
360 if (target_climb_rate <= 0.0
f) {
364 #if FRAME_CONFIG == HELI_FRAME 365 if (!
motors->rotor_runup_complete()) {
375 #if FRAME_CONFIG == HELI_FRAME 391 int32_t alt_above_ground;
392 if (
copter.rangefinder_alt_ok()) {
393 alt_above_ground =
copter.rangefinder_state.alt_cm_filt.get();
397 alt_above_ground =
copter.current_loc.alt;
400 return alt_above_ground;
405 #if PRECISION_LANDING == ENABLED 409 bool doing_precision_landing = !
ap.land_repo_active && precland.
target_acquired() && navigating;
411 bool doing_precision_landing =
false;
415 const float precland_acceptable_error = 15.0f;
416 const float precland_min_descent_speed = 10.0f;
420 if (!pause_descent) {
421 float max_land_descent_velocity;
429 max_land_descent_velocity =
MIN(max_land_descent_velocity, -abs(
g.
land_speed));
437 if (doing_precision_landing &&
copter.rangefinder_alt_ok() &&
copter.rangefinder_state.alt_cm > 35.0f &&
copter.rangefinder_state.alt_cm < 200.0f) {
440 cmb_rate =
MIN(-precland_min_descent_speed, -max_descent_speed+land_slowdown);
454 float target_roll = 0.0f;
455 float target_pitch = 0.0f;
456 float target_yaw_rate = 0;
459 if (
ap.land_complete_maybe) {
464 if (!
copter.failsafe.radio) {
482 ap.land_repo_active =
true;
490 #if PRECISION_LANDING == ENABLED 492 bool doing_precision_landing = !
ap.land_repo_active && precland.target_acquired();
494 if (doing_precision_landing) {
495 Vector2f target_pos, target_vel_rel;
496 if (!precland.get_target_position_cm(target_pos)) {
500 if (!precland.get_target_velocity_relative_cms(target_vel_rel)) {
528 float total_angle_cd =
norm(nav_roll, nav_pitch);
529 if (total_angle_cd > attitude_limit_cd) {
530 float ratio = attitude_limit_cd / total_angle_cd;
541 attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(nav_roll, nav_pitch, target_yaw_rate);
550 return copter.get_surface_tracking_climb_rate(target_rate, current_alt_target, dt);
555 return copter.get_pilot_desired_yaw_rate(stick_angle);
560 return copter.get_pilot_desired_climb_rate(throttle_control);
565 return copter.get_pilot_desired_throttle(throttle_control, thr_mid);
570 return copter.get_non_takeoff_throttle();
574 copter.update_simple_mode();
579 return copter.set_mode(mode, reason);
584 return copter.set_land_complete(b);
594 return copter.Log_Write_Event(
id);
599 return copter.set_throttle_takeoff();
604 return copter.takeoff_timer_start(alt_cm);
609 return copter.takeoff_stop();
614 return copter.takeoff_get_climb_rates(pilot_climb_rate, takeoff_climb_rate);
619 return copter.get_avoidance_adjusted_climbrate(target_rate);
624 return copter.get_pilot_speed_dn();
float norm(const T first, const U second, const Params... parameters)
AP_DEVO_Telem devo_telemetry
void zero_throttle_and_relax_ac()
bool set_mode(control_mode_t mode, mode_reason_t reason)
LowPassFilterFloat rc_throttle_control_in_filter
#define ERROR_SUBSYSTEM_FLIGHT_MODE
bool takeoff_triggered(float target_climb_rate) const
virtual const Vector3f & get_position() const=0
void update_flight_mode()
virtual void run_autopilot()
float get_surface_tracking_climb_rate(int16_t target_rate, float current_alt_target, float dt)
AP_Int16 throttle_behavior
ModeStabilize mode_stabilize
void takeoff_get_climb_rates(float &pilot_climb_rate, float &takeoff_climb_rate)
float get_avoidance_adjusted_climbrate(float target_rate)
virtual const char * name4() const =0
void notify_flight_mode()
struct _USB_OTG_GOTGCTL_TypeDef::@51 b
void update_z_controller()
void Log_Write_Event(uint8_t id)
void Log_Write_Mode(uint8_t mode, uint8_t reason)
virtual bool init(bool ignore_checks)=0
uint16_t get_pilot_speed_dn(void)
void update_simple_mode(void)
void set_limit_accel_xy(void)
float get_horizontal_error() const
AP_Frsky_Telem frsky_telemetry
void override_vehicle_velocity_xy(const Vector2f &vel_xy)
AC_AttitudeControl_t *& attitude_control
float get_pilot_desired_climb_rate(float throttle_control)
ModeSmartRTL mode_smartrtl
ModeGuidedNoGPS mode_guided_nogps
float get_pilot_desired_yaw_rate(int16_t stick_angle)
void soften_for_landing()
AP_Int8 land_repositioning
void land_run_horizontal_control()
Mode * mode_from_mode_num(const uint8_t mode)
void land_run_vertical_control(bool pause_descent=false)
void set_xy_target(float x, float y)
AP_MotorsMatrix motors(400)
bool is_active_xy() const
#define THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND
mode_reason_t control_mode_reason
void Log_Write_Error(uint8_t sub_system, uint8_t error_code)
#define MODE_FOLLOW_ENABLED
float linear_interpolate(float low_output, float high_output, float var_value, float var_low, float var_high)
int32_t get_alt_above_ground(void)
void update(float ekfGndSpdLimit, float ekfNavVelGainScaler)
void takeoff_timer_start(float alt_cm)
float get_non_takeoff_throttle(void)
int16_t get_control_in() const
void manual_recovery_start()
bool is_zero(const T fVal1)
ModeAvoidADSB mode_avoid_adsb
mission_state state() const
void update_control_mode(uint8_t mode)
void set_throttle_takeoff(void)
virtual bool is_autopilot() const
void update_control_mode(uint8_t mode)
void set_accel_throttle_I_from_pilot_throttle()
float get_accel_z() const
#define DATA_LAND_CANCELLED_BY_PILOT
bool set_mode(control_mode_t mode, mode_reason_t reason)
static float sqrt_controller(float error, float p, float second_ord_lim, float dt)
void send_text(MAV_SEVERITY severity, const char *fmt,...)
void get_pilot_desired_lean_angles(float &roll_out, float &pitch_out, float angle_max, float angle_limit) const
float get_pilot_desired_throttle(int16_t throttle_control, float thr_mid=0.0f)
float get_speed_down() const
virtual void set_alt_target_from_climb_rate_ff(float climb_rate_cms, float dt, bool force_descend)
DataFlash_Class DataFlash
RC_Channel *& channel_roll
AC_PosControl *& pos_control
const float & get() const
void set_flight_mode_str(const char *str)
float constrain_float(const float amt, const float low, const float high)
ModeAutoTune mode_autotune
static struct notify_flags_and_values_type flags
#define ROLL_PITCH_YAW_INPUT_MAX
AP_HAL_MAIN_CALLBACKS & copter
#define LAND_CANCEL_TRIGGER_THR
virtual bool has_manual_throttle() const =0
void set_pilot_desired_acceleration(float euler_roll_angle_cd, float euler_pitch_angle_cd, float dt)
void exit_mode(Mode *&old_flightmode, Mode *&new_flightmode)
float & ekfNavVelGainScaler
AP_InertialNav & inertial_nav
void set_mode_to_default()
virtual const Vector3f & get_velocity() const=0
RC_Channel *& channel_yaw
control_mode_t control_mode
int32_t get_pitch() const
void set_is_auto_mode(bool enable)
void set_land_complete(bool b)
AP_Vehicle::MultiCopter aparm
RC_Channel *& channel_pitch
float get_angle_max_cd() const