Paparazzi UAS  v7.0_unstable
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"
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 
59 #if PERIODIC_TELEMETRY
61 static void send_windtunnel_meas(struct transport_tx *trans, struct link_device *dev)
62 {
63  struct FloatQuat windtunnel_to_body;
64  struct FloatEulers windtunnel_to_body_e;
66  struct FloatQuat *ned_to_body = stateGetNedToBodyQuat_f();
67  float_quat_comp_inv(&windtunnel_to_body, ned_to_body, rotation);
68  float_eulers_of_quat(&windtunnel_to_body_e, &windtunnel_to_body);
69 
70  float aoa = DegOfRad(windtunnel_to_body_e.theta);
71  float power = electrical.vsupply * electrical.current;
72  pprz_msg_send_WINDTUNNEL_MEAS(trans, dev, AC_ID, &aoa, &air_data.airspeed, &electrical.vsupply, &electrical.current,
73  &power, COMMANDS_NB, stabilization.cmd);
74 }
75 #endif
76 
78 {
83 
84  // Create a rotation to the windtunnel from the body
85  struct FloatEulers windtunnel_to_body_eulers =
87  orientationSetEulers_f(&ctrl_windtunnel.rotation, &windtunnel_to_body_eulers);
88 
89 #if PERIODIC_TELEMETRY
91 #endif
92 }
93 
94 static void ctrl_module_run(bool in_flight __attribute__((unused)))
95 {
96  bool done = false;
97  // Increase step in steptime
99  // Increase throttle step if flaps at the end
101  // Only increase step if throttle is not at the end
106  } else {
107  // Finished
108  done = true;
109  }
110  } else {
111  // By default increase flaps
113 
114  // Double the amount of steptime during double transition
117  } else {
119  }
120  }
121  } else if (ctrl_windtunnel.rc_throttle < (MAX_PPRZ / 2)) {
122  // RESET
126  }
127 
128  stabilization.cmd[COMMAND_ROLL] = 0;
129  stabilization.cmd[COMMAND_PITCH] = 0;
130  stabilization.cmd[COMMAND_YAW] = 0;
131  stabilization.cmd[COMMAND_THRUST] = (done) ? 0 : ctrl_windtunnel_throttle.current;
132  stabilization.cmd[COMMAND_FLAPS] = (done) ? 0 : ctrl_windtunnel_flaps.current;
133 }
134 
135 
137 // Call our controller
138 // Implement own loops
140 {
145 }
146 
147 void guidance_module_run(bool in_flight)
148 {
149  // -MAX_PPRZ to MAX_PPRZ
154  // Call full inner-/outerloop / horizontal-/vertical controller:
155  ctrl_module_run(in_flight);
156 }
157 
struct AirData air_data
global AirData state
Definition: air_data.c:41
float airspeed
Equivalent Air Speed (equals to Calibrated Air Speed at low speed/altitude) (in m/s,...
Definition: air_data.h:44
void ctrl_windtunnel_init(void)
struct min_max_ctrl_t ctrl_windtunnel_flaps
static void send_windtunnel_meas(struct transport_tx *trans, struct link_device *dev)
#define WINDTUNNEL_TO_BODY_THETA
#define WINDTUNNEL_TO_BODY_PHI
struct min_max_ctrl_t ctrl_windtunnel_throttle
float ctrl_windtunnel_steptime
static void ctrl_module_run(bool in_flight)
void guidance_module_enter(void)
void guidance_module_run(bool in_flight)
static float last_time
#define WINDTUNNEL_TO_BODY_PSI
struct OrientationReps rotation
struct ctrl_windtunnel_struct ctrl_windtunnel
Windtunnel controller.
struct Electrical electrical
Definition: electrical.c:92
Interface for electrical status: supply voltage, current, battery status, etc.
float current
current in A
Definition: electrical.h:47
float vsupply
supply voltage in V
Definition: electrical.h:45
float theta
in radians
void float_quat_comp_inv(struct FloatQuat *a2b, struct FloatQuat *a2c, struct FloatQuat *b2c)
Composition (multiplication) of two quaternions.
void float_eulers_of_quat(struct FloatEulers *e, struct FloatQuat *q)
euler rotation 'ZYX'
euler angles
Roation quaternion.
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).
static struct FloatQuat * stateGetNedToBodyQuat_f(void)
Get vehicle body attitude quaternion (float).
Definition: state.h:1294
#define MAX_PPRZ
Definition: paparazzi.h:8
struct RadioControl radio_control
Definition: radio_control.c:33
Generic interface for radio control modules.
pprz_t values[RADIO_CONTROL_NB_CHANNEL]
Definition: radio_control.h:67
struct Stabilization stabilization
Definition: stabilization.c:41
General stabilization interface for rotorcrafts.
int32_t cmd[COMMANDS_NB]
output command vector, range from [-MAX_PPRZ:MAX_PPRZ] (store for messages)
API to get/set the generic vehicle states.
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
static float get_sys_time_float(void)
Get the time in seconds since startup.
Definition: sys_time.h:138
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:51
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66