Wind estimation (#2028)

* [nps] add angle of attack and sideslip to NPS

* [module] extra_dl can work with nps

* [module] add sideslip sensor to aoa_pwm module and fix apogee board file

* [module] add wind estimation module

This module is an experimental wind estimation filter based on UKF that
aims at estimating all 3 local wind components in real-time.
It is based on ChibiOS as the algorithm runs in a dedicated thread.
The algorithm itself is generated from a Matlab/Simulink model.

* [tool] read Meso-NH meteo data and feed NPS with wind information

* [module] remove nps target from extra_dl, uart not well supported on FW

* [mesonh] remove unused UDP interface

* [chibios] add compilation error message for wind estimator module

conf/firmwares/subsystems/shared/nps.makefile

@@ -49,6 +49,8 @@ nps.srcs +=                                      \
        $(NPSDIR)/nps_sensor_gps.c                \
        $(NPSDIR)/nps_sensor_airspeed.c           \
        $(NPSDIR)/nps_sensor_temperature.c        \
+       $(NPSDIR)/nps_sensor_aoa.c                \
+       $(NPSDIR)/nps_sensor_sideslip.c           \
        $(NPSDIR)/nps_electrical.c                \
        $(NPSDIR)/nps_atmosphere.c                \
        $(NPSDIR)/nps_ivy.c                       \

conf/modules/AOA_pwm.xml

@@ -2,7 +2,15 @@
 
 <module name="AOA_pwm" dir="sensors">
   <doc>
-    <description> Angle of Attack sensor using PWM input</description>
+    <description>
+      Angle of Attack sensor using PWM input
+
+      Driver for a PWM based angle of attack sensor
+      A second sensor can be defined for the sideslip angle
+      It is assumed that both sensors are the same, only sensitivity, offset and direction can differ.
+
+      Sensor example: US DIGITAL MA3-P12-125-B
+    </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"/>
@@ -12,12 +20,19 @@
     <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)"/>
+    <configure name="SSA_PWM_CHANNEL" value="PWM_INPUTX" description="Select PWM input channel for Sideslip sensor (optional)"/>
+    <define name="SSA_SENS" value="(2*3.14)/AOA_PWM_PERIOD" description="sensor sensitivity"/>
+    <define name="SSA_OFFSET" value="0."  description="offset in radian that can be set from settings"/>
+    <define name="SSA_REVERSE" value="TRUE|FALSE" description="set to TRUE to reverse rotation direction"/>
   </doc>
   <settings>
     <dl_settings>
       <dl_settings NAME="AOA">
+        <dl_setting MAX="1" MIN="0" STEP="1" VAR="aoa_send_type" shortname="send type" module="modules/sensors/aoa_pwm" values="AOA|SIDESLIP"/>
         <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_setting MAX="180" MIN="-180" STEP="0.1" VAR="ssa_pwm.offset" shortname="SSA_offset" module="modules/sensors/aoa_pwm" param="SSA_OFFSET" unit="rad" alt_unit="deg"/>
+        <dl_setting MAX="0.95" MIN="0" STEP="0.001" VAR="ssa_pwm.filter" shortname="SSA_filter" module="modules/sensors/aoa_pwm" param="SSA_FILTER"/>
       </dl_settings>
     </dl_settings>
   </settings>
@@ -35,5 +50,9 @@
     <define name="AOA_PWM_CHANNEL" value="$(AOA_PWM_CHANNEL)" cond="ifdef AOA_PWM_CHANNEL" />
     <define name="$(AOA_PWM_CHANNEL)_TICKS_PER_USEC" value="1" />
     <define name="USE_$(AOA_PWM_CHANNEL)" value="PWM_PULSE_TYPE_ACTIVE_LOW" />
+    <define name="SSA_PWM_CHANNEL" value="$(SSA_PWM_CHANNEL)" cond="ifdef SSA_PWM_CHANNEL" />
+    <define name="$(SSA_PWM_CHANNEL)_TICKS_PER_USEC" value="1" cond="ifdef SSA_PWM_CHANNEL" />
+    <define name="USE_$(SSA_PWM_CHANNEL)" value="PWM_PULSE_TYPE_ACTIVE_LOW" cond="ifdef SSA_PWM_CHANNEL" />
+    <define name="USE_SIDESLIP" cond="ifdef SSA_PWM_CHANNEL" />
   </makefile>
 </module>

conf/modules/nps.xml

@@ -55,6 +55,8 @@
     <file name="nps_sensor_gps.c" dir="nps"/>
     <file name="nps_sensor_airspeed.c" dir="nps"/>
     <file name="nps_sensor_temperature.c" dir="nps"/>
+    <file name="nps_sensor_aoa.c" dir="nps"/>
+    <file name="nps_sensor_sideslip.c" dir="nps"/>
     <file name="nps_electrical.c" dir="nps"/>
     <file name="nps_atmosphere.c" dir="nps"/>
     <file name="nps_ivy.c" dir="nps"/>

conf/modules/wind_estimator.xml

