Line data Source code
1 : #include "Copter.h"
2 :
3 : #if MODE_POSHOLD_ENABLED
4 :
5 : /*
6 : * Init and run calls for PosHold flight mode
7 : * PosHold tries to improve upon regular loiter by mixing the pilot input with the loiter controller
8 : */
9 :
10 : #define POSHOLD_SPEED_0 10 // speed below which it is always safe to switch to loiter
11 :
12 : #define POSHOLD_BRAKE_TIME_ESTIMATE_MAX_MS 6000 // Maximum duration (ms) allowed for braking before transitioning to loiter
13 : #define POSHOLD_BRAKE_TO_LOITER_TIME_MS 1500 // Duration (ms) over which braking is blended into loiter control during BRAKE_TO_LOITER phase
14 : #define POSHOLD_WIND_COMP_START_TIME_MS 1500 // Delay (ms) after entering loiter before wind compensation begins updating
15 : #define POSHOLD_CONTROLLER_TO_PILOT_MIX_TIME_MS 500 // Duration (ms) over which control is blended from autopilot to pilot input during CONTROLLER_TO_PILOT_OVERRIDE
16 : #define POSHOLD_SMOOTH_RATE_FACTOR 0.0125f // Low-pass filter factor for smoothing pilot roll/pitch input as it returns to center
17 : #define TC_WIND_COMP 0.0025f // Time constant for filtering wind compensation lean angle estimates (used in low-pass filter)
18 :
19 : // definitions that are independent of main loop rate
20 : #define POSHOLD_STICK_RELEASE_SMOOTH_ANGLE_RAD radians(18.0f) // max angle required (in radians) after which the smooth stick release effect is applied
21 : #define POSHOLD_WIND_COMP_ESTIMATE_SPEED_MAX_MS 0.10 // wind compensation estimates will only run when velocity is at or below this speed in cm/s
22 : #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
23 :
24 : #ifndef POSHOLD_BRAKE_ANGLE_DEG_DEFAULT
25 : #if FRAME_CONFIG == HELI_FRAME
26 : # define POSHOLD_BRAKE_ANGLE_DEG_DEFAULT 8 // default PHLD_BRK_ANGLE param value. Max lean angle during braking in degrees
27 : #else
28 : # define POSHOLD_BRAKE_ANGLE_DEG_DEFAULT 30 // default PHLD_BRK_ANGLE param value. Max lean angle during braking in degrees
29 : #endif
30 : #endif
31 :
32 : // table of user settable parameters
33 : const AP_Param::GroupInfo ModePosHold::var_info[] = {
34 :
35 : // @Param: BRK_ANGLE
36 : // @DisplayName: PosHold braking angle max
37 : // @Description: PosHold flight mode's max lean angle during braking in degrees
38 : // @Units: deg
39 : // @Increment: 1
40 : // @Range: 20 45
41 : // @User: Advanced
42 : AP_GROUPINFO("BRK_ANGLE", 1, ModePosHold, brake_angle_max_deg, POSHOLD_BRAKE_ANGLE_DEG_DEFAULT),
43 :
44 : AP_GROUPEND
45 : };
46 :
47 : // constructor
48 1001 : ModePosHold::ModePosHold() : Mode()
49 : {
50 : // load parameter defaults
51 1001 : AP_Param::setup_object_defaults(this, var_info);
52 1001 : }
53 :
54 : // convert parameters
55 1001 : void ModePosHold::convert_params()
56 : {
57 : // PARAMETER_CONVERSION - Added: Feb-2026 for ArduPilot-4.7
58 :
59 : // return immediately if parameter conversion has already been performed
60 1001 : if (brake_angle_max_deg.configured()) {
61 0 : return;
62 : }
63 :
64 : static const AP_Param::ConversionInfo conversion_info[] = {
65 : { Parameters::k_param_poshold_brake_angle_max, 0, AP_PARAM_INT16, "PHLD_BRK_ANGLE" }, // PHLD_BRAKE_ANGLE moved to PHLD_BRK_ANGLE
66 : };
67 1001 : AP_Param::convert_old_parameters_scaled(conversion_info, ARRAY_SIZE(conversion_info), 0.01, 0);
68 : }
69 :
70 : // poshold_init - initialise PosHold controller
71 12 : bool ModePosHold::init(bool ignore_checks)
72 : {
73 : // set vertical speed and acceleration limits
74 12 : pos_control->D_set_max_speed_accel_m(get_pilot_speed_dn_ms(), get_pilot_speed_up_ms(), get_pilot_accel_D_mss());
75 12 : pos_control->D_set_correction_speed_accel_m(get_pilot_speed_dn_ms(), get_pilot_speed_up_ms(), get_pilot_accel_D_mss());
76 :
77 : // initialise the vertical position controller
78 12 : if (!pos_control->D_is_active()) {
79 9 : pos_control->D_init_controller();
80 : }
81 :
82 : // initialise lean angles to current attitude
83 12 : pilot_roll_rad = 0.0f;
84 12 : pilot_pitch_rad = 0.0f;
85 :
86 : // compute brake_gain in rad/(m/s);
87 12 : brake.gain = radians((15.0f * (float)g.poshold_brake_rate_degs + 95.0f) * 0.01f);
88 :
89 12 : if (copter.ap.land_complete) {
90 : // if landed begin in loiter mode
91 11 : roll_mode = RPMode::LOITER;
92 11 : pitch_mode = RPMode::LOITER;
93 : } else {
94 : // if not landed start in pilot override to avoid hard twitch
95 1 : roll_mode = RPMode::PILOT_OVERRIDE;
96 1 : pitch_mode = RPMode::PILOT_OVERRIDE;
97 : }
98 :
99 : // initialise loiter
100 12 : loiter_nav->clear_pilot_desired_acceleration();
101 12 : loiter_nav->init_target();
102 :
103 : // initialise wind_comp each time PosHold is switched on
104 12 : init_wind_comp_estimate();
105 :
106 12 : return true;
107 : }
108 :
109 : // poshold_run - runs the PosHold controller
110 : // should be called at 100hz or more
111 216561 : void ModePosHold::run()
112 : {
113 216561 : const uint32_t now_ms = AP_HAL::millis();
114 : float controller_to_pilot_roll_mix; // mix of controller and pilot controls. 0 = fully last controller controls, 1 = fully pilot controls
115 : float controller_to_pilot_pitch_mix; // mix of controller and pilot controls. 0 = fully last controller controls, 1 = fully pilot controls
116 216561 : const Vector3f& vel_ned_ms = pos_control->get_vel_estimate_NED_ms();
117 :
118 : // enforce minimum allowed value for poshold_brake_rate_degs
119 216561 : if (g.poshold_brake_rate_degs < POSHOLD_BRAKE_RATE_MIN) {
120 0 : g.poshold_brake_rate_degs.set_and_save(POSHOLD_BRAKE_RATE_MIN);
121 : }
122 :
123 : // set vertical speed and acceleration limits
124 216561 : pos_control->D_set_max_speed_accel_m(get_pilot_speed_dn_ms(), get_pilot_speed_up_ms(), get_pilot_accel_D_mss());
125 216561 : loiter_nav->clear_pilot_desired_acceleration();
126 :
127 : // convert pilot input to lean angles
128 : float target_roll_rad, target_pitch_rad;
129 216561 : get_pilot_desired_lean_angles_rad(target_roll_rad, target_pitch_rad, attitude_control->lean_angle_max_rad(), attitude_control->get_althold_lean_angle_max_rad());
130 :
131 : // pilot desired yaw rate already rad/s
132 216561 : float target_yaw_rate_rads = get_pilot_desired_yaw_rate_rads();
133 :
134 : // pilot desired climb rate (m/s)
135 216561 : float target_climb_rate_ms = get_pilot_desired_climb_rate_ms();
136 :
137 : // relax loiter target if we might be landed
138 216561 : if (copter.ap.land_complete_maybe) {
139 114228 : loiter_nav->soften_for_landing();
140 : }
141 :
142 : // state machine
143 216561 : AltHoldModeState poshold_state = get_alt_hold_state_D_ms(target_climb_rate_ms);
144 :
145 216561 : switch (poshold_state) {
146 :
147 35432 : case AltHoldModeState::MotorStopped:
148 35432 : attitude_control->reset_rate_controller_I_terms();
149 35432 : attitude_control->reset_yaw_target_and_rate(false);
150 35432 : pos_control->D_relax_controller(0.0f); // forces throttle output to decay to zero
151 35432 : loiter_nav->clear_pilot_desired_acceleration();
152 35432 : loiter_nav->init_target();
153 :
154 : // set poshold state to pilot override
155 35432 : roll_mode = RPMode::PILOT_OVERRIDE;
156 35432 : pitch_mode = RPMode::PILOT_OVERRIDE;
157 :
158 : // initialise wind compensation estimate
159 35432 : init_wind_comp_estimate();
160 35432 : break;
161 :
162 6324 : case AltHoldModeState::Landed_Ground_Idle:
163 6324 : loiter_nav->clear_pilot_desired_acceleration();
164 6324 : loiter_nav->init_target();
165 6324 : attitude_control->reset_yaw_target_and_rate();
166 6324 : init_wind_comp_estimate();
167 : FALLTHROUGH;
168 :
169 75323 : case AltHoldModeState::Landed_Pre_Takeoff:
170 75323 : attitude_control->reset_rate_controller_I_terms_smoothly();
171 75323 : pos_control->D_relax_controller(0.0f); // forces throttle output to decay to zero
172 :
173 : // set poshold state to pilot override
174 75323 : roll_mode = RPMode::PILOT_OVERRIDE;
175 75323 : pitch_mode = RPMode::PILOT_OVERRIDE;
176 75323 : break;
177 :
178 8156 : case AltHoldModeState::Takeoff:
179 : // initiate take-off
180 8156 : if (!takeoff.running()) {
181 7 : takeoff.start_m(constrain_float(g2.pilot_takeoff_alt_m, 0.0f, 10.0f));
182 : }
183 :
184 : // avoidance-adjusted climb
185 8156 : target_climb_rate_ms = get_avoidance_adjusted_climbrate_ms(target_climb_rate_ms);
186 :
187 : // set position controller targets adjusted for pilot input
188 8156 : takeoff.do_pilot_takeoff_ms(target_climb_rate_ms);
189 :
190 : // init and update loiter although pilot is controlling lean angles
191 8156 : loiter_nav->clear_pilot_desired_acceleration();
192 8156 : loiter_nav->init_target();
193 :
194 : // pilot override
195 8156 : roll_mode = RPMode::PILOT_OVERRIDE;
196 8156 : pitch_mode = RPMode::PILOT_OVERRIDE;
197 8156 : break;
198 :
199 97650 : case AltHoldModeState::Flying:
200 : // avoidance-adjusted climb
201 97650 : target_climb_rate_ms = get_avoidance_adjusted_climbrate_ms(target_climb_rate_ms);
202 :
203 : #if AP_RANGEFINDER_ENABLED
204 : // update the vertical offset based on the surface measurement
205 97650 : copter.surface_tracking.update_surface_offset();
206 : #endif
207 :
208 : // Send the commanded climb rate to the position controller
209 97650 : pos_control->D_set_pos_target_from_climb_rate_ms(target_climb_rate_ms);
210 97650 : break;
211 : }
212 :
213 : // convert earth-frame velocities to body-frame (m/s)
214 216561 : float vel_fw_ms = vel_ned_ms.x * ahrs.cos_yaw() + vel_ned_ms.y * ahrs.sin_yaw();
215 216561 : float vel_right_ms = -vel_ned_ms.x * ahrs.sin_yaw() + vel_ned_ms.y * ahrs.cos_yaw();
216 :
217 : // If not in LOITER, retrieve latest wind compensation lean angles (radians) related to current yaw
218 216561 : if (roll_mode != RPMode::LOITER || pitch_mode != RPMode::LOITER) {
219 131462 : get_wind_comp_lean_angles_rad(wind_comp_roll_rad, wind_comp_pitch_rad);
220 : }
221 :
222 : // Roll state machine
223 : // Each state (aka mode) is responsible for:
224 : // 1. dealing with pilot input
225 : // 2. calculating the final roll output to the attitude controller
226 : // 3. checking if the state (aka mode) should be changed and if 'yes' perform any required initialisation for the new state
227 : // Roll state machine
228 216561 : switch (roll_mode) {
229 :
230 119702 : case RPMode::PILOT_OVERRIDE:
231 : // update pilot desired roll angle using latest radio input
232 : // this filters the input so that it returns to zero no faster than the brake-rate
233 119702 : update_pilot_lean_angle_rad(pilot_roll_rad, target_roll_rad);
234 :
235 : // switch to BRAKE mode for next iteration if no pilot input
236 238646 : if (is_zero(target_roll_rad) &&
237 118944 : (fabsf(pilot_roll_rad) < radians(2.0f * (float)g.poshold_brake_rate_degs))) {
238 : // initialise BRAKE mode
239 118913 : roll_mode = RPMode::BRAKE; // Set brake roll mode
240 118913 : brake.roll_rad = 0.0f; // initialise braking angle to zero
241 118913 : brake.angle_max_roll_rad = 0.0f; // reset tracked max brake angle (radians) so we can detect when vehicle begins to flatten out during braking
242 118913 : brake.start_time_roll_ms = now_ms; // timestamp (ms) marking start of roll-axis braking; updated during braking phase
243 118913 : brake.time_updated_roll = false; // flag the braking time can be re-estimated
244 : }
245 :
246 : // final lean angle should be pilot input plus wind compensation
247 119702 : roll_rad = pilot_roll_rad + wind_comp_roll_rad;
248 119702 : break;
249 :
250 7353 : case RPMode::BRAKE:
251 : case RPMode::BRAKE_READY_TO_LOITER:
252 : // calculate brake.roll angle to counter-act velocity
253 7353 : update_brake_angle_from_velocity(brake.roll_rad, vel_right_ms);
254 :
255 : // update braking time estimate
256 7353 : if (!brake.time_updated_roll) {
257 : // check if brake angle is increasing
258 1023 : if (fabsf(brake.roll_rad) >= brake.angle_max_roll_rad) {
259 1014 : brake.angle_max_roll_rad = fabsf(brake.roll_rad);
260 : } else {
261 : // braking angle has started decreasing so re-estimate braking time
262 9 : brake.start_time_roll_ms = now_ms;
263 9 : brake.time_updated_roll = true;
264 : }
265 : } else {
266 : // scaling factors:
267 : // 1.5 × (time to level, s) × 1000 -> ms
268 : // time to level ≈ angle_max / brake_rate
269 6330 : const uint32_t brake_timeout_roll_ms = MIN(POSHOLD_BRAKE_TIME_ESTIMATE_MAX_MS, (1.5f * 1000.0f * (brake.angle_max_roll_rad / radians(g.poshold_brake_rate_degs))));
270 :
271 : // if velocity is very low reduce braking time to 0.5 s
272 6330 : if ((fabsf(vel_right_ms) <= POSHOLD_SPEED_0) && (now_ms - brake.start_time_roll_ms > 500) && (brake_timeout_roll_ms > 500)) {
273 5015 : brake.start_time_roll_ms = now_ms - brake_timeout_roll_ms + 500;
274 : }
275 :
276 6330 : if (now_ms - brake.start_time_roll_ms > brake_timeout_roll_ms) {
277 : // indicate that we are ready to move to Loiter.
278 : // Loiter will only actually be engaged once both roll_mode and pitch_mode are changed to RPMode::BRAKE_READY_TO_LOITER
279 : // Logic for engaging loiter is handled below the roll and pitch mode switch statements
280 18 : roll_mode = RPMode::BRAKE_READY_TO_LOITER;
281 : }
282 : }
283 :
284 : // final lean angle is braking angle + wind compensation angle
285 7353 : roll_rad = brake.roll_rad + wind_comp_roll_rad;
286 :
287 : // check for pilot input
288 7353 : if (!is_zero(target_roll_rad)) {
289 : // init transition to pilot override
290 0 : roll_controller_to_pilot_override();
291 : }
292 7353 : break;
293 :
294 89305 : case RPMode::BRAKE_TO_LOITER:
295 : case RPMode::LOITER:
296 : // these modes are combined roll-pitch modes and are handled below
297 89305 : break;
298 :
299 201 : case RPMode::CONTROLLER_TO_PILOT_OVERRIDE:
300 : // update pilot desired roll angle using latest radio input
301 : // this filters the input so that it returns to zero no faster than the brake-rate
302 201 : update_pilot_lean_angle_rad(pilot_roll_rad, target_roll_rad);
303 :
304 : // count-down loiter to pilot timer
305 201 : if (now_ms - controller_to_pilot_start_time_roll_ms > POSHOLD_CONTROLLER_TO_PILOT_MIX_TIME_MS) {
306 : // when timer runs out switch to full pilot override for next iteration
307 1 : roll_mode = RPMode::PILOT_OVERRIDE;
308 : }
309 :
310 : // calculate controller_to_pilot mix ratio
311 201 : controller_to_pilot_roll_mix = (float)(now_ms - controller_to_pilot_start_time_roll_ms) / (float)POSHOLD_CONTROLLER_TO_PILOT_MIX_TIME_MS;
312 :
313 : // mix final loiter lean angle and pilot desired lean angles
314 201 : roll_rad = mix_controls(controller_to_pilot_roll_mix, pilot_roll_rad + wind_comp_roll_rad, controller_final_roll_rad);
315 201 : break;
316 : }
317 :
318 : // Pitch state machine
319 : // Each state (aka mode) is responsible for:
320 : // 1. dealing with pilot input
321 : // 2. calculating the final pitch output to the attitude contpitcher
322 : // 3. checking if the state (aka mode) should be changed and if 'yes' perform any required initialisation for the new state
323 : // Pitch state machine
324 216561 : switch (pitch_mode) {
325 :
326 119702 : case RPMode::PILOT_OVERRIDE:
327 : // update pilot desired pitch angle using latest radio input
328 : // this filters the input so that it returns to zero no faster than the brake-rate
329 119702 : update_pilot_lean_angle_rad(pilot_pitch_rad, target_pitch_rad);
330 :
331 : // switch to BRAKE next iteration if no pilot input
332 119702 : if (is_zero(target_pitch_rad) && (fabsf(pilot_pitch_rad) < radians(2 * g.poshold_brake_rate_degs))) {
333 : // initialise BRAKE mode
334 118913 : pitch_mode = RPMode::BRAKE; // set brake pitch mode
335 118913 : brake.pitch_rad = 0.0f; // initialise braking angle to zero
336 118913 : brake.angle_max_pitch_rad = 0.0f; // reset tracked max brake angle (radians) so we can detect when vehicle begins to flatten out during braking
337 118913 : brake.start_time_pitch_ms = now_ms; // timestamp (ms) marking the start of pitch-axis
338 118913 : brake.time_updated_pitch = false; // flag the braking time can be re-estimated
339 : }
340 :
341 : // final lean angle = pilot input + wind compensation
342 119702 : pitch_rad = pilot_pitch_rad + wind_comp_pitch_rad;
343 119702 : break;
344 :
345 7353 : case RPMode::BRAKE:
346 : case RPMode::BRAKE_READY_TO_LOITER:
347 : // calculate brake_pitch angle to counter-act velocity
348 7353 : update_brake_angle_from_velocity(brake.pitch_rad, -vel_fw_ms);
349 :
350 : // update braking time estimate
351 7353 : if (!brake.time_updated_pitch) {
352 : // check if brake angle is increasing
353 645 : if (fabsf(brake.pitch_rad) >= brake.angle_max_pitch_rad) {
354 637 : brake.angle_max_pitch_rad = fabsf(brake.pitch_rad);
355 : } else {
356 : // braking angle has started decreasing so re-estimate braking time
357 8 : brake.start_time_pitch_ms = now_ms;
358 8 : brake.time_updated_pitch = true;
359 : }
360 : } else {
361 : // timeout ≈ 1.5 * (angle / rate) in ms
362 6708 : const uint32_t brake_timeout_pitch_ms = MIN(POSHOLD_BRAKE_TIME_ESTIMATE_MAX_MS, (1.5 * 1000.0 * (brake.angle_max_pitch_rad / radians(g.poshold_brake_rate_degs))));
363 :
364 : // if velocity is very low reduce braking time to 0.5 seconds
365 6708 : if ((fabsf(vel_fw_ms) <= POSHOLD_SPEED_0) && (now_ms - brake.start_time_pitch_ms > 500) && (brake_timeout_pitch_ms > 500)) {
366 585 : brake.start_time_pitch_ms = now_ms - brake_timeout_pitch_ms + 500;
367 : }
368 :
369 6708 : if (now_ms - brake.start_time_pitch_ms > brake_timeout_pitch_ms) {
370 : // indicate that we are ready to move to Loiter.
371 : // Loiter will only actually be engaged once both pitch_mode and pitch_mode are changed to RPMode::BRAKE_READY_TO_LOITER
372 : // logic for engaging loiter is handled below the pitch and pitch mode switch statements
373 5690 : pitch_mode = RPMode::BRAKE_READY_TO_LOITER;
374 : }
375 : }
376 :
377 : // final lean angle is braking angle + wind compensation angle
378 7353 : pitch_rad = brake.pitch_rad + wind_comp_pitch_rad;
379 :
380 : // check for pilot input
381 7353 : if (!is_zero(target_pitch_rad)) {
382 : // init transition to pilot override
383 1 : pitch_controller_to_pilot_override();
384 : }
385 7353 : break;
386 :
387 89305 : case RPMode::BRAKE_TO_LOITER:
388 : case RPMode::LOITER:
389 : // these modes are combined pitch-pitch modes and are handled below
390 89305 : break;
391 :
392 201 : case RPMode::CONTROLLER_TO_PILOT_OVERRIDE:
393 : // update pilot desired pitch angle using latest radio input
394 : // this filters the input so that it returns to zero no faster than the brake-rate
395 201 : update_pilot_lean_angle_rad(pilot_pitch_rad, target_pitch_rad);
396 :
397 : // count-down loiter to pilot timer
398 201 : if (now_ms - controller_to_pilot_start_time_pitch_ms > POSHOLD_CONTROLLER_TO_PILOT_MIX_TIME_MS) {
399 : // when timer runs out switch to full pilot override for next iteration
400 1 : pitch_mode = RPMode::PILOT_OVERRIDE;
401 : }
402 :
403 : // calculate controller_to_pilot mix ratio
404 201 : controller_to_pilot_pitch_mix = (float)(now_ms - controller_to_pilot_start_time_pitch_ms) / (float)POSHOLD_CONTROLLER_TO_PILOT_MIX_TIME_MS;
405 :
406 : // mix final loiter lean angle and pilot desired lean angles
407 201 : pitch_rad = mix_controls(controller_to_pilot_pitch_mix, pilot_pitch_rad + wind_comp_pitch_rad, controller_final_pitch_rad);
408 201 : break;
409 : }
410 :
411 : //
412 : // Shared roll & pitch states (RPMode::BRAKE_TO_LOITER and RPMode::LOITER)
413 : //
414 :
415 : // switch into LOITER mode when both roll and pitch are ready
416 216561 : if (roll_mode == RPMode::BRAKE_READY_TO_LOITER && pitch_mode == RPMode::BRAKE_READY_TO_LOITER) {
417 7 : roll_mode = RPMode::BRAKE_TO_LOITER;
418 7 : pitch_mode = RPMode::BRAKE_TO_LOITER;
419 7 : brake.loiter_transition_start_time_ms = now_ms;
420 : // init loiter controller
421 7 : loiter_nav->init_target_m((pos_control->get_pos_estimate_NED_m().xy() - pos_control->get_pos_offset_NED_m().xy()));
422 : // set delay to start of wind compensation estimate updates
423 7 : wind_comp_start_time_ms = now_ms;
424 : }
425 :
426 : // roll-mode is used as the combined roll+pitch mode when in BRAKE_TO_LOITER or LOITER modes
427 216561 : if (roll_mode == RPMode::BRAKE_TO_LOITER || roll_mode == RPMode::LOITER) {
428 :
429 : // force pitch mode to be same as roll_mode just to keep it consistent (it's not actually used in these states)
430 89312 : pitch_mode = roll_mode;
431 :
432 : // handle combined roll+pitch mode
433 89312 : switch (roll_mode) {
434 4213 : case RPMode::BRAKE_TO_LOITER: {
435 : // reduce brake_to_loiter timer
436 4213 : if (now_ms - brake.loiter_transition_start_time_ms > POSHOLD_BRAKE_TO_LOITER_TIME_MS) {
437 : // progress to full loiter on next iteration
438 7 : roll_mode = RPMode::LOITER;
439 7 : pitch_mode = RPMode::LOITER;
440 : }
441 :
442 : // mix of brake and loiter controls
443 4213 : const float brake_to_loiter_mix = (float)(now_ms - brake.loiter_transition_start_time_ms) / (float)POSHOLD_BRAKE_TO_LOITER_TIME_MS;
444 :
445 : // calculate brake roll/pitch angles to counter velocity
446 4213 : update_brake_angle_from_velocity(brake.roll_rad, vel_right_ms);
447 4213 : update_brake_angle_from_velocity(brake.pitch_rad, -vel_fw_ms);
448 :
449 : // run loiter controller
450 4213 : loiter_nav->update(false);
451 :
452 : // calculate final roll and pitch output by mixing loiter and brake controls
453 4213 : roll_rad = mix_controls(brake_to_loiter_mix, brake.roll_rad + wind_comp_roll_rad, loiter_nav->get_roll_rad());
454 4213 : pitch_rad = mix_controls(brake_to_loiter_mix, brake.pitch_rad + wind_comp_pitch_rad, loiter_nav->get_pitch_rad());
455 :
456 : // check for pilot input
457 4213 : if (!is_zero(target_roll_rad) || !is_zero(target_pitch_rad)) {
458 : // if roll input switch to pilot override for roll
459 0 : if (!is_zero(target_roll_rad)) {
460 : // init transition to pilot override
461 0 : roll_controller_to_pilot_override();
462 : // switch pitch-mode to brake (but ready to go back to loiter anytime)
463 : // no need to reset brake.pitch here as wind comp has not been updated since last brake.pitch computation
464 0 : pitch_mode = RPMode::BRAKE_READY_TO_LOITER;
465 : }
466 : // if pitch input switch to pilot override for pitch
467 0 : if (!is_zero(target_pitch_rad)) {
468 : // init transition to pilot override
469 0 : pitch_controller_to_pilot_override();
470 0 : if (is_zero(target_roll_rad)) {
471 : // switch roll-mode to brake (but ready to go back to loiter anytime)
472 : // no need to reset brake.roll here as wind comp has not been updated since last brake.roll computation
473 0 : roll_mode = RPMode::BRAKE_READY_TO_LOITER;
474 : }
475 : }
476 : }
477 4213 : break;
478 : }
479 85099 : case RPMode::LOITER:
480 : // run loiter controller
481 85099 : loiter_nav->update(false);
482 :
483 : // set roll angle based on loiter controller outputs
484 85099 : roll_rad = loiter_nav->get_roll_rad();
485 85099 : pitch_rad = loiter_nav->get_pitch_rad();
486 :
487 : // update wind compensation estimate
488 85099 : update_wind_comp_estimate();
489 :
490 : // check for pilot input
491 85099 : if (!is_zero(target_roll_rad) || !is_zero(target_pitch_rad)) {
492 : // if roll input switch to pilot override for roll
493 1 : if (!is_zero(target_roll_rad)) {
494 : // init transition to pilot override
495 1 : roll_controller_to_pilot_override();
496 : // switch pitch-mode to brake (but ready to go back to loiter anytime)
497 1 : pitch_mode = RPMode::BRAKE_READY_TO_LOITER;
498 : // reset brake.pitch because wind_comp is now different and should give the compensation of the whole previous loiter angle
499 1 : brake.pitch_rad = 0.0f;
500 : }
501 : // if pitch input switch to pilot override for pitch
502 1 : if (!is_zero(target_pitch_rad)) {
503 : // init transition to pilot override on pitch
504 0 : pitch_controller_to_pilot_override();
505 : // if roll not overridden switch roll-mode to brake (but be ready to go back to loiter any time)
506 0 : if (is_zero(target_roll_rad)) {
507 0 : roll_mode = RPMode::BRAKE_READY_TO_LOITER;
508 0 : brake.roll_rad = 0.0f;
509 : }
510 : // if roll not overridden switch roll-mode to brake (but be ready to go back to loiter any time)
511 : }
512 : }
513 85099 : break;
514 :
515 0 : default:
516 : // do nothing for uncombined roll and pitch modes
517 0 : break;
518 : }
519 : }
520 :
521 : // constrain target pitch/roll angles
522 216561 : const float angle_max_rad = copter.attitude_control->lean_angle_max_rad();
523 216561 : roll_rad = constrain_float(roll_rad, -angle_max_rad, angle_max_rad);
524 216561 : pitch_rad = constrain_float(pitch_rad, -angle_max_rad, angle_max_rad);
525 :
526 : // call attitude controller
527 216561 : attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_rad(roll_rad, pitch_rad, target_yaw_rate_rads);
528 :
529 : // run the vertical position controller and set output throttle
530 216561 : pos_control->D_update_controller();
531 216561 : }
532 :
533 : // poshold_update_pilot_lean_angle - update the pilot's filtered lean angle with the latest raw input received
534 239806 : void ModePosHold::update_pilot_lean_angle_rad(float &lean_angle_filtered_rad, float &lean_angle_raw_rad)
535 : {
536 : // if raw input is large or reversing the vehicle's lean angle immediately set the filtered angle to the new raw angle
537 239806 : if ((lean_angle_filtered_rad > 0.0 && lean_angle_raw_rad < 0.0) || (lean_angle_filtered_rad < 0.0 && lean_angle_raw_rad > 0.0) || (fabsf(lean_angle_raw_rad) > POSHOLD_STICK_RELEASE_SMOOTH_ANGLE_RAD)) {
538 1918 : lean_angle_filtered_rad = lean_angle_raw_rad;
539 : } else {
540 : // lean_angle_raw must be pulling lean_angle_filtered towards zero, smooth the decrease
541 237888 : const float brake_rate_step_rad = radians((float)g.poshold_brake_rate_degs) * G_Dt;
542 237888 : if (lean_angle_filtered_rad > 0.0) {
543 : // reduce the filtered lean angle at 1.25% per step or the brake rate (whichever is faster).
544 64 : lean_angle_filtered_rad -= MAX(lean_angle_filtered_rad * POSHOLD_SMOOTH_RATE_FACTOR, brake_rate_step_rad);
545 : // do not let the filtered angle fall below the pilot's input lean angle.
546 : // the above line pulls the filtered angle down and the below line acts as a catch
547 64 : lean_angle_filtered_rad = MAX(lean_angle_filtered_rad, lean_angle_raw_rad);
548 : } else {
549 237824 : lean_angle_filtered_rad += MAX(-lean_angle_filtered_rad * POSHOLD_SMOOTH_RATE_FACTOR, brake_rate_step_rad);
550 237824 : lean_angle_filtered_rad = MIN(lean_angle_filtered_rad, lean_angle_raw_rad);
551 : }
552 : }
553 239806 : }
554 :
555 : // mix_controls - mixes two controls based on the mix_ratio
556 : // mix_ratio of 1 = use first_control completely, 0 = use second_control completely, 0.5 = mix evenly
557 8828 : float ModePosHold::mix_controls(float mix_ratio, float first_control, float second_control)
558 : {
559 8828 : mix_ratio = constrain_float(mix_ratio, 0.0f, 1.0f);
560 8828 : return mix_ratio * first_control + (1.0f - mix_ratio) * second_control;
561 : }
562 :
563 : // update_brake_angle_from_velocity - updates the brake_angle based on the vehicle's velocity and brake_gain
564 : // brake_angle is slewed with the wpnav.poshold_brake_rate_degs_degs and constrained by the wpnav.poshold_braking_angle_max
565 : // 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)
566 23132 : void ModePosHold::update_brake_angle_from_velocity(float &brake_angle_rad, float velocity_ms)
567 : {
568 : // slew limit per step (rad)
569 23132 : const float brake_delta_rad = radians((float)g.poshold_brake_rate_degs) * G_Dt;
570 :
571 : // velocity-shaped lean angle:
572 23132 : float lean_angle_rad = -brake.gain * velocity_ms * (1.0f + 5.0f / (fabsf(velocity_ms) + 0.60f));
573 :
574 : // do not let lean_angle be too far from brake_angle
575 23132 : brake_angle_rad = constrain_float(lean_angle_rad, brake_angle_rad - brake_delta_rad, brake_angle_rad + brake_delta_rad);
576 :
577 : // constrain final brake_angle
578 23132 : const float brake_angle_max_rad = radians(brake_angle_max_deg);
579 23132 : brake_angle_rad = constrain_float(brake_angle_rad, -brake_angle_max_rad, brake_angle_max_rad);
580 23132 : }
581 :
582 : // initialise wind compensation estimate back to zero
583 41768 : void ModePosHold::init_wind_comp_estimate()
584 : {
585 41768 : wind_comp_ne_mss.zero();
586 41768 : wind_comp_roll_rad = 0.0f;
587 41768 : wind_comp_pitch_rad = 0.0f;
588 41768 : }
589 :
590 : // update_wind_comp_estimate - updates wind compensation estimate
591 : // should be called at the maximum loop rate when loiter is engaged
592 85099 : void ModePosHold::update_wind_comp_estimate()
593 : {
594 85099 : const uint32_t now_ms = AP_HAL::millis();
595 : // check wind estimate start has not been delayed
596 85099 : if (now_ms - wind_comp_start_time_ms < POSHOLD_WIND_COMP_START_TIME_MS) {
597 0 : return;
598 : }
599 :
600 : // check horizontal velocity is low
601 85099 : if (pos_control->get_vel_estimate_NED_ms().xy().length() > POSHOLD_WIND_COMP_ESTIMATE_SPEED_MAX_MS) {
602 5410 : return;
603 : }
604 :
605 : // get position controller accel target
606 79689 : const Vector3f& accel_target_ned_mss = pos_control->get_accel_target_NED_mss();
607 :
608 : // update wind compensation in earth-frame lean angles
609 79689 : if (is_zero(wind_comp_ne_mss.x)) {
610 : // if wind compensation has not been initialised set it immediately to the pos controller's desired accel in north direction
611 7 : wind_comp_ne_mss.x = accel_target_ned_mss.x;
612 : } else {
613 : // low pass filter the position controller's lean angle output
614 79682 : wind_comp_ne_mss.x = (1.0f - TC_WIND_COMP) * wind_comp_ne_mss.x + TC_WIND_COMP * accel_target_ned_mss.x;
615 : }
616 79689 : if (is_zero(wind_comp_ne_mss.y)) {
617 : // if wind compensation has not been initialised set it immediately to the pos controller's desired accel in north direction
618 7 : wind_comp_ne_mss.y = accel_target_ned_mss.y;
619 : } else {
620 : // low pass filter the position controller's lean angle output
621 79682 : wind_comp_ne_mss.y = (1.0f - TC_WIND_COMP) * wind_comp_ne_mss.y + TC_WIND_COMP * accel_target_ned_mss.y;
622 : }
623 :
624 : // limit acceleration
625 79689 : const float accel_lim_mss = tanf(POSHOLD_WIND_COMP_LEAN_PCT_MAX * copter.attitude_control->lean_angle_max_rad()) * GRAVITY_MSS;
626 79689 : const float wind_comp_ef_len = wind_comp_ne_mss.length();
627 79689 : if (!is_zero(accel_lim_mss) && (wind_comp_ef_len > accel_lim_mss)) {
628 0 : wind_comp_ne_mss *= accel_lim_mss / wind_comp_ef_len;
629 : }
630 : }
631 :
632 : // get_wind_comp_lean_angles_rad - retrieve wind compensation angles in body frame roll and pitch angles
633 : // should be called at the maximum loop rate
634 131462 : void ModePosHold::get_wind_comp_lean_angles_rad(float &roll_angle_rad, float &pitch_angle_rad)
635 : {
636 : // convert earth frame desired accelerations to body frame roll and pitch lean angles
637 131462 : roll_angle_rad = atanf((-wind_comp_ne_mss.x * ahrs.sin_yaw() + wind_comp_ne_mss.y * ahrs.cos_yaw()) / GRAVITY_MSS);
638 131462 : pitch_angle_rad = atanf(-(wind_comp_ne_mss.x * ahrs.cos_yaw() + wind_comp_ne_mss.y * ahrs.sin_yaw()) / GRAVITY_MSS);
639 131462 : }
640 :
641 : // roll_controller_to_pilot_override - initialises transition from a controller submode (brake or loiter) to a pilot override on roll axis
642 1 : void ModePosHold::roll_controller_to_pilot_override()
643 : {
644 1 : const uint32_t now_ms = AP_HAL::millis();
645 1 : roll_mode = RPMode::CONTROLLER_TO_PILOT_OVERRIDE;
646 1 : controller_to_pilot_start_time_roll_ms = now_ms;
647 : // initialise pilot_roll_rad 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
648 1 : pilot_roll_rad = 0.0f;
649 : // store final controller output for mixing with pilot input
650 1 : controller_final_roll_rad = roll_rad;
651 1 : }
652 :
653 : // pitch_controller_to_pilot_override - initialises transition from a controller submode (brake or loiter) to a pilot override on roll axis
654 1 : void ModePosHold::pitch_controller_to_pilot_override()
655 : {
656 1 : const uint32_t now_ms = AP_HAL::millis();
657 1 : pitch_mode = RPMode::CONTROLLER_TO_PILOT_OVERRIDE;
658 1 : controller_to_pilot_start_time_pitch_ms = now_ms;
659 : // initialise pilot_pitch_rad 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
660 1 : pilot_pitch_rad = 0.0f;
661 : // store final loiter outputs for mixing with pilot input
662 1 : controller_final_pitch_rad = pitch_rad;
663 1 : }
664 :
665 : #endif
|
