From 3c4f223366da4a0052650118bdfd168938d00c40 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Mon, 7 Oct 2013 20:18:51 +0200 Subject: [PATCH] [modules] add baro_sim as a temporary workaround to get baro simulation in ocaml sim --- .../examples/Twinstar_energyadaptive.xml | 1 + .../subsystems/shared/baro_board.makefile | 5 ++- conf/modules/baro_sim.xml | 22 ++++++++++ sw/airborne/boards/pc_sim.h | 3 +- sw/airborne/modules/sensors/baro_sim.c | 44 +++++++++++++++++++ sw/airborne/modules/sensors/baro_sim.h | 37 ++++++++++++++++ sw/airborne/subsystems/ins/ins_alt_float.c | 6 ++- 7 files changed, 114 insertions(+), 4 deletions(-) create mode 100644 conf/modules/baro_sim.xml create mode 100644 sw/airborne/modules/sensors/baro_sim.c create mode 100644 sw/airborne/modules/sensors/baro_sim.h diff --git a/conf/airframes/examples/Twinstar_energyadaptive.xml b/conf/airframes/examples/Twinstar_energyadaptive.xml index d0f83f5147..972093d111 100644 --- a/conf/airframes/examples/Twinstar_energyadaptive.xml +++ b/conf/airframes/examples/Twinstar_energyadaptive.xml @@ -54,6 +54,7 @@ twog_1.0 + aspirin + ETS baro + ETS speed + diff --git a/conf/firmwares/subsystems/shared/baro_board.makefile b/conf/firmwares/subsystems/shared/baro_board.makefile index 012f3326b8..7bea12cf0b 100644 --- a/conf/firmwares/subsystems/shared/baro_board.makefile +++ b/conf/firmwares/subsystems/shared/baro_board.makefile @@ -125,8 +125,9 @@ endif # # add it for simulators # -sim.srcs += $(SRC_BOARD)/baro_board.c -jsbsim.srcs += $(SRC_BOARD)/baro_board.c +# only NPS for now... +#sim.srcs += $(SRC_BOARD)/baro_board.c +#jsbsim.srcs += $(SRC_BOARD)/baro_board.c nps.srcs += $(SRC_BOARD)/baro_board.c diff --git a/conf/modules/baro_sim.xml b/conf/modules/baro_sim.xml new file mode 100644 index 0000000000..ca694bb582 --- /dev/null +++ b/conf/modules/baro_sim.xml @@ -0,0 +1,22 @@ + + + + + + Simulated barometer. + Sends the BARO_ABS ABI message with gps.hmsl converted to absolute pressure. + + + +
+ +
+ + + + + + + + + diff --git a/sw/airborne/boards/pc_sim.h b/sw/airborne/boards/pc_sim.h index c15970eb21..c3583ebabd 100644 --- a/sw/airborne/boards/pc_sim.h +++ b/sw/airborne/boards/pc_sim.h @@ -11,7 +11,8 @@ #define DefaultVoltageOfAdc(adc) (1.0*adc) - +#ifndef USE_BARO_BOARD #define USE_BARO_BOARD 1 +#endif #endif /* CONFIG_PC_SIM_H */ diff --git a/sw/airborne/modules/sensors/baro_sim.c b/sw/airborne/modules/sensors/baro_sim.c new file mode 100644 index 0000000000..0b58664a2d --- /dev/null +++ b/sw/airborne/modules/sensors/baro_sim.c @@ -0,0 +1,44 @@ +/* + * Copyright (C) 2013 Felix Ruess + * + * 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 baro_sim.c + * + * Simulate barometer pressure measurement using gps.hmsl + */ + +#include "math/pprz_isa.h" +#include "subsystems/gps.h" +#include "subsystems/abi.h" + +#ifndef BARO_SIM_SENDER_ID +#define BARO_SIM_SENDER_ID 1 +#endif +PRINT_CONFIG_VAR(BARO_SIM_SENDER_ID) + +void baro_sim_init(void) { + +} + +void baro_sim_periodic(void) { + float pressure = pprz_isa_pressure_of_altitude(gps.hmsl / 1000.0); + AbiSendMsgBARO_ABS(BARO_SIM_SENDER_ID, &pressure); +} diff --git a/sw/airborne/modules/sensors/baro_sim.h b/sw/airborne/modules/sensors/baro_sim.h new file mode 100644 index 0000000000..a78d3218fa --- /dev/null +++ b/sw/airborne/modules/sensors/baro_sim.h @@ -0,0 +1,37 @@ +/* + * Copyright (C) 2013 Felix Ruess + * + * 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 baro_sim.h + * + * Simulate barometer pressure measurement using gps.hmsl + */ + +#ifndef BARO_SIM_H +#define BARO_SIM_H + +/// new measurement every baro_sim_periodic +#define BARO_SIM_DT BARO_SIM_PERIODIC_PERIOD + +extern void baro_sim_init(void); +extern void baro_sim_periodic(void); + +#endif diff --git a/sw/airborne/subsystems/ins/ins_alt_float.c b/sw/airborne/subsystems/ins/ins_alt_float.c index b12c50b66f..6e0afeb916 100644 --- a/sw/airborne/subsystems/ins/ins_alt_float.c +++ b/sw/airborne/subsystems/ins/ins_alt_float.c @@ -193,7 +193,11 @@ void alt_kalman(float z_meas) { float SIGMA2; #if USE_BAROMETER -#if USE_BARO_MS5534A +#ifdef SITL + DT = BARO_SIM_DT; + R = 0.5; + SIGMA2 = 0.1; +#elif USE_BARO_MS5534A if (alt_baro_enabled) { DT = BARO_DT; R = baro_MS5534A_r;