Paparazzi UAS  v5.0.5_stable-7-g4b8bbb7
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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 
26 #include "gain_scheduling.h"
27 
28 //Include for scheduling on transition_status
31 
32 // #include "state.h"
33 #include "math/pprz_algebra_int.h"
34 
35 #ifndef NUMBER_OF_GAINSETS
36 #error You must define the number of gainsets to use this module!
37 #endif
38 
39 #ifndef SCHEDULING_VARIABLE_FRAC
40 #define SCHEDULING_VARIABLE_FRAC 0
41 #pragma message "SCHEDULING_VARIABLE_FRAC not specified!"
42 #endif
43 
44 #define INT32_RATIO_FRAC 12
45 
46 struct Int32AttitudeGains gainlibrary[NUMBER_OF_GAINSETS];
47 
48 float scheduling_points[NUMBER_OF_GAINSETS] = SCHEDULING_POINTS;
49 
50 //Get the specified gains in the gainlibrary
52  int32_t phi_p[NUMBER_OF_GAINSETS] = PHI_P;
53  int32_t phi_d[NUMBER_OF_GAINSETS] = PHI_D;
54  int32_t phi_i[NUMBER_OF_GAINSETS] = PHI_I;
55  int32_t phi_dd[NUMBER_OF_GAINSETS] = PHI_DD;
56 
57  int32_t theta_p[NUMBER_OF_GAINSETS] = THETA_P;
58  int32_t theta_d[NUMBER_OF_GAINSETS] = THETA_D;
59  int32_t theta_i[NUMBER_OF_GAINSETS] = THETA_I;
60  int32_t theta_dd[NUMBER_OF_GAINSETS] = THETA_DD;
61 
62  int32_t psi_p[NUMBER_OF_GAINSETS] = PSI_P;
63  int32_t psi_d[NUMBER_OF_GAINSETS] = PSI_D;
64  int32_t psi_i[NUMBER_OF_GAINSETS] = PSI_I;
65  int32_t psi_dd[NUMBER_OF_GAINSETS] = PSI_DD;
66 
67  for(int i = 0; i<NUMBER_OF_GAINSETS; i++) {
68 
69  struct Int32AttitudeGains swap = {
70  {phi_p[i], theta_p[i], psi_p[i] },
71  {phi_d[i], theta_d[i], psi_d[i] },
72  {phi_dd[i], theta_dd[i], psi_dd[i] },
73  {phi_i[i], theta_i[i], psi_i[i] }
74  };
75 
76  gainlibrary[i]=swap;
77  }
79 }
80 
82 
83 #if NUMBER_OF_GAINSETS > 1
84  uint8_t section = 0;
85 
86  //Find out between which gainsets to interpolate
87  while(FLOAT_OF_BFP(SCHEDULING_VARIABLE,SCHEDULING_VARIABLE_FRAC) > scheduling_points[section]) {
88  section++;
89  if(section == NUMBER_OF_GAINSETS) break;
90  }
91 
92  //Get pointers for the two gainsets and the stabilization_gains
93  struct Int32AttitudeGains *ga, *gb, *gblend;
94 
95  gblend = &stabilization_gains;
96 
97  if (section == 0)
98  set_gainset(0);
99  else if (section == NUMBER_OF_GAINSETS)
100  set_gainset(NUMBER_OF_GAINSETS-1);
101  else {
102  ga = &gainlibrary[section-1];
103  gb = &gainlibrary[section];
104 
105  //Calculate the ratio between the scheduling points
106  int32_t ratio;
107  ratio = BFP_OF_REAL((FLOAT_OF_BFP(SCHEDULING_VARIABLE,SCHEDULING_VARIABLE_FRAC) - scheduling_points[section-1])/(scheduling_points[section] - scheduling_points[section-1]),INT32_RATIO_FRAC);
108 
109  int64_t g1, g2, gbl;
110 
111  //Loop through the gains and interpolate
112  for (int i=0; i < (sizeof(struct Int32AttitudeGains)/sizeof(int32_t)); i++)
113  {
114  g1 = *(((int32_t*) ga) + i);
115  g1 *= (1<<INT32_RATIO_FRAC) - ratio;
116  g2 = *(((int32_t*) gb) + i);
117  g2 *= ratio;
118 
119  gbl = (g1 + g2) >> INT32_RATIO_FRAC;
120 
121  *(((int32_t*) gblend) + i) = (int32_t) gbl;
122  }
123  }
124 #endif
125 }
126 
127 //Set one of the gainsets entirely
128 void set_gainset(int gainset) {
129  stabilization_gains = gainlibrary[gainset];
130 }
void set_gainset(int gainset)
#define INT32_RATIO_FRAC
float scheduling_points[NUMBER_OF_GAINSETS]
signed long long int64_t
Definition: types.h:21
void gain_scheduling_init(void)
Initialises periodic loop;.
#define SCHEDULING_VARIABLE_FRAC
void gain_scheduling_periodic(void)
Periodic function that interpolates between gain sets depending on the scheduling variable...
Module that interpolates between gain sets, depending on the scheduling variable. ...
struct Int32AttitudeGains stabilization_gains
#define BFP_OF_REAL(_vr, _frac)
signed long int32_t
Definition: types.h:19
unsigned char uint8_t
Definition: types.h:14
General stabilization interface for rotorcrafts.
Horizontal guidance for rotorcrafts.
#define FLOAT_OF_BFP(_vbfp, _frac)
struct Int32AttitudeGains gainlibrary[NUMBER_OF_GAINSETS]
Paparazzi fixed point algebra.