Paparazzi UAS  v5.14.0_stable-0-g3f680d1
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces 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 
31 
33 
34 #define GH_MAX_SPEED_REF_FRAC 7
35 
37 #ifndef GUIDANCE_H_REF_OMEGA
38 #define GUIDANCE_H_REF_OMEGA RadOfDeg(67.)
39 #endif
40 
41 #ifndef GUIDANCE_H_REF_ZETA
42 #define GUIDANCE_H_REF_ZETA 0.85
43 #endif
44 
45 #define GH_ZETA_OMEGA_FRAC 10
46 #define GH_OMEGA_2_FRAC 7
47 
48 
50 #ifndef GUIDANCE_H_REF_TAU
51 #define GUIDANCE_H_REF_TAU 0.5
52 #endif
53 #define GH_REF_INV_TAU_FRAC 16
54 
55 static void gh_compute_route_ref(struct Int32Vect2 *ref_vector);
56 static void gh_compute_ref_max(struct Int32Vect2 *ref_vector);
57 static void gh_compute_ref_max_accel(struct Int32Vect2 *ref_vector);
58 static void gh_compute_ref_max_speed(struct Int32Vect2 *ref_vector);
59 static void gh_saturate_ref_accel(void);
60 static void gh_saturate_ref_speed(void);
61 
62 void gh_ref_init(void)
63 {
67  gh_ref.omega_2 = BFP_OF_REAL((GUIDANCE_H_REF_OMEGA * GUIDANCE_H_REF_OMEGA), GH_OMEGA_2_FRAC);
70 }
71 
72 
74 {
75  /* limit to 100m/s as int version would overflow at 2^14 = 128 m/s */
76  gh_ref.max_speed = Min(fabs(max_speed), 100.0f);
78  return gh_ref.max_speed;
79 }
80 
81 float gh_set_tau(float tau)
82 {
83  gh_ref.tau = tau;
84  Bound(gh_ref.tau, 0.01f, 2.0f);
86  return gh_ref.tau;
87 }
88 
89 float gh_set_omega(float omega)
90 {
91  gh_ref.omega = omega;
92  Bound(gh_ref.omega, 0.1f, 5.0f);
95  return gh_ref.omega;
96 }
97 
98 float gh_set_zeta(float zeta)
99 {
100  gh_ref.zeta = zeta;
101  Bound(gh_ref.zeta, 0.7f, 1.2f);
103  return gh_ref.zeta;
104 }
105 
107 {
108  struct Int64Vect2 new_pos;
109  new_pos.x = ((int64_t)pos.x) << (GH_POS_REF_FRAC - INT32_POS_FRAC);
110  new_pos.y = ((int64_t)pos.y) << (GH_POS_REF_FRAC - INT32_POS_FRAC);
111  gh_ref.pos = new_pos;
114 }
115 
117 {
118 
121 
122  // compute the "speed part" of accel = -2*zeta*omega*speed -omega^2(pos - pos_sp)
123  struct Int32Vect2 speed;
125  VECT2_SMUL(speed, speed, -2 * gh_ref.zeta_omega);
126  INT32_VECT2_RSHIFT(speed, speed, GH_ZETA_OMEGA_FRAC);
127  // compute pos error in pos_sp resolution
128  struct Int32Vect2 pos_err;
130  VECT2_DIFF(pos_err, pos_err, pos_sp);
131  // convert to accel resolution
132  INT32_VECT2_RSHIFT(pos_err, pos_err, (INT32_POS_FRAC - GH_ACCEL_REF_FRAC));
133  // compute the "pos part" of accel
134  struct Int32Vect2 pos;
135  VECT2_SMUL(pos, pos_err, -gh_ref.omega_2);
137  // sum accel
138  VECT2_SUM(gh_ref.accel, speed, pos);
139 
140  /* Compute max ref accel/speed along route before saturation */
141  gh_compute_ref_max(&pos_err);
142 
145 }
146 
147 
149 {
150  /* WARNING: SPEED SATURATION UNTESTED */
153 
154  // compute speed error
155  struct Int32Vect2 speed_err;
156  INT32_VECT2_RSHIFT(speed_err, speed_sp, (INT32_SPEED_FRAC - GH_SPEED_REF_FRAC));
157  VECT2_DIFF(speed_err, gh_ref.speed, speed_err);
158  // convert to accel resolution
159  INT32_VECT2_RSHIFT(speed_err, speed_err, (GH_SPEED_REF_FRAC - GH_ACCEL_REF_FRAC));
160  // compute accel from speed_sp
161  VECT2_SMUL(gh_ref.accel, speed_err, -gh_ref.inv_tau);
163 
164  /* Compute max ref accel/speed along route before saturation */
165  gh_compute_ref_max_speed(&speed_sp);
166  gh_compute_ref_max_accel(&speed_err);
167 
170 }
171 
172 static void gh_compute_route_ref(struct Int32Vect2 *ref_vector)
173 {
174  float f_route_ref = atan2f(-ref_vector->y, -ref_vector->x);
175  gh_ref.route_ref = ANGLE_BFP_OF_REAL(f_route_ref);
176  /* Compute North and East route components */
181 }
182 
183 static void gh_compute_ref_max(struct Int32Vect2 *ref_vector)
184 {
185  /* Bound ref to max speed/accel along route reference angle.
186  * If angle can't be computed, simply set both axes to max magnitude/sqrt(2).
187  */
188  if (ref_vector->x == 0 && ref_vector->y == 0) {
191  } else {
192  gh_compute_route_ref(ref_vector);
193  /* Compute maximum acceleration*/
196  /* Compute maximum reference x/y velocity from absolute max_speed */
199  }
200  /* restore gh_ref.speed range (Q14.17) */
202 }
203 
204 static void gh_compute_ref_max_accel(struct Int32Vect2 *ref_vector)
205 {
206  /* Bound ref to max accel along reference vector.
207  * If angle can't be computed, simply set both axes to max magnitude/sqrt(2).
208  */
209  if (ref_vector->x == 0 && ref_vector->y == 0) {
211  } else {
212  gh_compute_route_ref(ref_vector);
213  /* Compute maximum acceleration*/
216  }
217 }
218 
219 static void gh_compute_ref_max_speed(struct Int32Vect2 *ref_vector)
220 {
221  /* Bound ref to max speed along reference vector.
222  * If angle can't be computed, simply set both axes to max magnitude/sqrt(2).
223  */
224  if (ref_vector->x == 0 && ref_vector->y == 0) {
226  } else {
227  gh_compute_route_ref(ref_vector);
228  /* Compute maximum reference x/y velocity from absolute max_speed */
231  }
232  /* restore gh_ref.speed range (Q14.17) */
234 }
235 
237 static void gh_saturate_ref_accel(void)
238 {
239  /* Saturate accelerations */
240  BoundAbs(gh_ref.accel.x, gh_ref.max_accel.x);
241  BoundAbs(gh_ref.accel.y, gh_ref.max_accel.y);
242 }
243 
245 static void gh_saturate_ref_speed(void)
246 {
247  if (gh_ref.speed.x < -gh_ref.max_vel.x) {
249  if (gh_ref.accel.x < 0) {
250  gh_ref.accel.x = 0;
251  }
252  } else if (gh_ref.speed.x > gh_ref.max_vel.x) {
254  if (gh_ref.accel.x > 0) {
255  gh_ref.accel.x = 0;
256  }
257  }
258  if (gh_ref.speed.y < -gh_ref.max_vel.y) {
260  if (gh_ref.accel.y < 0) {
261  gh_ref.accel.y = 0;
262  }
263  } else if (gh_ref.speed.y > gh_ref.max_vel.y) {
265  if (gh_ref.accel.y > 0) {
266  gh_ref.accel.y = 0;
267  }
268  }
269 }
270 
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
int32_t inv_tau
Reference generation for horizontal guidance.
void gh_update_ref_from_speed_sp(struct Int32Vect2 speed_sp)
#define INT32_ACCEL_FRAC
#define INT32_SPEED_FRAC
float gh_set_max_speed(float max_speed)
Set a new maximum speed for waypoint navigation.
#define GH_ZETA_OMEGA_FRAC
struct GuidanceHRef gh_ref
#define VECT2_SUM(_c, _a, _b)
Definition: pprz_algebra.h:86
static void gh_compute_route_ref(struct Int32Vect2 *ref_vector)
static void gh_compute_ref_max_accel(struct Int32Vect2 *ref_vector)
int32_t zeta_omega
#define VECT2_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:92
#define GUIDANCE_H_REF_TAU
first order time constant
signed long long int64_t
Definition: types.h:21
#define GUIDANCE_H_REF_MAX_SPEED
Default speed saturation.
#define VECT2_ADD(_a, _b)
Definition: pprz_algebra.h:74
struct Int64Vect2 pos
Reference model position.
#define GUIDANCE_H_REF_OMEGA
default second order model natural frequency
#define BFP_OF_REAL(_vr, _frac)
#define PPRZ_ITRIG_SIN(_s, _a)
#define GUIDANCE_H_REF_MAX_ACCEL
Accel saturation.
float gh_set_omega(float omega)
int32_t route_ref
float gh_set_tau(float tau)
#define GH_OMEGA_2_FRAC
int32_t s_route_ref
float max_speed
Current maximum speed for waypoint navigation.
#define GH_SPEED_REF_FRAC
#define INT32_POS_FRAC
#define Min(x, y)
Definition: main_fbw.c:52
#define GH_REF_INV_TAU_FRAC
float tau
first order time constant
struct Int32Vect2 max_vel
#define VECT2_SMUL(_vo, _vi, _s)
Definition: pprz_algebra.h:98
signed long int32_t
Definition: types.h:19
#define ANGLE_BFP_OF_REAL(_af)
#define INT32_TRIG_FRAC
#define GH_ACCEL_REF_FRAC
void gh_ref_init(void)
int32_t c_route_ref
struct Int32Vect2 speed
Reference model speed.
#define INT_MULT_RSHIFT(_a, _b, _r)
float omega
second order model natural frequency
int32_t max_speed_int
gh_max_speed in fixed point representation with GH_MAX_SPEED_REF_FRAC must be limited to 2^14 to avoi...
static void gh_saturate_ref_speed(void)
Saturate ref speed and adjust acceleration accordingly.
#define GUIDANCE_H_REF_ZETA
default second order model damping
int32_t omega_2
struct Int32Vect2 accel
Reference model acceleration.
static void gh_compute_ref_max_speed(struct Int32Vect2 *ref_vector)
static const int32_t gh_max_accel
struct Int32Vect2 max_accel
float zeta
second order model damping
#define INT32_VECT2_RSHIFT(_o, _i, _r)
#define GH_MAX_SPEED_REF_FRAC
#define INT32_VECT2_LSHIFT(_o, _i, _l)
float gh_set_zeta(float zeta)
static void gh_compute_ref_max(struct Int32Vect2 *ref_vector)
#define GH_POS_REF_FRAC
#define PPRZ_ITRIG_COS(_c, _a)