Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
ctrl_windtunnel.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2020 Freek van Tienen <freek.v.tienen@gmail.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, see
18  * <http://www.gnu.org/licenses/>.
19  */
20 
28 #include "state.h"
31 #include "subsystems/electrical.h"
32 
33 #ifndef WINDTUNNEL_TO_BODY_PHI
34 #define WINDTUNNEL_TO_BODY_PHI 0
35 #endif
36 
37 #ifndef WINDTUNNEL_TO_BODY_THETA
38 #define WINDTUNNEL_TO_BODY_THETA 0
39 #endif
40 
41 #ifndef WINDTUNNEL_TO_BODY_PSI
42 #define WINDTUNNEL_TO_BODY_PSI 0
43 #endif
44 
47  int rc_roll;
48  int rc_pitch;
49  int rc_yaw;
52 
53 float ctrl_windtunnel_steptime = CTRL_WINDTUNNEL_STEPTIME;
54 struct min_max_ctrl_t ctrl_windtunnel_throttle = {.min = CTRL_WINDTUNNEL_THR_MIN, .max = CTRL_WINDTUNNEL_THR_MAX, .step = CTRL_WINDTUNNEL_THR_STEP};
55 struct min_max_ctrl_t ctrl_windtunnel_flaps = {.min = CTRL_WINDTUNNEL_FLAP_MIN, .max = CTRL_WINDTUNNEL_FLAP_MAX, .step = CTRL_WINDTUNNEL_FLAP_STEP};
56 static float last_time = 0;
57 
58 void ctrl_module_init(void);
59 void ctrl_module_run(bool in_flight);
60 
61 #if PERIODIC_TELEMETRY
63 static void send_windtunnel_meas(struct transport_tx *trans, struct link_device *dev)
64 {
65  struct FloatQuat windtunnel_to_body;
66  struct FloatEulers windtunnel_to_body_e;
68  struct FloatQuat *ned_to_body = stateGetNedToBodyQuat_f();
69  float_quat_comp_inv(&windtunnel_to_body, ned_to_body, rotation);
70  float_eulers_of_quat(&windtunnel_to_body_e, &windtunnel_to_body);
71 
72  float aoa = DegOfRad(windtunnel_to_body_e.theta);
73  float power = electrical.vsupply * electrical.current;
74  pprz_msg_send_WINDTUNNEL_MEAS(trans, dev, AC_ID, &aoa, &air_data.airspeed, &electrical.vsupply, &electrical.current,
75  &power, COMMANDS_NB, stabilization_cmd);
76 }
77 #endif
78 
79 void ctrl_module_init(void)
80 {
85 
86  // Create a rotation to the windtunnel from the body
87  struct FloatEulers windtunnel_to_body_eulers =
89  orientationSetEulers_f(&ctrl_windtunnel.rotation, &windtunnel_to_body_eulers);
90 
91 #if PERIODIC_TELEMETRY
93 #endif
94 }
95 
96 void ctrl_module_run(bool in_flight __attribute__((unused)))
97 {
98  bool done = false;
99  // Increase step in steptime
101  // Increase throttle step if flaps at the end
103  // Only increase step if throttle is not at the end
108  } else {
109  // Finished
110  done = true;
111  }
112  } else {
113  // By default increase flaps
115 
116  // Double the amount of steptime during double transition
119  } else {
121  }
122  }
123  } else if (ctrl_windtunnel.rc_throttle < (MAX_PPRZ / 2)) {
124  // RESET
128  }
129 
130  stabilization_cmd[COMMAND_ROLL] = 0;
131  stabilization_cmd[COMMAND_PITCH] = 0;
132  stabilization_cmd[COMMAND_YAW] = 0;
133  stabilization_cmd[COMMAND_THRUST] = (done) ? 0 : ctrl_windtunnel_throttle.current;
134  stabilization_cmd[COMMAND_FLAPS] = (done) ? 0 : ctrl_windtunnel_flaps.current;
135 }
136 
137 
139 // Call our controller
140 // Implement own Horizontal loops
142 {
144 }
145 
147 {
149 }
150 
152 {
153  // -MAX_PPRZ to MAX_PPRZ
158 }
159 
160 void guidance_h_module_run(bool in_flight)
161 {
162  // Call full inner-/outerloop / horizontal-/vertical controller:
163  ctrl_module_run(in_flight);
164 }
165 
167 {
168  // initialization of your custom vertical controller goes here
169 }
170 
171 // Implement own Vertical loops
173 {
174  // your code that should be executed when entering this vertical mode goes here
175 }
176 
177 void guidance_v_module_run(UNUSED bool in_flight)
178 {
179  // your vertical controller goes here
180 }
radio_control.h
electrical.h
MAX_PPRZ
#define MAX_PPRZ
Definition: paparazzi.h:8
OrientationReps
Definition: pprz_orientation_conversion.h:79
air_data
struct AirData air_data
global AirData state
Definition: air_data.c:39
min_max_ctrl_t::step
float step
Definition: ctrl_windtunnel.h:36
RADIO_ROLL
#define RADIO_ROLL
Definition: intermcu_ap.h:41
ctrl_windtunnel_throttle
struct min_max_ctrl_t ctrl_windtunnel_throttle
Definition: ctrl_windtunnel.c:54
guidance_v_module_run
void guidance_v_module_run(UNUSED bool in_flight)
Definition: ctrl_windtunnel.c:177
min_max_ctrl_t::max
float max
Definition: ctrl_windtunnel.h:35
guidance_h_module_read_rc
void guidance_h_module_read_rc(void)
Definition: ctrl_windtunnel.c:151
get_sys_time_float
static float get_sys_time_float(void)
Get the time in seconds since startup.
Definition: sys_time.h:129
ctrl_windtunnel.h
Windtunnel controller.
ctrl_windtunnel_struct::rc_pitch
int rc_pitch
Definition: ctrl_windtunnel.c:48
ctrl_windtunnel_flaps
struct min_max_ctrl_t ctrl_windtunnel_flaps
Definition: ctrl_windtunnel.c:55
guidance_h_module_init
void guidance_h_module_init(void)
Definition: ctrl_windtunnel.c:141
WINDTUNNEL_TO_BODY_PHI
#define WINDTUNNEL_TO_BODY_PHI
Definition: ctrl_windtunnel.c:34
UNUSED
uint8_t last_wp UNUSED
Definition: navigation.c:96
Electrical::vsupply
float vsupply
supply voltage in V
Definition: electrical.h:45
ctrl_module_init
void ctrl_module_init(void)
Definition: ctrl_windtunnel.c:79
guidance_v_module_init
void guidance_v_module_init(void)
Definition: ctrl_windtunnel.c:166
FloatEulers::theta
float theta
in radians
Definition: pprz_algebra_float.h:86
telemetry.h
FloatQuat
Roation quaternion.
Definition: pprz_algebra_float.h:63
ctrl_windtunnel_struct
Definition: ctrl_windtunnel.c:45
orientationGetQuat_f
static struct FloatQuat * orientationGetQuat_f(struct OrientationReps *orientation)
Get vehicle body attitude quaternion (float).
Definition: pprz_orientation_conversion.h:225
stateGetNedToBodyQuat_f
static struct FloatQuat * stateGetNedToBodyQuat_f(void)
Get vehicle body attitude quaternion (float).
Definition: state.h:1131
guidance_h_module_run
void guidance_h_module_run(bool in_flight)
Definition: ctrl_windtunnel.c:160
guidance_v_module_enter
void guidance_v_module_enter(void)
Definition: ctrl_windtunnel.c:172
ctrl_windtunnel_steptime
float ctrl_windtunnel_steptime
Definition: ctrl_windtunnel.c:53
dev
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
AirData::airspeed
float airspeed
Equivalent Air Speed (equals to Calibrated Air Speed at low speed/altitude) (in m/s,...
Definition: air_data.h:42
min_max_ctrl_t::min
float min
Definition: ctrl_windtunnel.h:34
ctrl_windtunnel_struct::rc_roll
int rc_roll
Definition: ctrl_windtunnel.c:47
register_periodic_telemetry
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46
ctrl_windtunnel_struct::rc_throttle
int rc_throttle
Definition: ctrl_windtunnel.c:46
ctrl_windtunnel
struct ctrl_windtunnel_struct ctrl_windtunnel
orientationSetEulers_f
static void orientationSetEulers_f(struct OrientationReps *orientation, struct FloatEulers *eulers)
Set vehicle body attitude from euler angles (float).
Definition: pprz_orientation_conversion.h:189
last_time
static float last_time
Definition: ctrl_windtunnel.c:56
send_windtunnel_meas
static void send_windtunnel_meas(struct transport_tx *trans, struct link_device *dev)
Definition: ctrl_windtunnel.c:63
ctrl_windtunnel_struct::rc_yaw
int rc_yaw
Definition: ctrl_windtunnel.c:49
min_max_ctrl_t
Definition: ctrl_windtunnel.h:33
ctrl_windtunnel_struct::rotation
struct OrientationReps rotation
Definition: ctrl_windtunnel.c:50
RADIO_THROTTLE
#define RADIO_THROTTLE
Definition: intermcu_ap.h:40
WINDTUNNEL_TO_BODY_PSI
#define WINDTUNNEL_TO_BODY_PSI
Definition: ctrl_windtunnel.c:42
guidance_h_module_enter
void guidance_h_module_enter(void)
Definition: ctrl_windtunnel.c:146
min_max_ctrl_t::current
float current
Definition: ctrl_windtunnel.h:37
ctrl_module_run
void ctrl_module_run(bool in_flight)
Definition: ctrl_windtunnel.c:96
stabilization_cmd
int32_t stabilization_cmd[COMMANDS_NB]
Stabilization commands.
Definition: stabilization.c:32
RADIO_YAW
#define RADIO_YAW
Definition: intermcu_ap.h:43
FloatEulers
euler angles
Definition: pprz_algebra_float.h:84
electrical
struct Electrical electrical
Definition: electrical.c:66
stabilization.h
state.h
float_eulers_of_quat
void float_eulers_of_quat(struct FloatEulers *e, struct FloatQuat *q)
euler rotation 'ZYX'
Definition: pprz_algebra_float.c:650
WINDTUNNEL_TO_BODY_THETA
#define WINDTUNNEL_TO_BODY_THETA
Definition: ctrl_windtunnel.c:38
RADIO_PITCH
#define RADIO_PITCH
Definition: intermcu_ap.h:42
DefaultPeriodic
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
Electrical::current
float current
current in A
Definition: electrical.h:46
radio_control
struct RadioControl radio_control
Definition: radio_control.c:30
RadioControl::values
pprz_t values[RADIO_CONTROL_NB_CHANNEL]
Definition: radio_control.h:69
float_quat_comp_inv
void float_quat_comp_inv(struct FloatQuat *a2b, struct FloatQuat *a2c, struct FloatQuat *b2c)
Composition (multiplication) of two quaternions.
Definition: pprz_algebra_float.c:328