diff --git a/conf/firmwares/subsystems/fixedwing/fdm_crrcsim.makefile b/conf/firmwares/subsystems/fixedwing/fdm_crrcsim.makefile index 23677fb1f0..ff84c792fd 100644 --- a/conf/firmwares/subsystems/fixedwing/fdm_crrcsim.makefile +++ b/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 \ diff --git a/conf/firmwares/subsystems/fixedwing/fdm_jsbsim.makefile b/conf/firmwares/subsystems/fixedwing/fdm_jsbsim.makefile index 540c3286cb..05edd45b6a 100644 --- a/conf/firmwares/subsystems/fixedwing/fdm_jsbsim.makefile +++ b/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 \ diff --git a/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile b/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile index 1f18514478..b038e720f3 100644 --- a/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile +++ b/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 \ diff --git a/sw/simulator/nps/nps_autopilot_fixedwing.c b/sw/simulator/nps/nps_autopilot_fixedwing.c index e74b0c8c39..d21c097f92 100644 --- a/sw/simulator/nps/nps_autopilot_fixedwing.c +++ b/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); diff --git a/sw/simulator/nps/nps_sensor_airspeed.c b/sw/simulator/nps/nps_sensor_airspeed.c new file mode 100644 index 0000000000..96449452bc --- /dev/null +++ b/sw/simulator/nps/nps_sensor_airspeed.c @@ -0,0 +1,87 @@ +/* + * Copyright (C) 2014 Felix Ruess 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(<p_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; +} diff --git a/sw/simulator/nps/nps_sensor_airspeed.h b/sw/simulator/nps/nps_sensor_airspeed.h new file mode 100644 index 0000000000..b849f627d0 --- /dev/null +++ b/sw/simulator/nps/nps_sensor_airspeed.h @@ -0,0 +1,49 @@ +/* + * Copyright (C) 2014 Felix Ruess