Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
attitude_ref_saturate_naive.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2008-2015 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 
26 #ifndef ATTITUDE_REF_SATURATE_NAIVE_H
27 #define ATTITUDE_REF_SATURATE_NAIVE_H
28 
30 #include "math/pprz_algebra_int.h"
31 
32 struct FloatRefSat {
33  struct FloatRates max_rate;
34  struct FloatRates max_accel;
35 };
36 
37 struct Int32RefSat {
38  struct Int32Rates max_rate;
39  struct Int32Rates max_accel;
40 };
41 
43 #define SATURATE_SPEED_TRIM_ACCEL(_rate, _accel, _max_rate) { \
44  if ((_rate) >= (_max_rate)) { \
45  (_rate) = (_max_rate); \
46  if ((_accel) > 0) { \
47  (_accel) = 0; \
48  } \
49  } \
50  else if ((_rate) <= -(_max_rate)) { \
51  (_rate) = -(_max_rate); \
52  if ((_accel) < 0) { \
53  (_accel) = 0; \
54  } \
55  } \
56  }
57 
58 static inline void attitude_ref_float_saturate_naive(struct FloatRates *rate,
59  struct FloatRates *accel,
60  struct FloatRefSat *sat)
61 {
62  /* saturate angular acceleration */
63  RATES_BOUND_BOX_ABS(*accel, sat->max_accel);
64 
65  /* saturate angular speed and trim accel accordingly */
66  SATURATE_SPEED_TRIM_ACCEL(rate->p, accel->p, sat->max_rate.p);
67  SATURATE_SPEED_TRIM_ACCEL(rate->q, accel->q, sat->max_rate.q);
68  SATURATE_SPEED_TRIM_ACCEL(rate->r, accel->r, sat->max_rate.r);
69 }
70 
71 static inline void attitude_ref_int_saturate_naive(struct Int32Rates *rate,
72  struct Int32Rates *accel,
73  struct Int32RefSat *sat)
74 {
75  /* saturate angular acceleration */
76  RATES_BOUND_BOX_ABS(*accel, sat->max_accel);
77 
78  /* saturate angular speed and trim accel accordingly */
79  SATURATE_SPEED_TRIM_ACCEL(rate->p, accel->p, sat->max_rate.p);
80  SATURATE_SPEED_TRIM_ACCEL(rate->q, accel->q, sat->max_rate.q);
81  SATURATE_SPEED_TRIM_ACCEL(rate->r, accel->r, sat->max_rate.r);
82 }
83 
84 #endif /* ATTITUDE_REF_SATURATE_NAIVE_H */
struct FloatRates max_accel
#define SATURATE_SPEED_TRIM_ACCEL(_rate, _accel, _max_rate)
saturate angular speed and trim accel accordingly
struct Int32Rates max_rate
static void attitude_ref_float_saturate_naive(struct FloatRates *rate, struct FloatRates *accel, struct FloatRefSat *sat)
static void attitude_ref_int_saturate_naive(struct Int32Rates *rate, struct Int32Rates *accel, struct Int32RefSat *sat)
struct FloatRates max_rate
struct Int32Rates max_accel
float q
in rad/s
float p
in rad/s
float r
in rad/s
angular rates
#define RATES_BOUND_BOX_ABS(_v, _v_max)
Definition: pprz_algebra.h:413
int32_t p
in rad/s with INT32_RATE_FRAC
int32_t r
in rad/s with INT32_RATE_FRAC
int32_t q
in rad/s with INT32_RATE_FRAC
angular rates
Paparazzi floating point algebra.
Paparazzi fixed point algebra.