Paparazzi UAS v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
guidance_v_ref.c
Go to the documentation of this file.
1/*
2 * Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
3 * Copyright (C) 2013 Gautier Hattenberger
4 *
5 * This file is part of paparazzi.
6 *
7 * paparazzi is free software; you can redistribute it and/or modify
8 * it under the terms of the GNU General Public License as published by
9 * the Free Software Foundation; either version 2, or (at your option)
10 * any later version.
11 *
12 * paparazzi is distributed in the hope that it will be useful,
13 * but WITHOUT ANY WARRANTY; without even the implied warranty of
14 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15 * GNU General Public License for more details.
16 *
17 * You should have received a copy of the GNU General Public License
18 * along with paparazzi; see the file COPYING. If not, write to
19 * the Free Software Foundation, 59 Temple Place - Suite 330,
20 * Boston, MA 02111-1307, USA.
21 */
22
29#include "generated/airframe.h"
30
36
42
48
49
50/* Saturations definition */
51#ifndef GUIDANCE_V_REF_MIN_ZDD
52#define GUIDANCE_V_REF_MIN_ZDD (-2.0*9.81)
53#endif
54#define GV_MIN_ZDD BFP_OF_REAL(GUIDANCE_V_REF_MIN_ZDD, GV_ZDD_REF_FRAC)
55
56#ifndef GUIDANCE_V_REF_MAX_ZDD
57#define GUIDANCE_V_REF_MAX_ZDD ( 0.8*9.81)
58#endif
59#define GV_MAX_ZDD BFP_OF_REAL(GUIDANCE_V_REF_MAX_ZDD, GV_ZDD_REF_FRAC)
60
62#ifndef GUIDANCE_V_REF_MAX_Z_DIFF
63#define GUIDANCE_V_REF_MAX_Z_DIFF 10.0
64#endif
65#define GV_MAX_Z_DIFF BFP_OF_REAL(GUIDANCE_V_REF_MAX_Z_DIFF, GV_Z_REF_FRAC)
66
67#define GV_MIN_ZD BFP_OF_REAL(GUIDANCE_V_REF_MIN_ZD , GV_ZD_REF_FRAC)
68#define GV_MAX_ZD BFP_OF_REAL(GUIDANCE_V_REF_MAX_ZD , GV_ZD_REF_FRAC)
69
70/* second order model natural frequency and damping */
71#ifndef GUIDANCE_V_REF_OMEGA
72#define GUIDANCE_V_REF_OMEGA RadOfDeg(100.)
73#endif
74#ifndef GUIDANCE_V_REF_ZETA
75#define GUIDANCE_V_REF_ZETA 0.85
76#endif
77#define GV_ZETA_OMEGA_FRAC 10
78#define GV_ZETA_OMEGA BFP_OF_REAL((GUIDANCE_V_REF_ZETA*GUIDANCE_V_REF_OMEGA), GV_ZETA_OMEGA_FRAC)
79#define GV_OMEGA_2_FRAC 7
80#define GV_OMEGA_2 BFP_OF_REAL((GUIDANCE_V_REF_OMEGA*GUIDANCE_V_REF_OMEGA), GV_OMEGA_2_FRAC)
81
82/* first order time constant */
83#define GV_REF_THAU_F 0.25
84#define GV_REF_INV_THAU_FRAC 16
85#define GV_REF_INV_THAU BFP_OF_REAL((1./0.25), GV_REF_INV_THAU_FRAC)
86
94
96{
97
100
101 // compute the "speed part" of zdd = -2*zeta*omega*zd -omega^2(z_sp - z)
104 // compute z error in z_sp resolution
106 // convert to accel resolution
110
111 /* Saturate accelerations */
113
114 /* Saturate speed and adjust acceleration accordingly */
115 if (gv_zd_ref <= GV_MIN_ZD) {
117 if (gv_zdd_ref < 0) {
118 gv_zdd_ref = 0;
119 }
120 } else if (gv_zd_ref >= GV_MAX_ZD) {
122 if (gv_zdd_ref > 0) {
123 gv_zdd_ref = 0;
124 }
125 }
126}
127
128
130{
131
134
135 /* limit z_ref to GUIDANCE_V_REF_MAX_Z_DIFF from current z pos */
138
142
143 /* Saturate accelerations */
145
146 /* Saturate speed and adjust acceleration accordingly */
147 if (gv_zd_ref <= GV_MIN_ZD) {
149 if (gv_zdd_ref < 0) {
150 gv_zdd_ref = 0;
151 }
152 } else if (gv_zd_ref >= GV_MAX_ZD) {
154 if (gv_zdd_ref > 0) {
155 gv_zdd_ref = 0;
156 }
157 }
158}
159
#define INT32_POS_FRAC
#define INT32_SPEED_FRAC
#define INT32_ACCEL_FRAC
#define GV_ZETA_OMEGA
#define GV_MAX_ZDD
int32_t gv_zdd_ref
reference model vertical accel in meters/s^2 (output) fixed point representation with GV_ZDD_REF_FRAC...
#define GV_MAX_ZD
#define GV_MAX_Z_DIFF
int64_t gv_z_ref
reference model altitude in meters (output) fixed point representation with GV_Z_REF_FRAC Q37....
#define GV_REF_INV_THAU
#define GV_MIN_ZDD
#define GV_OMEGA_2_FRAC
void gv_set_ref(int32_t alt, int32_t speed, int32_t accel)
void gv_update_ref_from_z_sp(int32_t z_sp)
void gv_update_ref_from_zd_sp(int32_t zd_sp, int32_t z_pos)
update vertical reference from speed setpoint.
#define GV_OMEGA_2
#define GV_REF_INV_THAU_FRAC
#define GV_MIN_ZD
#define GV_ZETA_OMEGA_FRAC
int32_t gv_zd_ref
reference model vertical speed in meters/sec (output) fixed point representation with GV_ZD_REF_FRAC ...
Reference generation for vertical guidance.
#define GV_ZD_REF_FRAC
number of bits for the fractional part of gv_zd_ref
#define GV_Z_REF_FRAC
number of bits for the fractional part of gv_z_ref
#define GV_ZDD_REF_FRAC
number of bits for the fractional part of gv_zdd_ref
uint16_t foo
Definition main_demo5.c:58
int int32_t
Typedef defining 32 bit int type.