Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
guidance_v.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
3  *
4  * This file is part of paparazzi.
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  */
21 
27 #include "generated/airframe.h"
29 
33 
34 #include "state.h"
35 
36 #include "math/pprz_algebra_int.h"
37 
38 
39 /* error if some gains are negative */
40 #if (GUIDANCE_V_HOVER_KP < 0) || \
41  (GUIDANCE_V_HOVER_KD < 0) || \
42  (GUIDANCE_V_HOVER_KI < 0)
43 #error "ALL control gains must be positive!!!"
44 #endif
45 
46 
47 /* If only GUIDANCE_V_NOMINAL_HOVER_THROTTLE is defined,
48  * disable the adaptive throttle estimation by default.
49  * Otherwise enable adaptive estimation by default.
50  */
51 #ifdef GUIDANCE_V_NOMINAL_HOVER_THROTTLE
52 # ifndef GUIDANCE_V_ADAPT_THROTTLE_ENABLED
53 # define GUIDANCE_V_ADAPT_THROTTLE_ENABLED FALSE
54 # endif
55 #else
56 # define GUIDANCE_V_NOMINAL_HOVER_THROTTLE 0.4
57 # ifndef GUIDANCE_V_ADAPT_THROTTLE_ENABLED
58 # define GUIDANCE_V_ADAPT_THROTTLE_ENABLED TRUE
59 # endif
60 #endif
63 
64 
65 #ifndef GUIDANCE_V_CLIMB_RC_DEADBAND
66 #define GUIDANCE_V_CLIMB_RC_DEADBAND MAX_PPRZ/10
67 #endif
68 
69 #ifndef GUIDANCE_V_MAX_RC_CLIMB_SPEED
70 #define GUIDANCE_V_MAX_RC_CLIMB_SPEED GUIDANCE_V_REF_MIN_ZD
71 #endif
72 
73 #ifndef GUIDANCE_V_MAX_RC_DESCENT_SPEED
74 #define GUIDANCE_V_MAX_RC_DESCENT_SPEED GUIDANCE_V_REF_MAX_ZD
75 #endif
76 
77 #ifndef GUIDANCE_V_MIN_ERR_Z
78 #define GUIDANCE_V_MIN_ERR_Z POS_BFP_OF_REAL(-10.)
79 #endif
80 
81 #ifndef GUIDANCE_V_MAX_ERR_Z
82 #define GUIDANCE_V_MAX_ERR_Z POS_BFP_OF_REAL(10.)
83 #endif
84 
85 #ifndef GUIDANCE_V_MIN_ERR_ZD
86 #define GUIDANCE_V_MIN_ERR_ZD SPEED_BFP_OF_REAL(-10.)
87 #endif
88 
89 #ifndef GUIDANCE_V_MAX_ERR_ZD
90 #define GUIDANCE_V_MAX_ERR_ZD SPEED_BFP_OF_REAL(10.)
91 #endif
92 
93 #ifndef GUIDANCE_V_MAX_SUM_ERR
94 #define GUIDANCE_V_MAX_SUM_ERR 2000000
95 #endif
96 
101 
104 
105 
110 
116 
122 
126 
128 
130 
131 
132 #define GuidanceVSetRef(_pos, _speed, _accel) { \
133  gv_set_ref(_pos, _speed, _accel); \
134  guidance_v_z_ref = _pos; \
135  guidance_v_zd_ref = _speed; \
136  guidance_v_zdd_ref = _accel; \
137  }
138 
140 static void run_hover_loop(bool_t in_flight);
141 
142 #if PERIODIC_TELEMETRY
144 
145 static void send_vert_loop(void) {
146  DOWNLINK_SEND_VERT_LOOP(DefaultChannel, DefaultDevice,
147  &guidance_v_z_sp, &guidance_v_zd_sp,
148  &(stateGetPositionNed_i()->z),
149  &(stateGetSpeedNed_i()->z),
150  &(stateGetAccelNed_i()->z),
151  &guidance_v_z_ref, &guidance_v_zd_ref,
152  &guidance_v_zdd_ref,
153  &gv_adapt_X,
154  &gv_adapt_P,
156  &guidance_v_z_sum_err,
157  &guidance_v_ff_cmd,
158  &guidance_v_fb_cmd,
159  &guidance_v_delta_t);
160 }
161 
162 static void send_tune_vert(void) {
163  DOWNLINK_SEND_TUNE_VERT(DefaultChannel, DefaultDevice,
164  &guidance_v_z_sp,
165  &(stateGetPositionNed_i()->z),
166  &guidance_v_z_ref,
167  &guidance_v_zd_ref);
168 }
169 #endif
170 
171 void guidance_v_init(void) {
172 
173  guidance_v_mode = GUIDANCE_V_MODE_KILL;
174 
175  guidance_v_kp = GUIDANCE_V_HOVER_KP;
176  guidance_v_kd = GUIDANCE_V_HOVER_KD;
177  guidance_v_ki = GUIDANCE_V_HOVER_KI;
178 
179  guidance_v_z_sum_err = 0;
180 
181  guidance_v_nominal_throttle = GUIDANCE_V_NOMINAL_HOVER_THROTTLE;
182  guidance_v_adapt_throttle_enabled = GUIDANCE_V_ADAPT_THROTTLE_ENABLED;
183 
184  gv_adapt_init();
185 
186 #if PERIODIC_TELEMETRY
187  register_periodic_telemetry(DefaultPeriodic, "VERT_LOOP", send_vert_loop);
188  register_periodic_telemetry(DefaultPeriodic, "TUNE_VERT", send_tune_vert);
189 #endif
190 }
191 
192 
193 void guidance_v_read_rc(void) {
194 
195  /* used in RC_DIRECT directly and as saturation in CLIMB and HOVER */
196  guidance_v_rc_delta_t = (int32_t)radio_control.values[RADIO_THROTTLE];
197 
198  /* used in RC_CLIMB */
199  guidance_v_rc_zd_sp = (MAX_PPRZ/2) - (int32_t)radio_control.values[RADIO_THROTTLE];
200  DeadBand(guidance_v_rc_zd_sp, GUIDANCE_V_CLIMB_RC_DEADBAND);
201 
202  static const int32_t climb_scale = ABS(SPEED_BFP_OF_REAL(GUIDANCE_V_MAX_RC_CLIMB_SPEED) /
204  static const int32_t descent_scale = ABS(SPEED_BFP_OF_REAL(GUIDANCE_V_MAX_RC_DESCENT_SPEED) /
206 
207  if(guidance_v_rc_zd_sp > 0)
208  guidance_v_rc_zd_sp *= descent_scale;
209  else
210  guidance_v_rc_zd_sp *= climb_scale;
211 }
212 
214 
215  if (new_mode == guidance_v_mode)
216  return;
217 
218  switch (new_mode) {
220  guidance_v_z_sp = stateGetPositionNed_i()->z; // set current altitude as setpoint
221  guidance_v_z_sum_err = 0;
223  break;
224 
227  guidance_v_zd_sp = 0;
228  case GUIDANCE_V_MODE_NAV:
229  guidance_v_z_sum_err = 0;
231  break;
232 
233  default:
234  break;
235 
236  }
237 
238  guidance_v_mode = new_mode;
239 
240 }
241 
242 void guidance_v_notify_in_flight( bool_t in_flight) {
243  if (in_flight) {
244  gv_adapt_init();
245  }
246 }
247 
248 
249 void guidance_v_run(bool_t in_flight) {
250 
251  // FIXME... SATURATIONS NOT TAKEN INTO ACCOUNT
252  // AKA SUPERVISION and co
253  guidance_v_thrust_coeff = get_vertical_thrust_coeff();
254  if (in_flight) {
255  int32_t vertical_thrust = (stabilization_cmd[COMMAND_THRUST] * guidance_v_thrust_coeff) >> INT32_TRIG_FRAC;
256  gv_adapt_run(stateGetAccelNed_i()->z, vertical_thrust, guidance_v_zd_ref);
257  }
258  else {
259  /* reset estimate while not in_flight */
260  gv_adapt_init();
261  }
262 
263  switch (guidance_v_mode) {
264 
266  guidance_v_z_sp = stateGetPositionNed_i()->z; // for display only
267  stabilization_cmd[COMMAND_THRUST] = guidance_v_rc_delta_t;
268  break;
269 
271  guidance_v_zd_sp = guidance_v_rc_zd_sp;
272  gv_update_ref_from_zd_sp(guidance_v_zd_sp, stateGetPositionNed_i()->z);
273  run_hover_loop(in_flight);
274  stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t;
275  break;
276 
278  gv_update_ref_from_zd_sp(guidance_v_zd_sp, stateGetPositionNed_i()->z);
279  run_hover_loop(in_flight);
280 #if !NO_RC_THRUST_LIMIT
281  /* use rc limitation if available */
282  if (radio_control.status == RC_OK)
283  stabilization_cmd[COMMAND_THRUST] = Min(guidance_v_rc_delta_t, guidance_v_delta_t);
284  else
285 #endif
286  stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t;
287  break;
288 
290  guidance_v_zd_sp = 0;
291  gv_update_ref_from_z_sp(guidance_v_z_sp);
292  run_hover_loop(in_flight);
293 #if !NO_RC_THRUST_LIMIT
294  /* use rc limitation if available */
295  if (radio_control.status == RC_OK)
296  stabilization_cmd[COMMAND_THRUST] = Min(guidance_v_rc_delta_t, guidance_v_delta_t);
297  else
298 #endif
299  stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t;
300  break;
301 
302  case GUIDANCE_V_MODE_NAV:
303  {
305  guidance_v_z_sp = -nav_flight_altitude;
306  guidance_v_zd_sp = 0;
307  gv_update_ref_from_z_sp(guidance_v_z_sp);
308  run_hover_loop(in_flight);
309  }
310  else if (vertical_mode == VERTICAL_MODE_CLIMB) {
311  guidance_v_z_sp = stateGetPositionNed_i()->z;
312  guidance_v_zd_sp = -nav_climb;
313  gv_update_ref_from_zd_sp(guidance_v_zd_sp, stateGetPositionNed_i()->z);
314  run_hover_loop(in_flight);
315  }
316  else if (vertical_mode == VERTICAL_MODE_MANUAL) {
317  guidance_v_z_sp = stateGetPositionNed_i()->z;
318  guidance_v_zd_sp = stateGetSpeedNed_i()->z;
319  GuidanceVSetRef(guidance_v_z_sp, guidance_v_zd_sp, 0);
320  guidance_v_z_sum_err = 0;
321  guidance_v_delta_t = nav_throttle;
322  }
323 #if !NO_RC_THRUST_LIMIT
324  /* use rc limitation if available */
325  if (radio_control.status == RC_OK)
326  stabilization_cmd[COMMAND_THRUST] = Min(guidance_v_rc_delta_t, guidance_v_delta_t);
327  else
328 #endif
329  stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t;
330  break;
331  }
332  default:
333  break;
334  }
335 }
336 
339  static const int32_t max_bank_coef = BFP_OF_REAL(RadOfDeg(30.), INT32_TRIG_FRAC);
340 
341  struct Int32RMat* att = stateGetNedToBodyRMat_i();
342  /* thrust vector:
343  * INT32_RMAT_VMULT(thrust_vect, att, zaxis)
344  * same as last colum of rmat with INT32_TRIG_FRAC
345  * struct Int32Vect thrust_vect = {att.m[2], att.m[5], att.m[8]};
346  *
347  * Angle between two vectors v1 and v2:
348  * angle = acos(dot(v1, v2) / (norm(v1) * norm(v2)))
349  * since here both are already of unit length:
350  * angle = acos(dot(v1, v2))
351  * since we we want the cosine of the angle we simply need
352  * thrust_coeff = dot(v1, v2)
353  * also can be simplified considering: v1 is zaxis with (0,0,1)
354  * dot(v1, v2) = v1.z * v2.z = v2.z
355  */
356  int32_t coef = att->m[8];
357  if (coef < max_bank_coef)
358  coef = max_bank_coef;
359  return coef;
360 }
361 
362 
363 #define FF_CMD_FRAC 18
364 
365 static void run_hover_loop(bool_t in_flight) {
366 
367  /* convert our reference to generic representation */
369  guidance_v_z_ref = (int32_t)tmp;
370  guidance_v_zd_ref = gv_zd_ref<<(INT32_SPEED_FRAC - GV_ZD_REF_FRAC);
371  guidance_v_zdd_ref = gv_zdd_ref<<(INT32_ACCEL_FRAC - GV_ZDD_REF_FRAC);
372  /* compute the error to our reference */
373  int32_t err_z = guidance_v_z_ref - stateGetPositionNed_i()->z;
375  int32_t err_zd = guidance_v_zd_ref - stateGetSpeedNed_i()->z;
377 
378  if (in_flight) {
379  guidance_v_z_sum_err += err_z;
380  Bound(guidance_v_z_sum_err, -GUIDANCE_V_MAX_SUM_ERR, GUIDANCE_V_MAX_SUM_ERR);
381  }
382  else
383  guidance_v_z_sum_err = 0;
384 
385  /* our nominal command : (g + zdd)*m */
386  int32_t inv_m;
387  if (guidance_v_adapt_throttle_enabled) {
388  inv_m = gv_adapt_X >> (GV_ADAPT_X_FRAC - FF_CMD_FRAC);
389  }
390  else {
391  /* use the fixed nominal throttle */
392  inv_m = BFP_OF_REAL(9.81 / (guidance_v_nominal_throttle * MAX_PPRZ), FF_CMD_FRAC);
393  }
394 
395  const int32_t g_m_zdd = (int32_t)BFP_OF_REAL(9.81, FF_CMD_FRAC) -
396  (guidance_v_zdd_ref << (FF_CMD_FRAC - INT32_ACCEL_FRAC));
397 
398  guidance_v_ff_cmd = g_m_zdd / inv_m;
399  /* feed forward command */
400  guidance_v_ff_cmd = (guidance_v_ff_cmd << INT32_TRIG_FRAC) / guidance_v_thrust_coeff;
401 
402  /* bound the nominal command to 0.9*MAX_PPRZ */
403  Bound(guidance_v_ff_cmd, 0, 8640);
404 
405 
406  /* our error feed back command */
407  /* z-axis pointing down -> positive error means we need less thrust */
408  guidance_v_fb_cmd = ((-guidance_v_kp * err_z) >> 7) +
409  ((-guidance_v_kd * err_zd) >> 16) +
410  ((-guidance_v_ki * guidance_v_z_sum_err) >> 16);
411 
412  guidance_v_delta_t = guidance_v_ff_cmd + guidance_v_fb_cmd;
413 
414  /* bound the result */
415  Bound(guidance_v_delta_t, 0, MAX_PPRZ);
416 
417 }
int32_t guidance_v_z_sp
altitude setpoint in meters (input).
Definition: guidance_v.c:117
#define GV_ZD_REF_FRAC
number of bits for the fractional part of gv_zd_ref
int32_t guidance_v_kd
vertical control D-gain
Definition: guidance_v.c:124
#define GUIDANCE_V_MODE_NAV
Definition: guidance_v.h:40
#define Min(x, y)
#define GUIDANCE_V_NOMINAL_HOVER_THROTTLE
Definition: guidance_v.c:56
#define GV_Z_REF_FRAC
number of bits for the fractional part of gv_z_ref
int32_t guidance_v_fb_cmd
feed-back command
Definition: guidance_v.c:99
Periodic telemetry system header (includes downlink utility and generated code).
#define INT32_POS_FRAC
static struct Int32RMat * stateGetNedToBodyRMat_i(void)
Get vehicle body attitude rotation matrix (int).
Definition: state.h:1007
int32_t guidance_v_delta_t
thrust command.
Definition: guidance_v.c:100
void guidance_v_run(bool_t in_flight)
Definition: guidance_v.c:249
int32_t guidance_v_kp
vertical control P-gain
Definition: guidance_v.c:123
#define GV_ADAPT_X_FRAC
void guidance_v_init(void)
Definition: guidance_v.c:171
void guidance_v_notify_in_flight(bool_t in_flight)
Definition: guidance_v.c:242
#define GUIDANCE_V_MIN_ERR_Z
Definition: guidance_v.c:78
void guidance_v_mode_changed(uint8_t new_mode)
Definition: guidance_v.c:213
float guidance_v_nominal_throttle
nominal throttle for hover.
Definition: guidance_v.c:102
Vertical guidance for rotorcrafts.
static void run_hover_loop(bool_t in_flight)
Definition: guidance_v.c:365
#define GV_ZDD_REF_FRAC
number of bits for the fractional part of gv_zdd_ref
bool_t guidance_v_adapt_throttle_enabled
Use adaptive throttle command estimation.
Definition: guidance_v.c:103
signed long long int64_t
Definition: types.h:21
int32_t guidance_v_z_sum_err
accumulator for I-gain
Definition: guidance_v.c:127
#define INT32_TRIG_FRAC
bool_t register_periodic_telemetry(struct pprz_telemetry *_pt, const char *_msg, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:38
#define GUIDANCE_V_MODE_KILL
Definition: guidance_v.h:35
int32_t guidance_v_rc_zd_sp
Vertical speed setpoint from radio control.
Definition: guidance_v.c:115
void gv_update_ref_from_z_sp(int32_t z_sp)
int32_t z
Down.
static struct NedCoor_i * stateGetSpeedNed_i(void)
Get ground speed in local NED coordinates (int).
Definition: state.h:798
int32_t guidance_v_ki
vertical control I-gain
Definition: guidance_v.c:125
pprz_t values[RADIO_CONTROL_NB_CHANNEL]
Definition: radio_control.h:55
void gv_adapt_init(void)
#define GUIDANCE_V_MAX_RC_DESCENT_SPEED
Definition: guidance_v.c:74
#define GUIDANCE_V_MODE_CLIMB
Definition: guidance_v.h:38
int32_t gv_zd_ref
reference model vertical speed in meters/sec (output) fixed point representation with GV_ZD_REF_FRAC ...
#define BFP_OF_REAL(_vr, _frac)
int32_t guidance_v_zdd_ref
vertical acceleration reference in meter/s^2.
Definition: guidance_v.c:121
#define GUIDANCE_V_MAX_ERR_Z
Definition: guidance_v.c:82
#define GUIDANCE_V_MODE_HOVER
Definition: guidance_v.h:39
struct RadioControl radio_control
Definition: radio_control.c:25
void gv_adapt_run(int32_t zdd_meas, int32_t thrust_applied, int32_t zd_ref)
Adaptation function.
#define GUIDANCE_V_MAX_RC_CLIMB_SPEED
Definition: guidance_v.c:70
int32_t guidance_v_z_ref
altitude reference in meters.
Definition: guidance_v.c:119
void gv_update_ref_from_zd_sp(int32_t zd_sp, int32_t z_pos)
update vertical reference from speed setpoint.
int32_t guidance_v_zd_ref
vertical speed reference in meter/s.
Definition: guidance_v.c:120
int32_t gv_adapt_P
Covariance.
#define GuidanceVSetRef(_pos, _speed, _accel)
Definition: guidance_v.c:132
signed long int32_t
Definition: types.h:19
#define RC_OK
Definition: radio_control.h:45
uint8_t status
Definition: radio_control.h:50
#define GUIDANCE_V_CLIMB_RC_DEADBAND
Definition: guidance_v.c:66
uint8_t guidance_v_mode
Definition: guidance_v.c:97
#define RADIO_THROTTLE
Definition: spektrum_arch.h:39
#define GUIDANCE_V_ADAPT_THROTTLE_ENABLED
Definition: guidance_v.c:58
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.
int32_t gv_adapt_Xmeas
Measurement.
#define FF_CMD_FRAC
Definition: guidance_v.c:363
int32_t guidance_v_thrust_coeff
Definition: guidance_v.c:129
#define GUIDANCE_V_MIN_ERR_ZD
Definition: guidance_v.c:86
int64_t gv_z_ref
reference model altitude in meters (output) fixed point representation with GV_Z_REF_FRAC Q37...
static int32_t get_vertical_thrust_coeff(void)
get the cosine of the angle between thrust vector and gravity vector
Definition: guidance_v.c:338
General stabilization interface for rotorcrafts.
int32_t m[3 *3]
void guidance_v_read_rc(void)
Definition: guidance_v.c:193
int32_t stabilization_cmd[COMMANDS_NB]
Stabilization commands.
Definition: stabilization.c:28
#define GUIDANCE_V_MAX_ERR_ZD
Definition: guidance_v.c:90
rotation matrix
int32_t guidance_v_zd_sp
vertical speed setpoint in meter/s (input).
Definition: guidance_v.c:118
#define GUIDANCE_V_MAX_SUM_ERR
Definition: guidance_v.c:94
int32_t guidance_v_rc_delta_t
Direct throttle from radio control.
Definition: guidance_v.c:109
int32_t guidance_v_ff_cmd
feed-forward command
Definition: guidance_v.c:98
#define INT32_SPEED_FRAC
#define GUIDANCE_V_MODE_RC_CLIMB
Definition: guidance_v.h:37
#define MAX_PPRZ
Definition: paparazzi.h:8
static struct NedCoor_i * stateGetPositionNed_i(void)
Get position in local NED coordinates (int).
Definition: state.h:630
#define GUIDANCE_V_MODE_RC_DIRECT
Definition: guidance_v.h:36
uint8_t vertical_mode
Definition: sim_ap.c:37
#define INT32_ACCEL_FRAC
int32_t gv_adapt_X
State of the estimator.
#define SPEED_BFP_OF_REAL(_af)
int32_t gv_zdd_ref
reference model vertical accel in meters/s^2 (output) fixed point representation with GV_ZDD_REF_FRAC...
static struct NedCoor_i * stateGetAccelNed_i(void)
Get acceleration in NED coordinates (int).
Definition: state.h:924
Paparazzi fixed point algebra.