[module] add pwm based angle of attack sensor

- use pwm_meas module
- log on SD card if available
- shared message with aoa_adc

conf/messages.xml

@@ -593,9 +593,9 @@
     <field name="alt" type="float" unit="m"/>
   </message>
 
-  <message name="AOA_ADC" id="69">
-    <field name="adcVal" type="uint16"/>
-    <field name="AOA" type="float" unit="rad"/>
+  <message name="AOA" id="69">
+    <field name="raw" type="uint32"/>
+    <field name="angle" type="float" unit="rad" alt_unit="deg"/>
   </message>
 
   <message name="XTEND_RSSI" id="70">

conf/modules/AOA_pwm.xml

@@ -0,0 +1,39 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+
+<module name="AOA_pwm" dir="sensors">
+  <doc>
+    <description> Angle of Attack sensor using PWM input</description>
+    <configure name="AOA_PWM_CHANNEL" value="PWM_INPUTX" description="Select PWM input channel for AOA sensor"/>
+    <define name="AOA_SENS" value="(2*3.14)/AOA_PWM_PERIOD" description="sensor sensitivity"/>
+    <define name="AOA_OFFSET" value="0."  description="offset in radian that can be set from settings"/>
+    <define name="AOA_ANGLE_OFFSET" value="3.14"  description="global offset in radian corresponding to the sensor mounting (default: 3.14)"/>
+    <define name="AOA_REVERSE" value="TRUE|FALSE" description="set to TRUE to reverse rotation direction"/>
+    <define name="AOA_PWM_PERIOD" value="4096" description="period in microsec (e.g. 4096 for 12 bits sensor)"/>
+    <define name="AOA_PWM_OFFSET" value="1" description="initial offset on the raw pwm signal if needed (default: 1usec)"/>
+    <define name="SEND_AOA" value="TRUE|FALSE" description="enable telemetry report (default: TRUE)"/>
+    <define name="LOG_AOA" value="TRUE|FALSE" description="enable logging on SD card (default: FALSE)"/>
+  </doc>
+  <settings>
+    <dl_settings>
+      <dl_settings NAME="AOA">
+        <dl_setting MAX="180" MIN="-180" STEP="0.1" VAR="aoa_pwm.offset" shortname="AOA_offset" module="modules/sensors/aoa_pwm" param="AOA_OFFSET" unit="rad" alt_unit="deg"/>
+        <dl_setting MAX="0.95" MIN="0" STEP="0.001" VAR="aoa_pwm.filter" shortname="AOA_filter" module="modules/sensors/aoa_pwm" param="AOA_FILTER"/>
+      </dl_settings>
+    </dl_settings>
+  </settings>
+
+  <depends>pwm_meas</depends>
+  <header>
+    <file name="aoa_pwm.h" />
+  </header>
+
+  <init fun="aoa_pwm_init()" />
+  <periodic fun="aoa_pwm_update()" freq="20" />
+
+  <makefile target="ap">
+    <file name="aoa_pwm.c" />
+    <define name="AOA_PWM_CHANNEL" value="$(AOA_PWM_CHANNEL)" cond="ifdef AOA_PWM_CHANNEL" />
+    <define name="PWM_INPUT_TICKS_PER_USEC" value="1" />
+    <define name="USE_$(AOA_PWM_CHANNEL)" value="PWM_PULSE_TYPE_ACTIVE_LOW" />
+  </makefile>
+</module>

conf/telemetry/fixedwing_flight_recorder.xml

@@ -125,7 +125,8 @@
       <message name="IMU_ACCEL"           period=".02"/>
       <message name="IMU_GYRO"            period=".02"/>
       <message name="IMU_MAG"             period=".02"/>
-      <message name="AIR_DATA"            period="0.5"/>
+      <message name="AIR_DATA"            period="0.05"/>
+      <message name="AOA"                 period="0.05"/>
       <message name="RC"                  period="0.05"/>
       <message name="COMMANDS"            period="0.05"/>
       <message name="ACTUATORS"           period="0.05"/>

sw/airborne/modules/sensors/aoa_adc.c

