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_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 
53 #ifndef GUIDANCE_H_REF_MAX_ACCEL
54 #define GUIDANCE_H_REF_MAX_ACCEL 5.66
55 #endif
56 #define GH_MAX_ACCEL BFP_OF_REAL(GUIDANCE_H_REF_MAX_ACCEL, GH_ACCEL_REF_FRAC)
57 
59 #ifndef GUIDANCE_H_REF_MAX_SPEED
60 #define GUIDANCE_H_REF_MAX_SPEED 5.
61 #endif
62 
63 #define GH_MAX_SPEED_REF_FRAC 7
64 #define GH_MAX_SPEED BFP_OF_REAL(GUIDANCE_H_REF_MAX_SPEED, GH_MAX_SPEED_REF_FRAC)
65 
67 #ifndef GUIDANCE_H_REF_OMEGA
68 #define GUIDANCE_H_REF_OMEGA RadOfDeg(67.)
69 #endif
70 
71 #ifndef GUIDANCE_H_REF_ZETA
72 #define GUIDANCE_H_REF_ZETA 0.85
73 #endif
74 #define GH_ZETA_OMEGA_FRAC 10
75 #define GH_ZETA_OMEGA BFP_OF_REAL((GUIDANCE_H_REF_ZETA*GUIDANCE_H_REF_OMEGA), GH_ZETA_OMEGA_FRAC)
76 #define GH_OMEGA_2_FRAC 7
77 #define GH_OMEGA_2 BFP_OF_REAL((GUIDANCE_H_REF_OMEGA*GUIDANCE_H_REF_OMEGA), GH_OMEGA_2_FRAC)
78 
80 #define GH_REF_THAU_F 0.5
81 #define GH_REF_INV_THAU_FRAC 16
82 #define GH_REF_INV_THAU BFP_OF_REAL((1./GH_REF_THAU_F), GH_REF_INV_THAU_FRAC)
83 
86 
90 
91 void gh_set_ref(struct Int32Vect2 pos, struct Int32Vect2 speed, struct Int32Vect2 accel) {
92  struct Int64Vect2 new_pos;
93  new_pos.x = ((int64_t)pos.x)<<(GH_POS_REF_FRAC - INT32_POS_FRAC);
94  new_pos.y = ((int64_t)pos.y)<<(GH_POS_REF_FRAC - INT32_POS_FRAC);
95  gh_pos_ref = new_pos;
98 }
99 
101 
104 
105  // compute the "speed part" of accel = -2*zeta*omega*speed -omega^2(pos - pos_sp)
106  struct Int32Vect2 speed;
108  VECT2_SMUL(speed, speed, -2*GH_ZETA_OMEGA);
109  INT32_VECT2_RSHIFT(speed, speed, GH_ZETA_OMEGA_FRAC);
110  // compute pos error in pos_sp resolution
111  struct Int32Vect2 pos_err;
113  VECT2_DIFF(pos_err, pos_err, pos_sp);
114  // convert to accel resolution
115  INT32_VECT2_RSHIFT(pos_err, pos_err, (INT32_POS_FRAC - GH_ACCEL_REF_FRAC));
116  // compute the "pos part" of accel
117  struct Int32Vect2 pos;
118  VECT2_SMUL(pos, pos_err, (-GH_OMEGA_2));
120  // sum accel
121  VECT2_SUM(gh_accel_ref, speed, pos);
122 
123  /* Compute route reference before saturation */
124  float f_route_ref = atan2f(-pos_err.y, -pos_err.x);
125  route_ref = ANGLE_BFP_OF_REAL(f_route_ref);
126  /* Compute North and East route components */
129  c_route_ref = abs(c_route_ref);
130  s_route_ref = abs(s_route_ref);
131  /* Compute maximum acceleration*/
134  /* Compute maximum speed*/
137  /* restore gh_speed_ref range (Q14.17) */
139 
140  /* Saturate accelerations */
141  if (gh_accel_ref.x <= -gh_max_accel_ref.x) {
143  }
144  else if (gh_accel_ref.x >= gh_max_accel_ref.x) {
146  }
147  if (gh_accel_ref.y <= -gh_max_accel_ref.y) {
149  }
150  else if (gh_accel_ref.y >= gh_max_accel_ref.y) {
152  }
153 
154  /* Saturate speed and adjust acceleration accordingly */
155  if (gh_speed_ref.x <= -gh_max_speed_ref.x) {
157  if (gh_accel_ref.x < 0)
158  gh_accel_ref.x = 0;
159  }
160  else if (gh_speed_ref.x >= gh_max_speed_ref.x) {
162  if (gh_accel_ref.x > 0)
163  gh_accel_ref.x = 0;
164  }
165  if (gh_speed_ref.y <= -gh_max_speed_ref.y) {
167  if (gh_accel_ref.y < 0)
168  gh_accel_ref.y = 0;
169  }
170  else if (gh_speed_ref.y >= gh_max_speed_ref.y) {
172  if (gh_accel_ref.y > 0)
173  gh_accel_ref.y = 0;
174  }
175 }
176 
178 /* WARNING: SPEED SATURATION UNTESTED */
181 
182  // compute speed error
183  struct Int32Vect2 speed_err;
184  INT32_VECT2_RSHIFT(speed_err, speed_sp, (INT32_SPEED_FRAC - GH_SPEED_REF_FRAC));
185  VECT2_DIFF(speed_err, gh_speed_ref, speed_err);
186  // convert to accel resolution
187  INT32_VECT2_RSHIFT(speed_err, speed_err, (GH_SPEED_REF_FRAC - GH_ACCEL_REF_FRAC));
188  // compute accel from speed_sp
191 
192  /* Compute route reference before saturation */
193  float f_route_ref = atan2f(-speed_sp.y, -speed_sp.x);
194  route_ref = ANGLE_BFP_OF_REAL(f_route_ref);
195  /* Compute North and East route components */
198  c_route_ref = abs(c_route_ref);
199  s_route_ref = abs(s_route_ref);
200  /* Compute maximum acceleration*/
203  /* Compute maximum speed*/
206  /* restore gh_speed_ref range (Q14.17) */
208 
209  /* Saturate accelerations */
210  if (gh_accel_ref.x <= -gh_max_accel_ref.x) {
212  }
213  else if (gh_accel_ref.x >= gh_max_accel_ref.x) {
215  }
216  if (gh_accel_ref.y <= -gh_max_accel_ref.y) {
218  }
219  else if (gh_accel_ref.y >= gh_max_accel_ref.y) {
221  }
222 
223  /* Saturate speed and adjust acceleration accordingly */
224  if (gh_speed_ref.x <= -gh_max_speed_ref.x) {
226  if (gh_accel_ref.x < 0)
227  gh_accel_ref.x = 0;
228  }
229  else if (gh_speed_ref.x >= gh_max_speed_ref.x) {
231  if (gh_accel_ref.x > 0)
232  gh_accel_ref.x = 0;
233  }
234  if (gh_speed_ref.y <= -gh_max_speed_ref.y) {
236  if (gh_accel_ref.y < 0)
237  gh_accel_ref.y = 0;
238  }
239  else if (gh_speed_ref.y >= gh_max_speed_ref.y) {
241  if (gh_accel_ref.y > 0)
242  gh_accel_ref.y = 0;
243  }
244 }
245 
void gh_update_ref_from_pos_sp(struct Int32Vect2 pos_sp)
void gh_set_ref(struct Int32Vect2 pos, struct Int32Vect2 speed, struct Int32Vect2 accel)
#define INT32_VECT2_RSHIFT(_o, _i, _r)
Reference generation for horizontal guidance.
void gh_update_ref_from_speed_sp(struct Int32Vect2 speed_sp)
#define GH_MAX_SPEED
#define INT_MULT_RSHIFT(_a, _b, _r)
#define ANGLE_BFP_OF_REAL(_af)
#define INT32_POS_FRAC
#define GH_ZETA_OMEGA_FRAC
#define GH_REF_INV_THAU
#define GH_ZETA_OMEGA
struct Int64Vect2 gh_pos_ref
#define GH_OMEGA_2
struct Int32Vect2 gh_accel_ref
Reference model acceleration.
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
static int32_t route_ref
#define PPRZ_ITRIG_SIN(_s, _a)
Definition: pprz_trig_int.h:40
#define VECT2_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:67
#define GH_OMEGA_2_FRAC
#define GH_MAX_ACCEL
#define GH_SPEED_REF_FRAC
#define GH_REF_INV_THAU_FRAC
signed long int32_t
Definition: types.h:19
#define GH_ACCEL_REF_FRAC
#define VECT2_SUM(_c, _a, _b)
Definition: pprz_algebra.h:61
#define INT32_VECT2_LSHIFT(_o, _i, _l)
#define VECT2_ADD(_a, _b)
Definition: pprz_algebra.h:49
static struct Int32Vect2 gh_max_accel_ref
#define VECT2_SMUL(_vo, _vi, _s)
Definition: pprz_algebra.h:73
static struct Int32Vect2 gh_max_speed_ref
#define INT32_SPEED_FRAC
#define GH_MAX_SPEED_REF_FRAC
#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