Paparazzi UAS  v5.12_stable-4-g9b43e9b
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
aoa_pwm.c
Go to the documentation of this file.
1 /*
2 * Copyright (C) 2015 Jean-François Erdelyi, Gautier Hattenberger
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 
36 #include "mcu_periph/pwm_input.h"
37 #include "pprzlink/messages.h"
39 #include "generated/airframe.h"
40 
41 #if LOG_AOA
43 bool log_started;
44 #endif
45 
46 /* Config parameters for angle of attack sensor (mandatory) */
47 
48 #ifndef AOA_PWM_CHANNEL
49 #error "AOA_PWM_CHANNEL needs to be defined to use AOA_pwm module"
50 #endif
51 
53 #ifndef AOA_PWM_PERIOD
54 #define AOA_PWM_PERIOD 4096
55 #endif
56 #ifndef AOA_PWM_OFFSET
58 #define AOA_PWM_OFFSET 1
59 #endif
60 #ifndef AOA_ANGLE_OFFSET
62 #define AOA_ANGLE_OFFSET M_PI
63 #endif
64 #ifndef AOA_OFFSET
66 #define AOA_OFFSET 0.0f
67 #endif
68 #ifndef AOA_FILTER
70 #define AOA_FILTER 0.0f
71 #endif
72 #ifndef AOA_SENS
74 #define AOA_SENS ((2.0f*M_PI)/AOA_PWM_PERIOD)
75 #endif
76 // Set AOA_REVERSE to TRUE to change rotation direction
77 #if AOA_REVERSE
78 #define AOA_SIGN -1
79 #else
80 #define AOA_SIGN 1
81 #endif
82 // Enable telemetry report
83 #ifndef SEND_SYNC_AOA
84 #define SEND_SYNC_AOA TRUE
85 #endif
86 // Enable SD card logging
87 #ifndef LOG_AOA
88 #define LOG_AOA FALSE
89 #endif
90 
92 
93 
94 /* Config parameters for sideslip angle sensor (optional) */
95 
96 #if defined USE_SIDESLIP && !(defined SSA_PWM_CHANNEL)
97 #error "SSA_PWM_CHANNEL needs to be defined to use sideslip sensor"
98 #endif
99 
100 // Default extra offset that can be ajusted from settings
101 #ifndef SSA_OFFSET
102 #define SSA_OFFSET 0.0f
103 #endif
104 // Default filter value
105 #ifndef SSA_FILTER
106 #define SSA_FILTER 0.0f
107 #endif
108 // Default sensitivity (2*pi on a PWM of period AOA_PWM_PERIOD)
109 #ifndef SSA_SENS
110 #define SSA_SENS ((2.0f*M_PI)/AOA_PWM_PERIOD)
111 #endif
112 // Set SSA_REVERSE to TRUE to change rotation direction
113 #if SSA_REVERSE
114 #define SSA_SIGN -1
115 #else
116 #define SSA_SIGN 1
117 #endif
119 
120 
121 /* telemetry */
123 
124 #if PERIODIC_TELEMETRY
126 
127 static void send_aoa(struct transport_tx *trans, struct link_device *dev)
128 {
129  // FIXME use a second message, more fields or add a sensor ID to send sideslip
130  switch (aoa_send_type) {
131  case SEND_TYPE_SIDESLIP:
132  pprz_msg_send_AOA(trans, dev, AC_ID, &ssa_pwm.raw, &ssa_pwm.angle);
133  break;
134  case SEND_TYPE_AOA:
135  default:
136  pprz_msg_send_AOA(trans, dev, AC_ID, &aoa_pwm.raw, &aoa_pwm.angle);
137  break;
138  }
139 }
140 
141 #endif
142 
143 /* init */
144 void aoa_pwm_init(void)
145 {
149  aoa_pwm.angle = 0.0f;
150  aoa_pwm.raw = 0.0f;
154  ssa_pwm.angle = 0.0f;
155  ssa_pwm.raw = 0.0f;
156 #if LOG_AOA
157  log_started = false;
158 #endif
160 #if PERIODIC_TELEMETRY
162 #endif
163 }
164 
165 /* update, log and send */
166 void aoa_pwm_update(void) {
167  static float prev_aoa = 0.0f;
168  // raw duty cycle in usec
169  uint32_t aoa_duty_raw = get_pwm_input_duty_in_usec(AOA_PWM_CHANNEL);
170 
171  // remove some offset if needed
172  aoa_pwm.raw = aoa_duty_raw - AOA_PWM_OFFSET;
173  // FIXME for some reason, the last value of the MA3 encoder is not 4096 but 4097
174  // this case is not handled since we don't care about angles close to +- 180 deg
176  // filter angle
177  aoa_pwm.angle = aoa_pwm.filter * prev_aoa + (1.0f - aoa_pwm.filter) * aoa_pwm.angle;
178  prev_aoa = aoa_pwm.angle;
179 
180 #if USE_AOA
182 #endif
183 
184 #if USE_SIDESLIP
185  static float prev_ssa = 0.0f;
186  // raw duty cycle in usec
187  uint32_t ssa_duty_raw = get_pwm_input_duty_in_usec(SSA_PWM_CHANNEL);
188 
189  // remove some offset if needed
190  ssa_pwm.raw = ssa_duty_raw - AOA_PWM_OFFSET;
191  // FIXME for some reason, the last value of the MA3 encoder is not 4096 but 4097
192  // this case is not handled since we don't care about angles close to +- 180 deg
194  // filter angle
195  ssa_pwm.angle = ssa_pwm.filter * prev_ssa + (1.0f - ssa_pwm.filter) * ssa_pwm.angle;
196  prev_ssa = ssa_pwm.angle;
197 
199 #endif
200 
201 #if SEND_SYNC_AOA
202  RunOnceEvery(10, send_aoa(&(DefaultChannel).trans_tx, &(DefaultDevice).device));
203 #endif
204 
205 #if LOG_AOA
206  if(pprzLogFile != -1) {
207  if (!log_started) {
208  sdLogWriteLog(pprzLogFile, "AOA_PWM: AOA_ANGLE(deg) AOA_RAW(int16) SSA_ANGLE(deg) SSA_RAW(int16)\n");
209  log_started = true;
210  } else {
211  float aoa_angle = DegOfRad(aoa_pwm.angle);
212  float ssa_angle = DegOfRad(ssa_pwm.angle);
213  sdLogWriteLog(pprzLogFile, "AOA_PWM: %.3f %d %.3f %d\n", aoa_angle, aoa_pwm.raw, ssa_angle, ssa_pwm.raw);
214  }
215  }
216 #endif
217 }
218 
Aoa_Type
Selection of sensor type to be send over telemetry.
Definition: aoa_pwm.h:57
struct Aoa_Pwm ssa_pwm
Definition: aoa_pwm.c:118
Periodic telemetry system header (includes downlink utility and generated code).
arch independent PWM input capture API
#define AOA_PWM_OFFSET
Some sensor may need an initial PWM offset (1 usec in the case of an MA3 sensor)
Definition: aoa_pwm.c:58
#define AOA_OFFSET
Default extra offset that can be ajusted from settings.
Definition: aoa_pwm.c:66
void aoa_pwm_init(void)
Definition: aoa_pwm.c:144
struct Aoa_Pwm aoa_pwm
Definition: aoa_pwm.c:91
bool log_started
static void stateSetAngleOfAttack_f(float aoa)
Set angle of attack in radians (float).
Definition: state.h:1318
#define SSA_OFFSET
Definition: aoa_pwm.c:102
Angle of Attack sensor on PWM.
void aoa_pwm_update(void)
Definition: aoa_pwm.c:166
float angle
Angle of attack in radians.
Definition: aoa_pwm.h:41
#define AOA_SENS
Default sensitivity (2*pi on a PWM of period AOA_PWM_PERIOD)
Definition: aoa_pwm.c:74
#define AOA_FILTER
Default filter value.
Definition: aoa_pwm.c:70
unsigned long uint32_t
Definition: types.h:18
uint32_t raw
raw PWM value
Definition: aoa_pwm.h:40
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
FileDes pprzLogFile
Definition: sdlog_chibios.c:85
enum Aoa_Type aoa_send_type
Definition: aoa_pwm.c:122
#define AOA_SIGN
Definition: aoa_pwm.c:80
#define SSA_SIGN
Definition: aoa_pwm.c:116
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:73
float sens
sensitiviy, i.e. scale to conver raw to angle
Definition: aoa_pwm.h:43
#define SSA_FILTER
Definition: aoa_pwm.c:106
uint32_t get_pwm_input_duty_in_usec(uint32_t channel)
Definition: pwm_input.c:39
float filter
Filtering value [0-1] 0: no filtering 1: output is a constant value.
Definition: aoa_pwm.h:49
#define AOA_ANGLE_OFFSET
Default offset value (assuming 0 AOA is in the middle of the range)
Definition: aoa_pwm.c:62
static void send_aoa(struct transport_tx *trans, struct link_device *dev)
Definition: aoa_pwm.c:127
static void stateSetSideslip_f(float sideslip)
Set sideslip angle in radians (float).
Definition: state.h:1327
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46
float offset
Angle of attack offset in radians.
Definition: aoa_pwm.h:42
#define SSA_SENS
Definition: aoa_pwm.c:110