@@ -87,6 +87,6 @@ void aoa_adc_update(void)
   stateSetAngleOfAttack_f(aoa_adc.angle);
 #endif
 
-  RunOnceEvery(30, DOWNLINK_SEND_AOA_ADC(DefaultChannel, DefaultDevice, &aoa_adc.raw, &aoa_adc.angle));
+  RunOnceEvery(30, DOWNLINK_SEND_AOA(DefaultChannel, DefaultDevice, &aoa_adc.raw, &aoa_adc.angle));
 }
 

sw/airborne/modules/sensors/aoa_adc.h

@@ -38,7 +38,7 @@
  */
 struct Aoa_Adc {
   struct adc_buf buf;
-  uint16_t raw; ///< raw ADC value
+  uint32_t raw; ///< raw ADC value
   float angle;  ///< Angle of attack in radians
   float offset; ///< Angle of attack offset in radians
   float sens;   ///< sensitiviy, i.e. scale to conver raw to angle

sw/airborne/modules/sensors/aoa_pwm.c

@@ -0,0 +1,147 @@
+/*
+* Copyright (C) 2015 Jean-François Erdelyi, Gautier Hattenberger
+*
+* This file is part of paparazzi
+*
+* paparazzi is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License as published by
+* the Free Software Foundation; either version 2, or (at your option)
+* any later version.
+*
+* paparazzi is distributed in the hope that it will be useful,
+* but WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with paparazzi; see the file COPYING.  If not, see
+* <http://www.gnu.org/licenses/>.
+*/
+
+/**
+* @file "modules/sensors/aoa_pwm.c"
+* @author Jean-François Erdelyi
+* @brief Angle of Attack sensor on PWM
+*
+* SENSOR, exemple : US DIGITAL MA3-P12-125-B
+* @see http://www.usdigital.com/products/encoders/absolute/rotary/shaft/ma3
+*/
+
+#include "modules/sensors/aoa_pwm.h"
+#include "mcu_periph/pwm_input.h"
+#include "messages.h"
+#include "subsystems/datalink/downlink.h"
+#include "generated/airframe.h"
+
+#if LOG_AOA
+#include "sdLog.h"
+#include "subsystems/chibios-libopencm3/chibios_sdlog.h"
+bool_t log_started;
+#endif
+
+#ifndef AOA_PWM_CHANNEL
+#error "AOA_PWM_CHANNEL needs to be defined to use AOA_pwm module"
+#endif
+
+/// Default to a 12 bit PWM sensor
+#ifndef AOA_PWM_PERIOD
+#define AOA_PWM_PERIOD 4096
+#endif
+/// Some sensor may need an initial PWM offset (1 usec in the case of an MA3 sensor)
+#ifndef AOA_PWM_OFFSET
+#define AOA_PWM_OFFSET 1
+#endif
+/// Default offset value (assuming 0 AOA is in the middle of the range)
+#ifndef AOA_ANGLE_OFFSET
+#define AOA_ANGLE_OFFSET M_PI
+#endif
+/// Default extra offset that can be ajusted from settings
+#ifndef AOA_OFFSET
+#define AOA_OFFSET 0.0f
+#endif
+/// Default filter value
+#ifndef AOA_FILTER
+#define AOA_FILTER 0.0f
+#endif
+/// Default sensitivity (2*pi on a PWM of period AOA_PWM_PERIOD)
+#ifndef AOA_SENS
+#define AOA_SENS ((2.0f*M_PI)/AOA_PWM_PERIOD)
+#endif
+// Set AOA_REVERSE to TRUE to change rotation direction
+#if AOA_REVERSE
+#define AOA_SIGN -1
+#else
+#define AOA_SIGN 1
+#endif
+// Enable telemetry report
+#ifndef SEND_SYNC_AOA
+#define SEND_SYNC_AOA TRUE
+#endif
+// Enable SD card logging
+#ifndef LOG_AOA
+#define LOG_AOA FALSE
+#endif
+
+struct Aoa_Pwm aoa_pwm;
+
+#if PERIODIC_TELEMETRY
+#include "subsystems/datalink/telemetry.h"
+
+static void send_aoa(struct transport_tx *trans, struct link_device *dev)
+{
+  pprz_msg_send_AOA(trans, dev, AC_ID, &aoa_pwm.raw, &aoa_pwm.angle);
+}
+
+#endif
+
+void aoa_pwm_init(void)
+{
+  aoa_pwm.offset = AOA_OFFSET;
+  aoa_pwm.filter = AOA_FILTER;
+  aoa_pwm.sens = AOA_SENS;
+  aoa_pwm.angle = 0.0f;
+  aoa_pwm.raw = 0.0f;
+#if LOG_AOA
+  log_started = FALSE;
+#endif
+#if PERIODIC_TELEMETRY
+  register_periodic_telemetry(DefaultPeriodic, "AOA", send_aoa);
+#endif
+}
+
+void aoa_pwm_update(void) {
+  static float prev_aoa = 0.0f;
+
+  // raw duty cycle in usec
+  uint32_t duty_raw = pwm_input_duty_tics[AOA_PWM_CHANNEL] / PWM_INPUT_TICKS_PER_USEC;
+
+  // remove some offset if needed
+  aoa_pwm.raw = duty_raw - AOA_PWM_OFFSET;
+  // FIXME for some reason, the last value of the MA3 encoder is not 4096 but 4097
+  // this case is not handled since we don't care about angles close to +- 180 deg
+  aoa_pwm.angle = AOA_SIGN * (((float)aoa_pwm.raw * aoa_pwm.sens) - aoa_pwm.offset - AOA_ANGLE_OFFSET);
+  // filter angle
+  aoa_pwm.angle = aoa_pwm.filter * prev_aoa + (1.0f - aoa_pwm.filter) * aoa_pwm.angle;
+  prev_aoa = aoa_pwm.angle;
+
+#if USE_AOA
+  stateSetAngleOfAttack_f(aoa_adc.angle);
+#endif
+
+#if SEND_SYNC_AOA
+  RunOnceEvery(10, DOWNLINK_SEND_AOA(DefaultChannel, DefaultDevice, &aoa_pwm.raw, &aoa_pwm.angle));
+#endif
+
+#if LOG_AOA
+  if(pprzLogFile != -1) {
+    if (!log_started) {
+      sdLogWriteLog(pprzLogFile, "AOA_PWM: ANGLE(deg) RAW(int16)\n");
+      log_started = TRUE;
+    } else {
+      float angle = DegOfRad(aoa_pwm.angle);
+      sdLogWriteLog(pprzLogFile, "AOA_PWM: %.3f %d\n", angle, aoa_pwm.raw);
+    }
+  }
+#endif
+}
+

sw/airborne/modules/sensors/aoa_pwm.h

@@ -0,0 +1,53 @@
+/*
+* Copyright (C) 2015 Jean-François Erdelyi, Gautier Hattenberger
+*
+* This file is part of paparazzi
+*
+* paparazzi is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License as published by
+* the Free Software Foundation; either version 2, or (at your option)
+* any later version.
+*
+* paparazzi is distributed in the hope that it will be useful,
+* but WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with paparazzi; see the file COPYING.  If not, see
+* <http://www.gnu.org/licenses/>.
+*/
+
+/**
+* @file "modules/sensors/aoa_ppm.h"
+* @author Jean-François Erdelyi
+* @brief Angle of Attack sensor on PWM
+*
+* SENSOR, example : US DIGITAL MA3-P12-125-B
+*/
+
+#ifndef AOA_PWM_H
+#define AOA_PWM_H
+
+#include "std.h"
+
+struct Aoa_Pwm {
+  uint32_t raw; ///< raw PWM value
+  float angle;  ///< Angle of attack in radians
+  float offset; ///< Angle of attack offset in radians
+  float sens;   ///< sensitiviy, i.e. scale to conver raw to angle
+
+  /** Filtering value [0-1]
+   * 0: no filtering
+   * 1: output is a constant value
+   */
+  float filter;
+};
+
+extern struct Aoa_Pwm aoa_pwm;
+
+extern void aoa_pwm_init(void);
+extern void aoa_pwm_update(void);
+
+#endif /* AOA_PWM_H */
+