Paparazzi UAS  v5.0.5_stable-7-g4b8bbb7
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
guidance_h.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 
28 
32 
33 #include "state.h"
34 
35 #include "generated/airframe.h"
36 
37 /* error if some gains are negative */
38 #if (GUIDANCE_H_PGAIN < 0) || \
39  (GUIDANCE_H_DGAIN < 0) || \
40  (GUIDANCE_H_IGAIN < 0)
41 #error "ALL control gains have to be positive!!!"
42 #endif
43 
44 #ifndef GUIDANCE_H_AGAIN
45 #define GUIDANCE_H_AGAIN 0
46 #else
47 #if (GUIDANCE_H_AGAIN < 0)
48 #error "ALL control gains have to be positive!!!"
49 #endif
50 #endif
51 
52 #ifndef GUIDANCE_H_MAX_BANK
53 #define GUIDANCE_H_MAX_BANK RadOfDeg(20)
54 #endif
55 
56 PRINT_CONFIG_VAR(GUIDANCE_H_USE_REF)
57 
58 
61 
66 
71 
76 
81 
84 
85 
86 static void guidance_h_update_reference(void);
87 static void guidance_h_traj_run(bool_t in_flight);
88 static void guidance_h_hover_enter(void);
89 static void guidance_h_nav_enter(void);
90 static inline void transition_run(void);
91 
92 
93 void guidance_h_init(void) {
94 
95  guidance_h_mode = GUIDANCE_H_MODE_KILL;
96  guidance_h_use_ref = GUIDANCE_H_USE_REF;
97 
98  INT_VECT2_ZERO(guidance_h_pos_sp);
99  INT_VECT2_ZERO(guidance_h_pos_err_sum);
100  INT_EULERS_ZERO(guidance_h_rc_sp);
101  INT_EULERS_ZERO(guidance_h_command_body);
102  guidance_h_pgain = GUIDANCE_H_PGAIN;
103  guidance_h_igain = GUIDANCE_H_IGAIN;
104  guidance_h_dgain = GUIDANCE_H_DGAIN;
105  guidance_h_again = GUIDANCE_H_AGAIN;
106  transition_percentage = 0;
107  transition_theta_offset = 0;
108 }
109 
110 
116 
118 }
119 
121  if (new_mode == guidance_h_mode)
122  return;
123 
124  if (new_mode != GUIDANCE_H_MODE_FORWARD && new_mode != GUIDANCE_H_MODE_RATE) {
127  }
128 
129  switch (new_mode) {
132  break;
133 
136  break;
137 
143  break;
144 
147  /* reset attitude stabilization if previous mode was not using it */
151  }
152  break;
153 
154  case GUIDANCE_H_MODE_NAV:
156  /* reset attitude stabilization if previous mode was not using it */
160  }
161  break;
162 
163  default:
164  break;
165  }
166 
167  guidance_h_mode = new_mode;
168 
169 }
170 
171 
172 void guidance_h_read_rc(bool_t in_flight) {
173 
174  switch ( guidance_h_mode ) {
175 
178  break;
179 
181 #if SWITCH_STICKS_FOR_RATE_CONTROL
183 #else
185 #endif
186  break;
187 
192  break;
193 
196  break;
197 
198  case GUIDANCE_H_MODE_NAV:
199  if (radio_control.status == RC_OK) {
201  }
202  else {
204  }
205  break;
206  default:
207  break;
208  }
209 
210 }
211 
212 
213 void guidance_h_run(bool_t in_flight) {
214  switch ( guidance_h_mode ) {
215 
217  stabilization_none_run(in_flight);
218  break;
219 
221  stabilization_rate_run(in_flight);
222  break;
223 
226  transition_run();
227  }
230  stabilization_attitude_run(in_flight);
231  break;
232 
234  if (!in_flight)
236 
238 
239  /* set psi command */
241  /* compute roll and pitch commands and set final attitude setpoint */
242  guidance_h_traj_run(in_flight);
243 
244  stabilization_attitude_run(in_flight);
245  break;
246 
247  case GUIDANCE_H_MODE_NAV:
248  if (!in_flight)
250 
252  struct Int32Eulers sp_euler_i;
253  sp_euler_i.phi = nav_roll;
254  sp_euler_i.theta = nav_pitch;
255  /* FIXME: heading can't be set via attitude block yet, use current heading for now */
256  sp_euler_i.psi = stateGetNedToBodyEulers_i()->psi;
258  }
259  else {
261 
263 
264  /* set psi command */
266  /* compute roll and pitch commands and set final attitude setpoint */
267  guidance_h_traj_run(in_flight);
268  }
269  stabilization_attitude_run(in_flight);
270  break;
271 
272  default:
273  break;
274  }
275 }
276 
277 
278 static void guidance_h_update_reference(void) {
279  /* compute reference even if usage temporarily disabled via guidance_h_use_ref */
280 #if GUIDANCE_H_USE_REF
282 #endif
283 
284  /* either use the reference or simply copy the pos setpoint */
285  if (guidance_h_use_ref) {
286  /* convert our reference to generic representation */
290  } else {
294  }
295 }
296 
297 
298 #define MAX_POS_ERR POS_BFP_OF_REAL(16.)
299 #define MAX_SPEED_ERR SPEED_BFP_OF_REAL(16.)
300 #define MAX_POS_ERR_SUM ((int32_t)(MAX_POS_ERR)<< 12)
301 
302 /* with a pgain of 100 and a scale of 2,
303  * you get an angle of 5.6 degrees for 1m pos error */
304 #define GH_GAIN_SCALE 2
305 
307 #define TRAJ_MAX_BANK BFP_OF_REAL(GUIDANCE_H_MAX_BANK, INT32_ANGLE_FRAC)
308 
309 static void guidance_h_traj_run(bool_t in_flight) {
310 
311  /* compute position error */
313  /* saturate it */
315 
316  /* compute speed error */
318  /* saturate it */
320 
321  /* update pos error integral, zero it if not in_flight */
322  if (in_flight) {
324  /* saturate it */
326  } else {
328  }
329 
330  /* run PID */
335  ((guidance_h_again * guidance_h_accel_ref.x) >> 8); /* feedforward gain */
340  ((guidance_h_again * guidance_h_accel_ref.y) >> 8); /* feedforward gain */
341 
343 
344  /* Rotate to body frame */
345  int32_t s_psi, c_psi;
347  PPRZ_ITRIG_SIN(s_psi, psi);
348  PPRZ_ITRIG_COS(c_psi, psi);
349 
350  // Restore angle ref resolution after rotation
355 
356 
357  /* Add RC roll and pitch setpoints for emergency corrections */
360 
361  /* Set attitude setpoint from pseudo-eulers */
363 }
364 
365 static void guidance_h_hover_enter(void) {
366 
367  /* set horizontal setpoint to current position */
369 
371 
373 }
374 
375 static void guidance_h_nav_enter(void) {
376 
377  /* horizontal position setpoint from navigation/flightplan */
379 
381 
383 }
384 
385 static inline void transition_run(void) {
386  //Add 0.00625%
388 
389 #ifdef TRANSITION_MAX_OFFSET
390  const int32_t max_offset = ANGLE_BFP_OF_REAL(TRANSITION_MAX_OFFSET);
392 #endif
393 }
void gh_update_ref_from_pos_sp(struct Int32Vect2 pos_sp)
#define INT32_PERCENTAGE_FRAC
int32_t phi
in rad with INT32_ANGLE_FRAC
void gh_set_ref(struct Int32Vect2 pos, struct Int32Vect2 speed, struct Int32Vect2 accel)
#define INT32_VECT2_RSHIFT(_o, _i, _r)
void stabilization_attitude_read_rc(bool_t in_flight)
int32_t guidance_h_dgain
Definition: guidance_h.c:78
#define GUIDANCE_H_MODE_RC_DIRECT
Definition: guidance_h.h:49
void stabilization_rate_run(bool_t in_flight)
#define INT_MULT_RSHIFT(_a, _b, _r)
#define INT32_VECT2_NED_OF_ENU(_o, _i)
#define GUIDANCE_H_MODE_CARE_FREE
Definition: guidance_h.h:50
#define ANGLE_BFP_OF_REAL(_af)
#define INT32_POS_FRAC
void stabilization_rate_enter(void)
void stabilization_attitude_set_from_eulers_i(struct Int32Eulers *sp_euler)
struct Int64Vect2 gh_pos_ref
Read an attitude setpoint from the RC.
#define GUIDANCE_H_MODE_ATTITUDE
Definition: guidance_h.h:46
struct Int32Vect2 gh_accel_ref
Reference model acceleration.
static void transition_run(void)
Definition: guidance_h.c:385
int32_t theta
in rad with INT32_ANGLE_FRAC
#define GUIDANCE_H_AGAIN
Definition: guidance_h.c:45
#define INT_EULERS_ZERO(_e)
void stabilization_attitude_enter(void)
static void reset_guidance_reference_from_current_position(void)
Definition: guidance_h.c:111
int32_t transition_percentage
Definition: guidance_h.c:82
void stabilization_attitude_reset_care_free_heading(void)
reset the heading for care-free mode to current heading
#define INT32_TRIG_FRAC
bool_t guidance_h_use_ref
Definition: guidance_h.c:60
static void guidance_h_traj_run(bool_t in_flight)
Definition: guidance_h.c:309
struct Int32Vect2 guidance_h_pos_ref
with INT32_POS_FRAC
Definition: guidance_h.c:63
static void guidance_h_nav_enter(void)
Definition: guidance_h.c:375
struct Int32Vect2 guidance_h_pos_err_sum
Definition: guidance_h.c:69
void stabilization_none_enter(void)
#define PPRZ_ITRIG_SIN(_s, _a)
Definition: pprz_trig_int.h:40
static struct NedCoor_i * stateGetSpeedNed_i(void)
Get ground speed in local NED coordinates (int).
Definition: state.h:798
#define GUIDANCE_H_MODE_HOVER
Definition: guidance_h.h:47
struct Int32Vect2 guidance_h_accel_ref
with INT32_ACCEL_FRAC
Definition: guidance_h.c:65
#define MAX_POS_ERR
Definition: guidance_h.c:298
struct Int32Vect2 guidance_h_speed_err
Definition: guidance_h.c:68
void guidance_h_read_rc(bool_t in_flight)
Definition: guidance_h.c:172
int32_t guidance_h_igain
Definition: guidance_h.c:79
struct Int32Eulers guidance_h_rc_sp
with INT32_ANGLE_FRAC
Definition: guidance_h.c:72
#define INT32_ANGLE_FRAC
#define VECT2_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:67
void stabilization_attitude_run(bool_t in_flight)
void stabilization_rate_read_rc(void)
void stabilization_rate_read_rc_switched_sticks(void)
#define MAX_POS_ERR_SUM
Definition: guidance_h.c:300
struct Int32Vect2 guidance_h_pos_sp
horizontal position setpoint in NED.
Definition: guidance_h.c:62
void stabilization_attitude_read_rc_setpoint_eulers(struct Int32Eulers *sp, bool_t in_flight)
Read attitude setpoint from RC as euler angles.
void guidance_h_run(bool_t in_flight)
Definition: guidance_h.c:213
void stabilization_none_read_rc(void)
struct RadioControl radio_control
Definition: radio_control.c:25
int32_t guidance_h_again
Definition: guidance_h.c:80
#define GH_SPEED_REF_FRAC
#define GUIDANCE_H_MODE_NAV
Definition: guidance_h.h:48
#define TRAJ_MAX_BANK
maximum bank angle: default 20 deg
Definition: guidance_h.c:307
uint8_t guidance_h_mode
Definition: guidance_h.c:59
#define GUIDANCE_H_MODE_KILL
Definition: guidance_h.h:44
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 GH_ACCEL_REF_FRAC
#define INT32_VECT2_LSHIFT(_o, _i, _l)
#define VECT2_ADD(_a, _b)
Definition: pprz_algebra.h:49
unsigned char uint8_t
Definition: types.h:14
struct Int32Vect2 guidance_h_nav_err
Definition: guidance_h.c:70
API to get/set the generic vehicle states.
static void guidance_h_update_reference(void)
Definition: guidance_h.c:278
void guidance_h_mode_changed(uint8_t new_mode)
Definition: guidance_h.c:120
int32_t psi
in rad with INT32_ANGLE_FRAC
int32_t transition_theta_offset
Definition: guidance_h.c:83
General stabilization interface for rotorcrafts.
struct Int32Vect2 guidance_h_command_earth
Definition: guidance_h.c:73
Horizontal guidance for rotorcrafts.
void stabilization_none_run(bool_t in_flight)
#define INT_VECT2_ZERO(_v)
int32_t guidance_h_pgain
Definition: guidance_h.c:77
struct Int32Eulers guidance_h_command_body
with INT32_ANGLE_FRAC
Definition: guidance_h.c:75
#define MAX_SPEED_ERR
Definition: guidance_h.c:299
struct Int32Vect2 guidance_h_pos_err
Definition: guidance_h.c:67
#define INT32_SPEED_FRAC
struct Int32Vect2 guidance_h_speed_ref
with INT32_SPEED_FRAC
Definition: guidance_h.c:64
static struct NedCoor_i * stateGetPositionNed_i(void)
Get position in local NED coordinates (int).
Definition: state.h:630
#define GUIDANCE_H_MODE_FORWARD
Definition: guidance_h.h:51
static struct Int32Eulers * stateGetNedToBodyEulers_i(void)
Get vehicle body attitude euler angles (int).
Definition: state.h:1012
#define GUIDANCE_H_USE_REF
Use horizontal guidance reference trajectory.
Definition: guidance_h.h:41
#define GUIDANCE_H_MODE_RATE
Definition: guidance_h.h:45
#define INT32_ACCEL_FRAC
#define GH_POS_REF_FRAC
#define VECT2_STRIM(_v, _min, _max)
Definition: pprz_algebra.h:85
#define GH_GAIN_SCALE
Definition: guidance_h.c:304
#define VECT2_COPY(_a, _b)
Definition: pprz_algebra.h:43
static void guidance_h_hover_enter(void)
Definition: guidance_h.c:365
void guidance_h_init(void)
Definition: guidance_h.c:93
struct Int32Vect2 gh_speed_ref
Reference model speed.
#define PPRZ_ITRIG_COS(_c, _a)
Definition: pprz_trig_int.h:50
struct Int32Vect2 guidance_h_stick_earth_sp
Definition: guidance_h.c:74
euler angles