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

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 */