[nps] simulate airspeed sensor

conf/firmwares/subsystems/fixedwing/fdm_crrcsim.makefile

@@ -40,6 +40,7 @@ nps.srcs += $(NPSDIR)/nps_main.c                 \
        $(NPSDIR)/nps_sensor_baro.c               \
        $(NPSDIR)/nps_sensor_sonar.c              \
        $(NPSDIR)/nps_sensor_gps.c                \
+       $(NPSDIR)/nps_sensor_airspeed.c           \
        $(NPSDIR)/nps_electrical.c                \
        $(NPSDIR)/nps_atmosphere.c                \
        $(NPSDIR)/nps_radio_control.c             \

conf/firmwares/subsystems/fixedwing/fdm_jsbsim.makefile

@@ -55,6 +55,7 @@ nps.srcs += $(NPSDIR)/nps_main.c                 \
        $(NPSDIR)/nps_sensor_baro.c               \
        $(NPSDIR)/nps_sensor_sonar.c              \
        $(NPSDIR)/nps_sensor_gps.c                \
+       $(NPSDIR)/nps_sensor_airspeed.c           \
        $(NPSDIR)/nps_electrical.c                \
        $(NPSDIR)/nps_atmosphere.c                \
        $(NPSDIR)/nps_radio_control.c             \

conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile

@@ -51,6 +51,7 @@ nps.srcs += $(NPSDIR)/nps_main.c                 \
        $(NPSDIR)/nps_sensor_baro.c               \
        $(NPSDIR)/nps_sensor_sonar.c              \
        $(NPSDIR)/nps_sensor_gps.c                \
+       $(NPSDIR)/nps_sensor_airspeed.c           \
        $(NPSDIR)/nps_electrical.c                \
        $(NPSDIR)/nps_atmosphere.c                \
        $(NPSDIR)/nps_radio_control.c             \

sw/simulator/nps/nps_autopilot_fixedwing.c

@@ -127,6 +127,14 @@ void nps_autopilot_run_step(double time)
     Ap(event_task);
   }
 
