Paparazzi UAS  v5.18.0_stable-1-g6993852-dirty
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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
102  if ((ctrl_windtunnel_flaps.current + ctrl_windtunnel_flaps.step) > ctrl_windtunnel_flaps.max) {
103  // Only increase step if throttle is not at the end
104  if ((ctrl_windtunnel_throttle.current + ctrl_windtunnel_throttle.step) <= ctrl_windtunnel_throttle.max) {
105  ctrl_windtunnel_throttle.current += ctrl_windtunnel_throttle.step;
106  ctrl_windtunnel_flaps.current = ctrl_windtunnel_flaps.min;
108  } else {
109  // Finished
110  done = true;
111  }
112  } else {
113  // By default increase flaps
114  ctrl_windtunnel_flaps.current += ctrl_windtunnel_flaps.step;
115 
116  // Double the amount of steptime during double transition
117  if ((ctrl_windtunnel_flaps.current == ctrl_windtunnel_flaps.min)) {
119  } else {
121  }
122  }
123  } else if (ctrl_windtunnel.rc_throttle < (MAX_PPRZ / 2)) {
124  // RESET
125  ctrl_windtunnel_throttle.current = ctrl_windtunnel_throttle.min;
126  ctrl_windtunnel_flaps.current = ctrl_windtunnel_flaps.min;
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 }
void ctrl_module_init(void)
void float_quat_comp_inv(struct FloatQuat *a2b, struct FloatQuat *a2c, struct FloatQuat *b2c)
Composition (multiplication) of two quaternions.
#define RADIO_ROLL
Definition: intermcu_ap.h:41
void guidance_v_module_init(void)
Periodic telemetry system header (includes downlink utility and generated code).
uint8_t last_wp UNUSED
Definition: navigation.c:96
void guidance_h_module_read_rc(void)
float vsupply
supply voltage in V
Definition: electrical.h:45
#define WINDTUNNEL_TO_BODY_PHI
pprz_t values[RADIO_CONTROL_NB_CHANNEL]
Definition: radio_control.h:69
void guidance_v_module_enter(void)
static float get_sys_time_float(void)
Get the time in seconds since startup.
Definition: sys_time.h:129
static struct FloatQuat * stateGetNedToBodyQuat_f(void)
Get vehicle body attitude quaternion (float).
Definition: state.h:1131
euler angles
Roation quaternion.
float airspeed
Equivalent Air Speed (equals to Calibrated Air Speed at low speed/altitude) (in m/s, -1 if unknown.
Definition: air_data.h:42
struct min_max_ctrl_t ctrl_windtunnel_throttle
float theta
in radians
float ctrl_windtunnel_steptime
void guidance_h_module_enter(void)
Interface for electrical status: supply voltage, current, battery status, etc.
Windtunnel controller.
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
struct RadioControl radio_control
Definition: radio_control.c:30
#define WINDTUNNEL_TO_BODY_PSI
#define WINDTUNNEL_TO_BODY_THETA
#define RADIO_THROTTLE
Definition: intermcu_ap.h:40
void guidance_h_module_run(bool in_flight)
static float last_time
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
struct ctrl_windtunnel_struct ctrl_windtunnel
API to get/set the generic vehicle states.
#define RADIO_PITCH
Definition: intermcu_ap.h:42
void ctrl_module_run(bool in_flight)
General stabilization interface for rotorcrafts.
#define RADIO_YAW
Definition: intermcu_ap.h:43
int32_t stabilization_cmd[COMMANDS_NB]
Stabilization commands.
Definition: stabilization.c:32
struct OrientationReps rotation
static void send_windtunnel_meas(struct transport_tx *trans, struct link_device *dev)
struct Electrical electrical
Definition: electrical.c:66
#define MAX_PPRZ
Definition: paparazzi.h:8
float current
current in A
Definition: electrical.h:46
void guidance_v_module_run(UNUSED bool in_flight)
void float_eulers_of_quat(struct FloatEulers *e, struct FloatQuat *q)
euler rotation 'ZYX'
struct min_max_ctrl_t ctrl_windtunnel_flaps
struct AirData air_data
global AirData state
Definition: air_data.c:39
void guidance_h_module_init(void)
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46
static struct FloatQuat * orientationGetQuat_f(struct OrientationReps *orientation)
Get vehicle body attitude quaternion (float).
static void orientationSetEulers_f(struct OrientationReps *orientation, struct FloatEulers *eulers)
Set vehicle body attitude from euler angles (float).