Line data Source code
1 : #include "AC_Autorotation.h"
2 : #include <AP_Logger/AP_Logger.h>
3 : #include <AP_RPM/AP_RPM.h>
4 : #include <AP_AHRS/AP_AHRS.h>
5 : #include <GCS_MAVLink/GCS.h>
6 :
7 : extern const AP_HAL::HAL& hal;
8 :
9 : // Autorotation controller defaults
10 : #define HEAD_SPEED_TARGET_RATIO 1.0 // Normalised target main rotor head speed
11 :
12 : const AP_Param::GroupInfo AC_Autorotation::var_info[] = {
13 :
14 : // @Param: ENABLE
15 : // @DisplayName: Enable settings for RSC Setpoint
16 : // @Description: Allows you to enable (1) or disable (0) the autonomous autorotation capability.
17 : // @Values: 0:Disabled,1:Enabled
18 : // @User: Standard
19 : AP_GROUPINFO_FLAGS("ENABLE", 1, AC_Autorotation, _param_enable, 0, AP_PARAM_FLAG_ENABLE),
20 :
21 : // @Param: HS_P
22 : // @DisplayName: P gain for head speed controller
23 : // @Description: Increase value to increase sensitivity of head speed controller during autonomous autorotation.
24 : // @Range: 0.3 1
25 : // @Increment: 0.01
26 : // @User: Standard
27 : AP_SUBGROUPINFO(_p_hs, "HS_", 2, AC_Autorotation, AC_P),
28 :
29 : // @Param: HS_SET_PT
30 : // @DisplayName: Target Head Speed
31 : // @Description: The target head speed in RPM during autorotation. Start by setting to desired hover speed and tune from there as necessary.
32 : // @Units: RPM
33 : // @Range: 1000 2800
34 : // @Increment: 1
35 : // @User: Standard
36 : AP_GROUPINFO("HS_SET_PT", 3, AC_Autorotation, _param_head_speed_set_point, 1500),
37 :
38 : // @Param: FWD_SP_TARG
39 : // @DisplayName: Target Glide Body Frame Forward Speed
40 : // @Description: Target ground speed in cm/s for the autorotation controller to try and achieve/ maintain during the glide phase.
41 : // @Units: m/s
42 : // @Range: 8 20
43 : // @Increment: 0.5
44 : // @User: Standard
45 : AP_GROUPINFO("FWD_SP_TARG", 4, AC_Autorotation, _param_target_speed_ms, 11),
46 :
47 : // @Param: COL_FILT_E
48 : // @DisplayName: Entry Phase Collective Filter
49 : // @Description: Cut-off frequency for collective low pass filter. For the entry phase. Acts as a following trim. Must be higher than AROT_COL_FILT_G.
50 : // @Units: Hz
51 : // @Range: 0.2 0.5
52 : // @Increment: 0.01
53 : // @User: Standard
54 : AP_GROUPINFO("COL_FILT_E", 5, AC_Autorotation, _param_col_entry_cutoff_freq, 0.7),
55 :
56 : // @Param: COL_FILT_G
57 : // @DisplayName: Glide Phase Collective Filter
58 : // @Description: Cut-off frequency for collective low pass filter. For the glide phase. Acts as a following trim. Must be lower than AROT_COL_FILT_E.
59 : // @Units: Hz
60 : // @Range: 0.03 0.15
61 : // @Increment: 0.01
62 : // @User: Standard
63 : AP_GROUPINFO("COL_FILT_G", 6, AC_Autorotation, _param_col_glide_cutoff_freq, 0.1),
64 :
65 : // @Param: XY_ACC_MAX
66 : // @DisplayName: Body Frame XY Acceleration Limit
67 : // @Description: Maximum body frame acceleration allowed in the in speed controller. This limit defines a circular constraint in accel. Minimum used is 0.5 m/s/s.
68 : // @Units: m/s/s
69 : // @Range: 0.5 8.0
70 : // @Increment: 0.1
71 : // @User: Standard
72 : AP_GROUPINFO("XY_ACC_MAX", 7, AC_Autorotation, _param_accel_max_mss, 2.0),
73 :
74 : // @Param: HS_SENSOR
75 : // @DisplayName: Main Rotor RPM Sensor
76 : // @Description: Allocate the RPM sensor instance to use for measuring head speed. RPM1 = 0. RPM2 = 1.
77 : // @Units: s
78 : // @Range: 0.5 3
79 : // @Increment: 0.1
80 : // @User: Standard
81 : AP_GROUPINFO("HS_SENSOR", 8, AC_Autorotation, _param_rpm_instance, 0),
82 :
83 : // @Param: FWD_P
84 : // @DisplayName: Forward Speed Controller P Gain
85 : // @Description: Converts the difference between desired forward speed and actual speed into an acceleration target that is passed to the pitch angle controller.
86 : // @Range: 1.000 8.000
87 : // @User: Standard
88 :
89 : // @Param: FWD_I
90 : // @DisplayName: Forward Speed Controller I Gain
91 : // @Description: Corrects long-term difference in desired velocity to a target acceleration.
92 : // @Range: 0.02 1.00
93 : // @Increment: 0.01
94 : // @User: Advanced
95 :
96 : // @Param: FWD_IMAX
97 : // @DisplayName: Forward Speed Controller I Gain Maximum
98 : // @Description: Constrains the target acceleration that the I gain will output.
99 : // @Range: 1.000 8.000
100 : // @User: Standard
101 :
102 : // @Param: FWD_D
103 : // @DisplayName: Forward Speed Controller D Gain
104 : // @Description: Provides damping to velocity controller.
105 : // @Range: 0.00 1.00
106 : // @Increment: 0.001
107 : // @User: Advanced
108 :
109 : // @Param: FWD_FF
110 : // @DisplayName: Forward Speed Controller Feed Forward Gain
111 : // @Description: Produces an output that is proportional to the magnitude of the target.
112 : // @Range: 0 1
113 : // @Increment: 0.01
114 : // @User: Advanced
115 :
116 : // @Param: FWD_FLTE
117 : // @DisplayName: Forward Speed Controller Error Filter
118 : // @Description: This filter low pass filter is applied to the input for P and I terms.
119 : // @Range: 0 100
120 : // @Units: Hz
121 : // @User: Advanced
122 :
123 : // @Param: FWD_FLTD
124 : // @DisplayName: Forward Speed Controller input filter for D term
125 : // @Description: This filter low pass filter is applied to the input for D terms.
126 : // @Range: 0 100
127 : // @Units: Hz
128 : // @User: Advanced
129 : AP_SUBGROUPINFO(_fwd_speed_pid, "FWD_", 9, AC_Autorotation, AC_PID_Basic),
130 :
131 : AP_GROUPEND
132 : };
133 :
134 : // Constructor
135 83 : AC_Autorotation::AC_Autorotation(AP_MotorsHeli*& motors, AC_AttitudeControl*& att_crtl) :
136 83 : _motors_heli(motors),
137 83 : _attitude_control(att_crtl)
138 : {
139 83 : AP_Param::setup_object_defaults(this, var_info);
140 83 : }
141 :
142 1 : void AC_Autorotation::init(void)
143 : {
144 : // Initialisation of head speed controller
145 : // Set initial collective position to be the current collective position for smooth init
146 1 : const float collective_out = _motors_heli->get_throttle_out();
147 :
148 : // Reset feed forward filter
149 1 : col_trim_lpf.reset(collective_out);
150 :
151 : // Protect against divide by zero TODO: move this to an accessor function
152 1 : _param_head_speed_set_point.set(MAX(_param_head_speed_set_point, 500.0));
153 :
154 : // Reset the landed reason
155 1 : _landed_reason.min_speed = false;
156 1 : _landed_reason.land_col = false;
157 1 : _landed_reason.is_still = false;
158 1 : }
159 :
160 : // Functions and config that are only to be done once at the beginning of the entry
161 1 : void AC_Autorotation::init_entry(void)
162 : {
163 1 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AROT: Entry Phase");
164 :
165 : // Target head speed is set to rpm at initiation to prevent steps in controller
166 1 : if (!get_norm_head_speed(_target_head_speed)) {
167 : // Cannot get a valid RPM sensor reading so we default to not slewing the head speed target
168 0 : _target_head_speed = HEAD_SPEED_TARGET_RATIO;
169 : }
170 :
171 : // The rate to move the head speed from the current measurement to the target
172 1 : _hs_accel = (HEAD_SPEED_TARGET_RATIO - _target_head_speed) / (float(entry_time_ms)*1e-3);
173 :
174 : // Set collective following trim low pass filter cut off frequency
175 1 : col_trim_lpf.set_cutoff_frequency(_param_col_entry_cutoff_freq.get());
176 :
177 : // Set collective low-pass cut off filter at 2 Hz
178 1 : _motors_heli->set_throttle_filter_cutoff(2.0);
179 :
180 : // Set speed target to maintain the current speed whilst we enter the autorotation
181 1 : _desired_vel_ms = _param_target_speed_ms.get();
182 1 : _target_vel_ms = get_speed_forward_ms();
183 :
184 : // Reset I term of velocity PID
185 1 : _fwd_speed_pid.reset_filter();
186 1 : _fwd_speed_pid.set_integrator(0.0);
187 1 : }
188 :
189 : // The entry controller just a special case of the glide controller with head speed target slewing
190 800 : void AC_Autorotation::run_entry(float pilot_accel_norm)
191 : {
192 : // Slowly change the target head speed until the target head speed matches the parameter defined value
193 : float head_speed_norm;
194 800 : if (!get_norm_head_speed(head_speed_norm)) {
195 : // RPM sensor is bad, so we don't attempt to slew the head speed target as we do not know what head speed actually is
196 : // The collective output handling of the rpm sensor failure is handled later in the head speed controller
197 0 : head_speed_norm = HEAD_SPEED_TARGET_RATIO;
198 : }
199 :
200 : // Slew the head speed target from the initial condition to the target head speed ratio for the glide
201 800 : const float max_change = _hs_accel * _dt;
202 800 : _target_head_speed = constrain_float(HEAD_SPEED_TARGET_RATIO, _target_head_speed - max_change, _target_head_speed + max_change);
203 :
204 800 : run_glide(pilot_accel_norm);
205 800 : }
206 :
207 : // Functions and config that are only to be done once at the beginning of the glide
208 1 : void AC_Autorotation::init_glide(void)
209 : {
210 1 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AROT: Glide Phase");
211 :
212 : // Set collective following trim low pass filter cut off frequency
213 1 : col_trim_lpf.set_cutoff_frequency(_param_col_glide_cutoff_freq.get());
214 :
215 : // Ensure target head speed is set to setpoint, in case it didn't reach the target during entry
216 1 : _target_head_speed = HEAD_SPEED_TARGET_RATIO;
217 :
218 : // Ensure desired forward speed target is set to param value
219 1 : _desired_vel_ms = _param_target_speed_ms.get();
220 1 : }
221 :
222 : // Maintain head speed and forward speed as we glide to the ground
223 6720 : void AC_Autorotation::run_glide(float pilot_accel_norm)
224 : {
225 6720 : update_headspeed_controller();
226 :
227 6720 : update_forward_speed_controller(pilot_accel_norm);
228 6720 : }
229 :
230 6720 : void AC_Autorotation::update_headspeed_controller(void)
231 : {
232 : // Get current rpm and update healthy signal counters
233 : float head_speed_norm;
234 6720 : if (!get_norm_head_speed(head_speed_norm)) {
235 : // RPM sensor is bad, set collective to angle of -2 deg and hope for the best
236 0 : _motors_heli->set_coll_from_ang(-2.0);
237 0 : return;
238 : }
239 :
240 : // Calculate the head speed error.
241 6720 : _head_speed_error = head_speed_norm - _target_head_speed;
242 :
243 6720 : _p_term_hs = _p_hs.get_p(_head_speed_error);
244 :
245 : // Adjusting collective trim using feed forward (not yet been updated, so this value is the previous time steps collective position)
246 6720 : _ff_term_hs = col_trim_lpf.apply(_motors_heli->get_throttle(), _dt);
247 :
248 : // Calculate collective position to be set
249 6720 : const float collective_out = constrain_value((_p_term_hs + _ff_term_hs), 0.0f, 1.0f);
250 :
251 : // Send collective to setting to motors output library
252 6720 : _motors_heli->set_throttle(collective_out);
253 :
254 : #if HAL_LOGGING_ENABLED
255 : // @LoggerMessage: ARHS
256 : // @Vehicles: Copter
257 : // @Description: helicopter AutoRotation Head Speed (ARHS) controller information
258 : // @Field: TimeUS: Time since system startup
259 : // @Field: Tar: Normalised target head speed
260 : // @Field: Act: Normalised measured head speed
261 : // @Field: Err: Head speed controller error
262 : // @Field: P: P-term for head speed controller response
263 : // @Field: FF: FF-term for head speed controller response
264 :
265 : // Write to data flash log
266 13440 : AP::logger().WriteStreaming("ARHS",
267 : "TimeUS,Tar,Act,Err,P,FF",
268 : "s-----",
269 : "F00000",
270 : "Qfffff",
271 : AP_HAL::micros64(),
272 6720 : _target_head_speed,
273 : head_speed_norm,
274 6720 : _head_speed_error,
275 6720 : _p_term_hs,
276 6720 : _ff_term_hs);
277 : #endif
278 : }
279 :
280 : // Get measured head speed and normalise by head speed set point. Returns false if a valid rpm measurement cannot be obtained
281 7521 : bool AC_Autorotation::get_norm_head_speed(float& norm_rpm) const
282 : {
283 : // Assuming zero rpm is safer as it will drive collective in the direction of increasing head speed
284 7521 : float current_rpm = 0.0;
285 :
286 : #if AP_RPM_ENABLED
287 : // Get singleton for RPM library
288 7521 : const AP_RPM *rpm = AP_RPM::get_singleton();
289 :
290 : // Checking to ensure no nullptr, we do have a pre-arm check for this so it will be very bad if RPM has gone away
291 7521 : if (rpm == nullptr) {
292 0 : return false;
293 : }
294 :
295 : // Check RPM sensor is returning a healthy status
296 7521 : if (!rpm->get_rpm(_param_rpm_instance.get(), current_rpm)) {
297 0 : return false;
298 : }
299 : #endif
300 :
301 : // Protect against div by zeros later in the code
302 7521 : float head_speed_set_point = MAX(1.0, _param_head_speed_set_point.get());
303 :
304 : // Normalize the RPM by the setpoint
305 7521 : norm_rpm = current_rpm/head_speed_set_point;
306 7521 : return true;
307 : }
308 :
309 : // Update speed controller
310 14401 : void AC_Autorotation::update_forward_speed_controller(float pilot_accel_norm)
311 : {
312 : // Limiting the desired velocity based on the max acceleration limit to get an update target
313 14401 : const float min_vel_ms = _target_vel_ms - get_accel_max_mss() * _dt;
314 14401 : const float max_vel_ms = _target_vel_ms + get_accel_max_mss() * _dt;
315 14401 : _target_vel_ms = constrain_float(_desired_vel_ms, min_vel_ms, max_vel_ms); // (m/s)
316 :
317 : // Calculate acceleration target
318 14401 : const float fwd_accel_target_mss = _fwd_speed_pid.update_all(_target_vel_ms, get_speed_forward_ms(), _dt, _limit_accel); // (m/s/s)
319 :
320 : // Build the body frame XY accel vector.
321 : // Pilot can request as much as 1/2 of the max accel laterally to perform a turn.
322 : // We only allow up to half as we need to prioritize building/maintaining airspeed.
323 14401 : Vector2f bf_accel_target_mss = {fwd_accel_target_mss, pilot_accel_norm * get_accel_max_mss() * 0.5};
324 :
325 : // Ensure we do not exceed the accel limit
326 14401 : _limit_accel = bf_accel_target_mss.limit_length(get_accel_max_mss());
327 :
328 : // Calculate roll and pitch targets from angles, negative accel for negative pitch (pitch forward)
329 14401 : Vector2f angle_target_rad = { accel_mss_to_angle_rad(-bf_accel_target_mss.x), // Pitch
330 14401 : accel_mss_to_angle_rad(bf_accel_target_mss.y)}; // Roll
331 :
332 : // Ensure that the requested angles do not exceed angle max
333 14401 : _limit_accel |= angle_target_rad.limit_length(_attitude_control->lean_angle_max_rad());
334 :
335 : // we may have scaled the lateral accel in the angle limit scaling, so we need to
336 : // back calculate the resulting accel from this constrained angle for the yaw rate calc
337 14401 : const float bf_lat_accel_target_mss = angle_rad_to_accel_mss(angle_target_rad.y);
338 :
339 : // Calc yaw rate from desired body-frame accels
340 : // this seems suspiciously simple, but it is correct
341 : // accel = r * w^2, r = radius and w = angular rate
342 : // radius can be calculated as the distance traveled in the time it takes to do 360 deg
343 : // One rotation takes: (2*pi)/w seconds
344 : // Distance traveled in that time: (vel*2*pi)/w
345 : // radius for that distance: ((vel*2*pi)/w) / (2*pi)
346 : // r = vel / w
347 : // accel = (vel / w) * w^2
348 : // accel = vel * w
349 : // w = accel / vel
350 14401 : float yaw_rate_rads = 0.0;
351 14401 : if (!is_zero(_target_vel_ms)) {
352 8920 : yaw_rate_rads = bf_lat_accel_target_mss / _target_vel_ms;
353 : }
354 :
355 : // Output to attitude controller
356 14401 : _attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_rad(angle_target_rad.y, angle_target_rad.x, yaw_rate_rads);
357 :
358 : #if HAL_LOGGING_ENABLED
359 : // @LoggerMessage: ARSC
360 : // @Vehicles: Copter
361 : // @Description: Helicopter AutoRotation Speed Controller (ARSC) information
362 : // @Field: TimeUS: Time since system startup
363 : // @Field: Des: Desired forward velocity
364 : // @Field: Tar: Target forward velocity
365 : // @Field: Act: Measured forward velocity
366 : // @Field: P: Velocity to acceleration P-term component
367 : // @Field: I: Velocity to acceleration I-term component
368 : // @Field: D: Velocity to acceleration D-term component
369 : // @Field: FF: Velocity to acceleration feed forward component
370 : // @Field: Lim: Accel limit flag
371 : // @Field: FA: Forward acceleration target
372 : // @Field: LA: Lateral acceleration target
373 :
374 14401 : const AP_PIDInfo& pid_info = _fwd_speed_pid.get_pid_info();
375 28802 : AP::logger().WriteStreaming("ARSC",
376 : "TimeUS,Des,Tar,Act,P,I,D,FF,Lim,FA,LA",
377 : "snnn-----oo",
378 : "F0000000-00",
379 : "QfffffffBff",
380 : AP_HAL::micros64(),
381 14401 : _desired_vel_ms,
382 14401 : pid_info.target,
383 14401 : pid_info.actual,
384 14401 : pid_info.P,
385 14401 : pid_info.I,
386 14401 : pid_info.D,
387 14401 : pid_info.FF,
388 14401 : uint8_t(_limit_accel),
389 14401 : bf_accel_target_mss.x,
390 14401 : bf_accel_target_mss.y);
391 : #endif
392 14401 : }
393 :
394 : // smoothly zero velocity and accel
395 7681 : void AC_Autorotation::run_landed(void)
396 : {
397 7681 : _desired_vel_ms *= 0.95;
398 7681 : update_forward_speed_controller(0.0);
399 7681 : }
400 :
401 : // Determine the body frame forward speed
402 14402 : float AC_Autorotation::get_speed_forward_ms(void) const
403 : {
404 14402 : Vector3f vel_NED = {0,0,0};
405 14402 : const AP_AHRS &ahrs = AP::ahrs();
406 14402 : if (ahrs.get_velocity_NED(vel_NED)) {
407 14402 : vel_NED = ahrs.earth_to_body(vel_NED);
408 : }
409 : // TODO: need to improve the handling of the velocity NED not ok case
410 14402 : return vel_NED.x;
411 : }
412 :
413 : #if HAL_LOGGING_ENABLED
414 : // Logging of lower rate autorotation specific variables. This is meant for stuff that
415 : // doesn't need a high rate, e.g. controller variables that are need for tuning.
416 848 : void AC_Autorotation::log_write_autorotation(void) const
417 : {
418 : // enum class for bitmask documentation in logging
419 : enum class AC_Autorotation_Landed_Reason : uint8_t {
420 : LOW_SPEED = 1<<0, // true if below 1 m/s
421 : LAND_COL = 1<<1, // true if collective below land col min
422 : IS_STILL = 1<<2, // passes inertial nav is_still() check
423 : };
424 :
425 848 : uint8_t reason = 0;
426 848 : if (_landed_reason.min_speed) {
427 474 : reason |= uint8_t(AC_Autorotation_Landed_Reason::LOW_SPEED);
428 : }
429 848 : if (_landed_reason.land_col) {
430 453 : reason |= uint8_t(AC_Autorotation_Landed_Reason::LAND_COL);
431 : }
432 848 : if (_landed_reason.is_still) {
433 833 : reason |= uint8_t(AC_Autorotation_Landed_Reason::IS_STILL);
434 : }
435 :
436 : // @LoggerMessage: AROT
437 : // @Vehicles: Copter
438 : // @Description: Helicopter AutoROTation (AROT) information
439 : // @Field: TimeUS: Time since system startup
440 : // @Field: LR: Landed Reason state flags
441 : // @FieldBitmaskEnum: LR: AC_Autorotation_Landed_Reason
442 :
443 : // Write to data flash log
444 848 : AP::logger().WriteStreaming("AROT",
445 : "TimeUS,LR",
446 : "s-",
447 : "F-",
448 : "QB",
449 : AP_HAL::micros64(),
450 : reason);
451 848 : }
452 : #endif // HAL_LOGGING_ENABLED
453 :
454 : // Arming checks for autorotation, mostly checking for miss-configurations
455 2339 : bool AC_Autorotation::arming_checks(size_t buflen, char *buffer) const
456 : {
457 2339 : if (!enabled()) {
458 : // Don't run arming checks if not enabled
459 2206 : return true;
460 : }
461 :
462 : // Check for correct RPM sensor config
463 : #if AP_RPM_ENABLED
464 : // Get singleton for RPM library
465 133 : const AP_RPM *rpm = AP_RPM::get_singleton();
466 :
467 : // Get current rpm, checking to ensure no nullptr
468 133 : if (rpm == nullptr) {
469 0 : hal.util->snprintf(buffer, buflen, "Can't access RPM");
470 0 : return false;
471 : }
472 :
473 : // Sanity check that the designated rpm sensor instance is there
474 133 : if (_param_rpm_instance.get() < 0) {
475 21 : hal.util->snprintf(buffer, buflen, "RPM instance <0");
476 21 : return false;
477 : }
478 :
479 112 : if (!rpm->enabled(_param_rpm_instance.get())) {
480 32 : hal.util->snprintf(buffer, buflen, "RPM%i not enabled", _param_rpm_instance.get()+1);
481 32 : return false;
482 : }
483 : #endif
484 :
485 : // Check that heli motors is configured for autorotation
486 80 : if (!_motors_heli->rsc_autorotation_enabled()) {
487 38 : hal.util->snprintf(buffer, buflen, "H_RSC_AROT_* not configured");
488 38 : return false;
489 : }
490 :
491 42 : return true;
492 : }
493 :
494 : // Check if we believe we have landed. We need the landed state to zero all
495 : // controls and make sure that the copter landing detector will trip
496 6721 : bool AC_Autorotation::check_landed(void)
497 : {
498 : // minimum speed (m/s) used for "is moving" check
499 6721 : const float min_moving_speed = 1.0;
500 :
501 6721 : Vector3f velocity;
502 6721 : const AP_AHRS &ahrs = AP::ahrs();
503 6721 : _landed_reason.min_speed = ahrs.get_velocity_NED(velocity) && (velocity.length() < min_moving_speed);
504 6721 : _landed_reason.land_col = _motors_heli->get_below_land_min_coll();
505 6721 : _landed_reason.is_still = AP::ins().is_still();
506 :
507 6721 : return _landed_reason.min_speed && _landed_reason.land_col && _landed_reason.is_still;
508 : }
509 :
510 : // Dynamically update time step used in autorotation controllers
511 14401 : void AC_Autorotation::set_dt(float delta_sec)
512 : {
513 14401 : if (is_positive(delta_sec)) {
514 14401 : _dt = delta_sec;
515 14401 : return;
516 : }
517 0 : _dt = 2.5e-3; // Assume 400 Hz
518 : }
|