@@ -0,0 +1,41 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+
+<module name="wind_estimator" dir="meteo">
+  <doc>
+    <description>
+      Wind Estimator.
+      Using an UKF filter generated by MATLAB running in a CHibiOS thread
+      Requires:
+        - IMU for inertial data (rates and accel)
+        - GPS for ground speed vector
+        - magnetometer for true heading
+        - pitot for airspeed norm
+        - angle of attack probe (better and faster estimate of vertical component
+    </description>
+  </doc>
+  <settings>
+    <dl_settings>
+      <dl_settings name="wind">
+        <dl_setting MAX="1" MIN="1" STEP="1" values="RESET" module="modules/meteo/wind_estimator" VAR="wind_estimator.reset" shortname="reset"/>
+        <dl_setting MAX="2." MIN="0." STEP="0.01" module="modules/meteo/wind_estimator" VAR="wind_estimator.r_gs" shortname="R_GS" handler="Set_R_GS"/>
+        <dl_setting MAX="2." MIN="0." STEP="0.01" module="modules/meteo/wind_estimator" VAR="wind_estimator.r_va" shortname="R_VA" handler="Set_R_VA"/>
+        <dl_setting MAX="0.1" MIN="0." STEP="0.0001" module="modules/meteo/wind_estimator" VAR="wind_estimator.r_aoa" shortname="R_AOA" handler="Set_R_AOA"/>
+        <dl_setting MAX="0.1" MIN="0." STEP="0.0001" module="modules/meteo/wind_estimator" VAR="wind_estimator.r_ssa" shortname="R_SSA" handler="Set_R_SSA"/>
+        <dl_setting MAX="1." MIN="0." STEP="0.01" module="modules/meteo/wind_estimator" VAR="wind_estimator.q_va" shortname="Q_VA" handler="Set_Q_VA"/>
+        <dl_setting MAX="0.01" MIN="0." STEP="0.0001" module="modules/meteo/wind_estimator" VAR="wind_estimator.q_wind" shortname="Q_WIND" handler="Set_Q_WIND"/>
+        <dl_setting MAX="0.001" MIN="0." STEP="0.0001" module="modules/meteo/wind_estimator" VAR="wind_estimator.q_va_scale" shortname="Q_VA_SCALE" handler="Set_Q_VA_SCALE"/>
+      </dl_settings>
+    </dl_settings>
+  </settings>
+  <header>
+    <file name="wind_estimator.h"/>
+  </header>
+  <init fun="wind_estimator_init()"/>
+  <periodic fun="wind_estimator_periodic()" freq="10."  autorun="TRUE"/>
+  <event fun="wind_estimator_event()"/>
+  <makefile target="ap|nps">
+    <file name="wind_estimator.c"/>
+    <file name="lib_ukf_wind_estimator/UKF_Wind_Estimator.c"/>
+  </makefile>
+</module>
+

conf/simulator/nps/nps_sensors_params_wind_estimator.h

@@ -0,0 +1,171 @@
+/*
+ * Copyright (C) 2016 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, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+#ifndef NPS_SENSORS_PARAMS_H
+#define NPS_SENSORS_PARAMS_H
+
+#include "generated/airframe.h"
+#include "subsystems/imu.h"
+
+
+#define NPS_BODY_TO_IMU_PHI    IMU_BODY_TO_IMU_PHI
+#define NPS_BODY_TO_IMU_THETA  IMU_BODY_TO_IMU_THETA
+#define NPS_BODY_TO_IMU_PSI    IMU_BODY_TO_IMU_PSI
+
+/*
+ * Accelerometer
+ */
+/* MPU60x0 has 16bit resolution */
+#define NPS_ACCEL_MIN -32767
+#define NPS_ACCEL_MAX  32767
+/* ms-2 */
+/* aka 2^10/ACCEL_X_SENS  */
+#define NPS_ACCEL_SENSITIVITY_XX  (IMU_ACCEL_X_SIGN * ACCEL_BFP_OF_REAL(1./IMU_ACCEL_X_SENS))
+#define NPS_ACCEL_SENSITIVITY_YY  (IMU_ACCEL_Y_SIGN * ACCEL_BFP_OF_REAL(1./IMU_ACCEL_Y_SENS))
+#define NPS_ACCEL_SENSITIVITY_ZZ  (IMU_ACCEL_Z_SIGN * ACCEL_BFP_OF_REAL(1./IMU_ACCEL_Z_SENS))
+
+#define NPS_ACCEL_NEUTRAL_X       IMU_ACCEL_X_NEUTRAL
+#define NPS_ACCEL_NEUTRAL_Y       IMU_ACCEL_Y_NEUTRAL
+#define NPS_ACCEL_NEUTRAL_Z       IMU_ACCEL_Z_NEUTRAL
+/* m2s-4 */
+#define NPS_ACCEL_NOISE_STD_DEV_X 5.e-2
+#define NPS_ACCEL_NOISE_STD_DEV_Y 5.e-2
+#define NPS_ACCEL_NOISE_STD_DEV_Z 5.e-2
+/* ms-2 */
+#define NPS_ACCEL_BIAS_X          0
+#define NPS_ACCEL_BIAS_Y          0
+#define NPS_ACCEL_BIAS_Z          0
+/* s */
+#ifndef NPS_ACCEL_DT
+#define NPS_ACCEL_DT              (1./125.)
+#endif
+
+
+
+/*
+ * Gyrometer
+ */
+/* MPU60x0 has 16 bit resolution */
+#define NPS_GYRO_MIN -32767
+#define NPS_GYRO_MAX  32767
+
+/* 2^12/GYRO_X_SENS */
+#define NPS_GYRO_SENSITIVITY_PP   (IMU_GYRO_P_SIGN * RATE_BFP_OF_REAL(1./IMU_GYRO_P_SENS))
+#define NPS_GYRO_SENSITIVITY_QQ   (IMU_GYRO_Q_SIGN * RATE_BFP_OF_REAL(1./IMU_GYRO_Q_SENS))
+#define NPS_GYRO_SENSITIVITY_RR   (IMU_GYRO_R_SIGN * RATE_BFP_OF_REAL(1./IMU_GYRO_R_SENS))
+
+#define NPS_GYRO_NEUTRAL_P        IMU_GYRO_P_NEUTRAL
+#define NPS_GYRO_NEUTRAL_Q        IMU_GYRO_Q_NEUTRAL
+#define NPS_GYRO_NEUTRAL_R        IMU_GYRO_R_NEUTRAL
+
+#define NPS_GYRO_NOISE_STD_DEV_P  RadOfDeg(0.)
+#define NPS_GYRO_NOISE_STD_DEV_Q  RadOfDeg(0.)
+#define NPS_GYRO_NOISE_STD_DEV_R  RadOfDeg(0.)
+
+#define NPS_GYRO_BIAS_INITIAL_P   RadOfDeg( 0.0)
+#define NPS_GYRO_BIAS_INITIAL_Q   RadOfDeg( 0.0)
+#define NPS_GYRO_BIAS_INITIAL_R   RadOfDeg( 0.0)
+
+#define NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_P RadOfDeg(0.5)
+#define NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_Q RadOfDeg(0.5)
+#define NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_R RadOfDeg(0.5)
+/* s */
+#ifndef NPS_GYRO_DT
+#define NPS_GYRO_DT (1./125.)
+#endif
+
+
+
+/*
+ *  Magnetometer
+ */
+ /* HMC5843 has 12 bit resolution */
+#define NPS_MAG_MIN -2047
+#define NPS_MAG_MAX  2047
+
+#define NPS_MAG_IMU_TO_SENSOR_PHI    0.
+#define NPS_MAG_IMU_TO_SENSOR_THETA  0.
+#define NPS_MAG_IMU_TO_SENSOR_PSI    0.
+
+#define NPS_MAG_SENSITIVITY_XX   (IMU_MAG_X_SIGN * MAG_BFP_OF_REAL(1./IMU_MAG_X_SENS))
+#define NPS_MAG_SENSITIVITY_YY   (IMU_MAG_Y_SIGN * MAG_BFP_OF_REAL(1./IMU_MAG_Y_SENS))
+#define NPS_MAG_SENSITIVITY_ZZ   (IMU_MAG_Z_SIGN * MAG_BFP_OF_REAL(1./IMU_MAG_Z_SENS))
+
+#define NPS_MAG_NEUTRAL_X  IMU_MAG_X_NEUTRAL
+#define NPS_MAG_NEUTRAL_Y  IMU_MAG_Y_NEUTRAL
+#define NPS_MAG_NEUTRAL_Z  IMU_MAG_Z_NEUTRAL
+
+#define NPS_MAG_NOISE_STD_DEV_X  2e-3
+#define NPS_MAG_NOISE_STD_DEV_Y  2e-3
+#define NPS_MAG_NOISE_STD_DEV_Z  2e-3
+
+#define NPS_MAG_DT (1./60.)
+
+
+/*
+ *  Barometer
+ */
+/* m */
+/* aka 2^8/INS_BARO_SENS  */
+#define NPS_BARO_QNH             1013.25
+#define NPS_BARO_SENSITIVITY     4.0
+#define NPS_BARO_DT              (1./5.)
+#define NPS_BARO_NOISE_STD_DEV   5.e-2
+
+/*
+ *  GPS
+ */
+
+#ifndef GPS_PERFECT
+#define GPS_PERFECT 1
+#endif
+
+#if GPS_PERFECT
+
+#define NPS_GPS_SPEED_NOISE_STD_DEV            0.
+#define NPS_GPS_SPEED_LATENCY                  0.
+#define NPS_GPS_POS_NOISE_STD_DEV              0.001
+#define NPS_GPS_POS_BIAS_INITIAL_X             0.
+#define NPS_GPS_POS_BIAS_INITIAL_Y             0.
+#define NPS_GPS_POS_BIAS_INITIAL_Z             0.
+#define NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_X 0.
+#define NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Y 0.
+#define NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Z 0.
+#define NPS_GPS_POS_LATENCY                    0.
+
+#else
+
+#define NPS_GPS_SPEED_NOISE_STD_DEV            0.5
+#define NPS_GPS_SPEED_LATENCY                  0.2
+#define NPS_GPS_POS_NOISE_STD_DEV              2
+#define NPS_GPS_POS_BIAS_INITIAL_X             0e-1
+#define NPS_GPS_POS_BIAS_INITIAL_Y            -0e-1
+#define NPS_GPS_POS_BIAS_INITIAL_Z            -0e-1
+#define NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_X 1e-3
+#define NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Y 1e-3
+#define NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Z 1e-3
+#define NPS_GPS_POS_LATENCY                    0.2
+
+#endif /* GPS_PERFECT */
+
+#define NPS_GPS_DT                           (1./5.)
+
+#endif /* NPS_SENSORS_PARAMS_H */

conf/telemetry/fixedwing_flight_recorder.xml

@@ -137,6 +137,7 @@
       <message name="DATALINK_REPORT"     period="1.0"/>
       <message name="ESC"                 period="0.1"/>
       <message name="ADC_GENERIC"         period="0.05"/>
+      <message name="WIND_INFO_RET"       period="0.1"/>
     </mode>
   </process>
 </telemetry>

sw/airborne/boards/apogee/chibios/v1.0/board.h

@@ -932,7 +932,7 @@
 #if (USE_PWM1 && USE_PWM_INPUT2)
 #error "PW1 and PWM_INPUT2 are not compatible"
 #endif
-#define PWM_INPUT2_ICU            ICUD2
+#define PWM_INPUT2_ICU            ICUD9
 #define PWM_INPUT2_CHANNEL        ICU_CHANNEL_1
 #define PWM_INPUT2_GPIO_PORT      GPIOA
 #define PWM_INPUT2_GPIO_PIN       GPIO2

sw/airborne/boards/apogee/chibios/v1.0/mcuconf.h

@@ -194,11 +194,7 @@
  * ICU driver system settings.
  */
 #define STM32_ICU_USE_TIM1                  TRUE
-#ifdef USE_PWM_INPUT2
-#define STM32_ICU_USE_TIM2                  TRUE
-#else
 #define STM32_ICU_USE_TIM2                  FALSE
-#endif
 #define STM32_ICU_USE_TIM3                  FALSE
 #define STM32_ICU_USE_TIM4                  FALSE
 #define STM32_ICU_USE_TIM5                  FALSE

sw/airborne/modules/meteo/lib_ukf_wind_estimator/UKF_Wind_Estimator.h

@@ -0,0 +1,115 @@
+/*
+ * File: UKF_Wind_Estimator.h
+ *
+ * Code generated for Simulink model 'UKF_Wind_Estimator'.
+ *
+ * Model version                  : 1.120
+ * Simulink Coder version         : 8.10 (R2016a) 10-Feb-2016
+ * C/C++ source code generated on : Wed Nov  2 23:49:42 2016
+ *
+ * Target selection: ert.tlc
+ * Embedded hardware selection: Custom Processor->Custom
+ * Code generation objectives:
+ *    1. Execution efficiency
+ *    2. RAM efficiency
+ * Validation result: Not run
+ */
+
+#ifndef RTW_HEADER_UKF_Wind_Estimator_h_
+#define RTW_HEADER_UKF_Wind_Estimator_h_
+#include <math.h>
+#include <string.h>
+#ifndef UKF_Wind_Estimator_COMMON_INCLUDES_
+# define UKF_Wind_Estimator_COMMON_INCLUDES_
+#include "rtwtypes.h"
+#endif                                 /* UKF_Wind_Estimator_COMMON_INCLUDES_ */
+
+/* Macros for accessing real-time model data structure */
+
+/* Block signals and states (auto storage) for system '<Root>' */
+typedef struct {
+  real32_T Delay1_DSTATE[7];           /* '<Root>/ Delay1' */
+  real32_T Delay_DSTATE[49];           /* '<Root>/ Delay' */
+  boolean_T x_not_empty;               /* '<Root>/initialization' */
+  boolean_T P_not_empty;               /* '<Root>/initialization' */
+} DW;
+
+/* External inputs (root inport signals with auto storage) */
+typedef struct {
+  real32_T rates[3];                   /* '<Root>/rates' */
+  real32_T accel[3];                   /* '<Root>/accel' */
+  real32_T q[4];                       /* '<Root>/q' */
+  real32_T vk[3];                      /* '<Root>/vk' */
+  real32_T va;                         /* '<Root>/va ' */
+  real32_T aoa;                        /* '<Root>/aoa' */
+  real32_T sideslip;                   /* '<Root>/sideslip' */
+} ExtU;
+
+/* External outputs (root outports fed by signals with auto storage) */
+typedef struct {
+  real32_T xout[7];                    /* '<Root>/xout' */
+  real32_T Pout[49];                   /* '<Root>/Pout' */
+} ExtY;
+
+/* Type definition for custom storage class: Struct */
+typedef struct ukf_init_tag {
+  real32_T x0[7];
+  real32_T P0[49];
+  real32_T ki;
+  real32_T alpha;
+  real32_T beta;
+} ukf_init_type;
+
+typedef struct ukf_params_tag {
+  real32_T Q[49];
+  real32_T R[36];
+  real32_T dt;
+} ukf_params_type;
+
+/* Block signals and states (auto storage) */
+extern DW ukf_DW;
+
+/* External inputs (root inport signals with auto storage) */
+extern ExtU ukf_U;
+
+/* External outputs (root outports fed by signals with auto storage) */
+extern ExtY ukf_Y;
+
+/* Model entry point functions */
+extern void UKF_Wind_Estimator_initialize(void);
+extern void UKF_Wind_Estimator_step(void);
+
+/* Exported data declaration */
+
+/* Declaration for custom storage class: Struct */
+extern ukf_init_type ukf_init;
+extern ukf_params_type ukf_params;
+
+/*-
+ * The generated code includes comments that allow you to trace directly
+ * back to the appropriate location in the model.  The basic format
+ * is <system>/block_name, where system is the system number (uniquely
+ * assigned by Simulink) and block_name is the name of the block.
+ *
+ * Use the MATLAB hilite_system command to trace the generated code back
+ * to the model.  For example,
+ *
+ * hilite_system('<S3>')    - opens system 3
+ * hilite_system('<S3>/Kp') - opens and selects block Kp which resides in S3
+ *
+ * Here is the system hierarchy for this model
+ *
+ * '<Root>' : 'UKF_Wind_Estimator'
+ * '<S1>'   : 'UKF_Wind_Estimator/UKF_correction'
+ * '<S2>'   : 'UKF_Wind_Estimator/UKF_prediction'
+ * '<S3>'   : 'UKF_Wind_Estimator/initialization'
+ * '<S4>'   : 'UKF_Wind_Estimator/main'
+ * '<S5>'   : 'UKF_Wind_Estimator/sigmas'
+ */
+#endif                                 /* RTW_HEADER_UKF_Wind_Estimator_h_ */
+
+/*
+ * File trailer for generated code.
+ *
+ * [EOF]
+ */

sw/airborne/modules/meteo/lib_ukf_wind_estimator/rtwtypes.h

@@ -0,0 +1,96 @@
+/*
+ * File: rtwtypes.h
+ *
+ * Code generated for Simulink model 'UKF_Wind_Estimator'.
+ *
+ * Model version                  : 1.120
+ * Simulink Coder version         : 8.10 (R2016a) 10-Feb-2016
+ * C/C++ source code generated on : Wed Nov  2 23:49:42 2016
+ *
+ * Target selection: ert.tlc
+ * Embedded hardware selection: Custom Processor->Custom
+ * Code generation objectives:
+ *    1. Execution efficiency
+ *    2. RAM efficiency
+ * Validation result: Not run
+ */
+
+#ifndef RTWTYPES_H
+#define RTWTYPES_H
+
+/* Logical type definitions */
+#if (!defined(__cplusplus))
+#  ifndef false
+#   define false                       (0U)
+#  endif
+
+#  ifndef true
+#   define true                        (1U)
+#  endif
+#endif
+
+/*=======================================================================*
+ * Target hardware information
+ *   Device type: Custom Processor->Custom
+ *   Number of bits:     char:   8    short:   16    int:  32
+ *                       long:  32
+ *                       native word size:  32
+ *   Byte ordering: LittleEndian
+ *   Signed integer division rounds to: Zero
+ *   Shift right on a signed integer as arithmetic shift: on
+ *=======================================================================*/
+
+/*=======================================================================*
+ * Fixed width word size data types:                                     *
+ *   int8_T, int16_T, int32_T     - signed 8, 16, or 32 bit integers     *
+ *   uint8_T, uint16_T, uint32_T  - unsigned 8, 16, or 32 bit integers   *
+ *   real32_T, real64_T           - 32 and 64 bit floating point numbers *
+ *=======================================================================*/
+typedef signed char int8_T;
+typedef unsigned char uint8_T;
+typedef short int16_T;
+typedef unsigned short uint16_T;
+typedef int int32_T;
+typedef unsigned int uint32_T;
+typedef float real32_T;
+typedef double real64_T;
+
+/*===========================================================================*
+ * Generic type definitions: boolean_T, char_T, byte_T, int_T, uint_T,       *
+ *                           real_T, time_T, ulong_T.                        *
+ *===========================================================================*/
+typedef double real_T;
+typedef double time_T;
+typedef unsigned char boolean_T;
+typedef int int_T;
+typedef unsigned int uint_T;
+typedef unsigned long ulong_T;
+typedef char char_T;
+typedef unsigned char uchar_T;
+typedef char_T byte_T;
+
+/*=======================================================================*
+ * Min and Max:                                                          *
+ *   int8_T, int16_T, int32_T     - signed 8, 16, or 32 bit integers     *
+ *   uint8_T, uint16_T, uint32_T  - unsigned 8, 16, or 32 bit integers   *
+ *=======================================================================*/
+#define MAX_int8_T                     ((int8_T)(127))
+#define MIN_int8_T                     ((int8_T)(-128))
+#define MAX_uint8_T                    ((uint8_T)(255U))
+#define MAX_int16_T                    ((int16_T)(32767))
+#define MIN_int16_T                    ((int16_T)(-32768))
+#define MAX_uint16_T                   ((uint16_T)(65535U))
+#define MAX_int32_T                    ((int32_T)(2147483647))
+#define MIN_int32_T                    ((int32_T)(-2147483647-1))
+#define MAX_uint32_T                   ((uint32_T)(0xFFFFFFFFU))
+
+/* Block D-Work pointer type */
+typedef void * pointer_T;
+
+#endif                                 /* RTWTYPES_H */
+
+/*
+ * File trailer for generated code.
+ *
+ * [EOF]
+ */

sw/airborne/modules/meteo/wind_estimator.h

@@ -0,0 +1,66 @@
+/*
+ * Copyright (C) 2016 Johan Maurin, 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/meteo/wind_estimator.h"
+ * @author Maurin
+ *
+ * Wind Estimator based on generated library from Matlab
+ */
+
+#ifndef WIND_ESTIMATOR_H
+#define WIND_ESTIMATOR_H
+
+#include "std.h"
+#include "math/pprz_algebra_float.h"
+
+struct WindEstimator {
+  struct FloatVect3 airspeed;   ///< airspeed vector in body frame
+  struct FloatVect3 wind;       ///< wind vector in NED frame
+  bool data_available;          ///< new data available
+  bool reset;                   ///< reset filter flag
+
+  // filter parameters
+  float r_gs;       ///< noise associated to ground speed measurement
+  float r_va;       ///< noise associated to airspeed norm measurement
+  float r_aoa;      ///< noise associated to angle of attack measurement
+  float r_ssa;      ///< noise associated to sideslip angle measurement
+  float q_va;       ///< noise associated to airspeed vector model
+  float q_wind;     ///< noise associated to wind vector model
+  float q_va_scale; ///< noise associated to airspeed scale factor model
+};
+
+extern struct WindEstimator wind_estimator;
+
+extern void wind_estimator_init(void);
+extern void wind_estimator_periodic(void);
+extern void wind_estimator_event(void);
+
+// paramenters settings handler
+extern void wind_estimator_Set_R_GS(float _v);
+extern void wind_estimator_Set_R_VA(float _v);
+extern void wind_estimator_Set_R_AOA(float _v);
+extern void wind_estimator_Set_R_SSA(float _v);
+extern void wind_estimator_Set_Q_VA(float _v);
+extern void wind_estimator_Set_Q_WIND(float _v);
+extern void wind_estimator_Set_Q_VA_SCALE(float _v);
+
+#endif
+

sw/airborne/modules/sensors/aoa_pwm.c

@@ -23,6 +23,11 @@
 * @author Jean-François Erdelyi
 * @brief Angle of Attack sensor on PWM
 *
+* Driver for a PWM based angle of attack sensor
+* A second sensor can be defined for the sideslip angle
+* It is assumed that both sensors are the same, only
+* sensitivity, offset and direction can differ.
+*
 * SENSOR, exemple : US DIGITAL MA3-P12-125-B
 * @see http://www.usdigital.com/products/encoders/absolute/rotary/shaft/ma3
 */
@@ -38,6 +43,8 @@
 bool log_started;
 #endif
 
+/* Config parameters for angle of attack sensor (mandatory) */
+
 #ifndef AOA_PWM_CHANNEL
 #error "AOA_PWM_CHANNEL needs to be defined to use AOA_pwm module"
 #endif
@@ -83,16 +90,57 @@ bool log_started;
 
 struct Aoa_Pwm aoa_pwm;
 
+
+/* Config parameters for sideslip angle sensor (optional) */
+
+#if defined USE_SIDESLIP && !(defined SSA_PWM_CHANNEL)
+#error "SSA_PWM_CHANNEL needs to be defined to use sideslip sensor"
+#endif
+
+// Default extra offset that can be ajusted from settings
+#ifndef SSA_OFFSET
+#define SSA_OFFSET 0.0f
+#endif
+// Default filter value
+#ifndef SSA_FILTER
+#define SSA_FILTER 0.0f
+#endif
+// Default sensitivity (2*pi on a PWM of period AOA_PWM_PERIOD)
+#ifndef SSA_SENS
+#define SSA_SENS ((2.0f*M_PI)/AOA_PWM_PERIOD)
+#endif
+// Set SSA_REVERSE to TRUE to change rotation direction
+#if SSA_REVERSE
+#define SSA_SIGN -1
+#else
+#define SSA_SIGN 1
+#endif
+struct Aoa_Pwm ssa_pwm;
+
+
+/* telemetry */
+enum Aoa_Type aoa_send_type;
+
 #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);
+  // FIXME use a second message, more fields or add a sensor ID to send sideslip
+  switch (aoa_send_type) {
+    case SEND_TYPE_SIDESLIP:
+      pprz_msg_send_AOA(trans, dev, AC_ID, &ssa_pwm.raw, &ssa_pwm.angle);
+      break;
+    case SEND_TYPE_AOA:
+    default:
+      pprz_msg_send_AOA(trans, dev, AC_ID, &aoa_pwm.raw, &aoa_pwm.angle);
+      break;
+  }
 }
 
 #endif
 
