Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
guidance_plane.c
Go to the documentation of this file.
1/*
2 * Copyright (C) 2024 Gautier Hattenberger <gautier.hattenberger@enac.fr>
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
27
32#include "generated/airframe.h"
33#include "generated/flight_plan.h"
35#include "state.h"
36
37#ifndef GUIDANCE_PLANE_MAX_BANK
38#define GUIDANCE_PLANE_MAX_BANK RadOfDeg(45.f)
39#endif
40
41#ifndef GUIDANCE_PLANE_MAX_PITCH
42#define GUIDANCE_PLANE_MAX_PITCH RadOfDeg(30.f)
43#endif
44
45#ifndef GUIDANCE_PLANE_MIN_PITCH
46#define GUIDANCE_PLANE_MIN_PITCH RadOfDeg(-20.f)
47#endif
48
49#ifndef GUIDANCE_PLANCE_COURSE_PRE_BANK
50#define GUIDANCE_PLANCE_COURSE_PRE_BANK 1.f
51#endif
52
53#ifndef GUIDANCE_PLANE_MAX_CLIMB
54#define GUIDANCE_PLANE_MAX_CLIMB 2.f
55#endif
56
57#ifndef GUIDANCE_PLANE_PITCH_OF_VZ
58#define GUIDANCE_PLANE_PITCH_OF_VZ RadOfDeg(5.f)
59#endif
60
61#ifndef GUIDANCE_PLANE_PITCH_TRIM
62#define GUIDANCE_PLANE_PITCH_TRIM RadOfDeg(0.f)
63#endif
64
65#ifndef GUIDANCE_PLANE_CLIMB_THROTTLE_INCREMENT
66#define GUIDANCE_PLANE_CLIMB_THROTTLE_INCREMENT 0.1f
67#endif
68
69/*
70 * external variables
71 */
72
74
75/*
76 * internal variables
77 */
78
79static float pitch_sum_err;
80#define GUIDANCE_PLANE_PITCH_MAX_SUM_ERR (RadOfDeg(10.))
81
82static float throttle_sum_err;
83#define GUIDANCE_PLANE_THROTTLE_MAX_SUM_ERR 0.4
84
85#if PERIODIC_TELEMETRY
87
88#endif
89
91{
92 guidance_plane.roll_max_setpoint = GUIDANCE_PLANE_MAX_BANK;
93 guidance_plane.course_setpoint = 0.f;
94 guidance_plane.course_kp = GUIDANCE_PLANE_COURSE_KP;
95 guidance_plane.course_kd = GUIDANCE_PLANE_COURSE_KD;
96 guidance_plane.course_pre_bank_correction = GUIDANCE_PLANCE_COURSE_PRE_BANK;
97 guidance_plane.roll_cmd = 0.f;
98
99 guidance_plane.pitch_max_setpoint = GUIDANCE_PLANE_MAX_PITCH;
100 guidance_plane.pitch_min_setpoint = GUIDANCE_PLANE_MIN_PITCH;
101 guidance_plane.climb_max_setpoint = GUIDANCE_PLANE_MAX_CLIMB;
102 guidance_plane.altitude_setpoint = 0.f;
103 guidance_plane.climb_setpoint = 0.f;
104 guidance_plane.climb_kp = GUIDANCE_PLANE_CLIMB_KP;
108 guidance_plane.p_kp = GUIDANCE_PLANE_PITCH_KP;
109 guidance_plane.p_kd = GUIDANCE_PLANE_PITCH_KD;
110 guidance_plane.p_ki = GUIDANCE_PLANE_PITCH_KI;
111 guidance_plane.t_kp = GUIDANCE_PLANE_THROTTLE_KP;
112 guidance_plane.t_kd = GUIDANCE_PLANE_THROTTLE_KD;
113 guidance_plane.t_ki = GUIDANCE_PLANE_THROTTLE_KI;
114 guidance_plane.cruise_throttle = GUIDANCE_PLANE_NOMINAL_THROTTLE;
115 guidance_plane.pitch_cmd = 0.f;
116 guidance_plane.throttle_cmd = 0;
117
118 pitch_sum_err = 0.f;
119 throttle_sum_err = 0.f;
120
121#if PERIODIC_TELEMETRY
122#endif
123}
124
129{
130 struct FloatEulers att_sp;
131
132 // course control loop
133 if (nav.horizontal_mode == NAV_HORIZONTAL_MODE_NONE || nav.horizontal_mode == NAV_HORIZONTAL_MODE_GUIDED) {
134 guidance_plane.roll_cmd = 0.f;
135 guidance_plane.pitch_cmd = 0.f;
136 } else if (nav.horizontal_mode == NAV_HORIZONTAL_MODE_ATTITUDE) {
137 if (nav.setpoint_mode == NAV_SETPOINT_MODE_QUAT) {
138 struct FloatEulers e;
139 float_eulers_of_quat(&e, &nav.quat);
140 guidance_plane.roll_cmd = e.phi;
141 guidance_plane.pitch_cmd = e.theta;
142 }
143 else {
144 guidance_plane.roll_cmd = nav.roll;
145 guidance_plane.pitch_cmd = nav.pitch;
146 }
147 } else {
148 // update carrot for GCS display and convert ENU float -> NED int
149 // even if sp is changed later
150 // FIXME really needed here ?
151 guidance_h.sp.pos.x = POS_BFP_OF_REAL(nav.carrot.y);
152 guidance_h.sp.pos.y = POS_BFP_OF_REAL(nav.carrot.x);
153
154 switch (nav.setpoint_mode) {
156 guidance_plane.course_setpoint = atan2f(nav.carrot.x - stateGetPositionEnu_f()->x, nav.carrot.y - stateGetPositionNed_f()->y);
157 break;
159 guidance_plane.course_setpoint = atan2f(nav.speed.x, nav.speed.y);
160 break;
162 // not supported, flying towards HOME waypoint
163 guidance_plane.course_setpoint = atan2f(waypoint_get_x(WP_HOME) - stateGetPositionEnu_f()->x,
164 waypoint_get_y(WP_HOME) - stateGetPositionEnu_f()->y);
165 break;
166 default:
167 // nothing to do for other cases at the moment
168 break;
169 }
170 /* final attitude setpoint */
171
172 // Ground path error
173 static float last_err = 0.f;
174 float err = guidance_plane.course_setpoint - stateGetHorizontalSpeedDir_f();
175 NormRadAngle(err);
176 float d_err = err - last_err;
177 last_err = err;
178 NormRadAngle(d_err);
179 //float course_pre_bank = 0.f; // TODO: compute if flying a circle -> pb is that it is coming from nav
180 // guidance_plane.course_pre_bank_correction * course_pre_bank
181
182 guidance_plane.roll_cmd = guidance_plane.course_kp * err + guidance_plane.course_kd * d_err;
183 BoundAbs(guidance_plane.roll_cmd, guidance_plane.roll_max_setpoint);
184 }
185
186 att_sp.phi = guidance_plane.roll_cmd;
187 att_sp.theta = guidance_plane.pitch_cmd;
188 att_sp.psi = stateGetNedToBodyEulers_f()->psi;
189 return stab_sp_from_eulers_f(&att_sp);
190}
191
192
193static void guidance_plane_set_pitch(bool in_flight)
194{
195 static float last_err = 0.f;
196
197 if (!in_flight) {
198 pitch_sum_err = 0.f;
199 }
200
201 // Compute errors
202 float err = guidance_plane.climb_setpoint - stateGetSpeedEnu_f()->z;
203 float d_err = err - last_err;
204 last_err = err;
205
206 if (guidance_plane.p_ki > 0.) {
207 pitch_sum_err += err * (1. / PERIODIC_FREQUENCY);
209 }
210
211 // PID loop + feedforward ctl
212 guidance_plane.pitch_cmd = nav.pitch
213 + guidance_plane.pitch_trim
214 + guidance_plane.pitch_of_vz * guidance_plane.climb_setpoint
215 + guidance_plane.p_kp * err
216 + guidance_plane.p_kd * d_err
218
219}
220
221static void guidance_plane_set_throttle(bool in_flight)
222{
223 static float last_err = 0.;
224
225 if (!in_flight) {
227 }
228
229 // Compute errors
230 float err = guidance_plane.climb_setpoint - stateGetSpeedEnu_f()->z;
231 float d_err = err - last_err;
232 last_err = err;
233
234 if (guidance_plane.t_ki > 0.) {
235 throttle_sum_err += err * (1. / PERIODIC_FREQUENCY);
237 }
238
239 // PID loop + feedforward ctl
240 float th_cmd = guidance_plane.cruise_throttle
241 + guidance_plane.climb_throttle_increment * guidance_plane.climb_setpoint
242 + guidance_plane.t_kp * err
243 + guidance_plane.t_kd * d_err
245
246 guidance_plane.throttle_cmd = TRIM_UPPRZ(MAX_PPRZ * th_cmd);
247
248}
249
254{
255 if (nav.vertical_mode == NAV_VERTICAL_MODE_MANUAL) {
256 guidance_plane.throttle_cmd = (int32_t) nav.throttle;
257 } else {
258 if (nav.vertical_mode == NAV_VERTICAL_MODE_ALT) {
259 guidance_plane.altitude_setpoint = nav.nav_altitude;
260 float alt_err = guidance_plane.altitude_setpoint - stateGetPositionEnu_f()->z;
261 guidance_plane.climb_setpoint = guidance_plane.climb_kp * alt_err;
262 } else if (nav.vertical_mode == NAV_VERTICAL_MODE_CLIMB) {
263 guidance_plane.climb_setpoint = nav.climb;
264 } else {
265 guidance_plane.climb_setpoint = 0.f;
266 }
267 BoundAbs(guidance_plane.climb_setpoint, guidance_plane.climb_max_setpoint);
268
269 guidance_plane_set_pitch(in_flight);
271 }
272
274}
275
277{
278 /* set nav_heading to current heading */
279 //nav.heading = stateGetNedToBodyEulers_f()->psi;
280
281 pitch_sum_err = 0.f;
282 throttle_sum_err = 0.f;
283}
284
#define UNUSED(x)
if(GpsFixValid() &&e_identification_started)
float psi
in radians
euler angles
#define POS_BFP_OF_REAL(_af)
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition state.h:1325
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition state.h:859
static struct NedCoor_f * stateGetPositionNed_f(void)
Get position in local NED coordinates (float).
Definition state.h:850
static float stateGetHorizontalSpeedDir_f(void)
Get dir of horizontal ground speed (float).
Definition state.h:1096
static struct EnuCoor_f * stateGetSpeedEnu_f(void)
Get ground speed in local ENU coordinates (float).
Definition state.h:1069
#define GUIDANCE_PLANE_PITCH_TRIM
#define GUIDANCE_PLANCE_COURSE_PRE_BANK
struct GuidancePlane guidance_plane
Guidance PID structyre.
#define GUIDANCE_PLANE_MAX_PITCH
#define GUIDANCE_PLANE_MAX_BANK
#define GUIDANCE_PLANE_CLIMB_THROTTLE_INCREMENT
#define GUIDANCE_PLANE_PITCH_OF_VZ
void guidance_plane_enter(void)
#define GUIDANCE_PLANE_MAX_CLIMB
#define GUIDANCE_PLANE_THROTTLE_MAX_SUM_ERR
static void guidance_plane_set_pitch(bool in_flight)
void guidance_plane_init(void)
struct StabilizationSetpoint guidance_plane_attitude_from_nav(bool in_flight UNUSED)
run horizontal control loop for position and speed control
static float pitch_sum_err
#define GUIDANCE_PLANE_PITCH_MAX_SUM_ERR
static void guidance_plane_set_throttle(bool in_flight)
struct ThrustSetpoint guidance_plane_thrust_from_nav(bool in_flight)
run vertical control loop for position and speed control
#define GUIDANCE_PLANE_MIN_PITCH
static float throttle_sum_err
Guidance controller for planes in rotorcraft firmware using basic PID controller no airspeed control.
float waypoint_get_x(uint8_t wp_id)
Get X/East coordinate of waypoint in meters.
Definition waypoints.c:124
float waypoint_get_y(uint8_t wp_id)
Get Y/North coordinate of waypoint in meters.
Definition waypoints.c:132
#define TRIM_UPPRZ(pprz)
Definition paparazzi.h:14
#define MAX_PPRZ
Definition paparazzi.h:8
float z
in meters
struct HorizontalGuidance guidance_h
Definition guidance_h.c:45
struct RotorcraftNavigation nav
Definition navigation.c:51
Rotorcraft navigation functions.
#define NAV_SETPOINT_MODE_SPEED
Definition navigation.h:101
#define NAV_VERTICAL_MODE_CLIMB
Definition navigation.h:93
#define NAV_VERTICAL_MODE_MANUAL
Definition navigation.h:92
#define NAV_SETPOINT_MODE_ACCEL
Definition navigation.h:102
#define NAV_HORIZONTAL_MODE_GUIDED
Definition navigation.h:90
#define NAV_HORIZONTAL_MODE_ATTITUDE
Definition navigation.h:88
#define NAV_HORIZONTAL_MODE_NONE
Definition navigation.h:89
#define NAV_SETPOINT_MODE_POS
Nav setpoint modes these modes correspond to submodes defined by navigation routines to tell which se...
Definition navigation.h:100
struct StabilizationSetpoint stab_sp_from_eulers_f(struct FloatEulers *eulers)
struct ThrustSetpoint th_sp_from_thrust_i(int32_t thrust, uint8_t axis)
General stabilization interface for rotorcrafts.
#define THRUST_AXIS_X
API to get/set the generic vehicle states.
Stabilization setpoint.
Thrust setpoint // TODO to a setpoint header Structure to store the desired thrust vector with differ...
Periodic telemetry system header (includes downlink utility and generated code).