Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
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"
30 
35 
36 #include "state.h"
37 
38 #include "math/pprz_algebra_int.h"
39 
40 
41 #ifndef GUIDANCE_V_NOMINAL_HOVER_THROTTLE
42 #define GUIDANCE_V_NOMINAL_HOVER_THROTTLE 0.4
43 #endif
44 PRINT_CONFIG_VAR(GUIDANCE_V_NOMINAL_HOVER_THROTTLE)
45 
46 
47 #ifndef GUIDANCE_V_CLIMB_RC_DEADBAND
48 #define GUIDANCE_V_CLIMB_RC_DEADBAND MAX_PPRZ/10
49 #endif
50 
51 #ifndef GUIDANCE_V_MAX_RC_CLIMB_SPEED
52 #define GUIDANCE_V_MAX_RC_CLIMB_SPEED GUIDANCE_V_REF_MIN_ZD
53 #endif
54 
55 #ifndef GUIDANCE_V_MAX_RC_DESCENT_SPEED
56 #define GUIDANCE_V_MAX_RC_DESCENT_SPEED GUIDANCE_V_REF_MAX_ZD
57 #endif
58 
60 
61 static bool desired_zd_updated;
62 
63 #define GUIDANCE_V_GUIDED_MODE_ZHOLD 0
64 #define GUIDANCE_V_GUIDED_MODE_CLIMB 1
65 #define GUIDANCE_V_GUIDED_MODE_THROTTLE 2
66 
68 
69 
70 #if PERIODIC_TELEMETRY
72 
73 static void send_tune_vert(struct transport_tx *trans, struct link_device *dev)
74 {
75  pprz_msg_send_TUNE_VERT(trans, dev, AC_ID,
77  &(stateGetPositionNed_i()->z),
80 }
81 #endif
82 
83 void guidance_v_init(void)
84 {
85 
89 
91  desired_zd_updated = false;
92 
93  gv_adapt_init();
94 
95 #if GUIDANCE_V_MODE_MODULE_SETTING == GUIDANCE_V_MODE_MODULE
97 #endif
98 
99 #if PERIODIC_TELEMETRY
101 #endif
102 }
103 
104 
106 {
107 
108  /* used in RC_DIRECT directly and as saturation in CLIMB and HOVER */
110 
111  /* used in RC_CLIMB */
114 
115  static const int32_t climb_scale = ABS(SPEED_BFP_OF_REAL(GUIDANCE_V_MAX_RC_CLIMB_SPEED) /
117  static const int32_t descent_scale = ABS(SPEED_BFP_OF_REAL(GUIDANCE_V_MAX_RC_DESCENT_SPEED) /
119 
120  if (guidance_v.rc_zd_sp > 0) {
121  guidance_v.rc_zd_sp *= descent_scale;
122  } else {
123  guidance_v.rc_zd_sp *= climb_scale;
124  }
125 }
126 
128 {
129 
130  if (new_mode == guidance_v.mode) {
131  return;
132  }
133 
134  switch (new_mode) {
138  break;
139 
142  guidance_v.zd_sp = 0;
143  /* Falls through. */
144  case GUIDANCE_V_MODE_NAV:
147  break;
148 
149 #if GUIDANCE_V_MODE_MODULE_SETTING == GUIDANCE_V_MODE_MODULE
152  break;
153 #endif
154 
156  break;
157 
158  default:
159  break;
160 
161  }
162 
163  guidance_v.mode = new_mode;
164 
165 }
166 
167 void guidance_v_notify_in_flight(bool in_flight)
168 {
169  if (in_flight) {
170  gv_adapt_init();
171  }
172 }
173 
176 {
177  // cos(30°) = 0.8660254
178  static const int32_t max_bank_coef = BFP_OF_REAL(0.8660254f, INT32_TRIG_FRAC);
179 
180  struct Int32RMat *att = stateGetNedToBodyRMat_i();
181  /* thrust vector:
182  * int32_rmat_vmult(&thrust_vect, &att, &zaxis)
183  * same as last colum of rmat with INT32_TRIG_FRAC
184  * struct Int32Vect thrust_vect = {att.m[2], att.m[5], att.m[8]};
185  *
186  * Angle between two vectors v1 and v2:
187  * angle = acos(dot(v1, v2) / (norm(v1) * norm(v2)))
188  * since here both are already of unit length:
189  * angle = acos(dot(v1, v2))
190  * since we we want the cosine of the angle we simply need
191  * thrust_coeff = dot(v1, v2)
192  * also can be simplified considering: v1 is zaxis with (0,0,1)
193  * dot(v1, v2) = v1.z * v2.z = v2.z
194  */
195  int32_t coef = att->m[8];
196  if (coef < max_bank_coef) {
197  coef = max_bank_coef;
198  }
199  return coef;
200 }
201 
202 
203 void guidance_v_thrust_adapt(bool in_flight)
204 {
206 
207  if (in_flight) {
208  /* Only run adaptive throttle estimation if we are in flight and
209  * the desired vertical velocity (zd) was updated (i.e. we ran hover_loop before).
210  * This means that the estimation is not updated when using direct throttle commands.
211  *
212  * FIXME... SATURATIONS NOT TAKEN INTO ACCOUNT, AKA SUPERVISION and co
213  */
214  if (desired_zd_updated) {
215  int32_t vertical_thrust = (stabilization_cmd[COMMAND_THRUST] * guidance_v.thrust_coeff) >> INT32_TRIG_FRAC;
216  gv_adapt_run(stateGetAccelNed_i()->z, vertical_thrust, guidance_v.zd_ref);
217  }
218  } else {
219  /* reset estimate while not in_flight */
220  gv_adapt_init();
221  }
222 }
223 
224 void guidance_v_run(bool in_flight)
225 {
226  guidance_v_thrust_adapt(in_flight);
227 
228  /* reset flag indicating if desired zd was updated */
229  desired_zd_updated = false;
230 
231  switch (guidance_v.mode) {
232 
234  guidance_v.z_sp = stateGetPositionNed_i()->z; // for display only
235  stabilization_cmd[COMMAND_THRUST] = guidance_v.rc_delta_t;
236  break;
237 
242  stabilization_cmd[COMMAND_THRUST] = guidance_v.delta_t;
243  break;
244 
248 #if !NO_RC_THRUST_LIMIT
249  /* use rc limitation if available */
250  if (radio_control.status == RC_OK) {
252  } else
253 #endif
254  stabilization_cmd[COMMAND_THRUST] = guidance_v.delta_t;
255  break;
256 
259  /* Falls through. */
261  guidance_v_guided_run(in_flight);
262  break;
263 
264 #if GUIDANCE_V_MODE_MODULE_SETTING == GUIDANCE_V_MODE_MODULE
266  guidance_v_module_run(in_flight);
267  break;
268 #endif
269 
270  case GUIDANCE_V_MODE_NAV: {
271  guidance_v_from_nav(in_flight);
272  break;
273  }
274 
276  break;
277 
278  default:
279  break;
280  }
281 }
282 
283 
285 {
286  /* set current altitude as setpoint */
288 
289  /* reset guidance reference */
292 
293  /* reset speed setting */
294  guidance_v.zd_sp = 0;
295 }
296 
297 void guidance_v_set_ref(int32_t pos, int32_t speed, int32_t accel)
298 {
299  gv_set_ref(pos, speed, accel);
300  guidance_v.z_ref = pos;
301  guidance_v.zd_ref = speed;
302  guidance_v.zdd_ref = accel;
303 }
304 
305 
307 {
308  /* convert our reference to generic representation */
309  int64_t tmp = gv_z_ref >> (GV_Z_REF_FRAC - INT32_POS_FRAC);
310  guidance_v.z_ref = (int32_t)tmp;
313  /* set flag to indicate that desired zd was updated */
314  desired_zd_updated = true;
315 }
316 
317 void guidance_v_from_nav(bool in_flight)
318 {
321  guidance_v.zd_sp = 0;
325  } else if (nav.vertical_mode == NAV_VERTICAL_MODE_CLIMB) {
338  guidance_v_guided_run(in_flight);
339  }
340 #if !NO_RC_THRUST_LIMIT
341  /* use rc limitation if available */
342  if (radio_control.status == RC_OK) {
344  } else
345 #endif
346  stabilization_cmd[COMMAND_THRUST] = guidance_v.delta_t;
347 }
348 
350 {
351  /* set current altitude as setpoint */
353 
354  /* reset guidance reference */
357 }
358 
359 void guidance_v_guided_run(bool in_flight)
360 {
361  switch(guidance_v_guided_mode)
362  {
364  // Altitude Hold
365  guidance_v.zd_sp = 0;
369  break;
371  // Climb
375  break;
377  // Throttle
378  guidance_v.z_sp = stateGetPositionNed_i()->z; // for display only
380  break;
381  default:
382  break;
383  }
384 #if !NO_RC_THRUST_LIMIT
385  /* use rc limitation if available */
386  if (radio_control.status == RC_OK) {
388  } else
389 #endif
390  stabilization_cmd[COMMAND_THRUST] = guidance_v.delta_t;
391 }
392 
393 void guidance_v_set_z(float z)
394 {
395  /* disable vertical velocity setpoints */
397  /* set altitude setpoint */
399  /* reset speed setting */
400  guidance_v.zd_sp = 0;
401 }
402 
403 void guidance_v_set_vz(float vz)
404 {
405  /* enable vertical velocity setpoints */
407  /* set speed setting */
409 }
410 
411 void guidance_v_set_th(float th)
412 {
413  /* enable vertical velocity setpoints */
415 
416  /* reset guidance reference */
418  guidance_v.th_sp = (int32_t)(MAX_PPRZ * th);
419  Bound(guidance_v.th_sp, 0, MAX_PPRZ);
420 }
421 
422 
void guidance_v_module_enter(void)
void guidance_v_module_init(void)
void guidance_v_module_run(UNUSED bool in_flight)
#define Min(x, y)
Definition: esc_dshot.c:101
int32_t m[3 *3]
#define INT32_POS_FRAC
#define INT32_TRIG_FRAC
#define INT32_SPEED_FRAC
#define INT32_ACCEL_FRAC
#define BFP_OF_REAL(_vr, _frac)
#define POS_BFP_OF_REAL(_af)
#define SPEED_BFP_OF_REAL(_af)
rotation matrix
int32_t z
Down.
static struct NedCoor_i * stateGetAccelNed_i(void)
Get acceleration in NED coordinates (int).
Definition: state.h:1020
static struct Int32RMat * stateGetNedToBodyRMat_i(void)
Get vehicle body attitude rotation matrix (int).
Definition: state.h:1119
static struct NedCoor_f * stateGetPositionNed_f(void)
Get position in local NED coordinates (float).
Definition: state.h:710
static struct NedCoor_i * stateGetPositionNed_i(void)
Get position in local NED coordinates (int).
Definition: state.h:665
static struct NedCoor_i * stateGetSpeedNed_i(void)
Get ground speed in local NED coordinates (int).
Definition: state.h:863
Guidance controllers (horizontal and vertical) for Hybrid UAV configurations.
Guidance in a module file.
void guidance_v_run_enter(void)
int32_t guidance_v_run_speed(bool in_flight UNUSED, struct VerticalGuidance *gv)
int32_t guidance_v_run_pos(bool in_flight UNUSED, struct VerticalGuidance *gv)
void gv_adapt_run(int32_t zdd_meas, int32_t thrust_applied, int32_t zd_ref)
Adaptation function.
void gv_adapt_init(void)
int32_t gv_zdd_ref
reference model vertical accel in meters/s^2 (output) fixed point representation with GV_ZDD_REF_FRAC...
int64_t gv_z_ref
reference model altitude in meters (output) fixed point representation with GV_Z_REF_FRAC Q37....
void gv_set_ref(int32_t alt, int32_t speed, int32_t accel)
void gv_update_ref_from_z_sp(int32_t z_sp)
void gv_update_ref_from_zd_sp(int32_t zd_sp, int32_t z_pos)
update vertical reference from speed setpoint.
int32_t gv_zd_ref
reference model vertical speed in meters/sec (output) fixed point representation with GV_ZD_REF_FRAC ...
#define GV_ZD_REF_FRAC
number of bits for the fractional part of gv_zd_ref
#define GV_Z_REF_FRAC
number of bits for the fractional part of gv_z_ref
#define GV_ZDD_REF_FRAC
number of bits for the fractional part of gv_zdd_ref
#define MAX_PPRZ
Definition: paparazzi.h:8
Paparazzi fixed point algebra.
struct RadioControl radio_control
Definition: radio_control.c:33
Generic interface for radio control modules.
pprz_t values[RADIO_CONTROL_NB_CHANNEL]
Definition: radio_control.h:67
uint8_t status
Definition: radio_control.h:61
#define RC_OK
Definition: radio_control.h:49
void guidance_v_read_rc(void)
Definition: guidance_v.c:105
static void send_tune_vert(struct transport_tx *trans, struct link_device *dev)
Definition: guidance_v.c:73
void guidance_v_thrust_adapt(bool in_flight)
Definition: guidance_v.c:203
#define GUIDANCE_V_MAX_RC_DESCENT_SPEED
Definition: guidance_v.c:56
void guidance_v_set_vz(float vz)
Set z velocity setpoint.
Definition: guidance_v.c:403
void guidance_v_z_enter(void)
Definition: guidance_v.c:284
void guidance_v_notify_in_flight(bool in_flight)
Definition: guidance_v.c:167
#define GUIDANCE_V_MAX_RC_CLIMB_SPEED
Definition: guidance_v.c:52
static bool desired_zd_updated
Definition: guidance_v.c:61
#define GUIDANCE_V_NOMINAL_HOVER_THROTTLE
Definition: guidance_v.c:42
void guidance_v_run(bool in_flight)
Definition: guidance_v.c:224
#define GUIDANCE_V_GUIDED_MODE_ZHOLD
Definition: guidance_v.c:63
void guidance_v_set_z(float z)
Set z position setpoint.
Definition: guidance_v.c:393
void guidance_v_guided_enter(void)
Enter GUIDED mode control.
Definition: guidance_v.c:349
#define GUIDANCE_V_GUIDED_MODE_THROTTLE
Definition: guidance_v.c:65
static int32_t get_vertical_thrust_coeff(void)
get the cosine of the angle between thrust vector and gravity vector
Definition: guidance_v.c:175
void guidance_v_set_ref(int32_t pos, int32_t speed, int32_t accel)
Set guidance ref parameters.
Definition: guidance_v.c:297
#define GUIDANCE_V_GUIDED_MODE_CLIMB
Definition: guidance_v.c:64
void guidance_v_set_th(float th)
Set throttle setpoint.
Definition: guidance_v.c:411
void guidance_v_mode_changed(uint8_t new_mode)
Definition: guidance_v.c:127
void guidance_v_guided_run(bool in_flight)
Run GUIDED mode control.
Definition: guidance_v.c:359
struct VerticalGuidance guidance_v
Definition: guidance_v.c:59
#define GUIDANCE_V_CLIMB_RC_DEADBAND
Definition: guidance_v.c:48
void guidance_v_from_nav(bool in_flight)
Set guidance setpoint from NAV and run hover loop.
Definition: guidance_v.c:317
void guidance_v_update_ref(void)
Definition: guidance_v.c:306
void guidance_v_init(void)
Definition: guidance_v.c:83
static int guidance_v_guided_mode
Definition: guidance_v.c:67
Vertical guidance for rotorcrafts.
#define GuidanceVSetRef
Definition: guidance_v.h:128
float nominal_throttle
nominal throttle for hover.
Definition: guidance_v.h:103
int32_t z_sp
altitude setpoint in meters (input).
Definition: guidance_v.h:51
int32_t zd_sp
vertical speed setpoint in meter/s (input).
Definition: guidance_v.h:57
#define GUIDANCE_V_MODE_FLIP
Definition: guidance_v.h:41
int32_t z_ref
altitude reference in meters.
Definition: guidance_v.h:63
#define GUIDANCE_V_MODE_CLIMB
Definition: guidance_v.h:37
#define GUIDANCE_V_MODE_RC_CLIMB
Definition: guidance_v.h:36
int32_t zd_ref
vertical speed reference in meter/s.
Definition: guidance_v.h:69
#define GUIDANCE_V_MODE_KILL
Definition: guidance_v.h:34
int32_t th_sp
thrust setpoint.
Definition: guidance_v.h:91
int32_t rc_zd_sp
Vertical speed setpoint from radio control.
Definition: guidance_v.h:86
#define GUIDANCE_V_MODE_RC_DIRECT
Definition: guidance_v.h:35
#define GUIDANCE_V_MODE_NAV
Definition: guidance_v.h:39
int32_t delta_t
thrust command.
Definition: guidance_v.h:97
#define GUIDANCE_V_MODE_HOVER
Definition: guidance_v.h:38
#define GUIDANCE_V_MODE_MODULE
Definition: guidance_v.h:40
int32_t thrust_coeff
Definition: guidance_v.h:105
int32_t zdd_ref
vertical acceleration reference in meter/s^2.
Definition: guidance_v.h:75
#define GUIDANCE_V_MODE_GUIDED
Definition: guidance_v.h:42
int32_t rc_delta_t
Direct throttle from radio control.
Definition: guidance_v.h:80
struct RotorcraftNavigation nav
Definition: navigation.c:51
Rotorcraft navigation functions.
uint32_t throttle
throttle command (in pprz_t)
Definition: navigation.h:130
float climb
climb setpoint (in m/s)
Definition: navigation.h:137
#define NAV_VERTICAL_MODE_CLIMB
Definition: navigation.h:93
#define NAV_VERTICAL_MODE_MANUAL
Definition: navigation.h:92
float nav_altitude
current altitude setpoint (in meters): might differ from fp_altitude depending on altitude shift from...
Definition: navigation.h:139
#define NAV_VERTICAL_MODE_ALT
Definition: navigation.h:94
#define NAV_VERTICAL_MODE_GUIDED
Definition: navigation.h:95
int32_t stabilization_cmd[COMMANDS_NB]
Stabilization commands.
Definition: stabilization.c:34
General stabilization interface for rotorcrafts.
API to get/set the generic vehicle states.
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:51
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98