Paparazzi UAS v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
stabilization.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
33#include "modules/core/abi.h"
34#include "autopilot.h"
35#include "state.h"
36
37#if (STABILIZATION_FILTER_COMMANDS_ROLL_PITCH || STABILIZATION_FILTER_COMMANDS_YAW)
39#endif
40
42//int32_t stabilization.cmd[COMMANDS_NB];
43
44#if STABILIZATION_FILTER_CMD_ROLL_PITCH
45#ifndef STABILIZATION_FILTER_CMD_ROLL_CUTOFF
46#define STABILIZATION_FILTER_CMD_ROLL_CUTOFF 20.0
47#endif
48
49#ifndef STABILIZATION_FILTER_CMD_PITCH_CUTOFF
50#define STABILIZATION_FILTER_CMD_PITCH_CUTOFF 20.0
51#endif
52
55#endif
56
57#if STABILIZATION_FILTER_CMD_YAW
58#ifndef STABILIZATION_FILTER_CMD_YAW_CUTOFF
59#define STABILIZATION_FILTER_CMD_YAW_CUTOFF 20.0
60#endif
61
63#endif
64
65#ifndef STABILIZATION_RC_ID
66#define STABILIZATION_RC_ID ABI_BROADCAST
67#endif
71
72#if PERIODIC_TELEMETRY
74
75static void send_tune_hover(struct transport_tx *trans UNUSED, struct link_device *dev UNUSED)
76{
77#if defined(COMMAND_ROLL) && defined(COMMAND_PITCH) && defined(COMMAND_YAW)
87 &(stateGetNedToBodyEulers_i()->theta),
89#endif
90}
91
92#endif
93
123
124#if USE_STABILIZATION_RATE
125static void stabilization_rate_reset_rc(void)
126{
127 struct FloatRates zero = { 0., 0., 0. };
129}
130#endif
131
137
139{
141 return;
142 }
143
144 switch (new_mode) {
146 // nothing to do
147 break;
150 break;
151#if USE_STABILIZATION_RATE
155 break;
156#endif
161 }
163 break;
164 default:
165 break;
166 }
167
170}
171
183
185{
186 switch (stabilization.mode) {
187
190 break;
191#if USE_STABILIZATION_RATE
194 break;
195#endif
197 {
198 switch (stabilization.att_submode) {
201 break;
204 break;
207 break;
208 default:
209 break;
210 }
211 }
212 break;
213 default:
214 break;
215 }
216}
217
220#define TRANSITION_TO_HOVER false
221#define TRANSITION_TO_FORWARD true
222
223#ifndef TRANSITION_TIME
224#define TRANSITION_TIME 3.f
225#endif
226
228
241
242void stabilization_run(bool in_flight, struct StabilizationSetpoint *sp, struct ThrustSetpoint *thrust, int32_t *cmd)
243{
244 switch (stabilization.mode) {
245
247 stabilization_direct_run(in_flight, sp, thrust, cmd);
248 break;
249#if USE_STABILIZATION_RATE
251 stabilization_rate_run(in_flight, sp, thrust, cmd);
252 break;
253#endif
257 } else {
259 }
260 stabilization_attitude_run(in_flight, sp, thrust, cmd);
261#if (STABILIZATION_FILTER_CMD_ROLL_PITCH || STABILIZATION_FILTER_CMD_YAW)
262 if (in_flight) {
264 }
265#endif
266 break;
267 default:
268 break;
269 }
270
271 // store last attitude command
273}
274
276{
277 struct FloatEulers failsafe_sp = {
278 .phi = 0.f,
279 .theta = 0.f,
281 };
283}
284
285// compute sp_euler phi/theta for debugging/telemetry FIXME really needed ?
286/* Rotate horizontal commands to body frame by psi */
288{
289 struct Int32Eulers sp;
294 sp.phi = (-s_psi * vect->x + c_psi * vect->y) >> INT32_TRIG_FRAC;
295 sp.theta = -(c_psi * vect->x + s_psi * vect->y) >> INT32_TRIG_FRAC;
296 sp.psi = heading;
297 return sp;
298}
299
300/* Rotate horizontal commands to body frame by psi */
301static struct FloatEulers stab_sp_rotate_f(struct FloatVect2 *vect, float heading)
302{
303 struct FloatEulers sp;
305 float s_psi = sinf(psi);
306 float c_psi = cosf(psi);
307 sp.phi = -s_psi * vect->x + c_psi * vect->y;
308 sp.theta = -c_psi * vect->x + s_psi * vect->y;
309 sp.psi = heading;
310 return sp;
311}
312
313
330
332{
333 if ((sp->type == STAB_SP_QUAT) || (sp->type == STAB_SP_QUAT_FF_RATE)) {
334 if (sp->format == STAB_SP_INT) {
335 return sp->sp.quat_i;
336 } else {
337 struct Int32Quat quat;
338 QUAT_BFP_OF_REAL(quat, sp->sp.quat_f);
339 return quat;
340 }
341 } else if (sp->type == STAB_SP_EULERS) {
342 if (sp->format == STAB_SP_INT) {
343 struct Int32Quat quat;
344 int32_quat_of_eulers(&quat, &sp->sp.eulers_i);
345 return quat;
346 } else {
347 struct Int32Quat quat;
348 struct Int32Eulers eulers;
349 EULERS_BFP_OF_REAL(eulers, sp->sp.eulers_f);
350 int32_quat_of_eulers(&quat, &eulers);
351 return quat;
352 }
353 } else if (sp->type == STAB_SP_LTP) {
354 if (sp->format == STAB_SP_INT) {
355 struct Int32Quat quat;
356 quat_from_earth_cmd_i(&quat, &sp->sp.ltp_i.vect, sp->sp.ltp_i.heading);
357 return quat;
358 } else {
359 struct FloatQuat quat_f;
360 struct Int32Quat quat_i;
361 quat_from_earth_cmd_f(&quat_f, &sp->sp.ltp_f.vect, sp->sp.ltp_f.heading);
362 QUAT_BFP_OF_REAL(quat_i, quat_f);
363 return quat_i;
364 }
365 } else {
366 // error, rates setpoint
367 struct Int32Quat quat;
368 int32_quat_identity(&quat);
369 return quat;
370 }
371}
372
374{
375 if ((sp->type == STAB_SP_QUAT) || (sp->type == STAB_SP_QUAT_FF_RATE)) {
376 if (sp->format == STAB_SP_FLOAT) {
377 return sp->sp.quat_f;
378 } else {
379 struct FloatQuat quat;
380 QUAT_FLOAT_OF_BFP(quat, sp->sp.quat_i);
381 return quat;
382 }
383 } else if (sp->type == STAB_SP_EULERS) {
384 if (sp->format == STAB_SP_FLOAT) {
385 struct FloatQuat quat;
386 float_quat_of_eulers(&quat, &sp->sp.eulers_f);
387 return quat;
388 } else {
389 struct FloatQuat quat;
390 struct FloatEulers eulers;
391 EULERS_FLOAT_OF_BFP(eulers, sp->sp.eulers_i);
392 float_quat_of_eulers(&quat, &eulers);
393 return quat;
394 }
395 } else if (sp->type == STAB_SP_LTP) {
396 if (sp->format == STAB_SP_FLOAT) {
397 struct FloatQuat quat;
398 quat_from_earth_cmd_f(&quat, &sp->sp.ltp_f.vect, sp->sp.ltp_f.heading);
399 return quat;
400 } else {
401 struct FloatQuat quat_f;
402 struct Int32Quat quat_i;
403 quat_from_earth_cmd_i(&quat_i, &sp->sp.ltp_i.vect, sp->sp.ltp_i.heading);
404 QUAT_FLOAT_OF_BFP(quat_f, quat_i);
405 return quat_f;
406 }
407 } else {
408 // error, rates setpoint
409 struct FloatQuat quat;
410 float_quat_identity(&quat);
411 return quat;
412 }
413}
414
416{
417 if (sp->type == STAB_SP_EULERS) {
418 if (sp->format == STAB_SP_INT) {
419 return sp->sp.eulers_i;
420 } else {
421 struct Int32Eulers eulers;
422 EULERS_BFP_OF_REAL(eulers, sp->sp.eulers_f);
423 return eulers;
424 }
425 } else if ((sp->type == STAB_SP_QUAT) || (sp->type == STAB_SP_QUAT_FF_RATE)) {
426 if (sp->format == STAB_SP_INT) {
427 struct Int32Eulers eulers;
428 int32_eulers_of_quat(&eulers, &sp->sp.quat_i);
429 return eulers;
430 } else {
431 struct Int32Eulers eulers;
432 struct Int32Quat quat;
433 QUAT_BFP_OF_REAL(quat, sp->sp.quat_f);
434 int32_eulers_of_quat(&eulers, &quat);
435 return eulers;
436 }
437 } else if (sp->type == STAB_SP_LTP) {
438 if (sp->format == STAB_SP_INT) {
439 struct Int32Eulers eulers = stab_sp_rotate_i(&sp->sp.ltp_i.vect, sp->sp.ltp_i.heading);
440 return eulers;
441 } else {
442 struct FloatEulers eulers_f = stab_sp_rotate_f(&sp->sp.ltp_f.vect, sp->sp.ltp_f.heading);
443 struct Int32Eulers eulers_i;
444 EULERS_BFP_OF_REAL(eulers_i, eulers_f);
445 return eulers_i;
446 }
447 } else {
448 // error, rates setpoint
449 struct Int32Eulers eulers = {0};
450 return eulers;
451 }
452}
453
455{
456 if (sp->type == STAB_SP_EULERS) {
457 if (sp->format == STAB_SP_FLOAT) {
458 return sp->sp.eulers_f;
459 } else {
460 struct FloatEulers eulers;
461 EULERS_FLOAT_OF_BFP(eulers, sp->sp.eulers_i);
462 return eulers;
463 }
464 } else if ((sp->type == STAB_SP_QUAT) || (sp->type == STAB_SP_QUAT_FF_RATE)) {
465 if (sp->format == STAB_SP_FLOAT) {
466 struct FloatEulers eulers;
467 float_eulers_of_quat(&eulers, &sp->sp.quat_f);
468 return eulers;
469 } else {
470 struct FloatEulers eulers;
471 struct FloatQuat quat;
472 QUAT_FLOAT_OF_BFP(quat, sp->sp.quat_i);
473 float_eulers_of_quat(&eulers, &quat);
474 return eulers;
475 }
476 } else if (sp->type == STAB_SP_LTP) {
477 if (sp->format == STAB_SP_FLOAT) {
478 struct FloatEulers eulers = stab_sp_rotate_f(&sp->sp.ltp_f.vect, sp->sp.ltp_f.heading);
479 return eulers;
480 } else {
481 struct Int32Eulers eulers_i = stab_sp_rotate_i(&sp->sp.ltp_i.vect, sp->sp.ltp_i.heading);
482 struct FloatEulers eulers_f;
483 EULERS_FLOAT_OF_BFP(eulers_f, eulers_i);
484 return eulers_f;
485 }
486 } else {
487 // error, rates setpoint
488 struct FloatEulers eulers = {0, 0, 0};
489 return eulers;
490 }
491}
492
494{
495 if ((sp->type == STAB_SP_RATES) || (sp->type == STAB_SP_QUAT_FF_RATE)) {
496 if (sp->format == STAB_SP_INT) {
497 return sp->r_sp.rates_i;
498 } else {
499 struct Int32Rates rates;
500 RATES_BFP_OF_REAL(rates, sp->r_sp.rates_f);
501 return rates;
502 }
503 } else {
504 // error, attitude setpoint
505 struct Int32Rates rates = {0, 0, 0};
506 return rates;
507 }
508}
509
511{
512 if ((sp->type == STAB_SP_RATES) || (sp->type == STAB_SP_QUAT_FF_RATE)) {
513 if (sp->format == STAB_SP_FLOAT) {
514 return sp->r_sp.rates_f;
515 } else {
516 struct FloatRates rates;
517 RATES_FLOAT_OF_BFP(rates, sp->r_sp.rates_i);
518 return rates;
519 }
520 } else {
521 // error, attitude setpoint
522 struct FloatRates rates = {0, 0, 0};
523 return rates;
524 }
525}
526
528{
529 if (th->type == THRUST_SP) {
530 if (th->format == THRUST_SP_INT) {
531 return th->sp.thrust_i[axis];
532 } else {
533 return (int32_t) (th->sp.thrust_f[axis] * MAX_PPRZ);
534 }
535 } else {
536 if (th->format == THRUST_SP_INT) {
537 return thrust + th->sp.th_incr_i[axis];
538 } else {
539 return thrust + (int32_t)(th->sp.th_incr_f[axis] * MAX_PPRZ);
540 }
541 }
542}
543
545{
546 const float MAX_PPRZ_F = (float) MAX_PPRZ;
547 if (th->type == THRUST_SP) {
548 if (th->format == THRUST_SP_FLOAT) {
549 return th->sp.thrust_f[axis];
550 } else {
551 return (float)th->sp.thrust_i[axis] / MAX_PPRZ_F;
552 }
553 } else {
554 if (th->format == THRUST_SP_FLOAT) {
555 return ((float)thrust / MAX_PPRZ_F) + th->sp.th_incr_f[axis];
556 } else {
557 return (float)(thrust + th->sp.th_incr_f[axis]) / MAX_PPRZ_F;
558 }
559 }
560}
561
563{
564 if (th->type == THRUST_INCR_SP) {
565 if (th->format == THRUST_SP_INT) {
566 return th->sp.th_incr_i[axis];
567 } else {
568 return (int32_t) (th->sp.th_incr_f[axis] * MAX_PPRZ);
569 }
570 } else {
571 if (th->format == THRUST_SP_INT) {
572 return th->sp.thrust_i[axis] - thrust;
573 } else {
574 return (int32_t)(th->sp.thrust_f[axis] * MAX_PPRZ) - thrust;
575 }
576 }
577}
578
580{
581 const float MAX_PPRZ_F = (float) MAX_PPRZ;
582 if (th->type == THRUST_INCR_SP) {
583 if (th->format == THRUST_SP_FLOAT) {
584 return th->sp.th_incr_f[axis];
585 } else {
586 return (float)th->sp.th_incr_i[axis] / MAX_PPRZ_F;
587 }
588 } else {
589 if (th->format == THRUST_SP_FLOAT) {
590 return th->sp.thrust_f[axis] - ((float)thrust / MAX_PPRZ_F);
591 } else {
592 return (float)(th->sp.thrust_i[axis] - thrust) / MAX_PPRZ_F;
593 }
594 }
595}
596
598{
599 struct StabilizationSetpoint sp = {
602 .sp.quat_i = *quat
603 };
604 return sp;
605}
606
608{
609 struct StabilizationSetpoint sp = {
612 .sp.quat_f = *quat
613 };
614 return sp;
615}
616
618{
619 struct StabilizationSetpoint sp = {
622 .sp.quat_f = *quat,
623 .r_sp.rates_f = *rates
624 };
625 return sp;
626}
627
629{
630 struct StabilizationSetpoint sp = {
633 .sp.eulers_i = *eulers
634 };
635 return sp;
636}
637
639{
640 struct StabilizationSetpoint sp = {
643 .sp.eulers_f = *eulers
644 };
645 return sp;
646}
647
649{
650 struct StabilizationSetpoint sp = {
651 .type = STAB_SP_LTP,
653 .sp.ltp_i.vect = *vect,
654 .sp.ltp_i.heading = heading
655 };
656 return sp;
657}
658
660{
661 struct StabilizationSetpoint sp = {
662 .type = STAB_SP_LTP,
664 .sp.ltp_f.vect = *vect,
665 .sp.ltp_f.heading = heading
666 };
667 return sp;
668}
669
671{
672 struct StabilizationSetpoint sp = {
675 .r_sp.rates_i = *rates
676 };
677 return sp;
678}
679
681{
682 struct StabilizationSetpoint sp = {
685 .r_sp.rates_f = *rates
686 };
687 return sp;
688}
689
691{
692 struct ThrustSetpoint sp = {
693 .type = THRUST_SP,
695 };
696 sp.sp.thrust_i[axis] = thrust;
697 return sp;
698}
699
701{
702 struct ThrustSetpoint sp = {
703 .type = THRUST_SP,
705 };
706 sp.sp.thrust_f[axis] = thrust;
707 return sp;
708}
709
711{
712 struct ThrustSetpoint sp = {
715 };
716 sp.sp.th_incr_i[axis] = th_increment;
717 return sp;
718}
719
721{
722 struct ThrustSetpoint sp = {
725 };
726 sp.sp.th_incr_f[axis] = th_increment;
727 return sp;
728}
729
731{
732 struct ThrustSetpoint sp = {
733 .type = THRUST_SP,
735 };
736 sp.sp.thrust_i[0] = thrust[0];
737 sp.sp.thrust_i[1] = thrust[1];
738 sp.sp.thrust_i[2] = thrust[2];
739 return sp;
740}
741
743{
744 struct ThrustSetpoint sp = {
745 .type = THRUST_SP,
747 };
748 sp.sp.thrust_f[0] = thrust[0];
749 sp.sp.thrust_f[1] = thrust[1];
750 sp.sp.thrust_f[2] = thrust[2];
751 return sp;
752}
753
755{
756 struct ThrustSetpoint sp = {
759 };
760 sp.sp.th_incr_i[0] = th_increment[0];
761 sp.sp.th_incr_i[1] = th_increment[1];
762 sp.sp.th_incr_i[2] = th_increment[2];
763 return sp;
764}
765
767{
768 struct ThrustSetpoint sp = {
771 };
772 sp.sp.th_incr_f[0] = th_increment[0];
773 sp.sp.th_incr_f[1] = th_increment[1];
774 sp.sp.th_incr_f[2] = th_increment[2];
775 return sp;
776}
777
Main include for ABI (AirBorneInterface).
Event structure to store callbacks in a linked list.
Definition abi_common.h:67
bool autopilot_in_flight(void)
get in_flight flag
Definition autopilot.c:340
Core autopilot interface common to all firmwares.
#define UNUSED(x)
float phi
in radians
float theta
in radians
float psi
in radians
static void float_quat_identity(struct FloatQuat *q)
initialises a quaternion to identity
void float_quat_of_eulers(struct FloatQuat *q, struct FloatEulers *e)
quat of euler roation 'ZYX'
void float_eulers_of_quat(struct FloatEulers *e, struct FloatQuat *q)
euler rotation 'ZYX'
euler angles
Roation quaternion.
angular rates
#define EULERS_BFP_OF_REAL(_ei, _ef)
#define QUAT_BFP_OF_REAL(_qi, _qf)
#define RATES_BFP_OF_REAL(_ri, _rf)
#define QUAT_FLOAT_OF_BFP(_qf, _qi)
#define RATES_FLOAT_OF_BFP(_rf, _ri)
#define EULERS_FLOAT_OF_BFP(_ef, _ei)
int32_t phi
in rad with INT32_ANGLE_FRAC
int32_t psi
in rad with INT32_ANGLE_FRAC
int32_t theta
in rad with INT32_ANGLE_FRAC
void int32_eulers_of_quat(struct Int32Eulers *e, struct Int32Quat *q)
#define INT32_TRIG_FRAC
static void int32_quat_identity(struct Int32Quat *q)
initialises a quaternion to identity
void int32_quat_of_eulers(struct Int32Quat *q, struct Int32Eulers *e)
Quaternion from Euler angles.
euler angles
Rotation quaternion.
angular rates
static struct Int32Eulers * stateGetNedToBodyEulers_i(void)
Get vehicle body attitude euler angles (int).
Definition state.h:1288
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition state.h:1306
static struct FloatQuat * stateGetNedToBodyQuat_f(void)
Get vehicle body attitude quaternion (float).
Definition state.h:1294
Simple first order low pass filter with bilinear transform.
static void init_second_order_low_pass_int(struct SecondOrderLowPass_int *filter, float cut_off, float Q, float sample_time, int32_t value)
Init second order low pass filter(fixed point version).
static int32_t update_second_order_low_pass_int(struct SecondOrderLowPass_int *filter, int32_t value)
Update second order low pass filter state with a new value(fixed point version).
int32_t i[2]
input history
uint16_t foo
Definition main_demo5.c:58
PRINT_CONFIG_VAR(ONELOOP_ANDI_FILT_CUTOFF)
#define MAX_PPRZ
Definition paparazzi.h:8
#define PPRZ_ITRIG_SIN(_s, _a)
#define PPRZ_ITRIG_COS(_c, _a)
struct RadioControl radio_control
Generic interface for radio control modules.
pprz_t values[RADIO_CONTROL_NB_CHANNEL]
General attitude stabilization interface for rotorcrafts.
void stabilization_attitude_run(bool in_flight, struct StabilizationSetpoint *sp, struct ThrustSetpoint *thrust, int32_t *cmd)
Attitude control run function.
void stabilization_attitude_enter(void)
Attitude control enter function.
struct ThrustSetpoint th_sp_from_thrust_f(float thrust, uint8_t axis)
struct Stabilization stabilization
struct StabilizationSetpoint stab_sp_from_ltp_i(struct Int32Vect2 *vect, int32_t heading)
static struct Int32Eulers stab_sp_rotate_i(struct Int32Vect2 *vect, int32_t heading)
struct ThrustSetpoint th_sp_from_incr_i(int32_t th_increment, uint8_t axis)
struct FloatEulers stab_sp_to_eulers_f(struct StabilizationSetpoint *sp)
void stabilization_mode_changed(uint8_t new_mode, uint8_t submode)
Check mode change.
struct StabilizationSetpoint stab_sp_from_ltp_f(struct FloatVect2 *vect, float heading)
static void stabilization_attitude_reset_rc(void)
void stabilization_filter_commands(void)
Command filter for vibrating airframes.
struct StabilizationSetpoint stab_sp_from_rates_i(struct Int32Rates *rates)
static void rc_cb(uint8_t sender_id UNUSED, struct RadioControl *rc)
static void send_tune_hover(struct transport_tx *trans UNUSED, struct link_device *dev UNUSED)
struct Int32Rates stab_sp_to_rates_i(struct StabilizationSetpoint *sp)
struct ThrustSetpoint th_sp_from_incr_vect_f(float th_increment[3])
struct StabilizationSetpoint stab_sp_from_quat_ff_rates_f(struct FloatQuat *quat, struct FloatRates *rates)
void stabilization_run(bool in_flight, struct StabilizationSetpoint *sp, struct ThrustSetpoint *thrust, int32_t *cmd)
Call default stabilization control.
struct StabilizationSetpoint stab_sp_from_quat_f(struct FloatQuat *quat)
int32_t th_sp_to_incr_i(struct ThrustSetpoint *th, int32_t thrust, uint8_t axis)
struct ThrustSetpoint th_sp_from_thrust_vect_f(float thrust[3])
struct StabilizationSetpoint stab_sp_from_quat_i(struct Int32Quat *quat)
struct ThrustSetpoint th_sp_from_thrust_vect_i(int32_t thrust[3])
static struct FloatEulers stab_sp_rotate_f(struct FloatVect2 *vect, float heading)
struct ThrustSetpoint th_sp_from_incr_vect_i(int32_t th_increment[3])
struct FloatRates stab_sp_to_rates_f(struct StabilizationSetpoint *sp)
struct StabilizationSetpoint stab_sp_from_eulers_f(struct FloatEulers *eulers)
float th_sp_to_incr_f(struct ThrustSetpoint *th, int32_t thrust, uint8_t axis)
static const float transition_increment
struct Int32Quat stab_sp_to_quat_i(struct StabilizationSetpoint *sp)
float th_sp_to_thrust_f(struct ThrustSetpoint *th, int32_t thrust, uint8_t axis)
#define STABILIZATION_RC_ID
struct StabilizationSetpoint stab_sp_from_eulers_i(struct Int32Eulers *eulers)
struct ThrustSetpoint th_sp_from_incr_f(float th_increment, uint8_t axis)
struct Int32Eulers stab_sp_to_eulers_i(struct StabilizationSetpoint *sp)
#define TRANSITION_TIME
struct StabilizationSetpoint WEAK stabilization_attitude_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn, struct RadioControl *rc)
Retrun attitude setpoint from RC as euler angles.
#define TRANSITION_TO_FORWARD
static abi_event rc_ev
struct StabilizationSetpoint stabilization_get_failsafe_sp(void)
Get stabilization setpoint for failsafe.
struct ThrustSetpoint th_sp_from_thrust_i(int32_t thrust, uint8_t axis)
#define TRANSITION_TO_HOVER
Transition from 0 to 100%, used for pitch offset of hybrids.
void stabilization_init(void)
Init function.
static void transition_run(bool to_forward)
struct StabilizationSetpoint stab_sp_from_rates_f(struct FloatRates *rates)
struct FloatQuat stab_sp_to_quat_f(struct StabilizationSetpoint *sp)
int32_t th_sp_to_thrust_i(struct ThrustSetpoint *th, int32_t thrust, uint8_t axis)
General stabilization interface for rotorcrafts.
struct StabilizationSetpoint sp
current attitude setpoint (store for messages)
uint8_t mode
current mode
#define STABILIZATION_ATT_SUBMODE_CARE_FREE
#define STABILIZATION_MODE_ATTITUDE
#define STABILIZATION_ATT_SUBMODE_HEADING
Stabilization sub-modes for attitude.
uint8_t att_submode
current attitude sub-mode
#define STABILIZATION_ATT_SUBMODE_FORWARD
struct StabilizationSetpoint stab_sp_from_eulers_f(struct FloatEulers *eulers)
float transition_ratio
transition percentage for hybrids (0.: hover; 1.: forward)
struct StabilizationSetpoint rc_sp
Keep it ? FIXME.
int32_t cmd[COMMANDS_NB]
output command vector, range from [-MAX_PPRZ:MAX_PPRZ] (store for messages)
#define STABILIZATION_MODE_RATE
#define STAB_SP_SET_EULERS_ZERO(_sp)
#define STABILIZATION_MODE_DIRECT
#define STABILIZATION_MODE_NONE
Stabilization modes.
struct AttitudeRCInput rc_in
RC input.
Stabilization structure.
void quat_from_earth_cmd_f(struct FloatQuat *quat, struct FloatVect2 *cmd, float heading)
void quat_from_earth_cmd_i(struct Int32Quat *quat, struct Int32Vect2 *cmd, int32_t heading)
Quaternion transformation functions.
void stabilization_attitude_reset_care_free_heading(struct AttitudeRCInput *rc_sp)
reset the heading for care-free mode to current heading
void stabilization_attitude_reset_rc_setpoint(struct AttitudeRCInput *rc_sp)
reset to current state
void stabilization_attitude_rc_setpoint_init(struct AttitudeRCInput *rc_sp)
Init rc input.
void stabilization_attitude_read_rc_setpoint_earth_bound(struct AttitudeRCInput *rc_sp, bool in_flight, bool in_carefree, bool coordinated_turn, struct RadioControl *rc)
Read attitude setpoint from RC as quaternion in earth bound frame.
void stabilization_attitude_read_rc_setpoint(struct AttitudeRCInput *rc_sp, bool in_flight, bool in_carefree, bool coordinated_turn, struct RadioControl *rc)
Read attitude setpoint from RC as quaternion Interprets the stick positions as axes.
Read an attitude setpoint from the RC.
struct FloatQuat rc_quat
RC input in quaternion.
float transition_theta_offset
pitch offset for hybrids, add when in forward mode
void stabilization_direct_read_rc(void)
void stabilization_direct_enter(void)
void stabilization_direct_run(bool in_flight UNUSED, struct StabilizationSetpoint *sp UNUSED, struct ThrustSetpoint *thrust UNUSED, int32_t *cmd UNUSED)
Dummy stabilization for rotorcrafts.
void stabilization_rate_enter(void)
struct StabilizationSetpoint stabilization_rate_read_rc(struct RadioControl *rc)
void stabilization_rate_run(bool in_flight, struct StabilizationSetpoint *rate_sp, struct ThrustSetpoint *thrust, int32_t *cmd)
Rate stabilization for rotorcrafts.
API to get/set the generic vehicle states.
#define TRUE
Definition std.h:4
#define FALSE
Definition std.h:5
static const struct usb_device_descriptor dev
Definition usb_ser_hw.c:74
Stabilization setpoint.
@ STAB_SP_RATES
body rates
@ STAB_SP_EULERS
LTP to Body orientation in euler angles.
@ STAB_SP_LTP
banking and heading in LTP (NED) frame
@ STAB_SP_QUAT_FF_RATE
LTP to Body orientation in unit quaternion with precomputed feedforward rates.
@ STAB_SP_QUAT
LTP to Body orientation in unit quaternion.
union StabilizationSetpoint::@278 sp
union StabilizationSetpoint::@279 r_sp
enum StabilizationSetpoint::@277 format
enum StabilizationSetpoint::@276 type
Thrust setpoint // TODO to a setpoint header Structure to store the desired thrust vector with differ...
@ THRUST_SP
absolute thrust setpoint
@ THRUST_INCR_SP
thrust increment
@ THRUST_SP_INT
int is assumed to be normalized in [0:MAX_PPRZ]
@ THRUST_SP_FLOAT
float is assumed to be normalized in [0.:1.]
enum ThrustSetpoint::@282 type
enum ThrustSetpoint::@283 format
union ThrustSetpoint::@284 sp
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_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.
float heading
Definition wedgebug.c:258