Paparazzi UAS  v4.2.2_stable-4-gcc32f65
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros 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 
27 #define GUIDANCE_H_C
28 
30 
33 #include "subsystems/ahrs.h"
34 #include "subsystems/ins.h"
36 
37 #include "generated/airframe.h"
38 
40 
45 
50 
55 
60 
61 /* warn if some gains are still negative */
62 #if (GUIDANCE_H_PGAIN < 0) || \
63  (GUIDANCE_H_DGAIN < 0) || \
64  (GUIDANCE_H_IGAIN < 0)
65 #warning "ALL control gains are now positive!!!"
66 #endif
67 
68 
69 #ifndef GUIDANCE_H_AGAIN
70 #define GUIDANCE_H_AGAIN 0
71 #else
72 #if (GUIDANCE_H_AGAIN < 0)
73 #warning "ALL control gains are now positive!!!"
74 #endif
75 #endif
76 
77 #ifndef GUIDANCE_H_MAX_BANK
78 #define GUIDANCE_H_MAX_BANK RadOfDeg(20)
79 #endif
80 
81 static inline void guidance_h_update_reference(bool_t use_ref);
82 static inline void guidance_h_traj_run(bool_t in_flight);
83 static inline void guidance_h_hover_enter(void);
84 static inline void guidance_h_nav_enter(void);
85 
86 #define GuidanceHSetRef(_pos, _speed, _accel) { \
87  b2_gh_set_ref(_pos, _speed, _accel); \
88  VECT2_COPY(guidance_h_pos_ref, _pos); \
89  VECT2_COPY(guidance_h_speed_ref, _speed); \
90  VECT2_COPY(guidance_h_accel_ref, _accel); \
91  }
92 
93 
94 void guidance_h_init(void) {
95 
101  guidance_h_pgain = GUIDANCE_H_PGAIN;
102  guidance_h_igain = GUIDANCE_H_IGAIN;
103  guidance_h_dgain = GUIDANCE_H_DGAIN;
105 
106 }
107 
108 
110  if (new_mode == guidance_h_mode)
111  return;
112 
113  switch (new_mode) {
114 
117  break;
118 
121  break;
122 
125  break;
126 
129  break;
130 
131  case GUIDANCE_H_MODE_NAV:
133  break;
134  default:
135  break;
136  }
137 
138  guidance_h_mode = new_mode;
139 
140 }
141 
142 
143 void guidance_h_read_rc(bool_t in_flight) {
144 
145  switch ( guidance_h_mode ) {
146 
149  break;
150 
153  break;
154 
157  break;
158 
161  break;
162 
163  case GUIDANCE_H_MODE_NAV:
164  if (radio_control.status == RC_OK) {
166  }
167  else {
169  }
170  break;
171  default:
172  break;
173  }
174 
175 }
176 
177 
178 void guidance_h_run(bool_t in_flight) {
179  switch ( guidance_h_mode ) {
180 
182  stabilization_none_run(in_flight);
183  break;
184 
186  stabilization_rate_run(in_flight);
187  break;
188 
190  stabilization_attitude_run(in_flight);
191  break;
192 
195 
196  /* set psi command */
198  /* compute roll and pitch commands and set final attitude setpoint */
199  guidance_h_traj_run(in_flight);
200 
201  stabilization_attitude_run(in_flight);
202  break;
203 
204  case GUIDANCE_H_MODE_NAV:
205  {
206  if (!in_flight) guidance_h_nav_enter();
207 
211  /* FIXME: heading can't be set via attitude block yet, use current heading for now */
213 #ifdef STABILISATION_ATTITUDE_TYPE_QUAT
216 #endif
217  }
218  else {
220 
221 #if GUIDANCE_H_USE_REF
223 #else
225 #endif
226  /* set psi command */
228  /* compute roll and pitch commands and set final attitude setpoint */
229  guidance_h_traj_run(in_flight);
230  }
231  stabilization_attitude_run(in_flight);
232  break;
233  }
234  default:
235  break;
236  }
237 
238 }
239 
240 static inline void guidance_h_update_reference(bool_t use_ref) {
241  /* convert our reference to generic representation */
242  if (use_ref) {
243  b2_gh_update_ref_from_pos_sp(guidance_h_pos_sp);
247  } else {
251  }
252 }
253 
254 
255 #define MAX_POS_ERR POS_BFP_OF_REAL(16.)
256 #define MAX_SPEED_ERR SPEED_BFP_OF_REAL(16.)
257 #define MAX_POS_ERR_SUM ((int32_t)(MAX_POS_ERR)<< 12)
258 
259 /* with a pgain of 100 and a scale of 2,
260  * you get an angle of 5.6 degrees for 1m pos error */
261 #define GH_GAIN_SCALE 2
262 
264 #define TRAJ_MAX_BANK BFP_OF_REAL(GUIDANCE_H_MAX_BANK, INT32_ANGLE_FRAC)
265 
266 static inline void guidance_h_traj_run(bool_t in_flight) {
267 
268  /* compute position error */
270  /* saturate it */
272 
273  /* compute speed error */
275  /* saturate it */
277 
278  /* update pos error integral, zero it if not in_flight */
279  if (in_flight) {
281  /* saturate it */
283  } else {
285  }
286 
287  /* run PID */
292  guidance_h_again * (guidance_h_accel_ref.x >> 8); /* feedforward gain */
297  guidance_h_again * (guidance_h_accel_ref.y >> 8); /* feedforward gain */
298 
300 
301  /* Rotate to body frame */
302  int32_t s_psi, c_psi;
305 
306  // Restore angle ref resolution after rotation
311 
312 
313  /* Add RC roll and pitch setpoints for emergency corrections */
316 
317  /* Set attitude setpoint in eulers and as quaternion */
319 
320 #ifdef STABILISATION_ATTITUDE_TYPE_QUAT
323 #endif /* STABILISATION_ATTITUDE_TYPE_QUAT */
324 
325 }
326 
327 static inline void guidance_h_hover_enter(void) {
328 
330 
333 
335 
336 }
337 
338 static inline void guidance_h_nav_enter(void) {
339 
341  struct Int32Vect2 pos,speed,zero;
342  INT_VECT2_ZERO(zero);
343  VECT2_COPY(pos, ins_ltp_pos);
344  VECT2_COPY(speed, ins_ltp_speed);
345  GuidanceHSetRef(pos, speed, zero);
346 
347  /* reset psi reference, set psi setpoint to current psi */
350 
352 
353 }
int32_t phi
in rad with INT32_ANGLE_FRAC
#define INT32_VECT2_RSHIFT(_o, _i, _r)
void stabilization_attitude_read_rc(bool_t in_flight)
static void reset_psi_ref_from_body(void)
int32_t guidance_h_dgain
Definition: guidance_h.c:57
Attitude and Heading Reference System interface.
#define GUIDANCE_H_MODE_RC_DIRECT
Definition: guidance_h.h:40
void stabilization_rate_run(bool_t in_flight)
#define INT32_VECT2_NED_OF_ENU(_o, _i)
#define INT32_POS_FRAC
#define GuidanceHSetRef(_pos, _speed, _accel)
Definition: guidance_h.c:86
void stabilization_rate_enter(void)
float psi
in radians
Read an attitude setpoint from the RC.
struct Ahrs ahrs
global AHRS state (fixed point version)
Definition: ahrs.c:24
#define B2_GH_ACCEL_REF_FRAC
#define GUIDANCE_H_MODE_ATTITUDE
Definition: guidance_h.h:37
struct Int32Vect2 b2_gh_speed_ref
struct Int32Vect2 b2_gh_accel_ref
Device independent INS code.
int32_t theta
in rad with INT32_ANGLE_FRAC
#define GUIDANCE_H_AGAIN
Definition: guidance_h.c:70
#define INT_EULERS_ZERO(_e)
void stabilization_attitude_enter(void)
float theta
in radians
#define INT32_TRIG_FRAC
struct NedCoor_i ins_ltp_pos
Definition: ins.c:77
static void guidance_h_traj_run(bool_t in_flight)
Definition: guidance_h.c:266
#define FALSE
Definition: imu_chimu.h:141
struct Int32Vect2 guidance_h_pos_ref
Definition: guidance_h.c:42
static void guidance_h_nav_enter(void)
Definition: guidance_h.c:338
struct Int32Vect2 guidance_h_pos_err_sum
Definition: guidance_h.c:48
struct FloatEulers stab_att_sp_euler
with INT32_ANGLE_FRAC
void stabilization_none_enter(void)
#define PPRZ_ITRIG_SIN(_s, _a)
Definition: pprz_trig_int.h:42
#define GUIDANCE_H_MODE_HOVER
Definition: guidance_h.h:38
#define INT32_QUAT_OF_EULERS(_q, _e)
struct Int32Vect2 guidance_h_accel_ref
Definition: guidance_h.c:44
#define MAX_POS_ERR
Definition: guidance_h.c:255
struct Int32Vect2 guidance_h_speed_err
Definition: guidance_h.c:47
void guidance_h_read_rc(bool_t in_flight)
Definition: guidance_h.c:143
int32_t guidance_h_igain
Definition: guidance_h.c:58
struct Int32Eulers guidance_h_rc_sp
with INT32_ANGLE_FRAC
Definition: guidance_h.c:51
#define VECT2_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:69
float phi
in radians
static void stabilization_attitude_read_rc_setpoint_eulers(struct Int32Eulers *sp, bool_t in_flight)
void stabilization_attitude_run(bool_t in_flight)
void stabilization_rate_read_rc(void)
struct Int64Vect2 b2_gh_pos_ref
#define MAX_POS_ERR_SUM
Definition: guidance_h.c:257
struct Int32Vect2 guidance_h_pos_sp
horizontal position setpoint in NED.
Definition: guidance_h.c:41
struct NedCoor_i ins_ltp_speed
Definition: ins.c:78
void guidance_h_run(bool_t in_flight)
Definition: guidance_h.c:178
void stabilization_none_read_rc(void)
struct RadioControl radio_control
Definition: radio_control.c:27
int32_t guidance_h_again
Definition: guidance_h.c:59
#define GUIDANCE_H_MODE_NAV
Definition: guidance_h.h:39
#define TRAJ_MAX_BANK
maximum bank angle: default 20 deg
Definition: guidance_h.c:264
uint8_t guidance_h_mode
Definition: guidance_h.c:39
#define GUIDANCE_H_MODE_KILL
Definition: guidance_h.h:35
signed long int32_t
Definition: types.h:19
#define TRUE
Definition: imu_chimu.h:144
#define INT32_VECT2_LSHIFT(_o, _i, _l)
#define VECT2_ADD(_a, _b)
Definition: pprz_algebra.h:51
struct Int32Eulers ltp_to_body_euler
Rotation from LocalTangentPlane to body frame as Euler angles.
Definition: ahrs.h:50
struct FloatQuat stab_att_sp_quat
unsigned char uint8_t
Definition: types.h:14
struct Int32Vect2 guidance_h_nav_err
Definition: guidance_h.c:49
#define B2_GH_SPEED_REF_FRAC
void guidance_h_mode_changed(uint8_t new_mode)
Definition: guidance_h.c:109
#define B2_GH_POS_REF_FRAC
static void guidance_h_update_reference(bool_t use_ref)
Definition: guidance_h.c:240
int32_t psi
in rad with INT32_ANGLE_FRAC
General stabilization interface for rotorcrafts.
struct Int32Vect2 guidance_h_command_earth
Definition: guidance_h.c:52
#define INT_VECT2_ZERO(_v)
int32_t guidance_h_pgain
Definition: guidance_h.c:56
struct Int32Eulers guidance_h_command_body
with INT32_ANGLE_FRAC
Definition: guidance_h.c:54
#define MAX_SPEED_ERR
Definition: guidance_h.c:256
void stabilization_none_run(bool_t in_flight __attribute__((unused)))
struct Int32Vect2 guidance_h_pos_err
Definition: guidance_h.c:46
#define INT32_SPEED_FRAC
struct Int32Vect2 guidance_h_speed_ref
Definition: guidance_h.c:43
#define GUIDANCE_H_MODE_RATE
Definition: guidance_h.h:36
#define INT32_ACCEL_FRAC
#define VECT2_STRIM(_v, _min, _max)
Definition: pprz_algebra.h:87
#define GH_GAIN_SCALE
Definition: guidance_h.c:261
#define VECT2_COPY(_a, _b)
Definition: pprz_algebra.h:45
static void guidance_h_hover_enter(void)
Definition: guidance_h.c:327
void guidance_h_init(void)
Definition: guidance_h.c:94
#define INT32_QUAT_WRAP_SHORTEST(q)
#define EULERS_COPY(_a, _b)
Definition: pprz_algebra.h:236
#define PPRZ_ITRIG_COS(_c, _a)
Definition: pprz_trig_int.h:52
struct Int32Vect2 guidance_h_stick_earth_sp
Definition: guidance_h.c:53
euler angles