+/* init */
 void aoa_pwm_init(void)
 {
   aoa_pwm.offset = AOA_OFFSET;
@@ -100,22 +148,28 @@ void aoa_pwm_init(void)
   aoa_pwm.sens = AOA_SENS;
   aoa_pwm.angle = 0.0f;
   aoa_pwm.raw = 0.0f;
+  ssa_pwm.offset = SSA_OFFSET;
+  ssa_pwm.filter = SSA_FILTER;
+  ssa_pwm.sens = SSA_SENS;
+  ssa_pwm.angle = 0.0f;
+  ssa_pwm.raw = 0.0f;
 #if LOG_AOA
   log_started = false;
 #endif
+  aoa_send_type = SEND_TYPE_AOA;
 #if PERIODIC_TELEMETRY
   register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AOA, send_aoa);
 #endif
 }
 
+/* update, log and send */
 void aoa_pwm_update(void) {
   static float prev_aoa = 0.0f;
-
   // raw duty cycle in usec
-  uint32_t duty_raw = get_pwm_input_duty_in_usec(AOA_PWM_CHANNEL);
+  uint32_t aoa_duty_raw = get_pwm_input_duty_in_usec(AOA_PWM_CHANNEL);
 
   // remove some offset if needed
-  aoa_pwm.raw = duty_raw - AOA_PWM_OFFSET;
+  aoa_pwm.raw = aoa_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);
@@ -127,18 +181,36 @@ void aoa_pwm_update(void) {
   stateSetAngleOfAttack_f(aoa_pwm.angle);
 #endif
 
+#if USE_SIDESLIP
+  static float prev_ssa = 0.0f;
+  // raw duty cycle in usec
+  uint32_t ssa_duty_raw = get_pwm_input_duty_in_usec(SSA_PWM_CHANNEL);
+
+  // remove some offset if needed
+  ssa_pwm.raw = ssa_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
+  ssa_pwm.angle = SSA_SIGN * (((float)ssa_pwm.raw * ssa_pwm.sens) - ssa_pwm.offset - AOA_ANGLE_OFFSET);
+  // filter angle
+  ssa_pwm.angle = ssa_pwm.filter * prev_ssa + (1.0f - ssa_pwm.filter) * ssa_pwm.angle;
+  prev_ssa = ssa_pwm.angle;
+
+  stateSetSideslip_f(ssa_pwm.angle);
+#endif
+
 #if SEND_SYNC_AOA
-  RunOnceEvery(10, DOWNLINK_SEND_AOA(DefaultChannel, DefaultDevice, &aoa_pwm.raw, &aoa_pwm.angle));
+  RunOnceEvery(10, send_aoa(&(DefaultChannel).trans_tx, &(DefaultDevice).device));
 #endif
 
 #if LOG_AOA
   if(pprzLogFile != -1) {
     if (!log_started) {
-      sdLogWriteLog(pprzLogFile, "AOA_PWM: ANGLE(deg) RAW(int16)\n");
+      sdLogWriteLog(pprzLogFile, "AOA_PWM: AOA_ANGLE(deg) AOA_RAW(int16) SSA_ANGLE(deg) SSA_RAW(int16)\n");
       log_started = true;
     } else {
-      float angle = DegOfRad(aoa_pwm.angle);
-      sdLogWriteLog(pprzLogFile, "AOA_PWM: %.3f %d\n", angle, aoa_pwm.raw);
+      float aoa_angle = DegOfRad(aoa_pwm.angle);
+      float ssa_angle = DegOfRad(ssa_pwm.angle);
+      sdLogWriteLog(pprzLogFile, "AOA_PWM: %.3f %d %.3f %d\n", aoa_angle, aoa_pwm.raw, ssa_angle, ssa_pwm.raw);
     }
   }
 #endif

sw/airborne/modules/sensors/aoa_pwm.h

@@ -23,6 +23,11 @@
 * @author Jean-François Erdelyi
 * @brief Angle of Attack sensor on PWM
 *
+* Driver for a PWM based angle of attack sensor
+* A second sensor can be defined for the sideslip angle
+* It is assumed that both sensors are the same, only
+* sensitivity, offset and direction can differ.
+*
 * SENSOR, example : US DIGITAL MA3-P12-125-B
 */
 
@@ -44,7 +49,16 @@ struct Aoa_Pwm {
   float filter;
 };
 
-extern struct Aoa_Pwm aoa_pwm;
+extern struct Aoa_Pwm aoa_pwm; // angle of attack sensor
+extern struct Aoa_Pwm ssa_pwm; // sideslip angle sensor
+
+/** Selection of sensor type to be send over telemetry
+ */
+enum Aoa_Type {
+  SEND_TYPE_AOA,
+  SEND_TYPE_SIDESLIP
+};
+extern enum Aoa_Type aoa_send_type;
 
 extern void aoa_pwm_init(void);
 extern void aoa_pwm_update(void);

sw/simulator/mesonh/lauch_test

@@ -0,0 +1,2 @@
+python mesonh.py -t 3600 -f ARMHR.1.sc464.001diaKCL.nc ARMHR.1.sc464.002diaKCL.nc ARMHR.1.sc464.003diaKCL.nc ARMHR.1.sc464.004diaKCL.nc ARMHR.1.sc464.005diaKCL.nc ARMHR.1.sc464.006diaKCL.nc ARMHR.1.sc464.007diaKCL.nc ARMHR.1.sc464.008diaKCL.nc -x 2.2 -y 2.2
+

sw/simulator/mesonh/mesonh.py

@@ -0,0 +1,131 @@
+from __future__ import absolute_import, print_function, division
+from mesonh_atmosphere import MesoNHAtmosphere
+import os
+import sys
+import signal
+import time
+import socket
+import struct
+import cmath
+import numpy as np
+from os import getenv
+# if PAPARAZZI_SRC not set, then assume the tree containing this
+# file is a reasonable substitute
+PPRZ_SRC = getenv("PAPARAZZI_SRC", os.path.normpath(os.path.join(os.path.dirname(os.path.abspath(__file__)), '../../../')))
+sys.path.append(PPRZ_SRC + "/sw/ext/pprzlink/lib/v1.0/python")
+
+from pprzlink.ivy import IvyMessagesInterface
+from pprzlink.message import PprzMessage
+
+M_IN_KM = 1000.
+
+atm = None
+origin = np.array([0, 0, 0, 0])
+scale = np.array([1., 1/M_IN_KM, 1/M_IN_KM, 1/M_IN_KM])
+
+start_time = time.time()
+
+
+def get_wind(east, north, up):
+    t = time.time() - start_time
+    print("east :",east)
+    print("north :",north)
+    print("up :",up)
+    loc = np.array([t, up, east, north])
+    loc = loc*scale + origin
+    print("loc:",loc)
+    weast, wnorth, wup = atm.get_wind(loc)
+    return weast, wnorth, wup
+
+
+def ivy_request_callback(sender, msg, resp, *args, **kwargs):
+    """
+        Ivy Callback for Paparazzi Requests
+    """
+
+    if msg.msg_class == "ground" and msg.name == "WORLD_ENV_REQ":
+        return worldenv_cb(msg, resp)
+    else:
+        return None
+
+
+#def worldenv_cb(m, r):
+def worldenv_cb(ac_id, msg):
+    """
+        Callback for paparazzi WORLD_ENV requests
+    """
+    # request location (in meters)
+    east, north, up = float(msg.get_field(3)),\
+        float(msg.get_field(4)),\
+        float(msg.get_field(5))
+    up *= -1
+    # convert in km + translation with mesoNH origin
+    weast, wnorth, wup = get_wind(east, north, up)
+    print("wind_est:")
+    print(weast)
+    print(wnorth)
+    print(wup)
+    msg_back=PprzMessage("ground", "WORLD_ENV")
+    msg_back.set_value_by_name("wind_east",weast)
+    msg_back.set_value_by_name("wind_north",wnorth)
+    msg_back.set_value_by_name("wind_up",wup)
+    msg_back.set_value_by_name("ir_contrast",400)
+    msg_back.set_value_by_name("time_scale",1)
+    msg_back.set_value_by_name("gps_availability",1)
+    ivy.send(msg_back,None)
+
+
+def signal_handler(signal, frame):
+    print('\nShutting down IVY...')
+    ivy.shutdown()
+    print("Done.")
+
+
+def main():
+    # parse arguments
+    import argparse as ap
+
+    argp = ap.ArgumentParser(description="Environment variables provider "
+                             "for Paparazzi simulation from MesoNH data")
+
+    argp.add_argument("-t", "--time-step", required=True, type=int,
+                      help="Duration of a time step between MesoNH Files.")
+    argp.add_argument("-f", "--files", required=True, nargs='+',
+                      help="MesoNH netCDF files, in temporal ordering")
+    argp.add_argument("-x", "--origin-x", required=False, type=float,
+                      default=0.,
+                      help="Origin translation x.")
+    argp.add_argument("-y", "--origin-y", required=False, type=float,
+                      default=0.,
+                      help="Origin translation y.")
+    argp.add_argument("-z", "--origin-z", required=False, type=float,
+                      default=0.,
+                      help="Origin translation z.")
+    arguments = argp.parse_args()
+
+    print(arguments)
+
+    # register signal handler for ctrl+c to stop the program
+    signal.signal(signal.SIGINT, signal_handler)
+
+    # origin for translation from paparazzi to mesoNH frame
+    global origin
+    origin = np.array([0, arguments.origin_z, arguments.origin_x, arguments.origin_y])
+
+    # build atmosphere simulation source
+    global atm
+    atm = MesoNHAtmosphere(arguments.files, arguments.time_step, tinit=0)
+
+    # init ivy and register callback for WORLD_ENV_REQ and NPS_SPEED_POS
+    global ivy
+    ivy = IvyMessagesInterface("MesoNH");
+    ivy.subscribe(worldenv_cb,'(.* WORLD_ENV_REQ .*)')
+
+    # wait for ivy to stop
+    from ivy.std_api import IvyMainLoop
+
+    signal.pause()
+
+
+if __name__ == '__main__':
+    main()

sw/simulator/mesonh/mesonh_atmosphere.py

@@ -0,0 +1,147 @@
+"""
+ Reading mesoNH netCDF files and providing an interface to read chunks
+of data and interpolate points.
+
+ TODO : test with large netCDF files, add mapping of variables as an argument.
+"""
+
+from __future__ import division, print_function, absolute_import
+from netCDF4 import MFDataset, Dataset
+import numpy as np
+from scipy.interpolate import RegularGridInterpolator
+
+
+def find_lt(a, x):
+    'Find rightmost value less than x'
+    return np.searchsorted(a, x, side='left')-1
+
+
+def find_le(a, x):
+    'Find rightmost value less than or equal to x'
+    return np.searchsorted(a, x, side='right')-1
+
+
+def find_gt(a, x):
+    'Find leftmost value greater than x'
+    return np.searchsorted(a, x, side='right')
+
+
+def find_ge(a, x):
+    'Find leftmost item greater than or equal to x'
+    return np.searchsorted(a, x, side='left')
+
+WX = 'UT'  # Wind x,y and z composants
+WY = 'VT'
+WZ = 'WT'
+PTEMP = 'THT'  # potential temperature
+RVAP = 'RVT'  # vapor mixing ratio
+LWATER = 'RCT'  # liquid water
+ZSCALE = 'VLEV'  # vertical level (height) scale
+XSCALE = 'S_N_direction'  # south -> north axis scale
+YSCALE = 'W_E_direction'  # west -> east axis scale
+TIME = 'time'  # time dimension
+X = 2
+Y = 3
+Z = 1
+T = 0
+
+
+class MesoNHAtmosphere:
+    """
+     Interpolation in space-time from mesoNH data over a grid.
+
+     The functions valued on the grid are assumed to be periodic in the
+     x and y axis (south->north and west->east) :
+     f(x)=f(x+xmax-xmin) where [xmin,xmax] are the bounds on x
+     of the mesoNH grid.
+
+         Clipping is done on the z (height) and time axis :
+     f(z)=f(min(max(z,zmin),zmax)) where [zmin,zmax] are the bound on z
+     of the mesoNH grid.
+
+     Two interpolation methods are currently supported : nearest neighbour
+     and linear.
+
+     Uses RegularGridInterpolator from scipy package.
+
+     """
+    data = []
+
+    interpolator = None
+    boundsmax = None
+    boundsmin = None
+
+    def __init__(self, files, tstep, tinit=0):
+        if np.isscalar(files):
+            self.data = Dataset(files)
+        else:
+            self.data = MFDataset(files)
+        pz = self.data.variables[ZSCALE][:, 0, 0]
+        px = self.data.variables[XSCALE]
+        py = self.data.variables[YSCALE]
+        pt = np.array([tinit + i * tstep for i in
+                       range(self.data.variables[TIME].shape[0])])
+        self.boundsmax = [pt[-1], pz[-1], px[-1], py[-1]]
+        self.boundsmin = [pt[0], pz[0], px[0], py[0]]
+
+        self.grid_coordinates = (pt, pz, px, py)
+        self.grid_shape = (len(pt), len(pz), len(px), len(py))
+
+    def get_points(self, points, var, method='nearest'):
+        """ Get value of variable on points
+   
+        Arguments:
+        points: a ndarray containing the point coordinates on the last
+        dimension
+        var: the name of the variable in the mesoNH file(s)
+        method: 'nearest' and 'linear' interpolation are currently supported
+        """
+        points = np.array(points)
+        p = self._apply_bounds(points)
+        caxes = tuple(range(p.ndim-1))
+        bounds = zip(p.min(axis=caxes), p.max(axis=caxes))
+        interpolator = self._get_interpolator(bounds, var, method)
+        return interpolator(p, method).squeeze()
+
+    def _get_interpolator(self, bounds, var, method="nearest"):
+        slice_indexes, coordinates = self._slicyfy(bounds)
+        values = self._get_var_values(var, slice_indexes)
+        ip = RegularGridInterpolator(coordinates, values, method)
+        return ip
+
+    def _slicyfy(self, bounds):
+        slice_indexes = ()
+        coordinates = ()
+        for d, b in enumerate(bounds):
+            dslice = slice(find_le(self.grid_coordinates[d], b[0]),
+                           find_gt(self.grid_coordinates[d], b[1])+1)
+            slice_indexes += dslice,
+            coordinates += self.grid_coordinates[d][dslice],
+
+        return slice_indexes, coordinates
+
+    def _get_var_values(self, var, idx=Ellipsis):
+        return self.data[var][idx]
+
+    def _apply_bounds(self, point):
+        x = point[..., X]
+        y = point[..., Y]
+        z = point[..., Z]
+        t = point[..., T]
+
+        p = np.ndarray(point.shape)
+        p[..., T] = np.clip(t, self.boundsmin[T], self.boundsmax[T])
+        p[..., Z] = np.clip(z, self.boundsmin[Z], self.boundsmax[Z])
+        p[..., X] = (x-self.boundsmin[X]) % (self.boundsmax[X]-self.boundsmin[X])\
+            + self.boundsmin[X]
+        p[..., Y] = (y-self.boundsmin[Y]) % (self.boundsmax[Y]-self.boundsmin[Y])\
+            + self.boundsmin[Y]
+
+        return p
+
+    def get_wind(self, points, method='nearest'):
+        """Convenience method for getting 3D wind. See get_points."""
+        return np.array((self.get_points(points, WX, method),
+                         self.get_points(points, WY, method),
+                         self.get_points(points, WZ, method)
+                         ))

sw/simulator/nps/nps_autopilot_fixedwing.c

@@ -138,7 +138,7 @@ void nps_autopilot_run_step(double time)
     AbiSendMsgTEMPERATURE(BARO_SIM_SENDER_ID, (float)sensors.temp.value);
   }
 
