Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
scheduling_indi_simple.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 
30 #include "generated/airframe.h"
31 #include "state.h"
32 #define INDI_SCHEDULING_LOWER_BOUND_G1 0.0001
33 
35 
36 static float g_forward[3] = {STABILIZATION_INDI_FORWARD_G1_P, STABILIZATION_INDI_FORWARD_G1_Q, STABILIZATION_INDI_FORWARD_G1_R};
37 
38 static float g_hover[3] = {STABILIZATION_INDI_G1_P, STABILIZATION_INDI_G1_Q, STABILIZATION_INDI_G1_R};
39 
40 //Get the specified gains in the gainlibrary
42 {
43 }
44 
46 {
47  // Go from transition percentage to ratio
48  /*float ratio = FLOAT_OF_BFP(transition_percentage, INT32_PERCENTAGE_FRAC) / 100;*/
49  float ratio = 0.0;
50 
51  // Ratio is only based on pitch now, as the pitot tube is often not mounted.
52  if (use_scheduling == 1) {
53  ratio = fabs(stateGetNedToBodyEulers_f()->theta) / M_PI_2;
54  } else {
55  ratio = 0.0;
56  }
57 
58  //g_forward[0] = STABILIZATION_INDI_FORWARD_G1_P;
59  // When using the pitch slider to take the props out of the mix, adjust the pitch effectiveness
60  //g_forward[1] = STABILIZATION_INDI_FORWARD_G1_Q - (1.0 - pitch_slider) * STABILIZATION_INDI_MOT_PITCH_EFF;
61  // When using the yaw slider to take the props out of the mix, adjust the yaw effectiveness
62  //g_forward[2] = STABILIZATION_INDI_FORWARD_G1_R - (1.0 - yaw_slider) * STABILIZATION_INDI_MOT_YAW_EFF;
63 
64  indi.g1.p = g_hover[0] * (1.0 - ratio) + g_forward[0] * ratio;
65  indi.g1.q = g_hover[1] * (1.0 - ratio) + g_forward[1] * ratio;
66  indi.g1.r = g_hover[2] * (1.0 - ratio) + g_forward[2] * ratio;
67 
68  // Make sure g1 can never be negative, as the inverse is taken!
71  }
74  }
77  }
78 
79  float ratio_spec_force = 0.0;
80  float airspeed = stateGetAirspeed_f();
81  Bound(airspeed, 8.0, 20.0);
82  ratio_spec_force = (airspeed-8.0) / 12.0;
83  guidance_indi_specific_force_gain = GUIDANCE_INDI_SPECIFIC_FORCE_GAIN * (1.0 - ratio_spec_force)
84  + GUIDANCE_INDI_SPECIFIC_FORCE_GAIN_FWD * ratio_spec_force;
85 }
float q
in rad/s
float p
in rad/s
float r
in rad/s
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
static float stateGetAirspeed_f(void)
Get airspeed (float).
Definition: state.h:1407
float guidance_indi_specific_force_gain
A guidance mode based on Incremental Nonlinear Dynamic Inversion Come to ICRA2016 to learn more!
Horizontal guidance for rotorcrafts.
int32_t use_scheduling
void ctrl_eff_scheduling_periodic(void)
Periodic function that interpolates between gain sets depending on the scheduling variable.
#define INDI_SCHEDULING_LOWER_BOUND_G1
static float g_forward[3]
static float g_hover[3]
void ctrl_eff_scheduling_init(void)
Initialises periodic loop;.
struct IndiVariables indi
struct FloatRates g1
API to get/set the generic vehicle states.
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83