Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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
25
27
28// #include "state.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
55struct Int32AttitudeGains gainlibrary[NUMBER_OF_GAINSETS];
56
57float 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,
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
144void set_gainset(int gainset)
145{
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.
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.