-
Notifications
You must be signed in to change notification settings - Fork 17k
/
mode_poshold.cpp
640 lines (536 loc) · 30 KB
/
mode_poshold.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
#include "Copter.h"
#if MODE_POSHOLD_ENABLED == ENABLED
/*
* Init and run calls for PosHold flight mode
* PosHold tries to improve upon regular loiter by mixing the pilot input with the loiter controller
*/
#define POSHOLD_SPEED_0 10 // speed below which it is always safe to switch to loiter
// 400hz loop update rate
#define POSHOLD_BRAKE_TIME_ESTIMATE_MAX (600*4) // max number of cycles the brake will be applied before we switch to loiter
#define POSHOLD_BRAKE_TO_LOITER_TIMER (150*4) // Number of cycles to transition from brake mode to loiter mode. Must be lower than POSHOLD_LOITER_STAB_TIMER
#define POSHOLD_WIND_COMP_START_TIMER (150*4) // Number of cycles to start wind compensation update after loiter is engaged
#define POSHOLD_CONTROLLER_TO_PILOT_MIX_TIMER (50*4) // Set it from 100 to 200, the number of centiseconds loiter and manual commands are mixed to make a smooth transition.
#define POSHOLD_SMOOTH_RATE_FACTOR 0.0125f // filter applied to pilot's roll/pitch input as it returns to center. A lower number will cause the roll/pitch to return to zero more slowly if the brake_rate is also low.
#define POSHOLD_WIND_COMP_TIMER_10HZ 40 // counter value used to reduce wind compensation to 10hz
#define LOOP_RATE_FACTOR 4 // used to adapt PosHold params to loop_rate
#define TC_WIND_COMP 0.0025f // Time constant for poshold_update_wind_comp_estimate()
// definitions that are independent of main loop rate
#define POSHOLD_STICK_RELEASE_SMOOTH_ANGLE 1800 // max angle required (in centi-degrees) after which the smooth stick release effect is applied
#define POSHOLD_WIND_COMP_ESTIMATE_SPEED_MAX 10 // wind compensation estimates will only run when velocity is at or below this speed in cm/s
#define POSHOLD_WIND_COMP_LEAN_PCT_MAX 0.6666f // wind compensation no more than 2/3rds of angle max to ensure pilot can always override
// poshold_init - initialise PosHold controller
bool ModePosHold::init(bool ignore_checks)
{
// set vertical speed and acceleration limits
pos_control->set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
pos_control->set_correction_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
// initialise the vertical position controller
if (!pos_control->is_active_z()) {
pos_control->init_z_controller();
}
// initialise lean angles to current attitude
pilot_roll = 0.0f;
pilot_pitch = 0.0f;
// compute brake_gain
brake.gain = (15.0f * (float)g.poshold_brake_rate + 95.0f) * 0.01f;
if (copter.ap.land_complete) {
// if landed begin in loiter mode
roll_mode = RPMode::LOITER;
pitch_mode = RPMode::LOITER;
} else {
// if not landed start in pilot override to avoid hard twitch
roll_mode = RPMode::PILOT_OVERRIDE;
pitch_mode = RPMode::PILOT_OVERRIDE;
}
// initialise loiter
loiter_nav->clear_pilot_desired_acceleration();
loiter_nav->init_target();
// initialise wind_comp each time PosHold is switched on
init_wind_comp_estimate();
return true;
}
// poshold_run - runs the PosHold controller
// should be called at 100hz or more
void ModePosHold::run()
{
float controller_to_pilot_roll_mix; // mix of controller and pilot controls. 0 = fully last controller controls, 1 = fully pilot controls
float controller_to_pilot_pitch_mix; // mix of controller and pilot controls. 0 = fully last controller controls, 1 = fully pilot controls
const Vector3f& vel = inertial_nav.get_velocity_neu_cms();
// set vertical speed and acceleration limits
pos_control->set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
loiter_nav->clear_pilot_desired_acceleration();
// apply SIMPLE mode transform to pilot inputs
update_simple_mode();
// convert pilot input to lean angles
float target_roll, target_pitch;
get_pilot_desired_lean_angles(target_roll, target_pitch, copter.aparm.angle_max, attitude_control->get_althold_lean_angle_max_cd());
// get pilot's desired yaw rate
float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz());
// get pilot desired climb rate (for alt-hold mode and take-off)
float target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in());
target_climb_rate = constrain_float(target_climb_rate, -get_pilot_speed_dn(), g.pilot_speed_up);
// relax loiter target if we might be landed
if (copter.ap.land_complete_maybe) {
loiter_nav->soften_for_landing();
}
// Pos Hold State Machine Determination
AltHoldModeState poshold_state = get_alt_hold_state(target_climb_rate);
// state machine
switch (poshold_state) {
case AltHoldModeState::MotorStopped:
attitude_control->reset_rate_controller_I_terms();
attitude_control->reset_yaw_target_and_rate(false);
pos_control->relax_z_controller(0.0f); // forces throttle output to decay to zero
loiter_nav->clear_pilot_desired_acceleration();
loiter_nav->init_target();
// set poshold state to pilot override
roll_mode = RPMode::PILOT_OVERRIDE;
pitch_mode = RPMode::PILOT_OVERRIDE;
// initialise wind compensation estimate
init_wind_comp_estimate();
break;
case AltHoldModeState::Landed_Ground_Idle:
loiter_nav->clear_pilot_desired_acceleration();
loiter_nav->init_target();
attitude_control->reset_yaw_target_and_rate();
init_wind_comp_estimate();
FALLTHROUGH;
case AltHoldModeState::Landed_Pre_Takeoff:
attitude_control->reset_rate_controller_I_terms_smoothly();
pos_control->relax_z_controller(0.0f); // forces throttle output to decay to zero
// set poshold state to pilot override
roll_mode = RPMode::PILOT_OVERRIDE;
pitch_mode = RPMode::PILOT_OVERRIDE;
break;
case AltHoldModeState::Takeoff:
// initiate take-off
if (!takeoff.running()) {
takeoff.start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f));
}
// get avoidance adjusted climb rate
target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);
// set position controller targets adjusted for pilot input
takeoff.do_pilot_takeoff(target_climb_rate);
// init and update loiter although pilot is controlling lean angles
loiter_nav->clear_pilot_desired_acceleration();
loiter_nav->init_target();
// set poshold state to pilot override
roll_mode = RPMode::PILOT_OVERRIDE;
pitch_mode = RPMode::PILOT_OVERRIDE;
break;
case AltHoldModeState::Flying:
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// get avoidance adjusted climb rate
target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);
#if AP_RANGEFINDER_ENABLED
// update the vertical offset based on the surface measurement
copter.surface_tracking.update_surface_offset();
#endif
// Send the commanded climb rate to the position controller
pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate);
break;
}
// poshold specific behaviour to calculate desired roll, pitch angles
// convert inertial nav earth-frame velocities to body-frame
// To-Do: move this to AP_Math (or perhaps we already have a function to do this)
float vel_fw = vel.x*ahrs.cos_yaw() + vel.y*ahrs.sin_yaw();
float vel_right = -vel.x*ahrs.sin_yaw() + vel.y*ahrs.cos_yaw();
// If not in LOITER, retrieve latest wind compensation lean angles related to current yaw
if (roll_mode != RPMode::LOITER || pitch_mode != RPMode::LOITER) {
get_wind_comp_lean_angles(wind_comp_roll, wind_comp_pitch);
}
// Roll state machine
// Each state (aka mode) is responsible for:
// 1. dealing with pilot input
// 2. calculating the final roll output to the attitude controller
// 3. checking if the state (aka mode) should be changed and if 'yes' perform any required initialisation for the new state
switch (roll_mode) {
case RPMode::PILOT_OVERRIDE:
// update pilot desired roll angle using latest radio input
// this filters the input so that it returns to zero no faster than the brake-rate
update_pilot_lean_angle(pilot_roll, target_roll);
// switch to BRAKE mode for next iteration if no pilot input
if (is_zero(target_roll) && (fabsf(pilot_roll) < 2 * g.poshold_brake_rate)) {
// initialise BRAKE mode
roll_mode = RPMode::BRAKE; // Set brake roll mode
brake.roll = 0.0f; // initialise braking angle to zero
brake.angle_max_roll = 0.0f; // reset brake_angle_max so we can detect when vehicle begins to flatten out during braking
brake.timeout_roll = POSHOLD_BRAKE_TIME_ESTIMATE_MAX; // number of cycles the brake will be applied, updated during braking mode.
brake.time_updated_roll = false; // flag the braking time can be re-estimated
}
// final lean angle should be pilot input plus wind compensation
roll = pilot_roll + wind_comp_roll;
break;
case RPMode::BRAKE:
case RPMode::BRAKE_READY_TO_LOITER:
// calculate brake.roll angle to counter-act velocity
update_brake_angle_from_velocity(brake.roll, vel_right);
// update braking time estimate
if (!brake.time_updated_roll) {
// check if brake angle is increasing
if (fabsf(brake.roll) >= brake.angle_max_roll) {
brake.angle_max_roll = fabsf(brake.roll);
} else {
// braking angle has started decreasing so re-estimate braking time
brake.timeout_roll = 1+(uint16_t)(LOOP_RATE_FACTOR*15L*(int32_t)(fabsf(brake.roll))/(10L*(int32_t)g.poshold_brake_rate)); // the 1.2 (12/10) factor has to be tuned in flight, here it means 120% of the "normal" time.
brake.time_updated_roll = true;
}
}
// if velocity is very low reduce braking time to 0.5seconds
if ((fabsf(vel_right) <= POSHOLD_SPEED_0) && (brake.timeout_roll > 50*LOOP_RATE_FACTOR)) {
brake.timeout_roll = 50*LOOP_RATE_FACTOR;
}
// reduce braking timer
if (brake.timeout_roll > 0) {
brake.timeout_roll--;
} else {
// indicate that we are ready to move to Loiter.
// Loiter will only actually be engaged once both roll_mode and pitch_mode are changed to RPMode::BRAKE_READY_TO_LOITER
// logic for engaging loiter is handled below the roll and pitch mode switch statements
roll_mode = RPMode::BRAKE_READY_TO_LOITER;
}
// final lean angle is braking angle + wind compensation angle
roll = brake.roll + wind_comp_roll;
// check for pilot input
if (!is_zero(target_roll)) {
// init transition to pilot override
roll_controller_to_pilot_override();
}
break;
case RPMode::BRAKE_TO_LOITER:
case RPMode::LOITER:
// these modes are combined roll-pitch modes and are handled below
break;
case RPMode::CONTROLLER_TO_PILOT_OVERRIDE:
// update pilot desired roll angle using latest radio input
// this filters the input so that it returns to zero no faster than the brake-rate
update_pilot_lean_angle(pilot_roll, target_roll);
// count-down loiter to pilot timer
if (controller_to_pilot_timer_roll > 0) {
controller_to_pilot_timer_roll--;
} else {
// when timer runs out switch to full pilot override for next iteration
roll_mode = RPMode::PILOT_OVERRIDE;
}
// calculate controller_to_pilot mix ratio
controller_to_pilot_roll_mix = (float)controller_to_pilot_timer_roll / (float)POSHOLD_CONTROLLER_TO_PILOT_MIX_TIMER;
// mix final loiter lean angle and pilot desired lean angles
roll = mix_controls(controller_to_pilot_roll_mix, controller_final_roll, pilot_roll + wind_comp_roll);
break;
}
// Pitch state machine
// Each state (aka mode) is responsible for:
// 1. dealing with pilot input
// 2. calculating the final pitch output to the attitude contpitcher
// 3. checking if the state (aka mode) should be changed and if 'yes' perform any required initialisation for the new state
switch (pitch_mode) {
case RPMode::PILOT_OVERRIDE:
// update pilot desired pitch angle using latest radio input
// this filters the input so that it returns to zero no faster than the brake-rate
update_pilot_lean_angle(pilot_pitch, target_pitch);
// switch to BRAKE mode for next iteration if no pilot input
if (is_zero(target_pitch) && (fabsf(pilot_pitch) < 2 * g.poshold_brake_rate)) {
// initialise BRAKE mode
pitch_mode = RPMode::BRAKE; // set brake pitch mode
brake.pitch = 0.0f; // initialise braking angle to zero
brake.angle_max_pitch = 0.0f; // reset brake_angle_max so we can detect when vehicle begins to flatten out during braking
brake.timeout_pitch = POSHOLD_BRAKE_TIME_ESTIMATE_MAX; // number of cycles the brake will be applied, updated during braking mode.
brake.time_updated_pitch = false; // flag the braking time can be re-estimated
}
// final lean angle should be pilot input plus wind compensation
pitch = pilot_pitch + wind_comp_pitch;
break;
case RPMode::BRAKE:
case RPMode::BRAKE_READY_TO_LOITER:
// calculate brake_pitch angle to counter-act velocity
update_brake_angle_from_velocity(brake.pitch, -vel_fw);
// update braking time estimate
if (!brake.time_updated_pitch) {
// check if brake angle is increasing
if (fabsf(brake.pitch) >= brake.angle_max_pitch) {
brake.angle_max_pitch = fabsf(brake.pitch);
} else {
// braking angle has started decreasing so re-estimate braking time
brake.timeout_pitch = 1+(uint16_t)(LOOP_RATE_FACTOR*15L*(int32_t)(fabsf(brake.pitch))/(10L*(int32_t)g.poshold_brake_rate)); // the 1.2 (12/10) factor has to be tuned in flight, here it means 120% of the "normal" time.
brake.time_updated_pitch = true;
}
}
// if velocity is very low reduce braking time to 0.5seconds
if ((fabsf(vel_fw) <= POSHOLD_SPEED_0) && (brake.timeout_pitch > 50*LOOP_RATE_FACTOR)) {
brake.timeout_pitch = 50*LOOP_RATE_FACTOR;
}
// reduce braking timer
if (brake.timeout_pitch > 0) {
brake.timeout_pitch--;
} else {
// indicate that we are ready to move to Loiter.
// Loiter will only actually be engaged once both pitch_mode and pitch_mode are changed to RPMode::BRAKE_READY_TO_LOITER
// logic for engaging loiter is handled below the pitch and pitch mode switch statements
pitch_mode = RPMode::BRAKE_READY_TO_LOITER;
}
// final lean angle is braking angle + wind compensation angle
pitch = brake.pitch + wind_comp_pitch;
// check for pilot input
if (!is_zero(target_pitch)) {
// init transition to pilot override
pitch_controller_to_pilot_override();
}
break;
case RPMode::BRAKE_TO_LOITER:
case RPMode::LOITER:
// these modes are combined pitch-pitch modes and are handled below
break;
case RPMode::CONTROLLER_TO_PILOT_OVERRIDE:
// update pilot desired pitch angle using latest radio input
// this filters the input so that it returns to zero no faster than the brake-rate
update_pilot_lean_angle(pilot_pitch, target_pitch);
// count-down loiter to pilot timer
if (controller_to_pilot_timer_pitch > 0) {
controller_to_pilot_timer_pitch--;
} else {
// when timer runs out switch to full pilot override for next iteration
pitch_mode = RPMode::PILOT_OVERRIDE;
}
// calculate controller_to_pilot mix ratio
controller_to_pilot_pitch_mix = (float)controller_to_pilot_timer_pitch / (float)POSHOLD_CONTROLLER_TO_PILOT_MIX_TIMER;
// mix final loiter lean angle and pilot desired lean angles
pitch = mix_controls(controller_to_pilot_pitch_mix, controller_final_pitch, pilot_pitch + wind_comp_pitch);
break;
}
//
// Shared roll & pitch states (RPMode::BRAKE_TO_LOITER and RPMode::LOITER)
//
// switch into LOITER mode when both roll and pitch are ready
if (roll_mode == RPMode::BRAKE_READY_TO_LOITER && pitch_mode == RPMode::BRAKE_READY_TO_LOITER) {
roll_mode = RPMode::BRAKE_TO_LOITER;
pitch_mode = RPMode::BRAKE_TO_LOITER;
brake.to_loiter_timer = POSHOLD_BRAKE_TO_LOITER_TIMER;
// init loiter controller
loiter_nav->init_target(inertial_nav.get_position_xy_cm());
// set delay to start of wind compensation estimate updates
wind_comp_start_timer = POSHOLD_WIND_COMP_START_TIMER;
}
// roll-mode is used as the combined roll+pitch mode when in BRAKE_TO_LOITER or LOITER modes
if (roll_mode == RPMode::BRAKE_TO_LOITER || roll_mode == RPMode::LOITER) {
// force pitch mode to be same as roll_mode just to keep it consistent (it's not actually used in these states)
pitch_mode = roll_mode;
// handle combined roll+pitch mode
switch (roll_mode) {
case RPMode::BRAKE_TO_LOITER: {
// reduce brake_to_loiter timer
if (brake.to_loiter_timer > 0) {
brake.to_loiter_timer--;
} else {
// progress to full loiter on next iteration
roll_mode = RPMode::LOITER;
pitch_mode = RPMode::LOITER;
}
// mix of brake and loiter controls. 0 = fully brake
// controls, 1 = fully loiter controls
const float brake_to_loiter_mix = (float)brake.to_loiter_timer / (float)POSHOLD_BRAKE_TO_LOITER_TIMER;
// calculate brake.roll and pitch angles to counter-act velocity
update_brake_angle_from_velocity(brake.roll, vel_right);
update_brake_angle_from_velocity(brake.pitch, -vel_fw);
// run loiter controller
loiter_nav->update(false);
// calculate final roll and pitch output by mixing loiter and brake controls
roll = mix_controls(brake_to_loiter_mix, brake.roll + wind_comp_roll, loiter_nav->get_roll());
pitch = mix_controls(brake_to_loiter_mix, brake.pitch + wind_comp_pitch, loiter_nav->get_pitch());
// check for pilot input
if (!is_zero(target_roll) || !is_zero(target_pitch)) {
// if roll input switch to pilot override for roll
if (!is_zero(target_roll)) {
// init transition to pilot override
roll_controller_to_pilot_override();
// switch pitch-mode to brake (but ready to go back to loiter anytime)
// no need to reset brake.pitch here as wind comp has not been updated since last brake.pitch computation
pitch_mode = RPMode::BRAKE_READY_TO_LOITER;
}
// if pitch input switch to pilot override for pitch
if (!is_zero(target_pitch)) {
// init transition to pilot override
pitch_controller_to_pilot_override();
if (is_zero(target_roll)) {
// switch roll-mode to brake (but ready to go back to loiter anytime)
// no need to reset brake.roll here as wind comp has not been updated since last brake.roll computation
roll_mode = RPMode::BRAKE_READY_TO_LOITER;
}
}
}
break;
}
case RPMode::LOITER:
// run loiter controller
loiter_nav->update(false);
// set roll angle based on loiter controller outputs
roll = loiter_nav->get_roll();
pitch = loiter_nav->get_pitch();
// update wind compensation estimate
update_wind_comp_estimate();
// check for pilot input
if (!is_zero(target_roll) || !is_zero(target_pitch)) {
// if roll input switch to pilot override for roll
if (!is_zero(target_roll)) {
// init transition to pilot override
roll_controller_to_pilot_override();
// switch pitch-mode to brake (but ready to go back to loiter anytime)
pitch_mode = RPMode::BRAKE_READY_TO_LOITER;
// reset brake.pitch because wind_comp is now different and should give the compensation of the whole previous loiter angle
brake.pitch = 0.0f;
}
// if pitch input switch to pilot override for pitch
if (!is_zero(target_pitch)) {
// init transition to pilot override
pitch_controller_to_pilot_override();
// if roll not overriden switch roll-mode to brake (but be ready to go back to loiter any time)
if (is_zero(target_roll)) {
roll_mode = RPMode::BRAKE_READY_TO_LOITER;
brake.roll = 0.0f;
}
// if roll not overridden switch roll-mode to brake (but be ready to go back to loiter any time)
}
}
break;
default:
// do nothing for uncombined roll and pitch modes
break;
}
}
// constrain target pitch/roll angles
float angle_max = copter.aparm.angle_max;
roll = constrain_float(roll, -angle_max, angle_max);
pitch = constrain_float(pitch, -angle_max, angle_max);
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(roll, pitch, target_yaw_rate);
// run the vertical position controller and set output throttle
pos_control->update_z_controller();
}
// poshold_update_pilot_lean_angle - update the pilot's filtered lean angle with the latest raw input received
void ModePosHold::update_pilot_lean_angle(float &lean_angle_filtered, float &lean_angle_raw)
{
// if raw input is large or reversing the vehicle's lean angle immediately set the fitlered angle to the new raw angle
if ((lean_angle_filtered > 0 && lean_angle_raw < 0) || (lean_angle_filtered < 0 && lean_angle_raw > 0) || (fabsf(lean_angle_raw) > POSHOLD_STICK_RELEASE_SMOOTH_ANGLE)) {
lean_angle_filtered = lean_angle_raw;
} else {
// lean_angle_raw must be pulling lean_angle_filtered towards zero, smooth the decrease
if (lean_angle_filtered > 0) {
// reduce the filtered lean angle at 5% or the brake rate (whichever is faster).
lean_angle_filtered -= MAX(lean_angle_filtered * POSHOLD_SMOOTH_RATE_FACTOR, MAX(1.0f, g.poshold_brake_rate/(float)LOOP_RATE_FACTOR));
// do not let the filtered angle fall below the pilot's input lean angle.
// the above line pulls the filtered angle down and the below line acts as a catch
lean_angle_filtered = MAX(lean_angle_filtered, lean_angle_raw);
}else{
lean_angle_filtered += MAX(-lean_angle_filtered * POSHOLD_SMOOTH_RATE_FACTOR, MAX(1.0f, g.poshold_brake_rate/(float)LOOP_RATE_FACTOR));
lean_angle_filtered = MIN(lean_angle_filtered, lean_angle_raw);
}
}
}
// mix_controls - mixes two controls based on the mix_ratio
// mix_ratio of 1 = use first_control completely, 0 = use second_control completely, 0.5 = mix evenly
float ModePosHold::mix_controls(float mix_ratio, float first_control, float second_control)
{
mix_ratio = constrain_float(mix_ratio, 0.0f, 1.0f);
return mix_ratio * first_control + (1.0f - mix_ratio) * second_control;
}
// update_brake_angle_from_velocity - updates the brake_angle based on the vehicle's velocity and brake_gain
// brake_angle is slewed with the wpnav.poshold_brake_rate and constrained by the wpnav.poshold_braking_angle_max
// velocity is assumed to be in the same direction as lean angle so for pitch you should provide the velocity backwards (i.e. -ve forward velocity)
void ModePosHold::update_brake_angle_from_velocity(float &brake_angle, float velocity)
{
float lean_angle;
float brake_rate = g.poshold_brake_rate;
brake_rate /= (float)LOOP_RATE_FACTOR;
if (brake_rate <= 1.0f) {
brake_rate = 1.0f;
}
// calculate velocity-only based lean angle
if (velocity >= 0) {
lean_angle = -brake.gain * velocity * (1.0f + 500.0f / (velocity + 60.0f));
} else {
lean_angle = -brake.gain * velocity * (1.0f + 500.0f / (-velocity + 60.0f));
}
// do not let lean_angle be too far from brake_angle
brake_angle = constrain_float(lean_angle, brake_angle - brake_rate, brake_angle + brake_rate);
// constrain final brake_angle
brake_angle = constrain_float(brake_angle, -(float)g.poshold_brake_angle_max, (float)g.poshold_brake_angle_max);
}
// initialise wind compensation estimate back to zero
void ModePosHold::init_wind_comp_estimate()
{
wind_comp_ef.zero();
wind_comp_timer = 0;
wind_comp_roll = 0.0f;
wind_comp_pitch = 0.0f;
}
// update_wind_comp_estimate - updates wind compensation estimate
// should be called at the maximum loop rate when loiter is engaged
void ModePosHold::update_wind_comp_estimate()
{
// check wind estimate start has not been delayed
if (wind_comp_start_timer > 0) {
wind_comp_start_timer--;
return;
}
// check horizontal velocity is low
if (inertial_nav.get_speed_xy_cms() > POSHOLD_WIND_COMP_ESTIMATE_SPEED_MAX) {
return;
}
// get position controller accel target
const Vector3f& accel_target = pos_control->get_accel_target_cmss();
// update wind compensation in earth-frame lean angles
if (is_zero(wind_comp_ef.x)) {
// if wind compensation has not been initialised set it immediately to the pos controller's desired accel in north direction
wind_comp_ef.x = accel_target.x;
} else {
// low pass filter the position controller's lean angle output
wind_comp_ef.x = (1.0f-TC_WIND_COMP)*wind_comp_ef.x + TC_WIND_COMP*accel_target.x;
}
if (is_zero(wind_comp_ef.y)) {
// if wind compensation has not been initialised set it immediately to the pos controller's desired accel in north direction
wind_comp_ef.y = accel_target.y;
} else {
// low pass filter the position controller's lean angle output
wind_comp_ef.y = (1.0f-TC_WIND_COMP)*wind_comp_ef.y + TC_WIND_COMP*accel_target.y;
}
// limit acceleration
const float accel_lim_cmss = tanf(radians(POSHOLD_WIND_COMP_LEAN_PCT_MAX * copter.aparm.angle_max * 0.01f)) * 981.0f;
const float wind_comp_ef_len = wind_comp_ef.length();
if (!is_zero(accel_lim_cmss) && (wind_comp_ef_len > accel_lim_cmss)) {
wind_comp_ef *= accel_lim_cmss / wind_comp_ef_len;
}
}
// get_wind_comp_lean_angles - retrieve wind compensation angles in body frame roll and pitch angles
// should be called at the maximum loop rate
void ModePosHold::get_wind_comp_lean_angles(float &roll_angle, float &pitch_angle)
{
// reduce rate to 10hz
wind_comp_timer++;
if (wind_comp_timer < POSHOLD_WIND_COMP_TIMER_10HZ) {
return;
}
wind_comp_timer = 0;
// convert earth frame desired accelerations to body frame roll and pitch lean angles
roll_angle = atanf((-wind_comp_ef.x*ahrs.sin_yaw() + wind_comp_ef.y*ahrs.cos_yaw())/(GRAVITY_MSS*100))*(18000.0f/M_PI);
pitch_angle = atanf(-(wind_comp_ef.x*ahrs.cos_yaw() + wind_comp_ef.y*ahrs.sin_yaw())/(GRAVITY_MSS*100))*(18000.0f/M_PI);
}
// roll_controller_to_pilot_override - initialises transition from a controller submode (brake or loiter) to a pilot override on roll axis
void ModePosHold::roll_controller_to_pilot_override()
{
roll_mode = RPMode::CONTROLLER_TO_PILOT_OVERRIDE;
controller_to_pilot_timer_roll = POSHOLD_CONTROLLER_TO_PILOT_MIX_TIMER;
// initialise pilot_roll to 0, wind_comp will be updated to compensate and poshold_update_pilot_lean_angle function shall not smooth this transition at next iteration. so 0 is the right value
pilot_roll = 0.0f;
// store final controller output for mixing with pilot input
controller_final_roll = roll;
}
// pitch_controller_to_pilot_override - initialises transition from a controller submode (brake or loiter) to a pilot override on roll axis
void ModePosHold::pitch_controller_to_pilot_override()
{
pitch_mode = RPMode::CONTROLLER_TO_PILOT_OVERRIDE;
controller_to_pilot_timer_pitch = POSHOLD_CONTROLLER_TO_PILOT_MIX_TIMER;
// initialise pilot_pitch to 0, wind_comp will be updated to compensate and update_pilot_lean_angle function shall not smooth this transition at next iteration. so 0 is the right value
pilot_pitch = 0.0f;
// store final loiter outputs for mixing with pilot input
controller_final_pitch = pitch;
}
#endif