3 // Traditional helicopter variables and functions
5 #if FRAME_CONFIG == HELI_FRAME
7 #ifndef HELI_DYNAMIC_FLIGHT_SPEED_MIN
8 #define HELI_DYNAMIC_FLIGHT_SPEED_MIN 250 // we are in "dynamic flight" when the speed is over 2.5m/s for 2 seconds
11 // counter to control dynamic flight profile
12 static int8_t heli_dynamic_flight_counter
;
14 // heli_init - perform any special initialisation required for the tradheli
15 void Copter::heli_init()
17 // pre-load stab col values as mode is initialized as Stabilize, but stabilize_init() function is not run on start-up.
18 input_manager
.set_use_stab_col(true);
19 input_manager
.set_stab_col_ramp(1.0);
22 // heli_check_dynamic_flight - updates the dynamic_flight flag based on our horizontal velocity
23 // should be called at 50hz
24 void Copter::check_dynamic_flight(void)
26 if (motors
->get_spool_state() != AP_Motors::SpoolState::THROTTLE_UNLIMITED
||
27 flightmode
->is_landing()) {
28 heli_dynamic_flight_counter
= 0;
29 heli_flags
.dynamic_flight
= false;
35 // with GPS lock use inertial nav to determine if we are moving
37 // get horizontal speed
38 const float speed
= inertial_nav
.get_speed_xy_cms();
39 moving
= (speed
>= HELI_DYNAMIC_FLIGHT_SPEED_MIN
);
41 // with no GPS lock base it on throttle and forward lean angle
42 moving
= (motors
->get_throttle() > 0.8f
|| ahrs
.pitch_sensor
< -1500);
45 #if AP_RANGEFINDER_ENABLED
46 if (!moving
&& rangefinder_state
.enabled
&& rangefinder
.status_orient(ROTATION_PITCH_270
) == RangeFinder::Status::Good
) {
47 // when we are more than 2m from the ground with good
48 // rangefinder lock consider it to be dynamic flight
49 moving
= (rangefinder
.distance_cm_orient(ROTATION_PITCH_270
) > 200);
54 // if moving for 2 seconds, set the dynamic flight flag
55 if (!heli_flags
.dynamic_flight
) {
56 heli_dynamic_flight_counter
++;
57 if (heli_dynamic_flight_counter
>= 100) {
58 heli_flags
.dynamic_flight
= true;
59 heli_dynamic_flight_counter
= 100;
63 // if not moving for 2 seconds, clear the dynamic flight flag
64 if (heli_flags
.dynamic_flight
) {
65 if (heli_dynamic_flight_counter
> 0) {
66 heli_dynamic_flight_counter
--;
68 heli_flags
.dynamic_flight
= false;
74 // update_heli_control_dynamics - pushes several important factors up into AP_MotorsHeli.
75 // should be run between the rate controller and the servo updates.
76 void Copter::update_heli_control_dynamics(void)
79 if (!motors
->using_leaky_integrator()) {
81 attitude_control
->use_leaky_i(false);
82 if (ap
.land_complete
|| ap
.land_complete_maybe
) {
83 motors
->set_land_complete(true);
85 motors
->set_land_complete(false);
88 // Use Leaky_I if we are not moving fast
89 attitude_control
->use_leaky_i(!heli_flags
.dynamic_flight
);
90 motors
->set_land_complete(false);
93 if (ap
.land_complete
|| (is_zero(motors
->get_desired_rotor_speed()))) {
94 // if we are landed or there is no rotor power demanded, decrement slew scalar
95 hover_roll_trim_scalar_slew
--;
97 // if we are not landed and motor power is demanded, increment slew scalar
98 hover_roll_trim_scalar_slew
++;
100 hover_roll_trim_scalar_slew
= constrain_int16(hover_roll_trim_scalar_slew
, 0, scheduler
.get_loop_rate_hz());
102 // set hover roll trim scalar, will ramp from 0 to 1 over 1 second after we think helicopter has taken off
103 attitude_control
->set_hover_roll_trim_scalar((float) hover_roll_trim_scalar_slew
/(float) scheduler
.get_loop_rate_hz());
106 bool Copter::should_use_landing_swash() const
108 if (flightmode
->has_manual_throttle() ||
109 flightmode
->mode_number() == Mode::Number::DRIFT
||
110 attitude_control
->get_inverted_flight()) {
111 // manual modes or modes using inverted flight uses full swash range
114 if (flightmode
->is_landing()) {
115 // landing with non-manual throttle mode always uses limit swash range
118 if (ap
.land_complete
) {
119 // when landed in non-manual throttle mode limit swash range
122 if (!ap
.auto_armed
) {
123 // when waiting to takeoff in non-manual throttle mode limit swash range
126 if (!heli_flags
.dynamic_flight
) {
127 // Just in case we are unsure of being in non-manual throttle
128 // mode, limit swash range in low speed and hovering flight.
129 // This will catch any non-manual throttle mode attempting a
130 // landing and driving the collective too low before the land
131 // complete flag is set.
137 // heli_update_landing_swash - sets swash plate flag so higher minimum is used when landed or landing
138 // should be called soon after update_land_detector in main code
139 void Copter::heli_update_landing_swash()
141 motors
->set_collective_for_landing(should_use_landing_swash());
142 update_collective_low_flag(channel_throttle
->get_control_in());
145 // convert motor interlock switch's position to desired rotor speed expressed as a value from 0 to 1
146 // returns zero if motor interlock auxiliary switch hasn't been defined
147 float Copter::get_pilot_desired_rotor_speed() const
149 RC_Channel
*rc_ptr
= rc().find_channel_for_option(RC_Channel::AUX_FUNC::MOTOR_INTERLOCK
);
150 if (rc_ptr
!= nullptr) {
151 rc_ptr
->set_range(1000);
152 return (float)rc_ptr
->get_control_in() * 0.001f
;
157 // heli_update_rotor_speed_targets - reads pilot input and passes new rotor speed targets to heli motors object
158 void Copter::heli_update_rotor_speed_targets()
160 // get rotor control method
161 uint8_t rsc_control_mode
= motors
->get_rsc_mode();
163 switch (rsc_control_mode
) {
164 case ROTOR_CONTROL_MODE_PASSTHROUGH
:
165 // pass through pilot desired rotor speed from the RC
166 if (get_pilot_desired_rotor_speed() > 0.01) {
167 ap
.motor_interlock_switch
= true;
168 motors
->set_desired_rotor_speed(get_pilot_desired_rotor_speed());
170 ap
.motor_interlock_switch
= false;
171 motors
->set_desired_rotor_speed(0.0f
);
174 case ROTOR_CONTROL_MODE_SETPOINT
:
175 case ROTOR_CONTROL_MODE_THROTTLECURVE
:
176 case ROTOR_CONTROL_MODE_AUTOTHROTTLE
:
177 if (motors
->get_interlock()) {
178 motors
->set_desired_rotor_speed(motors
->get_rsc_setpoint());
180 motors
->set_desired_rotor_speed(0.0f
);
188 // heli_update_autorotation - determines if aircraft is in autorotation and sets motors flag and switches
189 // to autorotation flight mode if manual collective is not being used.
190 void Copter::heli_update_autorotation()
192 bool in_autorotation_mode
= false;
193 #if MODE_AUTOROTATE_ENABLED
194 in_autorotation_mode
= flightmode
== &mode_autorotate
;
197 // If we have landed then we do not want to be in autorotation and we do not want to via the bailout state
198 if (ap
.land_complete
|| ap
.land_complete_maybe
) {
199 motors
->force_deactivate_autorotation();
203 // if we got this far we are flying, check for conditions to set autorotation state
204 if (!motors
->get_interlock() && (flightmode
->has_manual_throttle() || in_autorotation_mode
)) {
205 // set state in motors to facilitate manual and assisted autorotations
206 motors
->set_autorotation_active(true);
208 // deactivate the autorotation state via the bailout case
209 motors
->set_autorotation_active(false);
213 // update collective low flag. Use a debounce time of 400 milliseconds.
214 void Copter::update_collective_low_flag(int16_t throttle_control
)
216 static uint32_t last_nonzero_collective_ms
= 0;
217 uint32_t tnow_ms
= millis();
219 if (throttle_control
> 0) {
220 last_nonzero_collective_ms
= tnow_ms
;
221 heli_flags
.coll_stk_low
= false;
222 } else if (tnow_ms
- last_nonzero_collective_ms
> 400) {
223 heli_flags
.coll_stk_low
= true;
227 #endif // FRAME_CONFIG == HELI_FRAME