Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
guidance_v.c
Go to the documentation of this file.
1/*
2 * Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
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
26
27#include "generated/airframe.h"
30
35#include "modules/core/abi.h"
36
37#include "state.h"
38
40
41
42#ifndef GUIDANCE_V_NOMINAL_HOVER_THROTTLE
43#define GUIDANCE_V_NOMINAL_HOVER_THROTTLE 0.4
44#endif
46
47
48#ifndef GUIDANCE_V_CLIMB_RC_DEADBAND
49#define GUIDANCE_V_CLIMB_RC_DEADBAND MAX_PPRZ/10
50#endif
51
52#ifndef GUIDANCE_V_MAX_RC_CLIMB_SPEED
53#define GUIDANCE_V_MAX_RC_CLIMB_SPEED GUIDANCE_V_REF_MIN_ZD
54#endif
55
56#ifndef GUIDANCE_V_MAX_RC_DESCENT_SPEED
57#define GUIDANCE_V_MAX_RC_DESCENT_SPEED GUIDANCE_V_REF_MAX_ZD
58#endif
59
61
63
64#define GUIDANCE_V_GUIDED_MODE_ZHOLD 0
65#define GUIDANCE_V_GUIDED_MODE_CLIMB 1
66#define GUIDANCE_V_GUIDED_MODE_THROTTLE 2
67#define GUIDANCE_V_GUIDED_MODE_ALL 3
68
70
71#ifndef GUIDANCE_V_RC_ID
72#define GUIDANCE_V_RC_ID ABI_BROADCAST
73#endif
76static void rc_cb(uint8_t sender_id UNUSED, struct RadioControl *rc);
77
78
79#if PERIODIC_TELEMETRY
81
82static void send_tune_vert(struct transport_tx *trans, struct link_device *dev)
83{
84 pprz_msg_send_TUNE_VERT(trans, dev, AC_ID,
85 &guidance_v.z_sp,
87 &guidance_v.z_ref,
88 &guidance_v.zd_ref);
89}
90#endif
91
93{
94
98
99 guidance_v.thrust_coeff = BFP_OF_REAL(1.f, INT32_TRIG_FRAC);
100 desired_zd_updated = false;
101
103
104 // bind ABI messages
105 AbiBindMsgRADIO_CONTROL(GUIDANCE_V_RC_ID, &rc_ev, rc_cb);
106
107#if PERIODIC_TELEMETRY
109#endif
110}
111
112static void rc_cb(uint8_t sender_id UNUSED, struct RadioControl *rc)
113{
114 /* used in RC_DIRECT directly and as saturation in CLIMB and HOVER */
115 guidance_v.rc_delta_t = (int32_t)rc->values[RADIO_THROTTLE];
116
117 /* used in RC_CLIMB */
118 guidance_v.rc_zd_sp = (MAX_PPRZ / 2) - (int32_t)rc->values[RADIO_THROTTLE];
119 DeadBand(guidance_v.rc_zd_sp, GUIDANCE_V_CLIMB_RC_DEADBAND);
120
121 static const int32_t climb_scale = ABS(SPEED_BFP_OF_REAL(GUIDANCE_V_MAX_RC_CLIMB_SPEED) /
123 static const int32_t descent_scale = ABS(SPEED_BFP_OF_REAL(GUIDANCE_V_MAX_RC_DESCENT_SPEED) /
125
126 if (guidance_v.rc_zd_sp > 0) {
127 guidance_v.rc_zd_sp *= descent_scale;
128 } else {
129 guidance_v.rc_zd_sp *= climb_scale;
130 }
131}
132
134{
135
136 if (new_mode == guidance_v.mode) {
137 return;
138 }
139
140 switch (new_mode) {
144 break;
145
148 guidance_v.zd_sp = 0;
149 guidance_v.zdd_sp = 0;
150 /* Falls through. */
154 break;
155
156 default:
157 break;
158
159 }
160
161 guidance_v.mode = new_mode;
162
163}
164
165void guidance_v_notify_in_flight(bool in_flight)
166{
167 if (in_flight) {
169 }
170}
171
174{
175 // cos(30°) = 0.8660254
176 static const int32_t max_bank_coef = BFP_OF_REAL(0.8660254f, INT32_TRIG_FRAC);
177
178 struct Int32RMat *att = stateGetNedToBodyRMat_i();
179 /* thrust vector:
180 * int32_rmat_vmult(&thrust_vect, &att, &zaxis)
181 * same as last colum of rmat with INT32_TRIG_FRAC
182 * struct Int32Vect thrust_vect = {att.m[2], att.m[5], att.m[8]};
183 *
184 * Angle between two vectors v1 and v2:
185 * angle = acos(dot(v1, v2) / (norm(v1) * norm(v2)))
186 * since here both are already of unit length:
187 * angle = acos(dot(v1, v2))
188 * since we we want the cosine of the angle we simply need
189 * thrust_coeff = dot(v1, v2)
190 * also can be simplified considering: v1 is zaxis with (0,0,1)
191 * dot(v1, v2) = v1.z * v2.z = v2.z
192 */
193 int32_t coef = att->m[8];
194 if (coef < max_bank_coef) {
195 coef = max_bank_coef;
196 }
197 return coef;
198}
199
200
201void guidance_v_thrust_adapt(bool in_flight)
202{
203 guidance_v.thrust_coeff = get_vertical_thrust_coeff();
204
205 if (in_flight) {
206 /* Only run adaptive throttle estimation if we are in flight and
207 * the desired vertical velocity (zd) was updated (i.e. we ran hover_loop before).
208 * This means that the estimation is not updated when using direct throttle commands.
209 *
210 * FIXME... SATURATIONS NOT TAKEN INTO ACCOUNT, AKA SUPERVISION and co
211 * FIXME get this out of here !!!! and don't get stab cmd directly
212 */
213 if (desired_zd_updated) {
214 int32_t vertical_thrust = (stabilization.cmd[COMMAND_THRUST] * guidance_v.thrust_coeff) >> INT32_TRIG_FRAC;
215 gv_adapt_run(stateGetAccelNed_i()->z, vertical_thrust, guidance_v.zd_ref);
216 }
217 } else {
218 /* reset estimate while not in_flight */
220 }
221}
222
223struct ThrustSetpoint guidance_v_run(bool in_flight)
224{
225 guidance_v_thrust_adapt(in_flight);
226
227 /* reset flag indicating if desired zd was updated */
228 desired_zd_updated = false;
229 /* reset setpoint */
231
232 switch (guidance_v.mode) {
233
235 guidance_v.z_sp = stateGetPositionNed_i()->z; // for display only
237 break;
238
240 guidance_v.zd_sp = guidance_v.rc_zd_sp;
242 guidance_v.thrust = guidance_v_run_speed(in_flight, &guidance_v);
243 break;
244
247 guidance_v.thrust = guidance_v_run_speed(in_flight, &guidance_v);
248 break;
249
252 /* Falls through. */
254 guidance_v.thrust = guidance_v_guided_run(in_flight);
255 break;
256
257 case GUIDANCE_V_MODE_NAV: {
258 guidance_v.thrust = guidance_v_from_nav(in_flight);
259 break;
260 }
261
262 default:
263 break;
264 }
265 return guidance_v.thrust;
266}
267
268
270{
271 /* set current altitude as setpoint */
273
274 /* reset guidance reference */
277
278 /* reset speed setting */
279 guidance_v.zd_sp = 0;
280 guidance_v.zdd_sp = 0;
281}
282
284{
285 gv_set_ref(pos, speed, accel);
286 guidance_v.z_ref = pos;
287 guidance_v.zd_ref = speed;
288 guidance_v.zdd_ref = accel;
289}
290
291
293{
294 /* convert our reference to generic representation */
295 int64_t tmp = gv_z_ref >> (GV_Z_REF_FRAC - INT32_POS_FRAC);
296 guidance_v.z_ref = (int32_t)tmp;
299 /* set flag to indicate that desired zd was updated */
300 desired_zd_updated = true;
301}
302
304{
305 struct ThrustSetpoint sp;
307 if (nav.vertical_mode == NAV_VERTICAL_MODE_ALT) {
308 guidance_v.z_sp = -POS_BFP_OF_REAL(nav.nav_altitude);
309 guidance_v.zd_sp = 0;
310 guidance_v.zdd_sp = 0;
313 sp = guidance_v_run_pos(in_flight, &guidance_v);
314 } else if (nav.vertical_mode == NAV_VERTICAL_MODE_CLIMB) {
316 guidance_v.zd_sp = -SPEED_BFP_OF_REAL(nav.climb);
317 guidance_v.zdd_sp = 0;
320 sp = guidance_v_run_speed(in_flight, &guidance_v);
321 } else if (nav.vertical_mode == NAV_VERTICAL_MODE_MANUAL) {
324 guidance_v.zdd_sp = 0;
325 GuidanceVSetRef(guidance_v.z_sp, guidance_v.zd_sp, 0);
328 } else if (nav.vertical_mode == NAV_VERTICAL_MODE_GUIDED) {
329 sp = guidance_v_guided_run(in_flight);
330 }
331 return sp;
332}
333
335{
336 /* set current altitude as setpoint */
338
339 /* reset guidance reference */
342}
343
345{
346 struct ThrustSetpoint sp;
349 {
351 // Altitude Hold
352 guidance_v.zd_sp = 0;
355 sp = guidance_v_run_pos(in_flight, &guidance_v);
356 break;
358 // Climb
361 sp = guidance_v_run_speed(in_flight, &guidance_v);
362 break;
364 // Throttle
365 guidance_v.z_sp = stateGetPositionNed_i()->z; // for display only
367 break;
369 // update full reference
371 sp = guidance_v_run_pos(in_flight, &guidance_v);
372 break;
373 default:
374 break;
375 }
376 return sp;
377}
378
379void guidance_v_set_z(float z)
380{
381 /* disable vertical velocity setpoints */
383 /* set altitude setpoint */
384 guidance_v.z_sp = POS_BFP_OF_REAL(z);
385 /* reset speed and accel setting */
386 guidance_v.zd_sp = 0;
387 guidance_v.zdd_sp = 0;
388}
389
390void guidance_v_set_vz(float vz)
391{
392 /* enable vertical velocity setpoints */
394 /* set speed setting */
395 guidance_v.zd_sp = SPEED_BFP_OF_REAL(vz);
396 /* reset accel setpoint */
397 guidance_v.zdd_sp = 0;
398}
399
400void guidance_v_set_th(float th)
401{
402 /* enable vertical velocity setpoints */
404
405 /* reset guidance reference */
407 guidance_v.th_sp = (int32_t)(MAX_PPRZ * th);
408 Bound(guidance_v.th_sp, 0, MAX_PPRZ);
409}
410
411void guidance_v_set_all(float z, float vz, float az)
412{
414 guidance_v.z_sp = POS_BFP_OF_REAL(z);
415 guidance_v.zd_sp = SPEED_BFP_OF_REAL(vz);
416 guidance_v.zdd_sp = ACCEL_BFP_OF_REAL(az);
417}
418
Main include for ABI (AirBorneInterface).
struct abi_struct abi_event
Definition abi_common.h:101
static void rc_cb(uint8_t sender_id, struct RadioControl *rc)
Definition autopilot.c:58
static abi_event rc_ev
Definition autopilot.c:56
#define UNUSED(x)
static struct uart_periph * dev
int32_t m[3 *3]
#define INT32_POS_FRAC
#define ACCEL_BFP_OF_REAL(_af)
#define INT32_TRIG_FRAC
#define INT32_SPEED_FRAC
#define INT32_ACCEL_FRAC
#define BFP_OF_REAL(_vr, _frac)
#define POS_BFP_OF_REAL(_af)
#define SPEED_BFP_OF_REAL(_af)
rotation matrix
int32_t z
Down.
static struct NedCoor_i * stateGetAccelNed_i(void)
Get acceleration in NED coordinates (int).
Definition state.h:1196
static struct Int32RMat * stateGetNedToBodyRMat_i(void)
Get vehicle body attitude rotation matrix (int).
Definition state.h:1301
static struct NedCoor_i * stateGetPositionNed_i(void)
Get position in local NED coordinates (int).
Definition state.h:805
static struct NedCoor_f * stateGetPositionNed_f(void)
Get position in local NED coordinates (float).
Definition state.h:850
static struct NedCoor_i * stateGetSpeedNed_i(void)
Get ground speed in local NED coordinates (int).
Definition state.h:1015
Guidance controllers (horizontal and vertical) for Hybrid UAV configurations.
Guidance in a module file.
void guidance_v_run_enter(void)
struct ThrustSetpoint guidance_v_run_pos(bool in_flight UNUSED, struct VerticalGuidance *gv)
struct ThrustSetpoint guidance_v_run_speed(bool in_flight UNUSED, struct VerticalGuidance *gv)
void gv_adapt_run(int32_t zdd_meas, int32_t thrust_applied, int32_t zd_ref)
Adaptation function.
void gv_adapt_init(void)
int32_t gv_zdd_ref
reference model vertical accel in meters/s^2 (output) fixed point representation with GV_ZDD_REF_FRAC...
int64_t gv_z_ref
reference model altitude in meters (output) fixed point representation with GV_Z_REF_FRAC Q37....
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.
int32_t gv_zd_ref
reference model vertical speed in meters/sec (output) fixed point representation with GV_ZD_REF_FRAC ...
#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
PRINT_CONFIG_VAR(ONELOOP_ANDI_FILT_CUTOFF)
#define MAX_PPRZ
Definition paparazzi.h:8
Paparazzi fixed point algebra.
Generic interface for radio control modules.
pprz_t values[RADIO_CONTROL_NB_CHANNEL]
static void send_tune_vert(struct transport_tx *trans, struct link_device *dev)
Definition guidance_v.c:82
void guidance_v_thrust_adapt(bool in_flight)
Definition guidance_v.c:201
#define GUIDANCE_V_MAX_RC_DESCENT_SPEED
Definition guidance_v.c:57
void guidance_v_set_vz(float vz)
Set z velocity setpoint.
Definition guidance_v.c:390
void guidance_v_z_enter(void)
Definition guidance_v.c:269
void guidance_v_notify_in_flight(bool in_flight)
Definition guidance_v.c:165
struct ThrustSetpoint guidance_v_from_nav(bool in_flight)
Set guidance setpoint from NAV and run hover loop.
Definition guidance_v.c:303
static void rc_cb(uint8_t sender_id UNUSED, struct RadioControl *rc)
Definition guidance_v.c:112
#define GUIDANCE_V_MAX_RC_CLIMB_SPEED
Definition guidance_v.c:53
static bool desired_zd_updated
Definition guidance_v.c:62
#define GUIDANCE_V_NOMINAL_HOVER_THROTTLE
Definition guidance_v.c:43
#define GUIDANCE_V_GUIDED_MODE_ZHOLD
Definition guidance_v.c:64
void guidance_v_set_z(float z)
Set z position setpoint.
Definition guidance_v.c:379
void guidance_v_guided_enter(void)
Enter GUIDED mode control.
Definition guidance_v.c:334
void guidance_v_set_all(float z, float vz, float az)
Set z position, velocity and acceleration setpoint.
Definition guidance_v.c:411
#define GUIDANCE_V_GUIDED_MODE_THROTTLE
Definition guidance_v.c:66
static int32_t get_vertical_thrust_coeff(void)
get the cosine of the angle between thrust vector and gravity vector
Definition guidance_v.c:173
void guidance_v_set_ref(int32_t pos, int32_t speed, int32_t accel)
Set guidance ref parameters.
Definition guidance_v.c:283
#define GUIDANCE_V_GUIDED_MODE_CLIMB
Definition guidance_v.c:65
#define GUIDANCE_V_GUIDED_MODE_ALL
Definition guidance_v.c:67
void guidance_v_set_th(float th)
Set throttle setpoint.
Definition guidance_v.c:400
struct ThrustSetpoint guidance_v_guided_run(bool in_flight)
Run GUIDED mode control.
Definition guidance_v.c:344
void guidance_v_mode_changed(uint8_t new_mode)
Definition guidance_v.c:133
struct VerticalGuidance guidance_v
Definition guidance_v.c:60
#define GUIDANCE_V_CLIMB_RC_DEADBAND
Definition guidance_v.c:49
void guidance_v_update_ref(void)
Definition guidance_v.c:292
void guidance_v_init(void)
Definition guidance_v.c:92
static int guidance_v_guided_mode
Definition guidance_v.c:69
struct ThrustSetpoint guidance_v_run(bool in_flight)
Guidance vertical run functions.
Definition guidance_v.c:223
#define GUIDANCE_V_RC_ID
Definition guidance_v.c:72
Vertical guidance for rotorcrafts.
#define GuidanceVSetRef
Definition guidance_v.h:135
#define GUIDANCE_V_MODE_CLIMB
Definition guidance_v.h:38
#define GUIDANCE_V_MODE_RC_CLIMB
Definition guidance_v.h:37
#define GUIDANCE_V_MODE_KILL
Definition guidance_v.h:35
#define GUIDANCE_V_MODE_RC_DIRECT
Definition guidance_v.h:36
#define GUIDANCE_V_MODE_NAV
Definition guidance_v.h:40
#define GUIDANCE_V_MODE_HOVER
Definition guidance_v.h:39
#define GUIDANCE_V_MODE_GUIDED
Definition guidance_v.h:41
struct RotorcraftNavigation nav
Definition navigation.c:51
Rotorcraft navigation functions.
#define NAV_VERTICAL_MODE_CLIMB
Definition navigation.h:93
#define NAV_VERTICAL_MODE_MANUAL
Definition navigation.h:92
#define NAV_VERTICAL_MODE_ALT
Definition navigation.h:94
#define NAV_VERTICAL_MODE_GUIDED
Definition navigation.h:95
struct Stabilization stabilization
General stabilization interface for rotorcrafts.
#define THRUST_SP_SET_ZERO(_sp)
#define THRUST_AXIS_Z
struct ThrustSetpoint th_sp_from_thrust_i(int32_t thrust, uint8_t axis)
API to get/set the generic vehicle states.
Thrust setpoint // TODO to a setpoint header Structure to store the desired thrust vector with differ...
union ThrustSetpoint::@302344065342070214103253314124156277107357236133 sp
int16_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint16_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition telemetry.c:51
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
Definition telemetry.h:66
int int32_t
Typedef defining 32 bit int type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.