Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
gain_scheduling.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012 Pranay Sinha <psinha@transition-robotics.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 
27 
28 // #include "state.h"
29 #include "math/pprz_algebra_int.h"
31 
32 #ifndef NUMBER_OF_GAINSETS
33 #error You must define the number of gainsets to use this module!
34 #endif
35 
36 #ifndef SCHEDULING_VARIABLE_FRAC
37 #define SCHEDULING_VARIABLE_FRAC 0
38 #pragma message "SCHEDULING_VARIABLE_FRAC not specified!"
39 #endif
40 
41 #ifndef PHI_FFD
42 #define PHI_FFD {0}
43 #endif
44 
45 #ifndef THETA_FFD
46 #define THETA_FFD {0}
47 #endif
48 
49 #ifndef PSI_FFD
50 #define PSI_FFD {0}
51 #endif
52 
53 #define INT32_RATIO_FRAC 12
54 
55 struct Int32AttitudeGains gainlibrary[NUMBER_OF_GAINSETS];
56 
57 float scheduling_points[NUMBER_OF_GAINSETS] = SCHEDULING_POINTS;
58 
59 //Get the specified gains in the gainlibrary
61 {
62  int32_t phi_p[NUMBER_OF_GAINSETS] = PHI_P;
63  int32_t phi_d[NUMBER_OF_GAINSETS] = PHI_D;
64  int32_t phi_i[NUMBER_OF_GAINSETS] = PHI_I;
65  int32_t phi_dd[NUMBER_OF_GAINSETS] = PHI_DD;
66  int32_t phi_ffd[NUMBER_OF_GAINSETS] = PHI_FFD;
67 
68  int32_t theta_p[NUMBER_OF_GAINSETS] = THETA_P;
69  int32_t theta_d[NUMBER_OF_GAINSETS] = THETA_D;
70  int32_t theta_i[NUMBER_OF_GAINSETS] = THETA_I;
71  int32_t theta_dd[NUMBER_OF_GAINSETS] = THETA_DD;
72  int32_t theta_ffd[NUMBER_OF_GAINSETS] = THETA_FFD;
73 
74  int32_t psi_p[NUMBER_OF_GAINSETS] = PSI_P;
75  int32_t psi_d[NUMBER_OF_GAINSETS] = PSI_D;
76  int32_t psi_i[NUMBER_OF_GAINSETS] = PSI_I;
77  int32_t psi_dd[NUMBER_OF_GAINSETS] = PSI_DD;
78  int32_t psi_ffd[NUMBER_OF_GAINSETS] = PSI_FFD;
79 
80  for (int i = 0; i < NUMBER_OF_GAINSETS; i++) {
81 
82  struct Int32AttitudeGains swap = {
83  {phi_p[i], theta_p[i], psi_p[i] },
84  {phi_d[i], theta_d[i], psi_d[i] },
85  {phi_dd[i], theta_dd[i], psi_dd[i] },
86  {phi_i[i], theta_i[i], psi_i[i] },
87  {phi_ffd[i], theta_ffd[i], psi_ffd[i] }
88  };
89 
90  gainlibrary[i] = swap;
91  }
93 }
94 
96 {
97 
98 #if NUMBER_OF_GAINSETS > 1
99  uint8_t section = 0;
100 
101  //Find out between which gainsets to interpolate
102  while (FLOAT_OF_BFP(SCHEDULING_VARIABLE, SCHEDULING_VARIABLE_FRAC) > scheduling_points[section]) {
103  section++;
104  if (section == NUMBER_OF_GAINSETS) { break; }
105  }
106 
107  //Get pointers for the two gainsets and the stabilization_gains
108  struct Int32AttitudeGains *ga, *gb, *gblend;
109 
110  gblend = &stabilization_gains;
111 
112  if (section == 0) {
113  set_gainset(0);
114  } else if (section == NUMBER_OF_GAINSETS) {
115  set_gainset(NUMBER_OF_GAINSETS - 1);
116  } else {
117  ga = &gainlibrary[section - 1];
118  gb = &gainlibrary[section];
119 
120  //Calculate the ratio between the scheduling points
121  int32_t ratio;
122  ratio = BFP_OF_REAL((FLOAT_OF_BFP(SCHEDULING_VARIABLE,
123  SCHEDULING_VARIABLE_FRAC) - scheduling_points[section - 1]) / (scheduling_points[section] -
124  scheduling_points[section - 1]), INT32_RATIO_FRAC);
125 
126  int64_t g1, g2, gbl;
127 
128  //Loop through the gains and interpolate
129  for (uint32_t i = 0; i < (sizeof(struct Int32AttitudeGains) / sizeof(int32_t)); i++) {
130  g1 = *(((int32_t *) ga) + i);
131  g1 *= (1 << INT32_RATIO_FRAC) - ratio;
132  g2 = *(((int32_t *) gb) + i);
133  g2 *= ratio;
134 
135  gbl = (g1 + g2) >> INT32_RATIO_FRAC;
136 
137  *(((int32_t *) gblend) + i) = (int32_t) gbl;
138  }
139  }
140 #endif
141 }
142 
143 //Set one of the gainsets entirely
144 void set_gainset(int gainset)
145 {
146  stabilization_gains = gainlibrary[gainset];
147 }
#define THETA_FFD
#define PSI_FFD
#define SCHEDULING_VARIABLE_FRAC
float scheduling_points[NUMBER_OF_GAINSETS]
void gain_scheduling_periodic(void)
Periodic function that interpolates between gain sets depending on the scheduling variable.
#define PHI_FFD
struct Int32AttitudeGains gainlibrary[NUMBER_OF_GAINSETS]
#define INT32_RATIO_FRAC
void set_gainset(int gainset)
void gain_scheduling_init(void)
Initialises periodic loop;.
Module that interpolates between gain sets, depending on the scheduling variable.
#define FLOAT_OF_BFP(_vbfp, _frac)
#define BFP_OF_REAL(_vr, _frac)
Paparazzi fixed point algebra.
Generic interface for radio control modules.
struct Int32AttitudeGains stabilization_gains
float g2[INDI_NUM_ACT]
float g1[INDI_OUTPUTS][INDI_NUM_ACT]
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98