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_h_ref.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2008-2013 The Paparazzi Team
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 #include "generated/airframe.h"
29 
36 
43 
44 /* Reference model position.
45  * in meters
46  * with fixedpoint representation: Q37.26
47  */
49 
50 
52 
54 
55 #define GH_MAX_SPEED_REF_FRAC 7
56 
60 
62 #ifndef GUIDANCE_H_REF_OMEGA
63 #define GUIDANCE_H_REF_OMEGA RadOfDeg(67.)
64 #endif
65 
66 #ifndef GUIDANCE_H_REF_ZETA
67 #define GUIDANCE_H_REF_ZETA 0.85
68 #endif
69 #define GH_ZETA_OMEGA_FRAC 10
70 #define GH_OMEGA_2_FRAC 7
73 
75 #ifndef GUIDANCE_H_REF_TAU
76 #define GUIDANCE_H_REF_TAU 0.5
77 #endif
78 #define GH_REF_INV_TAU_FRAC 16
80 
83 
87 
88 static void gh_compute_route_ref(struct Int32Vect2* ref_vector);
89 static void gh_compute_ref_max(struct Int32Vect2* ref_vector);
90 static void gh_compute_ref_max_accel(struct Int32Vect2* ref_vector);
91 static void gh_compute_ref_max_speed(struct Int32Vect2* ref_vector);
92 static void gh_saturate_ref_accel(void);
93 static void gh_saturate_ref_speed(void);
94 
95 
96 float gh_set_max_speed(float max_speed) {
97  /* limit to 100m/s as int version would overflow at 2^14 = 128 m/s */
98  gh_max_speed = Min(fabs(max_speed), 100.0f);
100  return gh_max_speed;
101 }
102 
103 void gh_set_ref(struct Int32Vect2 pos, struct Int32Vect2 speed, struct Int32Vect2 accel) {
104  struct Int64Vect2 new_pos;
105  new_pos.x = ((int64_t)pos.x)<<(GH_POS_REF_FRAC - INT32_POS_FRAC);
106  new_pos.y = ((int64_t)pos.y)<<(GH_POS_REF_FRAC - INT32_POS_FRAC);
107  gh_pos_ref = new_pos;
110 }
111 
113 
116 
117  // compute the "speed part" of accel = -2*zeta*omega*speed -omega^2(pos - pos_sp)
118  struct Int32Vect2 speed;
120  VECT2_SMUL(speed, speed, -2 * gh_zeta_omega);
121  INT32_VECT2_RSHIFT(speed, speed, GH_ZETA_OMEGA_FRAC);
122  // compute pos error in pos_sp resolution
123  struct Int32Vect2 pos_err;
125  VECT2_DIFF(pos_err, pos_err, pos_sp);
126  // convert to accel resolution
127  INT32_VECT2_RSHIFT(pos_err, pos_err, (INT32_POS_FRAC - GH_ACCEL_REF_FRAC));
128  // compute the "pos part" of accel
129  struct Int32Vect2 pos;
130  VECT2_SMUL(pos, pos_err, -gh_omega_2);
132  // sum accel
133  VECT2_SUM(gh_accel_ref, speed, pos);
134 
135  /* Compute max ref accel/speed along route before saturation */
136  gh_compute_ref_max(&pos_err);
137 
140 }
141 
142 
144 /* WARNING: SPEED SATURATION UNTESTED */
147 
148  // compute speed error
149  struct Int32Vect2 speed_err;
150  INT32_VECT2_RSHIFT(speed_err, speed_sp, (INT32_SPEED_FRAC - GH_SPEED_REF_FRAC));
151  VECT2_DIFF(speed_err, gh_speed_ref, speed_err);
152  // convert to accel resolution
153  INT32_VECT2_RSHIFT(speed_err, speed_err, (GH_SPEED_REF_FRAC - GH_ACCEL_REF_FRAC));
154  // compute accel from speed_sp
155  VECT2_SMUL(gh_accel_ref, speed_err, -gh_ref_inv_tau);
157 
158  /* Compute max ref accel/speed along route before saturation */
159  gh_compute_ref_max_speed(&speed_sp);
160  gh_compute_ref_max_accel(&speed_err);
161 
164 }
165 
166 static void gh_compute_route_ref(struct Int32Vect2* ref_vector) {
167  float f_route_ref = atan2f(-ref_vector->y, -ref_vector->x);
168  route_ref = ANGLE_BFP_OF_REAL(f_route_ref);
169  /* Compute North and East route components */
170  PPRZ_ITRIG_SIN(s_route_ref, route_ref);
171  PPRZ_ITRIG_COS(c_route_ref, route_ref);
172  c_route_ref = abs(c_route_ref);
173  s_route_ref = abs(s_route_ref);
174 }
175 
176 static void gh_compute_ref_max(struct Int32Vect2* ref_vector) {
177  /* Bound ref to max speed/accel along route reference angle.
178  * If angle can't be computed, simply set both axes to max magnitude/sqrt(2).
179  */
180  if (ref_vector->x == 0 && ref_vector->y == 0) {
183  }
184  else {
185  gh_compute_route_ref(ref_vector);
186  /* Compute maximum acceleration*/
189  /* Compute maximum reference x/y velocity from absolute max_speed */
192  }
193  /* restore gh_speed_ref range (Q14.17) */
195 }
196 
197 static void gh_compute_ref_max_accel(struct Int32Vect2* ref_vector) {
198  /* Bound ref to max accel along reference vector.
199  * If angle can't be computed, simply set both axes to max magnitude/sqrt(2).
200  */
201  if (ref_vector->x == 0 && ref_vector->y == 0) {
203  }
204  else {
205  gh_compute_route_ref(ref_vector);
206  /* Compute maximum acceleration*/
209  }
210 }
211 
212 static void gh_compute_ref_max_speed(struct Int32Vect2* ref_vector) {
213  /* Bound ref to max speed along reference vector.
214  * If angle can't be computed, simply set both axes to max magnitude/sqrt(2).
215  */
216  if (ref_vector->x == 0 && ref_vector->y == 0) {
218  }
219  else {
220  gh_compute_route_ref(ref_vector);
221  /* Compute maximum reference x/y velocity from absolute max_speed */
224  }
225  /* restore gh_speed_ref range (Q14.17) */
227 }
228 
230 static void gh_saturate_ref_accel(void) {
231  /* Saturate accelerations */
232  BoundAbs(gh_accel_ref.x, gh_max_accel_ref.x);
233  BoundAbs(gh_accel_ref.y, gh_max_accel_ref.y);
234 }
235 
237 static void gh_saturate_ref_speed(void) {
238  if (gh_speed_ref.x < -gh_max_speed_ref.x) {
240  if (gh_accel_ref.x < 0)
241  gh_accel_ref.x = 0;
242  }
243  else if (gh_speed_ref.x > gh_max_speed_ref.x) {
245  if (gh_accel_ref.x > 0)
246  gh_accel_ref.x = 0;
247  }
248  if (gh_speed_ref.y < -gh_max_speed_ref.y) {
250  if (gh_accel_ref.y < 0)
251  gh_accel_ref.y = 0;
252  }
253  else if (gh_speed_ref.y > gh_max_speed_ref.y) {
255  if (gh_accel_ref.y > 0)
256  gh_accel_ref.y = 0;
257  }
258 }
259 
void gh_update_ref_from_pos_sp(struct Int32Vect2 pos_sp)
void gh_set_ref(struct Int32Vect2 pos, struct Int32Vect2 speed, struct Int32Vect2 accel)
static void gh_saturate_ref_accel(void)
saturate reference accelerations
#define INT32_VECT2_RSHIFT(_o, _i, _r)
Reference generation for horizontal guidance.
void gh_update_ref_from_speed_sp(struct Int32Vect2 speed_sp)
#define Min(x, y)
#define INT_MULT_RSHIFT(_a, _b, _r)
#define ANGLE_BFP_OF_REAL(_af)
float gh_set_max_speed(float max_speed)
Set a new maximum speed for waypoint navigation.
#define INT32_POS_FRAC
#define GH_ZETA_OMEGA_FRAC
float gh_max_speed
Current maximum speed for waypoint navigation.
static void gh_compute_route_ref(struct Int32Vect2 *ref_vector)
static void gh_compute_ref_max_accel(struct Int32Vect2 *ref_vector)
struct Int64Vect2 gh_pos_ref
struct Int32Vect2 gh_accel_ref
Reference model acceleration.
#define GUIDANCE_H_REF_TAU
first order time constant
static int32_t c_route_ref
signed long long int64_t
Definition: types.h:21
#define INT32_TRIG_FRAC
static int32_t s_route_ref
#define GUIDANCE_H_REF_MAX_SPEED
Default speed saturation.
static const int32_t gh_zeta_omega
static int32_t route_ref
#define GUIDANCE_H_REF_OMEGA
second order model natural frequency
#define PPRZ_ITRIG_SIN(_s, _a)
Definition: pprz_trig_int.h:40
#define GUIDANCE_H_REF_MAX_ACCEL
Accel saturation.
#define VECT2_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:74
#define GH_OMEGA_2_FRAC
#define BFP_OF_REAL(_vr, _frac)
static const int32_t gh_ref_inv_tau
#define GH_SPEED_REF_FRAC
static const int32_t gh_omega_2
#define GH_REF_INV_TAU_FRAC
signed long int32_t
Definition: types.h:19
#define GH_ACCEL_REF_FRAC
#define VECT2_SUM(_c, _a, _b)
Definition: pprz_algebra.h:68
#define INT32_VECT2_LSHIFT(_o, _i, _l)
#define VECT2_ADD(_a, _b)
Definition: pprz_algebra.h:56
static struct Int32Vect2 gh_max_accel_ref
static void gh_saturate_ref_speed(void)
Saturate ref speed and adjust acceleration accordingly.
#define VECT2_SMUL(_vo, _vi, _s)
Definition: pprz_algebra.h:80
#define GUIDANCE_H_REF_ZETA
second order model damping
static struct Int32Vect2 gh_max_speed_ref
static void gh_compute_ref_max_speed(struct Int32Vect2 *ref_vector)
#define INT32_SPEED_FRAC
static const int32_t gh_max_accel
static int32_t gh_max_speed_int
gh_max_speed in fixed point representation with GH_MAX_SPEED_REF_FRAC must be limited to 2^14 to avoi...
#define GH_MAX_SPEED_REF_FRAC
static void gh_compute_ref_max(struct Int32Vect2 *ref_vector)
#define INT32_ACCEL_FRAC
#define GH_POS_REF_FRAC
struct Int32Vect2 gh_speed_ref
Reference model speed.
#define PPRZ_ITRIG_COS(_c, _a)
Definition: pprz_trig_int.h:50