Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
guidance_indi.c
Go to the documentation of this file.
1/*
2 * Copyright (C) 2015 Ewoud Smeur <ewoud.smeur@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, see
18 * <http://www.gnu.org/licenses/>.
19 */
20
33#include "generated/airframe.h"
40#include "mcu_periph/sys_time.h"
41#include "state.h"
42#include "autopilot.h"
45#include "modules/core/abi.h"
46
47// The acceleration reference is calculated with these gains. If you use GPS,
48// they are probably limited by the update rate of your GPS. The default
49// values are tuned for 4 Hz GPS updates. If you have high speed position updates, the
50// gains can be higher, depending on the speed of the inner loop.
51#ifdef GUIDANCE_INDI_POS_GAIN
53#else
55#endif
56
57#ifdef GUIDANCE_INDI_SPEED_GAIN
59#else
61#endif
62
63#ifndef GUIDANCE_INDI_ACCEL_SP_ID
64#define GUIDANCE_INDI_ACCEL_SP_ID ABI_BROADCAST
65#endif
67static void accel_sp_cb(uint8_t sender_id, uint8_t flag, struct FloatVect3 *accel_sp);
68struct FloatVect3 indi_accel_sp = {0.0f, 0.0f, 0.0f};
71
72// FIXME make a proper structure for these variables
73struct FloatVect3 speed_sp = {0.0f, 0.0f, 0.0f};
74struct FloatVect3 sp_accel = {0.0f, 0.0f, 0.0f};
75#ifdef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
76float guidance_indi_specific_force_gain = GUIDANCE_INDI_SPECIFIC_FORCE_GAIN;
77static void guidance_indi_filter_thrust(void);
78
79#ifndef GUIDANCE_INDI_THRUST_DYNAMICS_FREQ
80#ifndef STABILIZATION_INDI_ACT_FREQ_P
81#error "You need to define GUIDANCE_INDI_THRUST_DYNAMICS_FREQ to be able to use indi vertical control"
82#else // assume that the same actuators are used for thrust as for roll (e.g. quadrotor)
83#define GUIDANCE_INDI_THRUST_DYNAMICS_FREQ STABILIZATION_INDI_ACT_FREQ_P
84#endif
85#endif //GUIDANCE_INDI_THRUST_DYNAMICS_FREQ
86
87
88#endif //GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
89
90#ifndef GUIDANCE_INDI_FILTER_CUTOFF
91#ifdef STABILIZATION_INDI_FILT_CUTOFF
92#define GUIDANCE_INDI_FILTER_CUTOFF STABILIZATION_INDI_FILT_CUTOFF
93#else
94#define GUIDANCE_INDI_FILTER_CUTOFF 3.0
95#endif
96#endif
97
98float thrust_dyn = 0.f;
99float thrust_act = 0.f;
103
105
106#if GUIDANCE_INDI_USE_WLS
107#include "math/wls/wls_alloc.h"
108
109// priorities on control inputs (ax, ay, az)
110#ifndef GUIDANCE_INDI_WLS_PRIORITIES
111#define GUIDANCE_INDI_WLS_PRIORITIES {100.f, 100.f, 100.f}
112#endif
113
114// weighting of prefered control outputs (phi, theta, psi, Tx, Ty, Tz)
115#ifndef GUIDANCE_INDI_WLS_WU
116#define GUIDANCE_INDI_WLS_WU {1000.f,1000.f,10.f,1.f,1.f,1.f}
117#endif
118
119#define NU_MAX 6 // Example constant value // [dtheta, dphi, dthrust, dtx , dty]
120#define NV_MAX 3 // Example constant value (ax,ay,az)
121static float du_guidance[GUIDANCE_INDI_NU];
122static float *Bwls_gi[GUIDANCE_INDI_NV];
123
124static struct WLS_t wls_guid_p = {
125 .nu = GUIDANCE_INDI_NU,
126 .nv = GUIDANCE_INDI_NV,
127 .gamma_sq = 100.0,
128 .v = {0.0},
131 .u_pref = {0.0},
132 .u_min = {0.0},
133 .u_max = {0.0},
134 .PC = 0.0,
135 .SC = 0.0,
136 .iter = 0
137};
138
139#else
140
141// inverse matrix directly if not using WLS
144struct FloatVect3 control_increment; // [dtheta, dphi, dthrust]
145
146#endif
147
150
153
157float thrust_vect[3];
158
159static void guidance_indi_propagate_filters(void);
160
161//------------------------------------------------------------//
162
163#if PERIODIC_TELEMETRY
165static void send_indi_guidance(struct transport_tx *trans, struct link_device *dev)
166{
167 pprz_msg_send_GUIDANCE_INDI_HYBRID(trans, dev, AC_ID,
168 &sp_accel.x,
169 &sp_accel.y,
170 &sp_accel.z,
171#if GUIDANCE_INDI_USE_WLS
172 &du_guidance[0],
173 &du_guidance[1],
174 &du_guidance[2],
175#else
179#endif
180 &filt_accel_ned[0].o[0],
181 &filt_accel_ned[1].o[0],
182 &filt_accel_ned[2].o[0],
183 &speed_sp.x,
184 &speed_sp.y,
185 &speed_sp.z);
186}
187#endif
188
193{
197
198#if PERIODIC_TELEMETRY
199 register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GUIDANCE_INDI_HYBRID, send_indi_guidance);
200#endif
201}
202
208{
209 /* set nav_heading to current heading */
210 nav.heading = stateGetNedToBodyEulers_f()->psi;
211
212 thrust_in = stabilization.cmd[COMMAND_THRUST];
214
215#ifdef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
216 thrust_dyn = 1-exp(-GUIDANCE_INDI_THRUST_DYNAMICS_FREQ/PERIODIC_FREQUENCY);
217#endif //GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
218
219 float tau = 1.0 / (2.0 * M_PI * filter_cutoff);
220 float sample_time = 1.0 / PERIODIC_FREQUENCY;
221 for (int8_t i = 0; i < 3; i++) {
222 init_butterworth_2_low_pass(&filt_accel_ned[i], tau, sample_time, 0.0);
223 }
226}
227
235struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accel_sp, float heading_sp)
236{
237 struct FloatEulers eulers_filtered;
238 float_eulers_of_quat_yxz(&eulers_filtered, &quat_filt.quat);
239
240 // set global accel sp variable FIXME clean this
241 sp_accel = *accel_sp;
242
243 //filter accel to get rid of noise and filter attitude to synchronize with accel
245
246 struct FloatVect3 a_diff = {
247 sp_accel.x - filt_accel_ned[0].o[0],
248 sp_accel.y - filt_accel_ned[1].o[0],
249 sp_accel.z - filt_accel_ned[2].o[0]
250 };
251 Bound(a_diff.x, -6.0, 6.0);
252 Bound(a_diff.y, -6.0, 6.0);
253 Bound(a_diff.z, -9.0, 9.0);
254
255 //If the thrust to specific force ratio has been defined, include vertical control
256 //else ignore the vertical acceleration error
257#ifndef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
258#ifndef STABILIZATION_ATTITUDE_INDI_FULL
259 a_diff.z = 0.0;
260#endif
261#endif
262
263#if GUIDANCE_INDI_USE_WLS
264 const float m = GUIDANCE_INDI_MASS;
265 wls_guid_p.v[0] = m*a_diff.x;
266 wls_guid_p.v[1] = m*a_diff.y;
267 wls_guid_p.v[2] = m*a_diff.z;
268
269 guidance_indi_set_wls_settings(&wls_guid_p, &quat_filt.quat, heading_sp);
271 for (int i = 0; i < GUIDANCE_INDI_NV; i++) {
272 Bwls_gi[i] = Gmat[i];
273 }
274 wls_alloc(&wls_guid_p,Bwls_gi, 0, 0, 10);
275 for (int i = 0; i < GUIDANCE_INDI_NU; i++) {
276 du_guidance [i] = wls_guid_p.u[i];
277 }
278
279 guidance_euler_cmd.phi = eulers_filtered.phi + du_guidance[0];
280 guidance_euler_cmd.theta = eulers_filtered.theta + du_guidance[1];
281 guidance_euler_cmd.psi = eulers_filtered.psi + du_guidance[2];
282 thrust_vect[0] = du_guidance[3]; // (TX)
283 thrust_vect[1] = du_guidance[4]; // (TY)
284 thrust_vect[2] = du_guidance[5]; // (TZ)
286
287#else // !USE_WLS
288
289 // Calculate matrix of partial derivatives
291
292 RMAT_ELMT(Ga, 0, 0) = Gmat[0][0];
293 RMAT_ELMT(Ga, 1, 0) = Gmat[1][0];
294 RMAT_ELMT(Ga, 2, 0) = Gmat[2][0];
295 RMAT_ELMT(Ga, 0, 1) = Gmat[0][1];
296 RMAT_ELMT(Ga, 1, 1) = Gmat[1][1];
297 RMAT_ELMT(Ga, 2, 1) = Gmat[2][1];
298 RMAT_ELMT(Ga, 0, 2) = Gmat[0][2];
299 RMAT_ELMT(Ga, 1, 2) = Gmat[1][2];
300 RMAT_ELMT(Ga, 2, 2) = Gmat[2][2];
301 // Invert this matrix // FIXME input format
303 // Calculate roll,pitch and thrust command
305
306 guidance_euler_cmd.theta = eulers_filtered.theta + control_increment.x;
307 guidance_euler_cmd.phi = eulers_filtered.phi + control_increment.y;
308 guidance_euler_cmd.psi = heading_sp;
309
310 // Compute and store thust setpoint
311#ifdef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
312 guidance_indi_filter_thrust();
313 //Add the increment in specific force * specific_force_to_thrust_gain to the filtered thrust
315 Bound(thrust_in, 0, MAX_PPRZ);
317
318#else
319 // basic quad, no side force
320 thrust_vect[0] = 0.0f;
321 thrust_vect[1] = 0.0f;
323
324 // specific force not defined, return required increment
326#endif
327
328#endif // USE_WLS
329
330 // Bound euler angles to prevent flipping
333
334 // set the quat setpoint with the calculated roll and pitch
335 struct FloatQuat q_sp;
337
338 return stab_sp_from_quat_f(&q_sp);
339}
340
341struct FloatVect3 WEAK guidance_indi_controller(bool in_flight UNUSED, struct HorizontalGuidance *gh, struct VerticalGuidance *gv, enum GuidanceIndi_HMode h_mode, enum GuidanceIndi_VMode v_mode)
342{
343 struct FloatVect3 pos_err = { 0 };
344 struct FloatVect3 accel_sp = { 0 };
345
346 struct FloatVect3 speed_fb;
347
348 if (h_mode == GUIDANCE_INDI_H_ACCEL) {
349 // Speed feedback is included in the guidance when running in ACCEL mode
350 speed_fb.x = 0.;
351 speed_fb.y = 0.;
352 }
353 else {
354 // Generate speed feedback for acceleration, as it is estimated
355 if (h_mode == GUIDANCE_INDI_H_SPEED) {
356 speed_sp.x = SPEED_FLOAT_OF_BFP(gh->ref.speed.x);
357 speed_sp.y = SPEED_FLOAT_OF_BFP(gh->ref.speed.y);
358 }
359 else { // H_POS
360 pos_err.x = POS_FLOAT_OF_BFP(gh->ref.pos.x) - stateGetPositionNed_f()->x;
361 pos_err.y = POS_FLOAT_OF_BFP(gh->ref.pos.y) - stateGetPositionNed_f()->y;
362 speed_sp.x = pos_err.x * guidance_indi_pos_gain + SPEED_FLOAT_OF_BFP(gh->ref.speed.x);
363 speed_sp.y = pos_err.y * guidance_indi_pos_gain + SPEED_FLOAT_OF_BFP(gh->ref.speed.y);
364 }
367 }
368
369 if (v_mode == GUIDANCE_INDI_V_ACCEL) {
370 // Speed feedback is included in the guidance when running in ACCEL mode
371 speed_fb.z = 0;
372 }
373 else {
374 // Generate speed feedback for acceleration, as it is estimated
375 if (v_mode == GUIDANCE_INDI_V_SPEED) {
377 }
378 else { // V_POS
379 pos_err.z = POS_FLOAT_OF_BFP(gv->z_ref) - stateGetPositionNed_f()->z;
381 }
383 }
384
385 accel_sp.x = speed_fb.x + ACCEL_FLOAT_OF_BFP(gh->ref.accel.x);
386 accel_sp.y = speed_fb.y + ACCEL_FLOAT_OF_BFP(gh->ref.accel.y);
387 accel_sp.z = speed_fb.z + ACCEL_FLOAT_OF_BFP(gv->zdd_ref);
388
389 return accel_sp;
390}
391
392struct StabilizationSetpoint guidance_indi_run_mode(bool in_flight, struct HorizontalGuidance *gh, struct VerticalGuidance *gv, enum GuidanceIndi_HMode h_mode, enum GuidanceIndi_VMode v_mode)
393{
394 struct FloatVect3 accel_sp;
395
396 // If set and valid, the ABI message overwrite the accel setpoint
397 // TODO This is not ideal, guided mode with accel setpoint would be better
399 accel_sp.x = indi_accel_sp.x;
400 accel_sp.y = indi_accel_sp.y;
401 // In 2D the vertical motion is derived from the flight plan
404 // If the input command is not updated after a timeout, switch back to flight plan control
405 if (dt > 0.5) {
406 indi_accel_sp_set_2d = false;
407 }
408 } else if (indi_accel_sp_set_3d) {
409 accel_sp.x = indi_accel_sp.x;
410 accel_sp.y = indi_accel_sp.y;
411 accel_sp.z = indi_accel_sp.z;
413 // If the input command is not updated after a timeout, switch back to flight plan control
414 if (dt > 0.5) {
415 indi_accel_sp_set_3d = false;
416 }
417 }
418 else {
419 accel_sp = guidance_indi_controller(in_flight, gh, gv, h_mode, v_mode);
420 }
421
422 return guidance_indi_run(&accel_sp, gh->sp.heading);
423}
424
425
426#ifdef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
430void guidance_indi_filter_thrust(void)
431{
432 // Actuator dynamics
434
435 // same filter as for the acceleration
437}
438#endif
439
453
458static void accel_sp_cb(uint8_t sender_id __attribute__((unused)), uint8_t flag, struct FloatVect3 *accel_sp)
459{
460 if (flag == 0) {
461 indi_accel_sp.x = accel_sp->x;
462 indi_accel_sp.y = accel_sp->y;
465 } else if (flag == 1) {
466 indi_accel_sp.x = accel_sp->x;
467 indi_accel_sp.y = accel_sp->y;
468 indi_accel_sp.z = accel_sp->z;
471 }
472}
473
474#if GUIDANCE_INDI_USE_AS_DEFAULT
475// guidance indi control function is implementing the default functions of guidance
476
477void guidance_h_run_enter(void)
478{
480}
481
482void guidance_v_run_enter(void)
483{
484 // nothing to do
485}
486
487static struct VerticalGuidance *_gv = &guidance_v;
489
490struct StabilizationSetpoint guidance_h_run_pos(bool in_flight, struct HorizontalGuidance *gh)
491{
492 return guidance_indi_run_mode(in_flight, gh, _gv, GUIDANCE_INDI_H_POS, _v_mode);
493}
494
495struct StabilizationSetpoint guidance_h_run_speed(bool in_flight, struct HorizontalGuidance *gh)
496{
498}
499
500struct StabilizationSetpoint guidance_h_run_accel(bool in_flight, struct HorizontalGuidance *gh)
501{
503}
504
505struct ThrustSetpoint guidance_v_run_pos(bool in_flight UNUSED, struct VerticalGuidance *gv)
506{
507 _gv = gv;
509 return thrust_sp;
510}
511
512struct ThrustSetpoint guidance_v_run_speed(bool in_flight UNUSED, struct VerticalGuidance *gv)
513{
514 _gv = gv;
516 return thrust_sp;
517}
518
519struct ThrustSetpoint guidance_v_run_accel(bool in_flight UNUSED, struct VerticalGuidance *gv)
520{
521 _gv = gv;
523 return thrust_sp;
524}
525
526#endif
527
Main include for ABI (AirBorneInterface).
struct abi_struct abi_event
Definition abi_common.h:101
Core autopilot interface common to all firmwares.
#define UNUSED(x)
static struct uart_periph * dev
float phi
in radians
float theta
in radians
float psi
in radians
#define FLOAT_EULERS_ZERO(_e)
void float_eulers_of_quat_yxz(struct FloatEulers *e, const struct FloatQuat *q)
euler rotation 'YXZ' This function calculates from a quaternion the Euler angles with the order YXZ,...
void float_quat_of_eulers_yxz(struct FloatQuat *q, const struct FloatEulers *e)
quat from euler rotation 'YXZ' This function calculates a quaternion from Euler angles with the order...
euler angles
Roation quaternion.
#define MAT33_VECT3_MUL(_vout, _mat, _vin)
#define MAT33_INV(_minv, _m)
#define RMAT_ELMT(_rm, _row, _col)
#define POS_FLOAT_OF_BFP(_ai)
#define SPEED_FLOAT_OF_BFP(_ai)
#define ACCEL_FLOAT_OF_BFP(_ai)
static struct NedCoor_f * stateGetAccelNed_f(void)
Get acceleration in NED coordinates (float).
Definition state.h:1214
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition state.h:1325
static struct FloatQuat * stateGetNedToBodyQuat_f(void)
Get vehicle body attitude quaternion (float).
Definition state.h:1313
static struct NedCoor_f * stateGetPositionNed_f(void)
Get position in local NED coordinates (float).
Definition state.h:850
static struct NedCoor_f * stateGetSpeedNed_f(void)
Get ground speed in local NED coordinates (float).
Definition state.h:1060
#define GUIDANCE_INDI_ACCEL_SP_ID
QuatButterworthLowPass quat_filt
float thrust_in
struct StabilizationSetpoint guidance_indi_run_mode(bool in_flight, struct HorizontalGuidance *gh, struct VerticalGuidance *gv, enum GuidanceIndi_HMode h_mode, enum GuidanceIndi_VMode v_mode)
float guidance_indi_max_bank
bool indi_accel_sp_set_3d
float guidance_indi_pos_gain
struct FloatVect3 sp_accel
struct FloatEulers guidance_euler_cmd
struct FloatVect3 speed_sp
float thrust_act
struct FloatMat33 Ga_inv
void guidance_indi_enter(void)
Call upon entering indi guidance.
static void accel_sp_cb(uint8_t sender_id, uint8_t flag, struct FloatVect3 *accel_sp)
ABI callback that obtains the acceleration setpoint from telemetry flag: 0 -> 2D, 1 -> 3D.
abi_event accel_sp_ev
struct ThrustSetpoint thrust_sp
void guidance_indi_init(void)
Init function.
float time_of_accel_sp_3d
float filter_cutoff
#define GUIDANCE_INDI_FILTER_CUTOFF
struct FloatVect3 WEAK guidance_indi_controller(bool in_flight UNUSED, struct HorizontalGuidance *gh, struct VerticalGuidance *gv, enum GuidanceIndi_HMode h_mode, enum GuidanceIndi_VMode v_mode)
bool indi_accel_sp_set_2d
struct FloatVect3 indi_accel_sp
float thrust_vect[3]
struct FloatVect3 control_increment
struct FloatMat33 Ga
Butterworth2LowPass filt_accel_ned[3]
float guidance_indi_speed_gain
float time_of_accel_sp_2d
float thrust_dyn
static float Gmat[GUIDANCE_INDI_NV][GUIDANCE_INDI_NU]
struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accel_sp, float heading_sp)
static void guidance_indi_propagate_filters(void)
Low pass the accelerometer measurements to remove noise from vibrations.
Butterworth2LowPass thrust_filt
static void send_indi_guidance(struct transport_tx *trans, struct link_device *dev)
A guidance mode based on Incremental Nonlinear Dynamic Inversion.
#define GUIDANCE_INDI_NV
float guidance_indi_specific_force_gain
#define GUIDANCE_INDI_NU
GuidanceIndi_VMode
@ GUIDANCE_INDI_V_ACCEL
@ GUIDANCE_INDI_V_SPEED
@ GUIDANCE_INDI_V_POS
GuidanceIndi_HMode
@ GUIDANCE_INDI_H_SPEED
@ GUIDANCE_INDI_H_ACCEL
@ GUIDANCE_INDI_H_POS
void guidance_indi_calcG(float Gmat[GUIDANCE_INDI_NV][GUIDANCE_INDI_NU], const struct FloatQuat *att)
void guidance_indi_set_wls_settings(struct WLS_t *wls, const struct FloatQuat *att, const float heading_sp)
#define GUIDANCE_INDI_WLS_WU
#define GUIDANCE_INDI_POS_GAIN
#define GUIDANCE_INDI_WLS_PRIORITIES
#define GUIDANCE_INDI_SPEED_GAIN
static enum GuidanceOneloop_VMode _v_mode
void guidance_v_run_enter(void)
static struct VerticalGuidance * _gv
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)
struct ThrustSetpoint guidance_v_run_accel(bool in_flight UNUSED, struct VerticalGuidance *gv)
Simple first order low pass filter with bilinear transform.
struct SecondOrderLowPass Butterworth2LowPass
Second order Butterworth low pass filter.
static void init_butterworth_2_low_pass(Butterworth2LowPass *filter, const float tau, const float sample_time, const float value)
Init a second order Butterworth filter.
static float update_butterworth_2_low_pass(Butterworth2LowPass *filter, const float value)
Update second order Butterworth low pass filter state with a new value.
#define MAX_PPRZ
Definition paparazzi.h:8
float z
in meters
float x
in meters
float y
in meters
vector in North East Down coordinates Units: meters
Quaternion second order filter.
static struct FloatQuat update_quat_butterworth_low_pass(QuatButterworthLowPass *filter, const struct FloatQuat quat)
Update second order quaternion Butterworth low pass filter state with a new value.
static void init_quat_butterworth_low_pass(QuatButterworthLowPass *filter, const float omega, const float sample_time, const struct FloatQuat q0)
Init a second order quaternion Butterworth filter.
struct QuatSecondOrderLowPass QuatButterworthLowPass
Second order Butterworth low pass filter.
Generic interface for radio control modules.
Some helper functions to check RC sticks.
Horizontal guidance for rotorcrafts.
void guidance_h_run_enter(void)
struct StabilizationSetpoint guidance_h_run_pos(bool in_flight, struct HorizontalGuidance *gh)
struct StabilizationSetpoint guidance_h_run_speed(bool in_flight, struct HorizontalGuidance *gh)
struct StabilizationSetpoint guidance_h_run_accel(bool in_flight, struct HorizontalGuidance *gh)
#define GUIDANCE_H_MAX_BANK
Max bank controlled by guidance.
Definition guidance_h.h:64
struct VerticalGuidance guidance_v
Definition guidance_v.c:60
Vertical guidance for rotorcrafts.
int32_t z_ref
altitude reference in meters.
Definition guidance_v.h:68
int32_t zd_ref
vertical speed reference in meter/s.
Definition guidance_v.h:74
int32_t zdd_ref
vertical acceleration reference in meter/s^2.
Definition guidance_v.h:80
struct RotorcraftNavigation nav
Definition navigation.c:51
struct Stabilization stabilization
struct ThrustSetpoint th_sp_from_incr_vect_f(float th_increment[3])
struct StabilizationSetpoint stab_sp_from_quat_f(struct FloatQuat *quat)
struct ThrustSetpoint th_sp_from_thrust_i(int32_t thrust, uint8_t axis)
General stabilization interface for rotorcrafts.
#define THRUST_SP_SET_ZERO(_sp)
#define THRUST_AXIS_Z
API to get/set the generic vehicle states.
Stabilization setpoint.
Thrust setpoint // TODO to a setpoint header Structure to store the desired thrust vector with differ...
Architecture independent timing functions.
static float get_sys_time_float(void)
Get the time in seconds since startup.
Definition sys_time.h:241
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
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
signed char int8_t
Typedef defining 8 bit char type.
void wls_alloc(struct WLS_t *WLS_p, float **B, float *u_guess, float *W_init, int imax)
active set algorithm for control allocation
Definition wls_alloc.c:119
float u[WLS_N_U_MAX]
Definition wls_alloc.h:71
float v[WLS_N_V_MAX]
Definition wls_alloc.h:70