AP_DDS: pre-arm check service
[ardupilot.git] / ArduCopter / heli.cpp
blobbff2a3de170419df9e226ef1e3466b99970abfbc
1 #include "Copter.h"
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
9 #endif
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;
30 return;
33 bool moving = false;
35 // with GPS lock use inertial nav to determine if we are moving
36 if (position_ok()) {
37 // get horizontal speed
38 const float speed = inertial_nav.get_speed_xy_cms();
39 moving = (speed >= HELI_DYNAMIC_FLIGHT_SPEED_MIN);
40 } else {
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);
51 #endif
53 if (moving) {
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;
62 } else {
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--;
67 } else {
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()) {
80 //turn off leaky_I
81 attitude_control->use_leaky_i(false);
82 if (ap.land_complete || ap.land_complete_maybe) {
83 motors->set_land_complete(true);
84 } else {
85 motors->set_land_complete(false);
87 } else {
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--;
96 } else {
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
112 return false;
114 if (flightmode->is_landing()) {
115 // landing with non-manual throttle mode always uses limit swash range
116 return true;
118 if (ap.land_complete) {
119 // when landed in non-manual throttle mode limit swash range
120 return true;
122 if (!ap.auto_armed) {
123 // when waiting to takeoff in non-manual throttle mode limit swash range
124 return true;
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.
132 return true;
134 return false;
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;
154 return 0.0f;
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());
169 } else {
170 ap.motor_interlock_switch = false;
171 motors->set_desired_rotor_speed(0.0f);
173 break;
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());
179 } else {
180 motors->set_desired_rotor_speed(0.0f);
182 break;
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;
195 #endif
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();
200 return;
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);
207 } else {
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