Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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
34
37#include "pprzlink/messages.h"
39#include "modules/core/abi.h"
40#include "generated/airframe.h"
41
42#if LOG_AOA
44bool 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
58#ifndef AOA_PWM_OFFSET
59#define AOA_PWM_OFFSET 1
60#endif
62#ifndef AOA_ANGLE_OFFSET
63#define AOA_ANGLE_OFFSET M_PI
64#endif
66#ifndef AOA_OFFSET
67#define AOA_OFFSET 0.0f
68#endif
70#ifndef AOA_FILTER
71#define AOA_FILTER 0.0f
72#endif
74#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
128static 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) {
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 */
145void aoa_pwm_init(void)
146{
147 aoa_pwm.offset = AOA_OFFSET;
148 aoa_pwm.filter = AOA_FILTER;
149 aoa_pwm.sens = AOA_SENS;
150 aoa_pwm.angle = 0.0f;
151 aoa_pwm.raw = 0.0f;
152 ssa_pwm.offset = SSA_OFFSET;
153 ssa_pwm.filter = SSA_FILTER;
154 ssa_pwm.sens = SSA_SENS;
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 */
167void 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
179 aoa_pwm.angle = AOA_SIGN * (((float)aoa_pwm.raw * aoa_pwm.sens) - aoa_pwm.offset - AOA_ANGLE_OFFSET);
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
197 ssa_pwm.angle = SSA_SIGN * (((float)ssa_pwm.raw * ssa_pwm.sens) - ssa_pwm.offset - AOA_ANGLE_OFFSET);
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
Main include for ABI (AirBorneInterface).
#define AOA_PWM_ID
#define AOA_OFFSET
Default offset value (assuming 0 AOA is in the middle of the range).
Definition aoa_adc.c:43
#define AOA_SENS
Default sensitivity (2*pi on a 10 bit ADC).
Definition aoa_adc.c:51
#define AOA_FILTER
Default filter value.
Definition aoa_adc.c:47
struct Aoa_Pwm ssa_pwm
Definition aoa_pwm.c:119
#define SSA_SIGN
Definition aoa_pwm.c:117
#define SSA_OFFSET
Definition aoa_pwm.c:103
void aoa_pwm_init(void)
Definition aoa_pwm.c:145
#define SSA_FILTER
Definition aoa_pwm.c:107
struct Aoa_Pwm aoa_pwm
Definition aoa_pwm.c:92
#define AOA_ANGLE_OFFSET
Default offset value (assuming 0 AOA is in the middle of the range).
Definition aoa_pwm.c:63
#define SSA_SENS
Definition aoa_pwm.c:111
void aoa_pwm_update(void)
Definition aoa_pwm.c:167
enum Aoa_Type aoa_send_type
Definition aoa_pwm.c:123
static void send_aoa(struct transport_tx *trans, struct link_device *dev)
Definition aoa_pwm.c:128
#define AOA_SIGN
Definition aoa_pwm.c:81
#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
Angle of Attack sensor on PWM.
Aoa_Type
Selection of sensor type to be send over telemetry.
Definition aoa_pwm.h:57
@ SEND_TYPE_SIDESLIP
Definition aoa_pwm.h:59
@ SEND_TYPE_AOA
Definition aoa_pwm.h:58
static struct uart_periph * dev
static FILE * pprzLogFile
bool log_started
uint32_t get_pwm_input_duty_in_usec(uint32_t channel)
Definition pwm_input.c:39
arch independent PWM input capture API
SdioError sdLogWriteLog(const FileDes fd, const char *fmt,...)
int16_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint16_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
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.