-#if USE_AIRSPEED
+#if USE_AIRSPEED || USE_NPS_AIRSPEED
   if (nps_sensors_airspeed_available()) {
     stateSetAirspeed_f((float)sensors.airspeed.value);
     Fbw(event_task);
@@ -165,6 +165,22 @@ void nps_autopilot_run_step(double time)
   }
 #endif
 
+#if USE_NPS_AOA
+  if (nps_sensors_aoa_available()) {
+    stateSetAngleOfAttack_f((float)sensors.aoa.value);
+    Fbw(event_task);
+    Ap(event_task);
+  }
+#endif
+
+#if USE_NPS_SIDESLIP
+  if (nps_sensors_sideslip_available()) {
+    stateSetSideslip_f((float)sensors.sideslip.value);
+    Fbw(event_task);
+    Ap(event_task);
+  }
+#endif
+
   if (nps_bypass_ahrs) {
     sim_overwrite_ahrs();
   }

sw/simulator/nps/nps_fdm.h

@@ -112,6 +112,8 @@ struct NpsFdm {
   double dynamic_pressure; ///< dynamic pressure in Pascal
   double temperature;      ///< current temperature in degrees Celcius
   double pressure_sl;      ///< pressure at sea level in Pascal
+  double aoa;			         ///< angle of attack in rad
+  double sideslip;         ///< sideslip angle in rad
 
   // Control surface positions (normalized values)
   float elevator;

sw/simulator/nps/nps_fdm_jsbsim.cpp

@@ -42,6 +42,7 @@
 #include <models/FGAccelerations.h>
 #include <models/FGAuxiliary.h>
 #include <models/FGAtmosphere.h>
+#include <models/FGAircraft.h>
 #include <models/FGFCS.h>
 #include <models/atmosphere/FGWinds.h>
 
@@ -463,6 +464,12 @@ static void fetch_state(void)
   fdm.dynamic_pressure = PascalOfPsf(FDMExec->GetAuxiliary()->Getqbar());
   fdm.temperature = CelsiusOfRankine(FDMExec->GetAtmosphere()->GetTemperature());
 
+  /*
+   *  angle of attack and SlideSlip.
+   */
+  fdm.aoa= FDMExec->GetPropertyManager()->GetNode("aero/alpha-rad")->getDoubleValue();
+  fdm.sideslip = FDMExec->GetPropertyManager()->GetNode("aero/beta-rad")->getDoubleValue();
+
   /*
    * Control surface positions
    *

sw/simulator/nps/nps_sensor_aoa.c

@@ -0,0 +1,77 @@
+/*
+ * Copyright (C) 2016 Johan Maurin, 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 nps_sensor_aoa.c
+ *
+ * Simulated Angle of Attack of the Wind for NPS simulator.
+ *
+ */
+
+#include "nps_sensor_aoa.h"
+
+#include "generated/airframe.h"
+
+#include "std.h"
+#include "nps_fdm.h"
+#include "nps_random.h"
+#include NPS_SENSORS_PARAMS
+
+/// 10Hz default
+#ifndef NPS_AOA_DT
+#define NPS_AOA_DT 0.01
+#endif
+
+/// standard deviation in radian (default 0.001 rad)
+#ifndef NPS_AOA_NOISE_STD_DEV
+#define NPS_AOA_NOISE_STD_DEV 0.001
+#endif
+
+#ifndef NPS_AOA_OFFSET
+#define NPS_AOA_OFFSET 0
+#endif
+
+
+void nps_sensor_aoa_init(struct NpsSensorAngleOfAttack *aoa, double time)
+{
+  aoa->value = 0.;
+  aoa->offset = NPS_AOA_OFFSET;
+  aoa->noise_std_dev = NPS_AOA_NOISE_STD_DEV;
+  aoa->next_update = time;
+  aoa->data_available = FALSE;
+}
+
+
+void nps_sensor_aoa_run_step(struct NpsSensorAngleOfAttack *aoa, double time)
+{
+
+  if (time < aoa->next_update) {
+    return;
+  }
+
+  /* equivalent airspeed + sensor offset */
+  aoa->value = fdm.aoa + aoa->offset;
+  /* add noise with std dev rad */
+  aoa->value += get_gaussian_noise() * aoa->noise_std_dev;
+
+  aoa->next_update += NPS_AOA_DT;
+  aoa->data_available = TRUE;
+}
+

sw/simulator/nps/nps_sensor_aoa.h

@@ -0,0 +1,48 @@
+/*
+ * Copyright (C) 2016 Johan Maurin, 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 nps_sensor_aoa.h
+ *
+ * Simulated Angle Of Attack of the Wind for NPS simulator.
+ *
+ */
+
+#ifndef NPS_SENSOR_AOA_H
+#define NPS_SENSOR_AOA_H
+
+#include "math/pprz_algebra.h"
+#include "math/pprz_algebra_double.h"
+#include "math/pprz_algebra_float.h"
+#include "std.h"
+
+struct NpsSensorAngleOfAttack {
+  double value;          ///< angle of attack reading in radian
+  double offset;         ///< offset in meters/second
+  double noise_std_dev;  ///< noise standard deviation
+  double next_update;
+  bool data_available;
+};
+
+
+extern void nps_sensor_aoa_init(struct NpsSensorAngleOfAttack *aoa, double time);
+extern void nps_sensor_aoa_run_step(struct NpsSensorAngleOfAttack *aoa, double time);
+
+#endif /* NPS_SENSOR_AOA_H */

sw/simulator/nps/nps_sensor_sideslip.c

@@ -0,0 +1,78 @@
+/*
+ * Copyright (C) 2016 Johan Maurin, 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 nps_sensor_sideslip.c
+ *
+ * Simulated Angle of Attack of the Wind for NPS simulator.
+ *
+ */
+
+#include "nps_sensor_sideslip.h"
+
+#include "generated/airframe.h"
+#include <math.h>
+
+#include "std.h"
+#include "nps_fdm.h"
+#include "nps_random.h"
+#include NPS_SENSORS_PARAMS
+
+/// 10Hz default
+#ifndef NPS_SIDESLIP_DT
+#define NPS_SIDESLIP_DT 0.01
+#endif
+
+/// standard deviation in radian (default 0.001 rad)
+#ifndef NPS_SIDESLIP_NOISE_STD_DEV
+#define NPS_SIDESLIP_NOISE_STD_DEV 0.001
+#endif
+
+#ifndef NPS_SIDESLIP_OFFSET
+#define NPS_SIDESLIP_OFFSET 0
+#endif
+
+
+void nps_sensor_sideslip_init(struct NpsSensorSideSlip *sideslip, double time)
+{
+  sideslip->value = 0.;
+  sideslip->offset = NPS_SIDESLIP_OFFSET;
+  sideslip->noise_std_dev = NPS_SIDESLIP_NOISE_STD_DEV;
+  sideslip->next_update = time;
+  sideslip->data_available = FALSE;
+}
+
+
+void nps_sensor_sideslip_run_step(struct NpsSensorSideSlip *sideslip, double time)
+{
+
+  if (time < sideslip->next_update) {
+    return;
+  }
+
+  /* equivalent airspeed + sensor offset */
+  sideslip->value = fdm.sideslip + sideslip->offset;
+  /* add noise with std dev rad */
+  sideslip->value += get_gaussian_noise() * sideslip->noise_std_dev;
+
+  sideslip->next_update += NPS_SIDESLIP_DT;
+  sideslip->data_available = TRUE;
+}
+

sw/simulator/nps/nps_sensor_sideslip.h

@@ -0,0 +1,49 @@
+/*
+ * Copyright (C) 2016 Johan Maurin, 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 nps_sensor_sideslip.h
+ *
+ * Simulated Angle Of Attack of the Wind for NPS simulator.
+ *
+ */
+
+#ifndef NPS_SENSOR_SIDESLIP_H
+#define NPS_SENSOR_SIDESLIP_H
+
+#include "math/pprz_algebra.h"
+#include "math/pprz_algebra_double.h"
+#include "math/pprz_algebra_float.h"
+#include "std.h"
+
+struct NpsSensorSideSlip{
+  double value;          ///< sideslip reading in radian
+  double offset;         ///< offset in meters/second
+  double noise_std_dev;  ///< noise standard deviation
+  double next_update;
+  bool data_available;
+};
+
+
+extern void nps_sensor_sideslip_init(struct NpsSensorSideSlip *sideslip, double time);
+extern void nps_sensor_sideslip_run_step(struct NpsSensorSideSlip *sideslip, double time);
+
+#endif /* NPS_SENSOR_SIDESLIP_H */
+

sw/simulator/nps/nps_sensors.c

@@ -20,7 +20,8 @@ void nps_sensors_init(double time)
   nps_sensor_sonar_init(&sensors.sonar, time);
   nps_sensor_airspeed_init(&sensors.airspeed, time);
   nps_sensor_temperature_init(&sensors.temp, time);
-
+  nps_sensor_aoa_init(&sensors.aoa, time);
+  nps_sensor_sideslip_init(&sensors.sideslip,time);
 }
 
 
