Paparazzi UAS  v5.17_devel-24-g2ae834f
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
ctrl_effectiveness_scheduling.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2017 Ewoud Smeur <ewoud_smeur@msn.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 
29 #include "generated/airframe.h"
30 #include "state.h"
32 
33 #if STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
34 #error "You need to use WLS control allocation for this module"
35 #endif
36 
37 #ifndef INDI_FUNCTIONS_RC_CHANNEL
38 #error "You need to define an RC channel to switch between simple and advanced scheduling"
39 #endif
40 
41 static float g1g2_forward[INDI_OUTPUTS][INDI_NUM_ACT] = {FWD_G1_ROLL,
42  FWD_G1_PITCH, FWD_G1_YAW, FWD_G1_THRUST
43  };
44 
45 static float g1g2_hover[INDI_OUTPUTS][INDI_NUM_ACT] = {STABILIZATION_INDI_G1_ROLL,
46  STABILIZATION_INDI_G1_PITCH, STABILIZATION_INDI_G1_YAW, STABILIZATION_INDI_G1_THRUST
47  };
48 
49 static float g2_both[INDI_NUM_ACT] = STABILIZATION_INDI_G2; //scaled by INDI_G_SCALING
50 
51 //Get the specified gains in the gainlibrary
53 {
54  //sum of G1 and G2
55  int8_t i;
56  int8_t j;
57  for (i = 0; i < INDI_OUTPUTS; i++) {
58  for (j = 0; j < INDI_NUM_ACT; j++) {
59  if (i != 2) {
60  g1g2_hover[i][j] = g1g2_hover[i][j] / INDI_G_SCALING;
62  } else {
63  g1g2_forward[i][j] = (g1g2_forward[i][j] + g2_both[j]) / INDI_G_SCALING;
64  g1g2_hover[i][j] = (g1g2_hover[i][j] + g2_both[j]) / INDI_G_SCALING;
65  }
66  }
67  }
68 }
69 
71 {
72  if(radio_control.values[INDI_FUNCTIONS_RC_CHANNEL] > 0) {
74  } else {
76  }
77 
78 #ifdef INDI_THRUST_ON_PITCH_EFF
79  //State prioritization {W Roll, W pitch, W yaw, TOTAL THRUST}
80  if(radio_control.values[INDI_FUNCTIONS_RC_CHANNEL] > 0 && (actuator_state_filt_vect[0] < -7000) && (actuator_state_filt_vect[1] > 7000)) {
81  Bwls[1][2] = INDI_THRUST_ON_PITCH_EFF/INDI_G_SCALING;
82  Bwls[1][3] = INDI_THRUST_ON_PITCH_EFF/INDI_G_SCALING;
83  } else if(radio_control.values[INDI_FUNCTIONS_RC_CHANNEL] > 0 && (actuator_state_filt_vect[0] > 7000) && (actuator_state_filt_vect[1] < -7000)) {
84  Bwls[1][2] = -INDI_THRUST_ON_PITCH_EFF/INDI_G_SCALING;
85  Bwls[1][3] = -INDI_THRUST_ON_PITCH_EFF/INDI_G_SCALING;
86  } else {
87  Bwls[1][2] = 0.0;
88  Bwls[1][3] = 0.0;
89  }
90 #endif
91 
92 }
93 
95 {
96  // Go from transition percentage to ratio
98 
99  int8_t i;
100  int8_t j;
101  for (i = 0; i < INDI_OUTPUTS; i++) {
102  for (j = 0; j < INDI_NUM_ACT; j++) {
103  g1g2[i][j] = g1g2_hover[i][j] * (1.0 - ratio) + g1g2_forward[i][j] * ratio;
104  }
105  }
106 }
107 
109 {
110  float airspeed = stateGetAirspeed_f();
111  struct FloatEulers eulers_zxy;
112  if(airspeed < 6.0) {
114  float pitch_interp = DegOfRad(eulers_zxy.theta);
115  Bound(pitch_interp, -60.0, -30.0);
116  float ratio = (pitch_interp + 30.0)/(-30.);
117 
118  /*pitch*/
119  g1g2[1][0] = g1g2_hover[1][0]*(1-ratio) + -PITCH_EFF_AT_60/1000*ratio;
120  g1g2[1][1] = g1g2_hover[1][1]*(1-ratio) + PITCH_EFF_AT_60/1000*ratio;
121  /*yaw*/
122  g1g2[2][0] = g1g2_hover[2][0]*(1-ratio) + -YAW_EFF_AT_60/1000*ratio;
123  g1g2[2][1] = g1g2_hover[2][1]*(1-ratio) + -YAW_EFF_AT_60/1000*ratio;
124  } else {
125  // calculate squared airspeed
126  Bound(airspeed, 0.0, 30.0);
127  float airspeed2 = airspeed*airspeed;
128 
129  float pitch_eff = CE_PITCH_A + CE_PITCH_B*airspeed2;
130  g1g2[1][0] = -pitch_eff/1000;
131  g1g2[1][1] = pitch_eff/1000;
132 
133  float yaw_eff = CE_YAW_A + CE_YAW_B*airspeed2;
134  g1g2[2][0] = -yaw_eff/1000;
135  g1g2[2][1] = -yaw_eff/1000;
136  }
137 
138  g1g2[0][2] = -actuator_state_filt_vect[2]/1000*SQUARED_ROLL_EFF;
139  g1g2[0][3] = actuator_state_filt_vect[3]/1000*SQUARED_ROLL_EFF;
140  Bound(g1g2[0][2], -30.0/1000, -2.0/1000);
141  Bound(g1g2[0][3], 2.0/1000, 30.0/1000);
142 
143  /*Make pitch gain equal to roll gain for turns forward flight*/
144  if(airspeed > 12.0) {
146  } else {
148  }
149 
150 }
151 
int32_t transition_percentage
Definition: guidance_h.c:92
float g1g2[INDI_OUTPUTS][INDI_NUM_ACT]
struct ReferenceSystem reference_acceleration
static float g1g2_hover[INDI_OUTPUTS][INDI_NUM_ACT]
#define INT32_PERCENTAGE_FRAC
float * Bwls[INDI_OUTPUTS]
static float stateGetAirspeed_f(void)
Get airspeed (float).
Definition: state.h:1407
#define FLOAT_OF_BFP(_vbfp, _frac)
pprz_t values[RADIO_CONTROL_NB_CHANNEL]
Definition: radio_control.h:69
static struct FloatQuat * stateGetNedToBodyQuat_f(void)
Get vehicle body attitude quaternion (float).
Definition: state.h:1131
euler angles
static float g2_both[INDI_NUM_ACT]
float theta
in radians
void ctrl_eff_scheduling_periodic_b(void)
#define INDI_G_SCALING
void ctrl_eff_scheduling_periodic_a(void)
struct RadioControl radio_control
Definition: radio_control.c:30
static float g1g2_forward[INDI_OUTPUTS][INDI_NUM_ACT]
void float_eulers_of_quat_zxy(struct FloatEulers *e, struct FloatQuat *q)
euler rotation 'ZXY' This rotation order is useful if you need 90 deg pitch
API to get/set the generic vehicle states.
void ctrl_eff_scheduling_init(void)
Initialises periodic loop;.
Horizontal guidance for rotorcrafts.
signed char int8_t
Definition: types.h:15
void ctrl_eff_scheduling_periodic(void)
Periodic function that interpolates between gain sets depending on the scheduling variable...
float actuator_state_filt_vect[INDI_NUM_ACT]