Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
guidance_indi_hinf.c
Go to the documentation of this file.
1/*
2 * Copyright (C) Gautier Hattenberger, Mohamad Hachem
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, see
18 * <http://www.gnu.org/licenses/>.
19 */
20
23#include "state.h"
24
25// proportional control part (horizontal position)
26static float Ap = GUIDANCE_INDI_HINF_Ap;
27static float Bp = GUIDANCE_INDI_HINF_Bp;
28static float Cp = GUIDANCE_INDI_HINF_Cp;
29static float Dp = GUIDANCE_INDI_HINF_Dp;
30// derivative control part (horizontal speed)
31static float Ad = GUIDANCE_INDI_HINF_Ad;
32static float Bd = GUIDANCE_INDI_HINF_Bd;
33static float Cd = GUIDANCE_INDI_HINF_Cd;
34static float Dd = GUIDANCE_INDI_HINF_Dd;
35// proportional control part (vertical position)
36static float Apz = GUIDANCE_INDI_HINF_Apz;
37static float Bpz = GUIDANCE_INDI_HINF_Bpz;
38static float Cpz = GUIDANCE_INDI_HINF_Cpz;
39static float Dpz = GUIDANCE_INDI_HINF_Dpz;
40// derivative control part (vertical speed)
41static float Adz = GUIDANCE_INDI_HINF_Adz;
42static float Bdz = GUIDANCE_INDI_HINF_Bdz;
43static float Cdz = GUIDANCE_INDI_HINF_Cdz;
44static float Ddz = GUIDANCE_INDI_HINF_Ddz;
45
46static struct FloatVect3 pos_state = { 0 };
47static struct FloatVect3 speed_state = { 0 };
48
51struct FloatVect3 guidance_indi_controller(bool in_flight, struct HorizontalGuidance *gh, struct VerticalGuidance *gv, enum GuidanceIndi_HMode h_mode, enum GuidanceIndi_VMode v_mode)
52{
53 struct FloatVect3 pos_err = { 0 };
54 struct FloatVect3 speed_err = { 0 };
55
56 struct FloatVect3 accel_sp = { 0 };
57 struct FloatVect3 speed_sp = { 0 };
58 struct FloatVect3 speed_fb = { 0 };
59
60 if (!in_flight) {
63 // TODO return with no control ?
64 }
65
66 if (h_mode == GUIDANCE_INDI_H_ACCEL) {
67 // Speed feedback is included in the guidance when running in ACCEL mode
68 speed_fb.x = 0.f;
69 speed_fb.y = 0.f;
70 }
71 else {
72 // Generate speed feedback for acceleration, as it is estimated
73 if (h_mode == GUIDANCE_INDI_H_SPEED) {
74 speed_sp.x = SPEED_FLOAT_OF_BFP(gh->ref.speed.x);
75 speed_sp.y = SPEED_FLOAT_OF_BFP(gh->ref.speed.y);
76 }
77 else {
78 // H_POS
79 // speed setpoint from position error
80 pos_err.x = POS_FLOAT_OF_BFP(gh->ref.pos.x) - stateGetPositionNed_f()->x;
81 pos_err.y = POS_FLOAT_OF_BFP(gh->ref.pos.y) - stateGetPositionNed_f()->y;
82 speed_sp.x = Cp * pos_state.x + Dp * pos_err.x;
83 speed_sp.y = Cp * pos_state.y + Dp * pos_err.y;
84 pos_state.x = Ap * pos_state.x + Bd * pos_err.x;
85 pos_state.y = Ap * pos_state.y + Bp * pos_err.y;
86
87 // TODO where should we add the feed-forward ref speed ?
88 //speed_sp.x += SPEED_FLOAT_OF_BFP(gh->ref.speed.x);
89 //speed_sp.y += SPEED_FLOAT_OF_BFP(gh->ref.speed.y);
90 }
91 speed_err.x = speed_sp.x - stateGetSpeedNed_f()->x;
92 speed_err.y = speed_sp.y - stateGetSpeedNed_f()->y;
93 speed_fb.x = Cd * speed_state.x + Dd * speed_err.x;
94 speed_fb.y = Cd * speed_state.y + Dd * speed_err.y;
95 speed_state.x = Ad * speed_state.x + Bd * speed_err.x;
96 speed_state.y = Ad * speed_state.y + Bd * speed_err.y;
97 }
98
99 if (v_mode == GUIDANCE_INDI_V_ACCEL) {
100 // Speed feedback is included in the guidance when running in ACCEL mode
101 speed_fb.z = 0;
102 }
103 else {
104 // Generate speed feedback for acceleration, as it is estimated
105 if (v_mode == GUIDANCE_INDI_V_SPEED) {
107 }
108 else {
109 // V_POS
110 // vertical speed setpoint from altitude error
111 pos_err.z = POS_FLOAT_OF_BFP(gv->z_ref) - stateGetPositionNed_f()->z;
112 speed_sp.z = Cpz * pos_state.z + Dpz * pos_err.z;
113 pos_state.z = Apz * pos_state.z + Bpz * pos_err.z;
114 // TODO add speed feed-forward ?
115 // speed_sp.z += SPEED_FLOAT_OF_BFP(gv->zd_ref);
116 }
117 speed_err.z = speed_sp.z - stateGetSpeedNed_f()->z;
118 speed_fb.z = Cdz * speed_state.z + Ddz * speed_err.z;
119 speed_state.z = Adz * speed_state.z + Bdz * speed_err.z;
120 }
121
122 // TODO add accel feed-forward term ?
123 accel_sp.x = speed_fb.x; // + ACCEL_FLOAT_OF_BFP(gh->ref.accel.x);
124 accel_sp.y = speed_fb.y; // + ACCEL_FLOAT_OF_BFP(gh->ref.accel.y);
125 accel_sp.z = speed_fb.z; // + ACCEL_FLOAT_OF_BFP(gv->zdd_ref);
126
127 return accel_sp;
128}
129
130
#define FLOAT_VECT3_ZERO(_v)
#define POS_FLOAT_OF_BFP(_ai)
#define SPEED_FLOAT_OF_BFP(_ai)
static struct NedCoor_f * stateGetPositionNed_f(void)
Get position in local NED coordinates (float).
Definition state.h:850
static struct NedCoor_f * stateGetSpeedNed_f(void)
Get ground speed in local NED coordinates (float).
Definition state.h:1060
struct FloatVect3 speed_sp
struct FloatVect3 WEAK guidance_indi_controller(bool in_flight UNUSED, struct HorizontalGuidance *gh, struct VerticalGuidance *gv, enum GuidanceIndi_HMode h_mode, enum GuidanceIndi_VMode v_mode)
A guidance mode based on Incremental Nonlinear Dynamic Inversion.
GuidanceIndi_VMode
@ GUIDANCE_INDI_V_ACCEL
@ GUIDANCE_INDI_V_SPEED
GuidanceIndi_HMode
@ GUIDANCE_INDI_H_SPEED
@ GUIDANCE_INDI_H_ACCEL
static struct FloatVect3 pos_state
static float Ad
static float Dp
static struct FloatVect3 speed_state
static float Adz
static float Cp
static float Bd
static float Ap
static float Bdz
static float Bp
static float Cdz
static float Cpz
static float Apz
static float Cd
static float Ddz
static float Dpz
static float Bpz
static float Dd
Paparazzi floating point algebra.
float z
in meters
float x
in meters
float y
in meters
int32_t z_ref
altitude reference in meters.
Definition guidance_v.h:68
int32_t zd_ref
vertical speed reference in meter/s.
Definition guidance_v.h:74
API to get/set the generic vehicle states.