Paparazzi UAS  v5.15_devel-59-g8c145b7
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 "subsystems/abi.h"
40 #include "generated/airframe.h"
41 
42 #if LOG_AOA
44 bool log_started;
45 #endif
46 
47 /* Config parameters for angle of attack sensor (mandatory) */
48 
49 #ifndef AOA_PWM_CHANNEL
50 #error "AOA_PWM_CHANNEL needs to be defined to use AOA_pwm module"
51 #endif
52 
54 #ifndef AOA_PWM_PERIOD
55 #define AOA_PWM_PERIOD 4096
56 #endif
57 #ifndef AOA_PWM_OFFSET
59 #define AOA_PWM_OFFSET 1
60 #endif
61 #ifndef AOA_ANGLE_OFFSET
63 #define AOA_ANGLE_OFFSET M_PI
64 #endif
65 #ifndef AOA_OFFSET
67 #define AOA_OFFSET 0.0f
68 #endif
69 #ifndef AOA_FILTER
71 #define AOA_FILTER 0.0f
72 #endif
73 #ifndef AOA_SENS
75 #define AOA_SENS ((2.0f*M_PI)/AOA_PWM_PERIOD)
76 #endif
77 // Set AOA_REVERSE to TRUE to change rotation direction
78 #if AOA_REVERSE
79 #define AOA_SIGN -1
80 #else
81 #define AOA_SIGN 1
82 #endif
83 // Enable telemetry report
84 #ifndef SEND_SYNC_AOA
85 #define SEND_SYNC_AOA TRUE
86 #endif
87 // Enable SD card logging
88 #ifndef LOG_AOA
89 #define LOG_AOA FALSE
90 #endif
91 
93 
94 
95 /* Config parameters for sideslip angle sensor (optional) */
96 
97 #if defined USE_SIDESLIP && !(defined SSA_PWM_CHANNEL)
98 #error "SSA_PWM_CHANNEL needs to be defined to use sideslip sensor"
99 #endif
100 
101 // Default extra offset that can be ajusted from settings
102 #ifndef SSA_OFFSET
103 #define SSA_OFFSET 0.0f
104 #endif
105 // Default filter value
106 #ifndef SSA_FILTER
107 #define SSA_FILTER 0.0f
108 #endif
109 // Default sensitivity (2*pi on a PWM of period AOA_PWM_PERIOD)
110 #ifndef SSA_SENS
111 #define SSA_SENS ((2.0f*M_PI)/AOA_PWM_PERIOD)
112 #endif
113 // Set SSA_REVERSE to TRUE to change rotation direction
114 #if SSA_REVERSE
115 #define SSA_SIGN -1
116 #else
117 #define SSA_SIGN 1
118 #endif
120 
121 
122 /* telemetry */
124 
125 #if PERIODIC_TELEMETRY
127 
128 static void send_aoa(struct transport_tx *trans, struct link_device *dev)
129 {
130  // FIXME use a second message, more fields or add a sensor ID to send sideslip
131  switch (aoa_send_type) {
132  case SEND_TYPE_SIDESLIP:
133  pprz_msg_send_AOA(trans, dev, AC_ID, &ssa_pwm.raw, &ssa_pwm.angle);
134  break;
135  case SEND_TYPE_AOA:
136  default:
137  pprz_msg_send_AOA(trans, dev, AC_ID, &aoa_pwm.raw, &aoa_pwm.angle);
138  break;
139  }
140 }
141 
142 #endif
143 
144 /* init */
145 void aoa_pwm_init(void)
146 {
150  aoa_pwm.angle = 0.0f;
151  aoa_pwm.raw = 0.0f;
155  ssa_pwm.angle = 0.0f;
156  ssa_pwm.raw = 0.0f;
157 #if LOG_AOA
158  log_started = false;
159 #endif
161 #if PERIODIC_TELEMETRY
163 #endif
164 }
165 
166 /* update, log and send */
167 void aoa_pwm_update(void) {
168 #if USE_AOA || USE_SIDESLIP
169  uint8_t flag = 0;
170 #endif
171  static float prev_aoa = 0.0f;
172  // raw duty cycle in usec
173  uint32_t aoa_duty_raw = get_pwm_input_duty_in_usec(AOA_PWM_CHANNEL);
174 
175  // remove some offset if needed
176  aoa_pwm.raw = aoa_duty_raw - AOA_PWM_OFFSET;
177  // FIXME for some reason, the last value of the MA3 encoder is not 4096 but 4097
178  // this case is not handled since we don't care about angles close to +- 180 deg
180  // filter angle
181  aoa_pwm.angle = aoa_pwm.filter * prev_aoa + (1.0f - aoa_pwm.filter) * aoa_pwm.angle;
182  prev_aoa = aoa_pwm.angle;
183 
184 #if USE_AOA
185  SetBit(flag, 0);
186 #endif
187 
188 #if USE_SIDESLIP
189  static float prev_ssa = 0.0f;
190  // raw duty cycle in usec
191  uint32_t ssa_duty_raw = get_pwm_input_duty_in_usec(SSA_PWM_CHANNEL);
192 
193  // remove some offset if needed
194  ssa_pwm.raw = ssa_duty_raw - AOA_PWM_OFFSET;
195  // FIXME for some reason, the last value of the MA3 encoder is not 4096 but 4097
196  // this case is not handled since we don't care about angles close to +- 180 deg
198  // filter angle
199  ssa_pwm.angle = ssa_pwm.filter * prev_ssa + (1.0f - ssa_pwm.filter) * ssa_pwm.angle;
200  prev_ssa = ssa_pwm.angle;
201 
202  SetBit(flag, 1);
203 #endif
204 
205 #if USE_AOA || USE_SIDESLIP
206  AbiSendMsgINCIDENCE(AOA_PWM_ID, flag, aoa_pwm.angle, ssa_pwm.angle);
207 #endif
208 
209 #if SEND_SYNC_AOA
210  RunOnceEvery(10, send_aoa(&(DefaultChannel).trans_tx, &(DefaultDevice).device));
211 #endif
212 
213 #if LOG_AOA
214  if(pprzLogFile != -1) {
215  if (!log_started) {
216  sdLogWriteLog(pprzLogFile, "AOA_PWM: AOA_ANGLE(deg) AOA_RAW(int16) SSA_ANGLE(deg) SSA_RAW(int16)\n");
217  log_started = true;
218  } else {
219  float aoa_angle = DegOfRad(aoa_pwm.angle);
220  float ssa_angle = DegOfRad(ssa_pwm.angle);
221  sdLogWriteLog(pprzLogFile, "AOA_PWM: %.3f %d %.3f %d\n", aoa_angle, aoa_pwm.raw, ssa_angle, ssa_pwm.raw);
222  }
223  }
224 #endif
225 }
226 
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:119
Periodic telemetry system header (includes downlink utility and generated code).
arch independent PWM input capture API
Main include for ABI (AirBorneInterface).
#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:59
#define AOA_OFFSET
Default extra offset that can be ajusted from settings.
Definition: aoa_pwm.c:67
void aoa_pwm_init(void)
Definition: aoa_pwm.c:145
struct Aoa_Pwm aoa_pwm
Definition: aoa_pwm.c:92
bool log_started
#define SSA_OFFSET
Definition: aoa_pwm.c:103
Angle of Attack sensor on PWM.
void aoa_pwm_update(void)
Definition: aoa_pwm.c:167
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:75
#define AOA_FILTER
Default filter value.
Definition: aoa_pwm.c:71
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:123
#define AOA_SIGN
Definition: aoa_pwm.c:81
#define SSA_SIGN
Definition: aoa_pwm.c:117
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:107
unsigned char uint8_t
Definition: types.h:14
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:63
#define AOA_PWM_ID
static void send_aoa(struct transport_tx *trans, struct link_device *dev)
Definition: aoa_pwm.c:128
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:111