@@ -34,6 +35,8 @@ void nps_sensors_run_step(double time)
   nps_sensor_sonar_run_step(&sensors.sonar, time);
   nps_sensor_airspeed_run_step(&sensors.airspeed, time);
   nps_sensor_temperature_run_step(&sensors.temp, time);
+  nps_sensor_aoa_run_step(&sensors.aoa, time);
+  nps_sensor_sideslip_run_step(&sensors.sideslip,time);
 }
 
 
@@ -99,3 +102,21 @@ bool nps_sensors_temperature_available(void)
   }
   return FALSE;
 }
+
+bool nps_sensors_aoa_available(void)
+{
+  if (sensors.aoa.data_available) {
+    sensors.aoa.data_available = FALSE;
+    return TRUE;
+  }
+  return FALSE;
+}
+
+bool nps_sensors_sideslip_available(void)
+{
+  if (sensors.sideslip.data_available) {
+    sensors.sideslip.data_available = FALSE;
+    return TRUE;
+  }
+  return FALSE;
+}

sw/simulator/nps/nps_sensors.h

@@ -10,6 +10,8 @@
 #include "nps_sensor_sonar.h"
 #include "nps_sensor_airspeed.h"
 #include "nps_sensor_temperature.h"
+#include "nps_sensor_aoa.h"
+#include "nps_sensor_sideslip.h"
 
 struct NpsSensors {
   struct DoubleRMat body_to_imu_rmat;
@@ -21,6 +23,8 @@ struct NpsSensors {
   struct NpsSensorSonar sonar;
   struct NpsSensorAirspeed airspeed;
   struct NpsSensorTemperature temp;
+  struct NpsSensorAngleOfAttack aoa;
+  struct NpsSensorSideSlip sideslip;
 };
 
 extern struct NpsSensors sensors;
@@ -35,6 +39,7 @@ extern bool nps_sensors_gps_available();
 extern bool nps_sensors_sonar_available();
 extern bool nps_sensors_airspeed_available();
 extern bool nps_sensors_temperature_available();
-
+extern bool nps_sensors_aoa_available();
+extern bool nps_sensors_sideslip_available();
 
 #endif /* NPS_SENSORS_H */