Paparazzi UAS  v6.2_unstable
Paparazzi is a free software Unmanned Aircraft System.
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 
35 #ifndef GUIDANCE_H_REF_OMEGA
36 #define GUIDANCE_H_REF_OMEGA RadOfDeg(67.)
37 #endif
38 
39 #ifndef GUIDANCE_H_REF_ZETA
40 #define GUIDANCE_H_REF_ZETA 0.85f
41 #endif
42 
44 #ifndef GUIDANCE_H_REF_TAU
45 #define GUIDANCE_H_REF_TAU 0.5f
46 #endif
47 
48 static void gh_saturate_speed(struct FloatVect2 *speed_sp);
49 static void gh_saturate_accel(struct FloatVect2 *accel_sp);
50 
51 void gh_ref_init(void)
52 {
59  gh_ref.dt = (1.0f/PERIODIC_FREQUENCY);
60 }
61 
62 
64 {
65  /* limit to 100m/s as int version would overflow at 2^14 = 128 m/s */
66  gh_ref.max_speed = Min(fabsf(max_speed), 100.0f);
67  return gh_ref.max_speed;
68 }
69 
70 float gh_set_tau(float tau)
71 {
72  gh_ref.tau = tau;
73  Bound(gh_ref.tau, 0.01f, 2.0f);
74  gh_ref.inv_tau = (1.f / gh_ref.tau);
75  return gh_ref.tau;
76 }
77 
78 float gh_set_omega(float omega)
79 {
80  gh_ref.omega = omega;
81  Bound(gh_ref.omega, 0.1f, 5.0f);
84  return gh_ref.omega;
85 }
86 
87 float gh_set_zeta(float zeta)
88 {
89  gh_ref.zeta = zeta;
90  Bound(gh_ref.zeta, 0.7f, 1.2f);
92  return gh_ref.zeta;
93 }
94 
96 {
97  struct Int64Vect2 new_pos;
98  new_pos.x = ((int64_t)pos.x) << (GH_POS_REF_FRAC - INT32_POS_FRAC);
99  new_pos.y = ((int64_t)pos.y) << (GH_POS_REF_FRAC - INT32_POS_FRAC);
100  gh_ref.pos = new_pos;
101  VECT2_COPY(gh_ref.speed, speed);
102  VECT2_COPY(gh_ref.accel, accel);
103 }
104 
106 {
107  struct FloatVect2 pos_step, speed_step;
108 
109  VECT2_SMUL(pos_step, gh_ref.speed, gh_ref.dt);
110  VECT2_SMUL(speed_step, gh_ref.accel, gh_ref.dt);
111 
112  struct Int64Vect2 pos_update;
113  pos_update.x = LBFP_OF_REAL(pos_step.x, GH_POS_REF_FRAC);
114  pos_update.y = LBFP_OF_REAL(pos_step.y, GH_POS_REF_FRAC);
115 
116  VECT2_ADD(gh_ref.pos, pos_update);
117  VECT2_ADD(gh_ref.speed, speed_step);
118 
119  // compute pos error in pos_frac resolution
120  struct FloatVect2 pos_err;
121  pos_err.x = POS_FLOAT_OF_BFP(pos_sp.x - (gh_ref.pos.x >> (GH_POS_REF_FRAC - INT32_POS_FRAC)));
122  pos_err.y = POS_FLOAT_OF_BFP(pos_sp.y - (gh_ref.pos.y >> (GH_POS_REF_FRAC - INT32_POS_FRAC)));
123 
124  // Calculate velocity error
125  struct FloatVect2 vel_sp;
126  VECT2_SMUL(vel_sp, pos_err, gh_ref.omega*0.5/gh_ref.zeta);
127 
128  // Saturate vel_sp
129  gh_saturate_speed(&vel_sp);
130 
131  // compute the "speed part" of accel = -2*zeta*omega*speed -omega^2(pos - pos_sp)
132  struct FloatVect2 accel_sp;
133  struct FloatVect2 speed_err;
134  VECT2_DIFF(speed_err, vel_sp, gh_ref.speed);
135  VECT2_SMUL(accel_sp, speed_err, 2 * gh_ref.zeta_omega);
136 
137  gh_saturate_accel(&accel_sp);
138 
139  // copy accel
140  VECT2_COPY(gh_ref.accel, accel_sp);
141 }
142 
144 {
145  struct FloatVect2 pos_step, speed_step;
146 
147  VECT2_SMUL(pos_step, gh_ref.speed, gh_ref.dt);
148  VECT2_SMUL(speed_step, gh_ref.accel, gh_ref.dt);
149 
150  struct Int64Vect2 pos_update;
151  pos_update.x = LBFP_OF_REAL(pos_step.x, GH_POS_REF_FRAC);
152  pos_update.y = LBFP_OF_REAL(pos_step.y, GH_POS_REF_FRAC);
153 
154  VECT2_ADD(gh_ref.pos, pos_update);
155  VECT2_ADD(gh_ref.speed, speed_step);
156 
157  // compute speed error
158  struct FloatVect2 speed_err;
159  VECT2_DIFF(speed_err, gh_ref.speed, speed_sp);
160  // compute accel from speed_sp
161  struct FloatVect2 accel_sp;
162  VECT2_SMUL(accel_sp, speed_err, -gh_ref.inv_tau);
163 
164  gh_saturate_accel(&accel_sp);
165 
166  // copy accel
167  VECT2_COPY(gh_ref.accel, accel_sp);
168 }
169 
171 {
172  // Speed squared
173  float v_norm2 = VECT2_NORM2(*speed_sp);
174 
175  // Apply saturation if above max speed
176  if (v_norm2 > (gh_ref.max_speed * gh_ref.max_speed)) {
177  // speed_sp/sqrt(v_norm2)*vmax
178  float factor = gh_ref.max_speed / sqrtf(v_norm2);
179  VECT2_SMUL(*speed_sp, *speed_sp, factor);
180  }
181 }
182 
183 static void gh_saturate_accel(struct FloatVect2 *accel_sp)
184 {
185  // Accel squared
186  float a_norm2 = VECT2_NORM2(*accel_sp);
187 
188  // Apply saturation if above max speed
189  if (a_norm2 > (gh_max_accel * gh_max_accel)) {
190  // accel_sp/sqrt(a_norm2)*amax
191  float factor = gh_max_accel / sqrtf(a_norm2);
192  VECT2_SMUL(*accel_sp, *accel_sp, factor);
193  }
194 }
195 
LBFP_OF_REAL
#define LBFP_OF_REAL(_vr, _frac)
Definition: pprz_algebra_int.h:205
GUIDANCE_H_REF_TAU
#define GUIDANCE_H_REF_TAU
first order time constant
Definition: guidance_h_ref.c:45
Int64Vect2::x
int64_t x
Definition: pprz_algebra_int.h:193
VECT2_SMUL
#define VECT2_SMUL(_vo, _vi, _s)
Definition: pprz_algebra.h:98
gh_saturate_accel
static void gh_saturate_accel(struct FloatVect2 *accel_sp)
Definition: guidance_h_ref.c:183
gh_ref
struct GuidanceHRef gh_ref
Definition: guidance_h_ref.c:30
gh_set_omega
float gh_set_omega(float omega)
Definition: guidance_h_ref.c:78
GUIDANCE_H_REF_ZETA
#define GUIDANCE_H_REF_ZETA
default second order model damping
Definition: guidance_h_ref.c:40
Int32Vect2::y
int32_t y
Definition: pprz_algebra_int.h:85
GuidanceHRef::accel
struct FloatVect2 accel
Reference model acceleration.
Definition: guidance_h_ref.h:64
POS_FLOAT_OF_BFP
#define POS_FLOAT_OF_BFP(_ai)
Definition: pprz_algebra_int.h:218
VECT2_ADD
#define VECT2_ADD(_a, _b)
Definition: pprz_algebra.h:74
gh_set_tau
float gh_set_tau(float tau)
Definition: guidance_h_ref.c:70
gh_set_ref
void gh_set_ref(struct Int32Vect2 pos, struct FloatVect2 speed, struct FloatVect2 accel)
Definition: guidance_h_ref.c:95
VECT2_DIFF
#define VECT2_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:92
VECT2_COPY
#define VECT2_COPY(_a, _b)
Definition: pprz_algebra.h:68
FloatVect2
Definition: pprz_algebra_float.h:49
Int32Vect2::x
int32_t x
Definition: pprz_algebra_int.h:84
gh_update_ref_from_pos_sp
void gh_update_ref_from_pos_sp(struct Int32Vect2 pos_sp)
Definition: guidance_h_ref.c:105
Int32Vect2
Definition: pprz_algebra_int.h:83
GuidanceHRef::inv_tau
float inv_tau
Definition: guidance_h_ref.h:92
GuidanceHRef::dt
float dt
Integration timestep.
Definition: guidance_h_ref.h:96
GuidanceHRef::speed
struct FloatVect2 speed
Reference model speed.
Definition: guidance_h_ref.h:70
gh_ref_init
void gh_ref_init(void)
Definition: guidance_h_ref.c:51
guidance_h_ref.h
Int64Vect2::y
int64_t y
Definition: pprz_algebra_int.h:194
VECT2_NORM2
#define VECT2_NORM2(_v)
Definition: pprz_algebra.h:118
GuidanceHRef::tau
float tau
first order time constant
Definition: guidance_h_ref.h:78
GuidanceHRef
Definition: guidance_h_ref.h:60
f
uint16_t f
Camera baseline, in meters (i.e. horizontal distance between the two cameras of the stereo setup)
Definition: wedgebug.c:204
FloatVect2::y
float y
Definition: pprz_algebra_float.h:51
GUIDANCE_H_REF_MAX_ACCEL
#define GUIDANCE_H_REF_MAX_ACCEL
Accel saturation.
Definition: guidance_h_ref.h:51
Int64Vect2
Definition: pprz_algebra_int.h:192
gh_saturate_speed
static void gh_saturate_speed(struct FloatVect2 *speed_sp)
Definition: guidance_h_ref.c:170
GH_POS_REF_FRAC
#define GH_POS_REF_FRAC
fixedpoint representation: Q26.37 will give a range of 67e3 km and a resolution of 1....
Definition: guidance_h_ref.h:58
gh_set_zeta
float gh_set_zeta(float zeta)
Definition: guidance_h_ref.c:87
FloatVect2::x
float x
Definition: pprz_algebra_float.h:50
GuidanceHRef::zeta_omega
float zeta_omega
Definition: guidance_h_ref.h:90
gh_update_ref_from_speed_sp
void gh_update_ref_from_speed_sp(struct FloatVect2 speed_sp)
Definition: guidance_h_ref.c:143
gh_set_max_speed
float gh_set_max_speed(float max_speed)
Set a new maximum speed for waypoint navigation.
Definition: guidance_h_ref.c:63
gh_max_accel
static const float gh_max_accel
Definition: guidance_h_ref.c:32
GuidanceHRef::zeta
float zeta
second order model damping
Definition: guidance_h_ref.h:80
GuidanceHRef::max_speed
float max_speed
Current maximum speed for waypoint navigation.
Definition: guidance_h_ref.h:85
GuidanceHRef::omega
float omega
second order model natural frequency
Definition: guidance_h_ref.h:79
INT32_POS_FRAC
#define INT32_POS_FRAC
Definition: pprz_algebra_int.h:68
GuidanceHRef::pos
struct Int64Vect2 pos
Reference model position.
Definition: guidance_h_ref.h:76
GuidanceHRef::omega_2
float omega_2
Definition: guidance_h_ref.h:91
speed_sp
struct FloatVect3 speed_sp
Definition: guidance_indi.c:76
Min
#define Min(x, y)
Definition: esc_dshot.c:85
GUIDANCE_H_REF_OMEGA
#define GUIDANCE_H_REF_OMEGA
default second order model natural frequency
Definition: guidance_h_ref.c:36
GUIDANCE_H_REF_MAX_SPEED
#define GUIDANCE_H_REF_MAX_SPEED
Default speed saturation.
Definition: guidance_h_ref.h:39