Paparazzi UAS  v5.15_devel-88-gb3ad7fe
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces 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 
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 #define INT32_RATIO_FRAC 12
42 
43 struct Int32AttitudeGains gainlibrary[NUMBER_OF_GAINSETS];
44 
45 float scheduling_points[NUMBER_OF_GAINSETS] = SCHEDULING_POINTS;
46 
47 //Get the specified gains in the gainlibrary
49 {
50  int32_t phi_p[NUMBER_OF_GAINSETS] = PHI_P;
51  int32_t phi_d[NUMBER_OF_GAINSETS] = PHI_D;
52  int32_t phi_i[NUMBER_OF_GAINSETS] = PHI_I;
53  int32_t phi_dd[NUMBER_OF_GAINSETS] = PHI_DD;
54 
55  int32_t theta_p[NUMBER_OF_GAINSETS] = THETA_P;
56  int32_t theta_d[NUMBER_OF_GAINSETS] = THETA_D;
57  int32_t theta_i[NUMBER_OF_GAINSETS] = THETA_I;
58  int32_t theta_dd[NUMBER_OF_GAINSETS] = THETA_DD;
59 
60  int32_t psi_p[NUMBER_OF_GAINSETS] = PSI_P;
61  int32_t psi_d[NUMBER_OF_GAINSETS] = PSI_D;
62  int32_t psi_i[NUMBER_OF_GAINSETS] = PSI_I;
63  int32_t psi_dd[NUMBER_OF_GAINSETS] = PSI_DD;
64 
65  for (int i = 0; i < NUMBER_OF_GAINSETS; i++) {
66 
67  struct Int32AttitudeGains swap = {
68  {phi_p[i], theta_p[i], psi_p[i] },
69  {phi_d[i], theta_d[i], psi_d[i] },
70  {phi_dd[i], theta_dd[i], psi_dd[i] },
71  {phi_i[i], theta_i[i], psi_i[i] }
72  };
73 
74  gainlibrary[i] = swap;
75  }
77 }
78 
80 {
81 
82 #if NUMBER_OF_GAINSETS > 1
83  uint8_t section = 0;
84 
85  //Find out between which gainsets to interpolate
86  while (FLOAT_OF_BFP(SCHEDULING_VARIABLE, SCHEDULING_VARIABLE_FRAC) > scheduling_points[section]) {
87  section++;
88  if (section == NUMBER_OF_GAINSETS) { break; }
89  }
90 
91  //Get pointers for the two gainsets and the stabilization_gains
92  struct Int32AttitudeGains *ga, *gb, *gblend;
93 
94  gblend = &stabilization_gains;
95 
96  if (section == 0) {
97  set_gainset(0);
98  } else if (section == NUMBER_OF_GAINSETS) {
99  set_gainset(NUMBER_OF_GAINSETS - 1);
100  } else {
101  ga = &gainlibrary[section - 1];
102  gb = &gainlibrary[section];
103 
104  //Calculate the ratio between the scheduling points
105  int32_t ratio;
106  ratio = BFP_OF_REAL((FLOAT_OF_BFP(SCHEDULING_VARIABLE,
107  SCHEDULING_VARIABLE_FRAC) - scheduling_points[section - 1]) / (scheduling_points[section] -
108  scheduling_points[section - 1]), INT32_RATIO_FRAC);
109 
110  int64_t g1, g2, gbl;
111 
112  //Loop through the gains and interpolate
113  for (uint32_t i = 0; i < (sizeof(struct Int32AttitudeGains) / sizeof(int32_t)); i++) {
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 {
130  stabilization_gains = gainlibrary[gainset];
131 }
void set_gainset(int gainset)
#define INT32_RATIO_FRAC
#define FLOAT_OF_BFP(_vbfp, _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
#define BFP_OF_REAL(_vr, _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
unsigned long uint32_t
Definition: types.h:18
signed long int32_t
Definition: types.h:19
unsigned char uint8_t
Definition: types.h:14
float g2[INDI_NUM_ACT]
float g1[INDI_OUTPUTS][INDI_NUM_ACT]
struct Int32AttitudeGains gainlibrary[NUMBER_OF_GAINSETS]
Paparazzi fixed point algebra.