CoCalc Logo Icon
StoreFeaturesDocsShareSupportNewsAboutSign UpSign In
Ardupilot

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.

GitHub Repository: Ardupilot/ardupilot
Path: blob/master/libraries/AC_AttitudeControl/AC_PosControl.h
Views: 1798
1
#pragma once
2
3
#include <AP_Common/AP_Common.h>
4
#include <AP_Param/AP_Param.h>
5
#include <AP_Math/AP_Math.h>
6
#include <AC_PID/AC_P.h> // P library
7
#include <AC_PID/AC_PID.h> // PID library
8
#include <AC_PID/AC_P_1D.h> // P library (1-axis)
9
#include <AC_PID/AC_P_2D.h> // P library (2-axis)
10
#include <AC_PID/AC_PI_2D.h> // PI library (2-axis)
11
#include <AC_PID/AC_PID_Basic.h> // PID library (1-axis)
12
#include <AC_PID/AC_PID_2D.h> // PID library (2-axis)
13
#include <AP_InertialNav/AP_InertialNav.h> // Inertial Navigation library
14
#include <AP_Scripting/AP_Scripting_config.h>
15
#include "AC_AttitudeControl.h" // Attitude control library
16
17
#include <AP_Logger/LogStructure.h>
18
19
// position controller default definitions
20
#define POSCONTROL_ACCEL_XY 100.0f // default horizontal acceleration in cm/s/s. This is overwritten by waypoint and loiter controllers
21
#define POSCONTROL_JERK_XY 5.0f // default horizontal jerk m/s/s/s
22
23
#define POSCONTROL_STOPPING_DIST_UP_MAX 300.0f // max stopping distance (in cm) vertically while climbing
24
#define POSCONTROL_STOPPING_DIST_DOWN_MAX 200.0f // max stopping distance (in cm) vertically while descending
25
26
#define POSCONTROL_SPEED 500.0f // default horizontal speed in cm/s
27
#define POSCONTROL_SPEED_DOWN -150.0f // default descent rate in cm/s
28
#define POSCONTROL_SPEED_UP 250.0f // default climb rate in cm/s
29
30
#define POSCONTROL_ACCEL_Z 250.0f // default vertical acceleration in cm/s/s.
31
#define POSCONTROL_JERK_Z 5.0f // default vertical jerk m/s/s/s
32
33
#define POSCONTROL_THROTTLE_CUTOFF_FREQ_HZ 2.0f // low-pass filter on acceleration error (unit: Hz)
34
35
#define POSCONTROL_OVERSPEED_GAIN_Z 2.0f // gain controlling rate at which z-axis speed is brought back within SPEED_UP and SPEED_DOWN range
36
37
#define POSCONTROL_RELAX_TC 0.16f // This is used to decay the I term to 5% in half a second.
38
39
class AC_PosControl
40
{
41
public:
42
43
/// Constructor
44
AC_PosControl(AP_AHRS_View& ahrs, const AP_InertialNav& inav,
45
const class AP_Motors& motors, AC_AttitudeControl& attitude_control);
46
47
// do not allow copying
48
CLASS_NO_COPY(AC_PosControl);
49
50
/// set_dt / get_dt - dt is the time since the last time the position controllers were updated
51
/// _dt should be set based on the time of the last IMU read used by these controllers
52
/// the position controller should run updates for active controllers on each loop to ensure normal operation
53
void set_dt(float dt) { _dt = dt; }
54
float get_dt() const { return _dt; }
55
56
/// get_shaping_jerk_xy_cmsss - gets the jerk limit of the xy kinematic path generation in cm/s/s/s
57
float get_shaping_jerk_xy_cmsss() const { return _shaping_jerk_xy * 100.0; }
58
59
60
///
61
/// 3D position shaper
62
///
63
64
/// input_pos_xyz - calculate a jerk limited path from the current position, velocity and acceleration to an input position.
65
/// The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
66
/// The kinematic path is constrained by the maximum acceleration and jerk set using the function set_max_speed_accel_xy.
67
void input_pos_xyz(const Vector3p& pos, float pos_terrain_target, float terrain_buffer);
68
69
/// pos_offset_z_scaler - calculates a multiplier used to reduce the horizontal velocity to allow the z position controller to stay within the provided buffer range
70
float pos_offset_z_scaler(float pos_offset_z, float pos_offset_z_buffer) const;
71
72
///
73
/// Lateral position controller
74
///
75
76
/// set_max_speed_accel_xy - set the maximum horizontal speed in cm/s and acceleration in cm/s/s
77
/// This function only needs to be called if using the kinematic shaping.
78
/// This can be done at any time as changes in these parameters are handled smoothly
79
/// by the kinematic shaping.
80
void set_max_speed_accel_xy(float speed_cms, float accel_cmss);
81
82
/// set_max_speed_accel_xy - set the position controller correction velocity and acceleration limit
83
/// This should be done only during initialisation to avoid discontinuities
84
void set_correction_speed_accel_xy(float speed_cms, float accel_cmss);
85
86
/// get_max_speed_xy_cms - get the maximum horizontal speed in cm/s
87
float get_max_speed_xy_cms() const { return _vel_max_xy_cms; }
88
89
/// get_max_accel_xy_cmss - get the maximum horizontal acceleration in cm/s/s
90
float get_max_accel_xy_cmss() const { return _accel_max_xy_cmss; }
91
92
// set the maximum horizontal position error that will be allowed in the horizontal plane
93
void set_pos_error_max_xy_cm(float error_max) { _p_pos_xy.set_error_max(error_max); }
94
float get_pos_error_max_xy_cm() { return _p_pos_xy.get_error_max(); }
95
96
/// init_xy_controller_stopping_point - initialise the position controller to the stopping point with zero velocity and acceleration.
97
/// This function should be used when the expected kinematic path assumes a stationary initial condition but does not specify a specific starting position.
98
/// The starting position can be retrieved by getting the position target using get_pos_target_cm() after calling this function.
99
void init_xy_controller_stopping_point();
100
101
// relax_velocity_controller_xy - initialise the position controller to the current position and velocity with decaying acceleration.
102
/// This function decays the output acceleration by 95% every half second to achieve a smooth transition to zero requested acceleration.
103
void relax_velocity_controller_xy();
104
105
/// reduce response for landing
106
void soften_for_landing_xy();
107
108
// init_xy_controller - initialise the position controller to the current position, velocity, acceleration and attitude.
109
/// This function is the default initialisation for any position control that provides position, velocity and acceleration.
110
/// This function is private and contains all the shared xy axis initialisation functions
111
void init_xy_controller();
112
113
/// input_accel_xy - calculate a jerk limited path from the current position, velocity and acceleration to an input acceleration.
114
/// The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
115
/// The kinematic path is constrained by the maximum acceleration and jerk set using the function set_max_speed_accel_xy.
116
/// The jerk limit defines the acceleration error decay in the kinematic path as the system approaches constant acceleration.
117
/// The jerk limit also defines the time taken to achieve the maximum acceleration.
118
void input_accel_xy(const Vector3f& accel);
119
120
/// input_vel_accel_xy - calculate a jerk limited path from the current position, velocity and acceleration to an input velocity and acceleration.
121
/// The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
122
/// The kinematic path is constrained by the maximum acceleration and jerk set using the function set_max_speed_accel_xy.
123
/// The function alters the vel to be the kinematic path based on accel
124
/// The parameter limit_output specifies if the velocity and acceleration limits are applied to the sum of commanded and correction values or just correction.
125
void input_vel_accel_xy(Vector2f& vel, const Vector2f& accel, bool limit_output = true);
126
127
/// input_pos_vel_accel_xy - calculate a jerk limited path from the current position, velocity and acceleration to an input position velocity and acceleration.
128
/// The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
129
/// The kinematic path is constrained by the maximum acceleration and jerk set using the function set_max_speed_accel_xy.
130
/// The function alters the pos and vel to be the kinematic path based on accel
131
/// The parameter limit_output specifies if the velocity and acceleration limits are applied to the sum of commanded and correction values or just correction.
132
void input_pos_vel_accel_xy(Vector2p& pos, Vector2f& vel, const Vector2f& accel, bool limit_output = true);
133
134
// is_active_xy - returns true if the xy position controller has been run in the previous 5 loop times
135
bool is_active_xy() const;
136
137
/// stop_pos_xy_stabilisation - sets the target to the current position to remove any position corrections from the system
138
void stop_pos_xy_stabilisation();
139
140
/// stop_vel_xy_stabilisation - sets the target to the current position and velocity to the current velocity to remove any position and velocity corrections from the system
141
void stop_vel_xy_stabilisation();
142
143
/// update_xy_controller - runs the horizontal position controller correcting position, velocity and acceleration errors.
144
/// Position and velocity errors are converted to velocity and acceleration targets using PID objects
145
/// Desired velocity and accelerations are added to these corrections as they are calculated
146
/// Kinematically consistent target position and desired velocity and accelerations should be provided before calling this function
147
void update_xy_controller();
148
149
///
150
/// Vertical position controller
151
///
152
153
/// set_max_speed_accel_z - set the maximum vertical speed in cm/s and acceleration in cm/s/s
154
/// speed_down can be positive or negative but will always be interpreted as a descent speed
155
/// This can be done at any time as changes in these parameters are handled smoothly
156
/// by the kinematic shaping.
157
void set_max_speed_accel_z(float speed_down, float speed_up, float accel_cmss);
158
159
/// set_correction_speed_accel_z - set the position controller correction velocity and acceleration limit
160
/// speed_down can be positive or negative but will always be interpreted as a descent speed
161
/// This should be done only during initialisation to avoid discontinuities
162
void set_correction_speed_accel_z(float speed_down, float speed_up, float accel_cmss);
163
164
/// get_max_accel_z_cmss - get the maximum vertical acceleration in cm/s/s
165
float get_max_accel_z_cmss() const { return _accel_max_z_cmss; }
166
167
// get_pos_error_z_up_cm - get the maximum vertical position error up that will be allowed
168
float get_pos_error_z_up_cm() { return _p_pos_z.get_error_max(); }
169
170
// get_pos_error_z_down_cm - get the maximum vertical position error down that will be allowed
171
float get_pos_error_z_down_cm() { return _p_pos_z.get_error_min(); }
172
173
/// get_max_speed_up_cms - accessors for current maximum up speed in cm/s
174
float get_max_speed_up_cms() const { return _vel_max_up_cms; }
175
176
/// get_max_speed_down_cms - accessors for current maximum down speed in cm/s. Will be a negative number
177
float get_max_speed_down_cms() const { return _vel_max_down_cms; }
178
179
/// init_z_controller_no_descent - initialise the position controller to the current position, velocity, acceleration and attitude.
180
/// This function is the default initialisation for any position control that provides position, velocity and acceleration.
181
/// This function does not allow any negative velocity or acceleration
182
void init_z_controller_no_descent();
183
184
/// init_z_controller_stopping_point - initialise the position controller to the stopping point with zero velocity and acceleration.
185
/// This function should be used when the expected kinematic path assumes a stationary initial condition but does not specify a specific starting position.
186
/// The starting position can be retrieved by getting the position target using get_pos_target_cm() after calling this function.
187
void init_z_controller_stopping_point();
188
189
// relax_z_controller - initialise the position controller to the current position and velocity with decaying acceleration.
190
/// This function decays the output acceleration by 95% every half second to achieve a smooth transition to zero requested acceleration.
191
void relax_z_controller(float throttle_setting);
192
193
// init_z_controller - initialise the position controller to the current position, velocity, acceleration and attitude.
194
/// This function is the default initialisation for any position control that provides position, velocity and acceleration.
195
/// This function is private and contains all the shared z axis initialisation functions
196
void init_z_controller();
197
198
/// input_accel_z - calculate a jerk limited path from the current position, velocity and acceleration to an input acceleration.
199
/// The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
200
/// The kinematic path is constrained by the maximum acceleration and jerk set using the function set_max_speed_accel_z.
201
virtual void input_accel_z(float accel);
202
203
/// input_vel_accel_z - calculate a jerk limited path from the current position, velocity and acceleration to an input velocity and acceleration.
204
/// The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
205
/// The kinematic path is constrained by the maximum acceleration and jerk set using the function set_max_speed_accel_z.
206
/// The function alters the vel to be the kinematic path based on accel
207
/// The parameter limit_output specifies if the velocity and acceleration limits are applied to the sum of commanded and correction values or just correction.
208
virtual void input_vel_accel_z(float &vel, float accel, bool limit_output = true);
209
210
/// set_pos_target_z_from_climb_rate_cm - adjusts target up or down using a commanded climb rate in cm/s
211
/// using the default position control kinematic path.
212
/// The zero target altitude is varied to follow pos_offset_z
213
void set_pos_target_z_from_climb_rate_cm(float vel);
214
215
/// land_at_climb_rate_cm - adjusts target up or down using a commanded climb rate in cm/s
216
/// using the default position control kinematic path.
217
/// ignore_descent_limit turns off output saturation handling to aid in landing detection. ignore_descent_limit should be true unless landing.
218
void land_at_climb_rate_cm(float vel, bool ignore_descent_limit);
219
220
/// input_pos_vel_accel_z - calculate a jerk limited path from the current position, velocity and acceleration to an input position velocity and acceleration.
221
/// The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
222
/// The function alters the pos and vel to be the kinematic path based on accel
223
/// The parameter limit_output specifies if the velocity and acceleration limits are applied to the sum of commanded and correction values or just correction.
224
void input_pos_vel_accel_z(float &pos, float &vel, float accel, bool limit_output = true);
225
226
/// set_alt_target_with_slew - adjusts target up or down using a commanded altitude in cm
227
/// using the default position control kinematic path.
228
void set_alt_target_with_slew(float pos);
229
230
// is_active_z - returns true if the z position controller has been run in the previous 5 loop times
231
bool is_active_z() const;
232
233
/// update_z_controller - runs the vertical position controller correcting position, velocity and acceleration errors.
234
/// Position and velocity errors are converted to velocity and acceleration targets using PID objects
235
/// Desired velocity and accelerations are added to these corrections as they are calculated
236
/// Kinematically consistent target position and desired velocity and accelerations should be provided before calling this function
237
void update_z_controller();
238
239
240
241
///
242
/// Accessors
243
///
244
245
/// set commanded position (cm), velocity (cm/s) and acceleration (cm/s/s) inputs when the path is created externally.
246
void set_pos_vel_accel(const Vector3p& pos, const Vector3f& vel, const Vector3f& accel);
247
void set_pos_vel_accel_xy(const Vector2p& pos, const Vector2f& vel, const Vector2f& accel);
248
249
250
/// Position
251
252
/// get_pos_target_cm - returns the position target, frame NEU in cm relative to the EKF origin
253
const Vector3p& get_pos_target_cm() const { return _pos_target; }
254
255
/// set_pos_desired_xy_cm - sets the position target, frame NEU in cm relative to the EKF origin
256
void set_pos_desired_xy_cm(const Vector2f& pos) { _pos_desired.xy() = pos.topostype(); }
257
258
/// get_pos_desired_cm - returns the position desired, frame NEU in cm relative to the EKF origin
259
const Vector3p& get_pos_desired_cm() const { return _pos_desired; }
260
261
/// get_pos_target_z_cm - get target altitude (in cm above the EKF origin)
262
float get_pos_target_z_cm() const { return _pos_target.z; }
263
264
/// set_pos_desired_z_cm - set altitude target in cm above the EKF origin
265
void set_pos_desired_z_cm(float pos_z) { _pos_desired.z = pos_z; }
266
267
/// get_pos_desired_z_cm - get target altitude (in cm above the EKF origin)
268
float get_pos_desired_z_cm() const { return _pos_desired.z; }
269
270
271
/// Stopping Point
272
273
/// get_stopping_point_xy_cm - calculates stopping point in NEU cm based on current position, velocity, vehicle acceleration
274
void get_stopping_point_xy_cm(Vector2p &stopping_point) const;
275
276
/// get_stopping_point_z_cm - calculates stopping point in NEU cm based on current position, velocity, vehicle acceleration
277
void get_stopping_point_z_cm(postype_t &stopping_point) const;
278
279
280
/// Position Error
281
282
/// get_pos_error_cm - get position error vector between the current and target position
283
const Vector3f get_pos_error_cm() const { return Vector3f(_p_pos_xy.get_error().x, _p_pos_xy.get_error().y, _p_pos_z.get_error()); }
284
285
/// get_pos_error_xy_cm - get the length of the position error vector in the xy plane
286
float get_pos_error_xy_cm() const { return _p_pos_xy.get_error().length(); }
287
288
/// get_pos_error_z_cm - returns altitude error in cm
289
float get_pos_error_z_cm() const { return _p_pos_z.get_error(); }
290
291
292
/// Velocity
293
294
/// set_vel_desired_cms - sets desired velocity in NEU cm/s
295
void set_vel_desired_cms(const Vector3f &des_vel) { _vel_desired = des_vel; }
296
297
/// set_vel_desired_xy_cms - sets horizontal desired velocity in NEU cm/s
298
void set_vel_desired_xy_cms(const Vector2f &vel) {_vel_desired.xy() = vel; }
299
300
/// get_vel_desired_cms - returns desired velocity in cm/s in NEU
301
const Vector3f& get_vel_desired_cms() { return _vel_desired; }
302
303
// get_vel_target_cms - returns the target velocity in NEU cm/s
304
const Vector3f& get_vel_target_cms() const { return _vel_target; }
305
306
/// set_vel_desired_z_cms - sets desired velocity in cm/s in z axis
307
void set_vel_desired_z_cms(float vel_z_cms) {_vel_desired.z = vel_z_cms;}
308
309
/// get_vel_target_z_cms - returns target vertical speed in cm/s
310
float get_vel_target_z_cms() const { return _vel_target.z; }
311
312
313
/// Acceleration
314
315
// set_accel_desired_xy_cmss - set desired acceleration in cm/s in xy axis
316
void set_accel_desired_xy_cmss(const Vector2f &accel_cms) { _accel_desired.xy() = accel_cms; }
317
318
// get_accel_target_cmss - returns the target acceleration in NEU cm/s/s
319
const Vector3f& get_accel_target_cmss() const { return _accel_target; }
320
321
322
/// Terrain
323
324
// set_pos_terrain_target_cm - set target terrain altitude in cm
325
void set_pos_terrain_target_cm(float pos_terrain_target) {_pos_terrain_target = pos_terrain_target;}
326
327
// init_pos_terrain_cm - initialises the current terrain altitude and target altitude to pos_offset_terrain_cm
328
void init_pos_terrain_cm(float pos_offset_terrain_cm);
329
330
// get_pos_terrain_cm - returns the current terrain altitude in cm
331
float get_pos_terrain_cm() { return _pos_terrain; }
332
333
334
/// Offset
335
336
#if AP_SCRIPTING_ENABLED
337
// position, velocity and acceleration offset target (only used by scripting)
338
// gets or sets an additional offset to the vehicle's target position, velocity and acceleration
339
// units are m, m/s and m/s/s in NED frame
340
bool set_posvelaccel_offset(const Vector3f &pos_offset_NED, const Vector3f &vel_offset_NED, const Vector3f &accel_offset_NED);
341
bool get_posvelaccel_offset(Vector3f &pos_offset_NED, Vector3f &vel_offset_NED, Vector3f &accel_offset_NED);
342
343
// get target velocity in m/s in NED frame
344
bool get_vel_target(Vector3f &vel_target_NED);
345
346
// get target acceleration in m/s/s in NED frame
347
bool get_accel_target(Vector3f &accel_target_NED);
348
#endif
349
350
/// set the horizontal position, velocity and acceleration offset targets in cm, cms and cm/s/s from EKF origin in NE frame
351
/// these must be set every 3 seconds (or less) or they will timeout and return to zero
352
void set_posvelaccel_offset_target_xy_cm(const Vector2p& pos_offset_target_xy_cm, const Vector2f& vel_offset_target_xy_cms, const Vector2f& accel_offset_target_xy_cmss);
353
void set_posvelaccel_offset_target_z_cm(float pos_offset_target_z_cm, float vel_offset_target_z_cms, float accel_offset_target_z_cmss);
354
355
/// get the position, velocity or acceleration offets in cm from EKF origin in NEU frame
356
const Vector3p& get_pos_offset_cm() const { return _pos_offset; }
357
const Vector3f& get_vel_offset_cms() const { return _vel_offset; }
358
const Vector3f& get_accel_offset_cmss() const { return _accel_offset; }
359
360
/// set_pos_offset_z_cm - set altitude offset in cm above the EKF origin
361
void set_pos_offset_z_cm(float pos_offset_z) { _pos_offset.z = pos_offset_z; }
362
363
/// get_pos_offset_z_cm - returns altitude offset in cm above the EKF origin
364
float get_pos_offset_z_cm() const { return _pos_offset.z; }
365
366
/// get_vel_offset_z_cm - returns current vertical offset speed in cm/s
367
float get_vel_offset_z_cms() const { return _vel_offset.z; }
368
369
/// get_accel_offset_z_cm - returns current vertical offset acceleration in cm/s/s
370
float get_accel_offset_z_cmss() const { return _accel_offset.z; }
371
372
/// Outputs
373
374
/// get desired roll and pitch to be passed to the attitude controller
375
float get_roll_cd() const { return _roll_target; }
376
float get_pitch_cd() const { return _pitch_target; }
377
378
/// get desired yaw to be passed to the attitude controller
379
float get_yaw_cd() const { return _yaw_target; }
380
381
/// get desired yaw rate to be passed to the attitude controller
382
float get_yaw_rate_cds() const { return _yaw_rate_target; }
383
384
/// get desired roll and pitch to be passed to the attitude controller
385
Vector3f get_thrust_vector() const;
386
387
/// get_bearing_to_target_cd - get bearing to target position in centi-degrees
388
int32_t get_bearing_to_target_cd() const;
389
390
/// get_lean_angle_max_cd - returns the maximum lean angle the autopilot may request
391
float get_lean_angle_max_cd() const;
392
393
/*
394
set_lean_angle_max_cd - set the maximum lean angle. A value of zero means to use the ANGLE_MAX parameter.
395
This is reset to zero on init_xy_controller()
396
*/
397
void set_lean_angle_max_cd(float angle_max_cd) { _angle_max_override_cd = angle_max_cd; }
398
399
400
/// Other
401
402
/// get pid controllers
403
AC_P_2D& get_pos_xy_p() { return _p_pos_xy; }
404
AC_P_1D& get_pos_z_p() { return _p_pos_z; }
405
AC_PID_2D& get_vel_xy_pid() { return _pid_vel_xy; }
406
AC_PID_Basic& get_vel_z_pid() { return _pid_vel_z; }
407
AC_PID& get_accel_z_pid() { return _pid_accel_z; }
408
409
/// set_limit_accel_xy - mark that accel has been limited
410
/// this prevents integrator buildup
411
void set_externally_limited_xy() { _limit_vector.x = _accel_target.x; _limit_vector.y = _accel_target.y; }
412
413
// lean_angles_to_accel - convert roll, pitch lean angles to lat/lon frame accelerations in cm/s/s
414
Vector3f lean_angles_to_accel(const Vector3f& att_target_euler) const;
415
416
// write PSC and/or PSCZ logs
417
void write_log();
418
419
// provide feedback on whether arming would be a good idea right now:
420
bool pre_arm_checks(const char *param_prefix,
421
char *failure_msg,
422
const uint8_t failure_msg_len);
423
424
// enable or disable high vibration compensation
425
void set_vibe_comp(bool on_off) { _vibe_comp_enabled = on_off; }
426
427
/// get_vel_z_error_ratio - returns the proportion of error relative to the maximum request
428
float get_vel_z_control_ratio() const { return constrain_float(_vel_z_control_ratio, 0.0f, 1.0f); }
429
430
/// crosstrack_error - returns horizontal error to the closest point to the current track
431
float crosstrack_error() const;
432
433
/// standby_xyz_reset - resets I terms and removes position error
434
/// This function will let Loiter and Alt Hold continue to operate
435
/// in the event that the flight controller is in control of the
436
/// aircraft when in standby.
437
void standby_xyz_reset();
438
439
// get earth-frame Z-axis acceleration with gravity removed in cm/s/s with +ve being up
440
float get_z_accel_cmss() const { return -(_ahrs.get_accel_ef().z + GRAVITY_MSS) * 100.0f; }
441
442
/// returns true when the forward pitch demand is limited by the maximum allowed tilt
443
bool get_fwd_pitch_is_limited() const { return _fwd_pitch_is_limited; }
444
445
// set disturbance north
446
void set_disturb_pos_cm(Vector2f disturb_pos) {_disturb_pos = disturb_pos;}
447
448
// set disturbance north
449
void set_disturb_vel_cms(Vector2f disturb_vel) {_disturb_vel = disturb_vel;}
450
451
static const struct AP_Param::GroupInfo var_info[];
452
453
static void Write_PSCN(float pos_desired, float pos_target, float pos, float vel_desired, float vel_target, float vel, float accel_desired, float accel_target, float accel);
454
static void Write_PSCE(float pos_desired, float pos_target, float pos, float vel_desired, float vel_target, float vel, float accel_desired, float accel_target, float accel);
455
static void Write_PSCD(float pos_desired, float pos_target, float pos, float vel_desired, float vel_target, float vel, float accel_desired, float accel_target, float accel);
456
static void Write_PSON(float pos_target_offset_cm, float pos_offset_cm, float vel_target_offset_cms, float vel_offset_cms, float accel_target_offset_cmss, float accel_offset_cmss);
457
static void Write_PSOE(float pos_target_offset_cm, float pos_offset_cm, float vel_target_offset_cms, float vel_offset_cms, float accel_target_offset_cmss, float accel_offset_cmss);
458
static void Write_PSOD(float pos_target_offset_cm, float pos_offset_cm, float vel_target_offset_cms, float vel_offset_cms, float accel_target_offset_cmss, float accel_offset_cmss);
459
static void Write_PSOT(float pos_target_offset_cm, float pos_offset_cm, float vel_target_offset_cms, float vel_offset_cms, float accel_target_offset_cmss, float accel_offset_cmss);
460
461
// singleton
462
static AC_PosControl *get_singleton(void) { return _singleton; }
463
464
protected:
465
466
// get throttle using vibration-resistant calculation (uses feed forward with manually calculated gain)
467
float get_throttle_with_vibration_override();
468
469
// lean_angles_to_accel - convert roll, pitch lean angles to lat/lon frame accelerations in cm/s/s
470
void accel_to_lean_angles(float accel_x_cmss, float accel_y_cmss, float& roll_target, float& pitch_target) const;
471
472
// lean_angles_to_accel - convert roll, pitch lean angles to lat/lon frame accelerations in cm/s/s
473
void lean_angles_to_accel_xy(float& accel_x_cmss, float& accel_y_cmss) const;
474
475
// calculate_yaw_and_rate_yaw - calculate the vehicle yaw and rate of yaw.
476
void calculate_yaw_and_rate_yaw();
477
478
// calculate_overspeed_gain - calculated increased maximum acceleration and jerk if over speed condition is detected
479
float calculate_overspeed_gain();
480
481
482
/// Terrain Following
483
484
/// set the position, velocity and acceleration offsets in cm, cms and cm/s/s from EKF origin in NE frame
485
/// this is used to initiate the offsets when initialise the position controller or do an offset reset
486
/// note that this sets the actual offsets, not the offset targets
487
void init_terrain();
488
489
/// update_terrain - updates the terrain position, velocity and acceleration estimation
490
/// this moves the estimated terrain position _pos_terrain towards the target _pos_terrain_target
491
void update_terrain();
492
493
494
/// Offsets
495
496
/// init_offsets - set the position, velocity and acceleration offsets in cm, cms and cm/s/s from EKF origin in NE frame
497
/// this is used to initiate the offsets when initialise the position controller or do an offset reset
498
/// note that this sets the actual offsets, not the offset targets
499
void init_offsets_xy();
500
void init_offsets_z();
501
502
/// update_offsets - update the position and velocity offsets
503
/// this moves the offsets (e.g _pos_offset, _vel_offset, _accel_offset) towards the targets (e.g. _pos_offset_target or _vel_offset_target)
504
void update_offsets_xy();
505
void update_offsets_z();
506
507
/// initialise and check for ekf position resets
508
void init_ekf_xy_reset();
509
void handle_ekf_xy_reset();
510
void init_ekf_z_reset();
511
void handle_ekf_z_reset();
512
513
// references to inertial nav and ahrs libraries
514
AP_AHRS_View& _ahrs;
515
const AP_InertialNav& _inav;
516
const class AP_Motors& _motors;
517
AC_AttitudeControl& _attitude_control;
518
519
// parameters
520
AP_Float _lean_angle_max; // Maximum autopilot commanded angle (in degrees). Set to zero for Angle Max
521
AP_Float _shaping_jerk_xy; // Jerk limit of the xy kinematic path generation in m/s^3 used to determine how quickly the aircraft varies the acceleration target
522
AP_Float _shaping_jerk_z; // Jerk limit of the z kinematic path generation in m/s^3 used to determine how quickly the aircraft varies the acceleration target
523
AC_P_2D _p_pos_xy; // XY axis position controller to convert distance error to desired velocity
524
AC_P_1D _p_pos_z; // Z axis position controller to convert altitude error to desired climb rate
525
AC_PID_2D _pid_vel_xy; // XY axis velocity controller to convert velocity error to desired acceleration
526
AC_PID_Basic _pid_vel_z; // Z axis velocity controller to convert climb rate error to desired acceleration
527
AC_PID _pid_accel_z; // Z axis acceleration controller to convert desired acceleration to throttle output
528
529
// internal variables
530
float _dt; // time difference (in seconds) since the last loop time
531
uint32_t _last_update_xy_ticks; // ticks of last last update_xy_controller call
532
uint32_t _last_update_z_ticks; // ticks of last update_z_controller call
533
float _vel_max_xy_cms; // max horizontal speed in cm/s used for kinematic shaping
534
float _vel_max_up_cms; // max climb rate in cm/s used for kinematic shaping
535
float _vel_max_down_cms; // max descent rate in cm/s used for kinematic shaping
536
float _accel_max_xy_cmss; // max horizontal acceleration in cm/s/s used for kinematic shaping
537
float _accel_max_z_cmss; // max vertical acceleration in cm/s/s used for kinematic shaping
538
float _jerk_max_xy_cmsss; // Jerk limit of the xy kinematic path generation in cm/s^3 used to determine how quickly the aircraft varies the acceleration target
539
float _jerk_max_z_cmsss; // Jerk limit of the z kinematic path generation in cm/s^3 used to determine how quickly the aircraft varies the acceleration target
540
float _vel_z_control_ratio = 2.0f; // confidence that we have control in the vertical axis
541
Vector2f _disturb_pos; // position disturbance generated by system ID mode
542
Vector2f _disturb_vel; // velocity disturbance generated by system ID mode
543
544
// output from controller
545
float _roll_target; // desired roll angle in centi-degrees calculated by position controller
546
float _pitch_target; // desired roll pitch in centi-degrees calculated by position controller
547
float _yaw_target; // desired yaw in centi-degrees calculated by position controller
548
float _yaw_rate_target; // desired yaw rate in centi-degrees per second calculated by position controller
549
550
// position controller internal variables
551
Vector3p _pos_desired; // desired location, frame NEU in cm relative to the EKF origin. This is equal to the _pos_target minus offsets
552
Vector3p _pos_target; // target location, frame NEU in cm relative to the EKF origin. This is equal to the _pos_desired plus offsets
553
Vector3f _vel_desired; // desired velocity in NEU cm/s
554
Vector3f _vel_target; // velocity target in NEU cm/s calculated by pos_to_rate step
555
Vector3f _accel_desired; // desired acceleration in NEU cm/s/s (feed forward)
556
Vector3f _accel_target; // acceleration target in NEU cm/s/s
557
Vector3f _limit_vector; // the direction that the position controller is limited, zero when not limited
558
559
bool _fwd_pitch_is_limited; // true when the forward pitch demand is being limited to meet acceleration limits
560
561
// terrain handling variables
562
float _pos_terrain_target; // position terrain target in cm relative to the EKF origin in NEU frame
563
float _pos_terrain; // position terrain in cm from the EKF origin in NEU frame. this terrain moves towards _pos_terrain_target
564
float _vel_terrain; // velocity terrain in NEU cm/s calculated by pos_to_rate step. this terrain moves towards _vel_terrain_target
565
float _accel_terrain; // acceleration terrain in NEU cm/s/s
566
567
// offset handling variables
568
Vector3p _pos_offset_target; // position offset target in cm relative to the EKF origin in NEU frame
569
Vector3p _pos_offset; // position offset in cm from the EKF origin in NEU frame. this offset moves towards _pos_offset_target
570
Vector3f _vel_offset_target; // velocity offset target in cm/s in NEU frame
571
Vector3f _vel_offset; // velocity offset in NEU cm/s calculated by pos_to_rate step. this offset moves towards _vel_offset_target
572
Vector3f _accel_offset_target; // acceleration offset target in cm/s/s in NEU frame
573
Vector3f _accel_offset; // acceleration offset in NEU cm/s/s
574
uint32_t _posvelaccel_offset_target_xy_ms; // system time that pos, vel, accel targets were set (used to implement timeouts)
575
uint32_t _posvelaccel_offset_target_z_ms; // system time that pos, vel, accel targets were set (used to implement timeouts)
576
577
// ekf reset handling
578
uint32_t _ekf_xy_reset_ms; // system time of last recorded ekf xy position reset
579
uint32_t _ekf_z_reset_ms; // system time of last recorded ekf altitude reset
580
581
// high vibration handling
582
bool _vibe_comp_enabled; // true when high vibration compensation is on
583
584
// angle max override, if zero then use ANGLE_MAX parameter
585
float _angle_max_override_cd;
586
587
// return true if on a real vehicle or SITL with lock-step scheduling
588
bool has_good_timing(void) const;
589
590
private:
591
// convenience method for writing out the identical PSCE, PSCN, PSCD - and
592
// to save bytes
593
static void Write_PSCx(LogMessages ID, float pos_desired, float pos_target, float pos, float vel_desired, float vel_target, float vel, float accel_desired, float accel_target, float accel);
594
595
// a convenience function for writing out the position controller offsets
596
static void Write_PSOx(LogMessages id, float pos_target_offset_cm, float pos_offset_cm,
597
float vel_target_offset_cms, float vel_offset_cms,
598
float accel_target_offset_cmss, float accel_offset_cmss);
599
600
// singleton
601
static AC_PosControl *_singleton;
602
};
603
604