APM:Copter
mode_sport.cpp
Go to the documentation of this file.
1 #include "Copter.h"
2 
3 /*
4  * Init and run calls for sport flight mode
5  */
6 
7 // sport_init - initialise sport controller
8 bool Copter::ModeSport::init(bool ignore_checks)
9 {
10  // initialize vertical speed and acceleration
13 
14  // initialise position and desired velocity
15  if (!pos_control->is_active_z()) {
18  }
19 
20  return true;
21 }
22 
23 // sport_run - runs the sport controller
24 // should be called at 100hz or more
26 {
27  SportModeState sport_state;
28  float takeoff_climb_rate = 0.0f;
29 
30  // initialize vertical speed and acceleration
33 
34  // apply SIMPLE mode transform
36 
37  // get pilot's desired roll and pitch rates
38 
39  // calculate rate requests
40  float target_roll_rate = channel_roll->get_control_in() * g.acro_rp_p;
41  float target_pitch_rate = channel_pitch->get_control_in() * g.acro_rp_p;
42 
43  // get attitude targets
44  const Vector3f att_target = attitude_control->get_att_target_euler_cd();
45 
46  // Calculate trainer mode earth frame rate command for roll
47  int32_t roll_angle = wrap_180_cd(att_target.x);
49 
50  // Calculate trainer mode earth frame rate command for pitch
51  int32_t pitch_angle = wrap_180_cd(att_target.y);
52  target_pitch_rate -= constrain_int32(pitch_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE) * g.acro_balance_pitch;
53 
55  if (roll_angle > aparm.angle_max){
56  target_roll_rate -= g.acro_rp_p*(roll_angle-aparm.angle_max);
57  }else if (roll_angle < -aparm.angle_max) {
58  target_roll_rate -= g.acro_rp_p*(roll_angle+aparm.angle_max);
59  }
60 
61  if (pitch_angle > aparm.angle_max){
62  target_pitch_rate -= g.acro_rp_p*(pitch_angle-aparm.angle_max);
63  }else if (pitch_angle < -aparm.angle_max) {
64  target_pitch_rate -= g.acro_rp_p*(pitch_angle+aparm.angle_max);
65  }
66 
67  // get pilot's desired yaw rate
68  float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
69 
70  // get pilot desired climb rate
71  float target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in());
72  target_climb_rate = constrain_float(target_climb_rate, -get_pilot_speed_dn(), g.pilot_speed_up);
73 
74  // State Machine Determination
75  if (!motors->armed() || !motors->get_interlock()) {
76  sport_state = Sport_MotorStopped;
77  } else if (takeoff_state.running || takeoff_triggered(target_climb_rate)) {
78  sport_state = Sport_Takeoff;
79  } else if (!ap.auto_armed || ap.land_complete) {
80  sport_state = Sport_Landed;
81  } else {
82  sport_state = Sport_Flying;
83  }
84 
85  // State Machine
86  switch (sport_state) {
87 
88  case Sport_MotorStopped:
89 
90  motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN);
91  attitude_control->input_euler_rate_roll_pitch_yaw(target_roll_rate, target_pitch_rate, target_yaw_rate);
92 #if FRAME_CONFIG == HELI_FRAME
93  // force descent rate and call position controller
95 #else
96  attitude_control->relax_attitude_controllers();
97  attitude_control->reset_rate_controller_I_terms();
98  attitude_control->set_yaw_target_to_current_heading();
99  pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero
100 #endif
102  break;
103 
104  case Sport_Takeoff:
105  // set motors to full range
106  motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
107 
108  // initiate take-off
109  if (!takeoff_state.running) {
111  // indicate we are taking off
112  set_land_complete(false);
113  // clear i terms
115  }
116 
117  // get take-off adjusted pilot and takeoff climb rates
118  takeoff_get_climb_rates(target_climb_rate, takeoff_climb_rate);
119 
120  // get avoidance adjusted climb rate
121  target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);
122 
123  // call attitude controller
124  attitude_control->input_euler_rate_roll_pitch_yaw(target_roll_rate, target_pitch_rate, target_yaw_rate);
125 
126  // call position controller
127  pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
128  pos_control->add_takeoff_climb_rate(takeoff_climb_rate, G_Dt);
130  break;
131 
132  case Sport_Landed:
133  // set motors to spin-when-armed if throttle below deadzone, otherwise full range (but motors will only spin at min throttle)
134  if (target_climb_rate < 0.0f) {
135  motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
136  } else {
137  motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
138  }
139 
140  attitude_control->reset_rate_controller_I_terms();
141  attitude_control->set_yaw_target_to_current_heading();
142  attitude_control->input_euler_rate_roll_pitch_yaw(target_roll_rate, target_pitch_rate, target_yaw_rate);
143  pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero
145  break;
146 
147  case Sport_Flying:
148  motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
149  // call attitude controller
150  attitude_control->input_euler_rate_roll_pitch_yaw(target_roll_rate, target_pitch_rate, target_yaw_rate);
151 
152  // adjust climb rate using rangefinder
153  if (copter.rangefinder_alt_ok()) {
154  // if rangefinder is ok, use surface tracking
155  target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt);
156  }
157 
158  // get avoidance adjusted climb rate
159  target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);
160 
161  // call position controller
162  pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
164  break;
165  }
166 }
DESIRED_SPIN_WHEN_ARMED
virtual float get_velocity_z() const=0
float get_alt_target() const
bool takeoff_triggered(float target_climb_rate) const
Definition: mode.cpp:354
AP_Float acro_balance_roll
Definition: Parameters.h:471
float get_surface_tracking_climb_rate(int16_t target_rate, float current_alt_target, float dt)
Definition: mode.cpp:548
virtual void set_alt_target_from_climb_rate(float climb_rate_cms, float dt, bool force_descend)
void takeoff_get_climb_rates(float &pilot_climb_rate, float &takeoff_climb_rate)
Definition: mode.cpp:612
float get_avoidance_adjusted_climbrate(float target_rate)
Definition: mode.cpp:617
void update_z_controller()
uint16_t get_pilot_speed_dn(void)
Definition: mode.cpp:622
void update_simple_mode(void)
Definition: mode.cpp:573
AC_AttitudeControl_t *& attitude_control
Definition: Copter.h:123
auto wrap_180_cd(const T angle) -> decltype(wrap_180(angle, 100.f))
float get_pilot_desired_climb_rate(float throttle_control)
Definition: mode.cpp:558
AP_Int16 pilot_speed_up
Definition: Parameters.h:416
float get_pilot_desired_yaw_rate(int16_t stick_angle)
Definition: mode.cpp:553
AP_Int16 pilot_accel_z
Definition: Parameters.h:417
AP_Float pilot_takeoff_alt
Definition: Parameters.h:389
void add_takeoff_climb_rate(float climb_rate_cms, float dt)
bool is_active_z() const
void takeoff_timer_start(float alt_cm)
Definition: mode.cpp:602
int16_t get_control_in() const
void relax_alt_hold_controllers(float throttle_setting)
#define f(i)
int32_t constrain_int32(const int32_t amt, const int32_t low, const int32_t high)
void set_desired_velocity_z(float vel_z_cms)
AP_Int16 land_speed
Definition: Parameters.h:414
void set_speed_z(float speed_down, float speed_up)
void set_throttle_takeoff(void)
Definition: mode.cpp:597
void set_accel_z(float accel_cmss)
DESIRED_THROTTLE_UNLIMITED
virtual void set_alt_target_from_climb_rate_ff(float climb_rate_cms, float dt, bool force_descend)
RC_Channel *& channel_roll
Definition: Copter.h:125
AC_PosControl *& pos_control
Definition: Copter.h:120
void run() override
Definition: mode_sport.cpp:25
takeoff_state_t & takeoff_state
Definition: Copter.h:131
float constrain_float(const float amt, const float low, const float high)
MOTOR_CLASS *& motors
Definition: Copter.h:124
AP_HAL_MAIN_CALLBACKS & copter
Definition: ArduCopter.cpp:581
float & G_Dt
Definition: Copter.h:129
SportModeState
Definition: defines.h:251
AP_InertialNav & inertial_nav
Definition: Copter.h:121
AP_Float acro_balance_pitch
Definition: Parameters.h:472
bool init(bool ignore_checks) override
Definition: mode_sport.cpp:8
#define ACRO_LEVEL_MAX_ANGLE
Definition: config.h:467
AP_Float acro_rp_p
Definition: Parameters.h:469
RC_Channel *& channel_yaw
Definition: Copter.h:128
Parameters & g
Definition: Copter.h:116
void set_land_complete(bool b)
Definition: mode.cpp:582
AP_Vehicle::MultiCopter aparm
Definition: Copter.h:205
RC_Channel *& channel_throttle
Definition: Copter.h:127
RC_Channel *& channel_pitch
Definition: Copter.h:126
void set_alt_target_to_current_alt()