Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
control_mixing_heewing.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
25
28#include "generated/modules.h"
31#include "autopilot.h"
32#include "state.h"
39
40// Tilt position in forward flight
41#ifndef CMH_TILT_FORWARD
42#define CMH_TILT_FORWARD 0
43#endif
44
45// Tilt vertical position for hovering
46#ifndef CMH_TILT_VERTICAL
47#define CMH_TILT_VERTICAL 8700
48#endif
49
50// Max tilt differential for yaw
51#ifndef CMH_TILT_DIFF_MAX
52#define CMH_TILT_DIFF_MAX (MAX_PPRZ-CMH_TILT_VERTICAL)
53#endif
54
55// Motor idle position
56#ifndef CMH_MOTOR_IDLE
57#define CMH_MOTOR_IDLE 800
58#endif
59
60// Time for forward transition
61#ifndef CMH_TRANSITION_TIME
62#define CMH_TRANSITION_TIME 4.f
63#endif
64
65// Transition airspeed, set a negative value to disable
66#ifndef CMH_TRANSITION_AIRSPEED
67#define CMH_TRANSITION_AIRSPEED 10.f
68#endif
69
70#define TRANSITION_TO_HOVER false
71#define TRANSITION_TO_FORWARD true
72
73static float transition_ratio;
74static const float transition_increment = 1.f / (CMH_TRANSITION_TIME * PERIODIC_FREQUENCY);
75
76static void transition_run(bool to_forward) {
77 if (to_forward && transition_ratio < 1.f) {
82 }
85 }
86 }
87 } else if (!to_forward && transition_ratio > 0.f) {
88 transition_ratio = 0.f; // immediately switch back to hover
89 }
90 Bound(transition_ratio, 0.f, 1.f);
91}
92
93static int32_t command_from_transition(int32_t hover_cmd, int32_t forward_cmd) {
94 Bound(transition_ratio, 0.f, 1.f);
95 return (int32_t) (hover_cmd + transition_ratio * (forward_cmd - hover_cmd));
96}
97
99{
100 transition_ratio = 0.f; // 0. for hover to 1. for forward flight
101}
102
104{
108 commands[COMMAND_YAW] = 0;
109 commands[COMMAND_TILT] = CMH_TILT_FORWARD;
110 commands[COMMAND_MOTOR_RIGHT] = radio_control_get(RADIO_THROTTLE);
111 commands[COMMAND_MOTOR_LEFT] = radio_control_get(RADIO_THROTTLE);
112 commands[COMMAND_MOTOR_TAIL] = MIN_PPRZ;
113 commands[COMMAND_THRUST] = (commands[COMMAND_MOTOR_RIGHT] + commands[COMMAND_MOTOR_LEFT]) / 2;
114 autopilot.throttle = commands[COMMAND_THRUST];
115}
116
118{
120 commands[COMMAND_TILT] = CMH_TILT_VERTICAL;
126 commands[COMMAND_MOTOR_RIGHT] = actuators_pprz[CMH_ACT_MOTOR_RIGHT];
127 commands[COMMAND_MOTOR_LEFT] = actuators_pprz[CMH_ACT_MOTOR_LEFT];
128 commands[COMMAND_MOTOR_TAIL] = actuators_pprz[CMH_ACT_MOTOR_TAIL];
129 if (autopilot_in_flight()) {
130 commands[COMMAND_YAW] = actuators_pprz[CMH_ACT_YAW];
131 } else {
133 }
134 commands[COMMAND_THRUST] = stabilization.cmd[COMMAND_THRUST];
135 } else {
136 commands[COMMAND_MOTOR_RIGHT] = -MAX_PPRZ;
137 commands[COMMAND_MOTOR_LEFT] = -MAX_PPRZ;
138 commands[COMMAND_MOTOR_TAIL] = -MAX_PPRZ;
139 commands[COMMAND_YAW] = 0;
140 commands[COMMAND_THRUST] = 0;
141 }
142 autopilot.throttle = commands[COMMAND_THRUST];
143}
144
152
172
181
183{
185
186 if (transition_ratio < 0.5f) {
187 struct ThrustSetpoint hover_th_sp = guidance_v_run(autopilot_in_flight());
189 }
191 struct FloatEulers rc_sp = {
192 .phi = STABILIZATION_ATTITUDE_SP_MAX_PHI * radio_control_get(RADIO_ROLL) / MAX_PPRZ,
193 .theta = STABILIZATION_ATTITUDE_SP_MAX_THETA * radio_control_get(RADIO_PITCH) / MAX_PPRZ,
194 .psi = 0.f
195 };
196 struct StabilizationSetpoint stab_sp = stab_sp_from_eulers_f(&rc_sp);
198
201 commands[COMMAND_THRUST] = stabilization.cmd[COMMAND_THRUST];
202 if (autopilot_in_flight()) {
203 if (transition_ratio < 0.5f) {
204 // only hover stabilization
205 commands[COMMAND_MOTOR_RIGHT] = actuators_pprz[CMH_ACT_MOTOR_RIGHT];
206 commands[COMMAND_MOTOR_LEFT] = actuators_pprz[CMH_ACT_MOTOR_LEFT];
207 commands[COMMAND_MOTOR_TAIL] = actuators_pprz[CMH_ACT_MOTOR_TAIL];
208 } else {
209 // blend with plane control
210 commands[COMMAND_MOTOR_RIGHT] = command_from_transition(actuators_pprz[CMH_ACT_MOTOR_RIGHT], stabilization.cmd[COMMAND_THRUST]);
211 commands[COMMAND_MOTOR_LEFT] = command_from_transition(actuators_pprz[CMH_ACT_MOTOR_LEFT], stabilization.cmd[COMMAND_THRUST]);
213 }
214 } else {
215 commands[COMMAND_MOTOR_RIGHT] = stabilization.cmd[COMMAND_THRUST];
216 commands[COMMAND_MOTOR_LEFT] = stabilization.cmd[COMMAND_THRUST];
217 commands[COMMAND_MOTOR_TAIL] = 0;
218 }
220 commands[COMMAND_YAW] = 0;
221 autopilot.throttle = commands[COMMAND_THRUST];
222
223}
224
231
233{
234 if (nav.horizontal_mode == NAV_HORIZONTAL_MODE_ROUTE ||
235 nav.horizontal_mode == NAV_HORIZONTAL_MODE_CIRCLE) {
237
239 if (transition_ratio < 0.8f) {
240 guidance_plane.pitch_cmd = 0.f;
241 }
243 if (transition_ratio < 0.5f) {
245 struct ThrustSetpoint hover_th_sp = guidance_v_run(autopilot_in_flight());
246 struct FloatEulers eulers_sp = { .phi = 0.f , .theta = 0.f, .psi = stateGetNedToBodyEulers_f()->psi };
247 struct StabilizationSetpoint hover_stab_sp = stab_sp_from_eulers_f(&eulers_sp);
248 stabilization_run(autopilot_in_flight(), &hover_stab_sp, &hover_th_sp, stabilization.cmd); // will overwrite COMMAND_THRUST
249 } else {
251 }
252 nav.heading = stateGetNedToBodyEulers_f()->psi; // overwrite nav heading to avoid problems when transition to hover
253
259 if (transition_ratio < 0.5f) {
260 // only hover stabilization
261 commands[COMMAND_MOTOR_RIGHT] = actuators_pprz[CMH_ACT_MOTOR_RIGHT];
262 commands[COMMAND_MOTOR_LEFT] = actuators_pprz[CMH_ACT_MOTOR_LEFT];
263 commands[COMMAND_MOTOR_TAIL] = actuators_pprz[CMH_ACT_MOTOR_TAIL];
264 } else {
265 // blend with plane control
266 commands[COMMAND_MOTOR_RIGHT] = command_from_transition(actuators_pprz[CMH_ACT_MOTOR_RIGHT], stabilization.cmd[COMMAND_THRUST]);
267 commands[COMMAND_MOTOR_LEFT] = command_from_transition(actuators_pprz[CMH_ACT_MOTOR_LEFT], stabilization.cmd[COMMAND_THRUST]);
269 }
270 commands[COMMAND_THRUST] = stabilization.cmd[COMMAND_THRUST];
271 } else {
272 commands[COMMAND_THRUST] = 0;
273 commands[COMMAND_MOTOR_RIGHT] = MIN_PPRZ;
274 commands[COMMAND_MOTOR_LEFT] = MIN_PPRZ;
275 commands[COMMAND_MOTOR_TAIL] = 0;
276 }
277
278 } else {
279 // all other nav modes are in rotorcraft flight mode
284
287 stabilization_run(autopilot_in_flight(), &stab_sp, &th_sp, stabilization.cmd);
288
290 commands[COMMAND_MOTOR_RIGHT] = actuators_pprz[CMH_ACT_MOTOR_RIGHT];
291 commands[COMMAND_MOTOR_LEFT] = actuators_pprz[CMH_ACT_MOTOR_LEFT];
293 if (autopilot_in_flight()) {
294 commands[COMMAND_YAW] = actuators_pprz[CMH_ACT_YAW];
295 } else {
297 }
298 commands[COMMAND_THRUST] = stabilization.cmd[COMMAND_THRUST];
299 } else {
300 commands[COMMAND_MOTOR_RIGHT] = -MAX_PPRZ;
301 commands[COMMAND_MOTOR_LEFT] = -MAX_PPRZ;
302 commands[COMMAND_MOTOR_TAIL] = -MAX_PPRZ;
303 commands[COMMAND_YAW] = 0;
304 commands[COMMAND_THRUST] = 0;
305 }
306 }
307 autopilot.throttle = commands[COMMAND_THRUST];
308}
struct pprz_autopilot autopilot
Global autopilot structure.
Definition autopilot.c:50
bool autopilot_get_motors_on(void)
get motors status
Definition autopilot.c:303
bool autopilot_in_flight(void)
get in_flight flag
Definition autopilot.c:351
Core autopilot interface common to all firmwares.
Hardware independent code for commands handling.
void control_mixing_heewing_nav_enter(void)
void control_mixing_heewing_nav_run(void)
void stabilization_indi_set_wls_settings(void)
Function that sets the u_min, u_max and u_pref if function not elsewhere defined.
static float transition_ratio
void control_mixing_heewing_manual(void)
Direct manual control in plane style flight.
void control_mixing_heewing_attitude_plane(void)
void control_mixing_heewing_init(void)
#define CMH_TILT_FORWARD
#define CMH_TILT_VERTICAL
void control_mixing_heewing_attitude_direct(void)
Stabilization in attitude direct mode.
void control_mixing_heewing_attitude_plane_enter(void)
static int32_t command_from_transition(int32_t hover_cmd, int32_t forward_cmd)
#define CMH_TRANSITION_AIRSPEED
#define CMH_MOTOR_IDLE
#define CMH_TRANSITION_TIME
#define CMH_TILT_DIFF_MAX
static void transition_run(bool to_forward)
void control_mixing_heewing_attitude_direct_enter(void)
#define CMH_ACT_MOTOR_RIGHT
#define CMH_ACT_YAW
#define CMH_ACT_MOTOR_TAIL
#define CMH_ACT_MOTOR_LEFT
#define COMMAND_PITCH
#define COMMAND_ROLL
float psi
in radians
euler angles
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition state.h:1325
static bool stateIsAirspeedValid(void)
test if air speed is available.
Definition state.h:1436
static float stateGetAirspeed_f(void)
Get airspeed (float).
Definition state.h:1615
struct GuidancePlane guidance_plane
Guidance PID structyre.
struct StabilizationSetpoint guidance_plane_attitude_from_nav(bool in_flight UNUSED)
run horizontal control loop for position and speed control
struct ThrustSetpoint guidance_plane_thrust_from_nav(bool in_flight)
run vertical control loop for position and speed control
Guidance controller for planes in rotorcraft firmware using basic PID controller no airspeed control.
Hardware independent API for actuators (servos, motor controllers).
#define MAX_PPRZ
Definition paparazzi.h:8
#define MIN_PPRZ
Definition paparazzi.h:9
Generic interface for radio control modules.
static pprz_t radio_control_get(uint8_t idx)
Get a radio control channel value.
void guidance_h_mode_changed(uint8_t new_mode)
Definition guidance_h.c:128
struct StabilizationSetpoint guidance_h_run(bool in_flight)
Definition guidance_h.c:244
#define GUIDANCE_H_MODE_NAV
Definition guidance_h.h:58
#define GUIDANCE_H_MODE_NONE
Definition guidance_h.h:56
void guidance_v_mode_changed(uint8_t new_mode)
Definition guidance_v.c:133
struct ThrustSetpoint guidance_v_run(bool in_flight)
Guidance vertical run functions.
Definition guidance_v.c:223
#define GUIDANCE_V_MODE_RC_DIRECT
Definition guidance_v.h:36
#define GUIDANCE_V_MODE_NAV
Definition guidance_v.h:40
struct RotorcraftNavigation nav
Definition navigation.c:51
Rotorcraft navigation functions.
#define NAV_HORIZONTAL_MODE_CIRCLE
Definition navigation.h:87
#define NAV_HORIZONTAL_MODE_ROUTE
Definition navigation.h:86
static const ShellCommand commands[]
Definition shell_arch.c:77
struct Stabilization stabilization
void stabilization_mode_changed(uint8_t new_mode, uint8_t submode)
Check mode change.
void stabilization_run(bool in_flight, struct StabilizationSetpoint *sp, struct ThrustSetpoint *thrust, int32_t *cmd)
Call default stabilization control.
struct StabilizationSetpoint stab_sp_from_eulers_f(struct FloatEulers *eulers)
static const float transition_increment
#define TRANSITION_TO_FORWARD
struct ThrustSetpoint th_sp_from_thrust_i(int32_t thrust, uint8_t axis)
#define TRANSITION_TO_HOVER
Transition from 0 to 100%, used for pitch offset of hybrids.
General stabilization interface for rotorcrafts.
#define STABILIZATION_MODE_ATTITUDE
#define STABILIZATION_ATT_SUBMODE_HEADING
Stabilization sub-modes for attitude.
#define THRUST_AXIS_X
#define STABILIZATION_MODE_NONE
Stabilization modes.
struct WLS_t wls_stab_p
void stabilization_attitude_plane_pid_run(bool in_flight, struct StabilizationSetpoint *sp, struct ThrustSetpoint *thrust, int32_t *cmd)
void stabilization_attitude_plane_pid_enter(void)
Basic fixed-wing attitude stabilization in euler float version.
float act_pref[INDI_NUM_ACT]
int16_t actuators_pprz[INDI_NUM_ACT+1]
PPRZ command to each actuator Can be used to directly control actuators from the control algorithm if...
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...
int int32_t
Typedef defining 32 bit int type.