+#if USE_AIRSPEED
+  if (nps_sensors_airspeed_available()) {
+    stateSetAirspeed_f((float)sensors.airspeed.value);
+    Fbw(event_task);
+    Ap(event_task);
+  }
+#endif
+
   if (nps_sensors_gps_available()) {
     gps_feed_value();
     Fbw(event_task);

sw/simulator/nps/nps_sensor_airspeed.c

@@ -0,0 +1,87 @@
+/*
+ * Copyright (C) 2014 Felix Ruess <felix.ruess@gmail.com
+ *
+ * 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.
+ */
+
+/**
+ * @file nps_sensor_airspeed.c
+ *
+ * Simulated airspeed for NPS simulator.
+ *
+ */
+
+#include "nps_sensor_airspeed.h"
+
+#include "generated/airframe.h"
+
+#include "std.h"
+#include "nps_fdm.h"
+#include "nps_random.h"
+#include NPS_SENSORS_PARAMS
+
+/// 10Hz default
+#ifndef NPS_AIRSPEED_DT
+#define NPS_AIRSPEED_DT 0.01
+#endif
+
+/// standard deviation in meters/second (default 0.1 m/s)
+#ifndef NPS_AIRSPEED_NOISE_STD_DEV
+#define NPS_AIRSPEED_NOISE_STD_DEV 0.1
+#endif
+
+#ifndef NPS_AIRSPEED_OFFSET
+#define NPS_AIRSPEED_OFFSET 0
+#endif
+
+
+void nps_sensor_airspeed_init(struct NpsSensorAirspeed *airspeed, double time)
+{
+  airspeed->value = 0.;
+  airspeed->offset = NPS_AIRSPEED_OFFSET;
+  airspeed->noise_std_dev = NPS_AIRSPEED_NOISE_STD_DEV;
+  airspeed->next_update = time;
+  airspeed->data_available = FALSE;
+}
+
+
+void nps_sensor_airspeed_run_step(struct NpsSensorAirspeed *airspeed, double time)
+{
+
+  if (time < airspeed->next_update) {
+    return;
+  }
+
+  /* super simple approximation for now:
+   * airspeed = ground speed + wind
+   */
+  struct DoubleVect3 ltp_air_vel;
+  VECT3_SUM(ltp_air_vel, fdm.ltpprz_ecef_vel, fdm.wind);
+  double speed = double_vect3_norm(&ltp_air_vel);
+  /* sensor offset */
+  airspeed->value = speed + airspeed->offset;
+  /* add noise with std dev meters/second */
+  airspeed->value += get_gaussian_noise() * airspeed->noise_std_dev;
+  /* can't be negative, min is zero */
+  if (airspeed->value < 0) {
+    airspeed->value = 0.0;
+  }
+
+  airspeed->next_update += NPS_AIRSPEED_DT;
+  airspeed->data_available = TRUE;
+}

sw/simulator/nps/nps_sensor_airspeed.h

@@ -0,0 +1,49 @@
+/*
+ * Copyright (C) 2014 Felix Ruess <felix.ruess@gmail.com
+ *
+ * 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.
+ */
+
+/**
+ * @file nps_sensor_airspeed.h
+ *
+ * Simulated airspeed for NPS simulator.
+ *
+ */
+
+#ifndef NPS_SENSOR_AIRSPEED_H
+#define NPS_SENSOR_AIRSPEED_H
+
+#include "math/pprz_algebra.h"
+#include "math/pprz_algebra_double.h"
+#include "math/pprz_algebra_float.h"
+#include "std.h"
+
+struct NpsSensorAirspeed {
+  double value;          ///< airspeed reading in meters/second
+  double offset;         ///< offset in meters/second
+  double noise_std_dev;  ///< noise standard deviation
+  double next_update;
+  bool_t data_available;
+};
+
+
+extern void nps_sensor_airspeed_init(struct NpsSensorAirspeed *airspeed, double time);
+extern void nps_sensor_airspeed_run_step(struct NpsSensorAirspeed *airspeed, double time);
+
+#endif /* NPS_SENSOR_AIRSPEED_H */

sw/simulator/nps/nps_sensors.c

@@ -18,6 +18,7 @@ void nps_sensors_init(double time)
   nps_sensor_baro_init(&sensors.baro, time);
   nps_sensor_gps_init(&sensors.gps, time);
   nps_sensor_sonar_init(&sensors.sonar, time);
+  nps_sensor_airspeed_init(&sensors.airspeed, time);
 
 }
 
@@ -30,6 +31,7 @@ void nps_sensors_run_step(double time)
   nps_sensor_baro_run_step(&sensors.baro, time);
   nps_sensor_gps_run_step(&sensors.gps, time);
   nps_sensor_sonar_run_step(&sensors.sonar, time);
+  nps_sensor_airspeed_run_step(&sensors.airspeed, time);
 }
 
 
@@ -77,3 +79,12 @@ bool_t nps_sensors_sonar_available(void)
   }
   return FALSE;
 }
+
+bool_t nps_sensors_airspeed_available(void)
+{
+  if (sensors.airspeed.data_available) {
+    sensors.airspeed.data_available = FALSE;
+    return TRUE;
+  }
+  return FALSE;
+}

sw/simulator/nps/nps_sensors.h

@@ -8,6 +8,7 @@
 #include "nps_sensor_baro.h"
 #include "nps_sensor_gps.h"
 #include "nps_sensor_sonar.h"
+#include "nps_sensor_airspeed.h"
 
 struct NpsSensors {
   struct DoubleRMat body_to_imu_rmat;
@@ -17,6 +18,7 @@ struct NpsSensors {
   struct NpsSensorBaro  baro;
   struct NpsSensorGps   gps;
   struct NpsSensorSonar sonar;
+  struct NpsSensorAirspeed airspeed;
 };
 
 extern struct NpsSensors sensors;
@@ -29,6 +31,7 @@ extern bool_t nps_sensors_mag_available();
 extern bool_t nps_sensors_baro_available();
 extern bool_t nps_sensors_gps_available();
 extern bool_t nps_sensors_sonar_available();
+extern bool_t nps_sensors_airspeed_available();
 
 
 #endif /* NPS_SENSORS_H */