From 41451d5422535baa651c4ec92d88044fa57c6691 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Tue, 7 Nov 2023 10:21:12 +0100 Subject: [PATCH] [sim] moving the old ocaml simulator to NPS (#3167) - the exact same basic model is now a NPS FDM - sim target still woks, it is just an alias to NPS with the proper FDM - the old ocaml files are removed - AHRS and INS are bypassed, since the accelerations are not well calculated by the model --- Makefile | 8 +- conf/Makefile.sim | 159 +-------- conf/modules/actuators_nps.xml | 2 +- conf/modules/fdm_fixedwing_sim.xml | 19 + conf/modules/firmwares/fixedwing.xml | 2 +- conf/modules/gps_nps.xml | 2 +- conf/modules/imu_common.xml | 2 +- conf/modules/imu_nps.xml | 2 +- conf/modules/ins_arduimu.xml | 3 - conf/modules/ins_arduimu_basic.xml | 3 - conf/modules/ins_nps.xml | 4 +- conf/modules/system_core.xml | 1 - conf/modules/targets/nps.xml | 2 +- conf/modules/targets/nps_common.xml | 2 +- conf/modules/targets/sim.xml | 19 +- conf/modules/telemetry_nps.xml | 2 +- conf/modules/udp.xml | 2 +- sw/airborne/arch/sim/led_hw.c | 20 -- sw/airborne/arch/sim/led_hw.h | 19 - .../arch/sim/modules/radio_control/ppm_arch.c | 30 +- .../sim/modules/radio_control/rc_datalink.c | 35 -- .../sim/modules/radio_control/spektrum_arch.c | 34 -- sw/airborne/arch/sim/sim_adc_generic.c | 19 - sw/airborne/arch/sim/sim_ahrs.c | 38 -- sw/airborne/arch/sim/sim_airspeed.c | 17 - sw/airborne/arch/sim/sim_ap.c | 123 ------- sw/airborne/arch/sim/sim_gps.c | 82 ----- sw/airborne/arch/sim/sim_uart_hw.c | 5 - sw/simulator/Makefile | 24 +- sw/simulator/data.ml | 73 ---- sw/simulator/data.mli | 36 -- sw/simulator/diffusion.ml | 138 -------- sw/simulator/fg.c | 57 --- sw/simulator/fg_environment.xml | 53 --- sw/simulator/flightModel.ml | 280 --------------- sw/simulator/flightModel.mli | 53 --- sw/simulator/gps.ml | 81 ----- sw/simulator/gps.mli | 36 -- sw/simulator/{ => nps}/flight_gear.h | 0 sw/simulator/nps/nps_fdm.h | 2 +- sw/simulator/nps/nps_fdm_fixedwing_sim.c | 309 +++++++++++++++++ sw/simulator/nps/nps_flightgear.c | 2 +- sw/simulator/pprzsim-launch | 79 ++--- sw/simulator/sim.ml | 325 ------------------ sw/simulator/sim.mli | 26 -- sw/simulator/simlib.ml | 65 ---- sw/simulator/simlib.mli | 37 -- sw/simulator/simsitl.ml | 14 - sw/simulator/sitl.ml | 221 ------------ sw/simulator/sitl.mli | 6 - sw/supervision/python/session_widget.py | 20 +- tests/sim/01_Microjet.t | 87 ----- 52 files changed, 376 insertions(+), 2304 deletions(-) create mode 100644 conf/modules/fdm_fixedwing_sim.xml delete mode 100644 sw/airborne/arch/sim/led_hw.c delete mode 100644 sw/airborne/arch/sim/led_hw.h delete mode 100644 sw/airborne/arch/sim/modules/radio_control/rc_datalink.c delete mode 100644 sw/airborne/arch/sim/sim_adc_generic.c delete mode 100644 sw/airborne/arch/sim/sim_ahrs.c delete mode 100644 sw/airborne/arch/sim/sim_airspeed.c delete mode 100644 sw/airborne/arch/sim/sim_ap.c delete mode 100644 sw/airborne/arch/sim/sim_gps.c delete mode 100644 sw/airborne/arch/sim/sim_uart_hw.c delete mode 100644 sw/simulator/data.ml delete mode 100644 sw/simulator/data.mli delete mode 100644 sw/simulator/diffusion.ml delete mode 100644 sw/simulator/fg.c delete mode 100644 sw/simulator/fg_environment.xml delete mode 100644 sw/simulator/flightModel.ml delete mode 100644 sw/simulator/flightModel.mli delete mode 100644 sw/simulator/gps.ml delete mode 100644 sw/simulator/gps.mli rename sw/simulator/{ => nps}/flight_gear.h (100%) create mode 100644 sw/simulator/nps/nps_fdm_fixedwing_sim.c delete mode 100644 sw/simulator/sim.ml delete mode 100644 sw/simulator/sim.mli delete mode 100644 sw/simulator/simlib.ml delete mode 100644 sw/simulator/simlib.mli delete mode 100644 sw/simulator/simsitl.ml delete mode 100644 sw/simulator/sitl.ml delete mode 100644 sw/simulator/sitl.mli delete mode 100644 tests/sim/01_Microjet.t diff --git a/Makefile b/Makefile index 3d9a3291ef..c7ba714579 100644 --- a/Makefile +++ b/Makefile @@ -343,14 +343,8 @@ test_all_confs: all opencv_bebop test_math: make -C tests/math -# super simple simulator test, needs X -# always uses conf/conf.xml, so that needs to contain the appropriate aircrafts -# (only Microjet right now) -test_sim: all - prove tests/sim - .PHONY: all print_build_version _print_building _save_build_version init dox ground_segment ground_segment.opt \ subdirs $(SUBDIRS) conf ext libpprz libpprzlink.update libpprzlink.install cockpit cockpit.opt tmtc tmtc.opt generators\ static sim_static opencv_bebop\ clean cleanspaces ab_clean dist_clean distclean dist_clean_irreversible \ -test test_examples test_math test_sim test_all_confs +test test_examples test_math test_all_confs diff --git a/conf/Makefile.sim b/conf/Makefile.sim index d6f47a5477..c8a79d76e7 100644 --- a/conf/Makefile.sim +++ b/conf/Makefile.sim @@ -1,158 +1 @@ -# Hey Emacs, this is a -*- makefile -*- -# -# Copyright (C) 2006 Pascal Brisset, Antoine Drouin -# -# 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. -# - -# -# This is the common Makefile for the sim target. -# - -# this should not be needed -SRC_ARCH = arch/sim - -include $(PAPARAZZI_SRC)/sw/Makefile.ocaml - -CC = gcc -SIMDIR = $(PAPARAZZI_SRC)/sw/simulator -CAMLINCLUDES = -I $(LIBPPRZDIR) -I $(SIMDIR) -I $(OBJDIR) -PKG = -package glibivy,pprz -LINKPKG = $(PKG) -linkpkg -dllpath-pkg pprz,pprzlink -SIMSITLML = $(OBJDIR)/simsitl.ml -SITLCMA = $(SIMDIR)/sitl.cma -OPT ?= 2 - -# Launch with "make Q=''" to get full command display -Q=@ - -# -# End of configuration part. -# - -INCLUDES += -I $(shell $(OCAMLC) -where) - -CFLAGS = -W -Wall -CFLAGS += $(INCLUDES) -CFLAGS += $($(TARGET).CFLAGS) -CFLAGS += $(USER_CFLAGS) $(BOARD_CFLAGS) -CFLAGS += -fPIC -CFLAGS += -O$(OPT) -CFLAGS += -g -CFLAGS += -std=gnu99 -CFLAGS += $(shell pkg-config --cflags-only-I ivy-glib) -CFLAGS += -D_GNU_SOURCE -CFLAGS += -DPPRZLINK_UNALIGNED_ACCESS=1 - -LDFLAGS = -lm -LDFLAGS += $(BOARD_LDFLAGS) - -LIBFLAGS = -shared - -UNAME = $(shell uname -s) -ifeq ($(UNAME),Darwin) -LIBFLAGS = -dynamiclib -undefined suppress -flat_namespace -endif - -# build sim in custom mode for ARM systems for now... -MNAME = $(shell uname -m) -ifneq (,$(findstring arm,$(MNAME))) -SIMSITLCUSTOM=TRUE -endif - -ifdef SIMSITLCUSTOM - CAMLINCLUDES += $(shell ocamlfind query -r -i-format lablgtk2) $(shell ocamlfind query -r -i-format xml-light) $(shell ocamlfind query -r -i-format glibivy) - CAMLCMAS = unix.cma str.cma xml-light.cma glibivy-ocaml.cma lib-pprz.cma lablgtk.cma -endif - - -# -# General rules -# - -$(TARGET).srcsnd = $(notdir $($(TARGET).srcs)) -$(TARGET).objso = $($(TARGET).srcs:%.c=$(OBJDIR)/%.o) -$(TARGET).objs = $($(TARGET).objso:%.S=$(OBJDIR)/%.o) - -all compile: $(OBJDIR) $(OBJDIR)/simsitl - -$(OBJDIR): - @echo CREATING object dir $(OBJDIR) - @test -d $(OBJDIR) || mkdir -p $(OBJDIR) - -# shared library of the C autopilot part -# Note: On darwin the file should actually have the .dylib ending but ocamlc -# only assumes that .so files are shared libraries, even though this is only -# correct on linux. -autopilot.so : $($(TARGET).objs) - @echo BUILD $@ - $(Q)$(CC) $(LIBFLAGS) $^ -o $(OBJDIR)/$@ - -ifdef SIMSITLCUSTOM -$(OBJDIR)/simsitl : $($(TARGET).objs) $(SITLCMA) $(SIMSITLML) - @echo LD $@ - $(Q)$(OCAMLC) -g -custom $(CAMLINCLUDES) -o $@ $(CAMLCMAS) $^ -else -$(OBJDIR)/simsitl : autopilot.so $(SITLCMA) $(SIMSITLML) - @echo LD $@ - $(Q)$(OCAMLC) -g $(CAMLINCLUDES) -o $@ $(LINKPKG) $^ -dllpath $(OBJDIR) -dllpath $(SIMDIR) -endif - - -# The id of the A/C is hardcoded in the code (to be improved with dynlink ?) -$(SIMSITLML) : $(SIMDIR)/simsitl.ml - @echo "Sim.ac_name := \"$(AIRCRAFT)\"" > $@ - @cat $< >> $@ - -%.s: %.c - $(CC) $(CFLAGS) -S -o $@ $< - -%.s: %.cpp - $(CC) $(CFLAGS) -S -o $@ $< - -$(OBJDIR)/%.s: %.c - @echo CC $@ - $(Q)test -d $(dir $@) || mkdir -p $(dir $@) - $(CC) $(CFLAGS) -S -o $@ $< - -$(OBJDIR)/%.s: %.cpp - @echo CC $@ - $(Q)test -d $(dir $@) || mkdir -p $(dir $@) - $(CC) $(CFLAGS) -S -o $@ $< - -$(OBJDIR)/%.o: %.c $(OBJDIR)/../Makefile.ac - @echo CC $@ - $(Q)test -d $(dir $@) || mkdir -p $(dir $@) - $(Q)$(CC) -MMD $(CFLAGS) -c -o $@ $< - -$(OBJDIR)/%.o: %.cpp $(OBJDIR)/../Makefile.ac - @echo CC $@ - $(Q)test -d $(dir $@) || mkdir -p $(dir $@) - $(Q)$(CC) -MMD $(CFLAGS) -c -o $@ $< - -.PHONY: all compile - -# -# Dependencies -# - -DEPS = $(addprefix $(OBJDIR)/,$($(TARGET).srcs:.c=.d)) - -ifneq ($(MAKECMDGOALS),clean) --include $(DEPS) -endif +include $(PAPARAZZI_SRC)/conf/Makefile.nps diff --git a/conf/modules/actuators_nps.xml b/conf/modules/actuators_nps.xml index a6571aa52f..614178227c 100644 --- a/conf/modules/actuators_nps.xml +++ b/conf/modules/actuators_nps.xml @@ -10,7 +10,7 @@ actuators actuators - + diff --git a/conf/modules/fdm_fixedwing_sim.xml b/conf/modules/fdm_fixedwing_sim.xml new file mode 100644 index 0000000000..bd58810552 --- /dev/null +++ b/conf/modules/fdm_fixedwing_sim.xml @@ -0,0 +1,19 @@ + + + + + + Basic fixedwing FDM for NPS simulator + + This flight dynamic model is the copy of the legacy simulator that was written in OCaml. + Its only purpose is to allow to test the flight plans with a very simple airplane model. + + +
+ + + + + + + diff --git a/conf/modules/firmwares/fixedwing.xml b/conf/modules/firmwares/fixedwing.xml index 54194df182..fe2f67d3c8 100644 --- a/conf/modules/firmwares/fixedwing.xml +++ b/conf/modules/firmwares/fixedwing.xml @@ -28,7 +28,7 @@ - + diff --git a/conf/modules/gps_nps.xml b/conf/modules/gps_nps.xml index 68f6e15860..051e2abb7f 100644 --- a/conf/modules/gps_nps.xml +++ b/conf/modules/gps_nps.xml @@ -17,7 +17,7 @@
- + diff --git a/conf/modules/imu_common.xml b/conf/modules/imu_common.xml index 9905db46d2..2225a137c8 100644 --- a/conf/modules/imu_common.xml +++ b/conf/modules/imu_common.xml @@ -40,7 +40,7 @@ - + diff --git a/conf/modules/imu_nps.xml b/conf/modules/imu_nps.xml index 1137664499..7cbb2dcace 100644 --- a/conf/modules/imu_nps.xml +++ b/conf/modules/imu_nps.xml @@ -16,7 +16,7 @@ - + diff --git a/conf/modules/ins_arduimu.xml b/conf/modules/ins_arduimu.xml index eeb9a6f64e..a75b0ff1fc 100644 --- a/conf/modules/ins_arduimu.xml +++ b/conf/modules/ins_arduimu.xml @@ -17,8 +17,5 @@ - - - diff --git a/conf/modules/ins_arduimu_basic.xml b/conf/modules/ins_arduimu_basic.xml index 98073a2934..f0dfc38324 100644 --- a/conf/modules/ins_arduimu_basic.xml +++ b/conf/modules/ins_arduimu_basic.xml @@ -29,8 +29,5 @@ - - - diff --git a/conf/modules/ins_nps.xml b/conf/modules/ins_nps.xml index e29123cd56..8d7e807e81 100644 --- a/conf/modules/ins_nps.xml +++ b/conf/modules/ins_nps.xml @@ -14,7 +14,7 @@ - + @@ -34,7 +34,7 @@ - + diff --git a/conf/modules/system_core.xml b/conf/modules/system_core.xml index a55bf0b3e7..aed9d8e3b8 100644 --- a/conf/modules/system_core.xml +++ b/conf/modules/system_core.xml @@ -31,7 +31,6 @@ - VPATH += $(PAPARAZZI_HOME)/var/share diff --git a/conf/modules/targets/nps.xml b/conf/modules/targets/nps.xml index 63b7386138..feacd206b2 100644 --- a/conf/modules/targets/nps.xml +++ b/conf/modules/targets/nps.xml @@ -15,7 +15,7 @@ baro,airspeed,sonar,incidence,temperature gps_nps,imu_nps,ins_nps,actuators_nps,telemetry_nps - + diff --git a/conf/modules/targets/nps_common.xml b/conf/modules/targets/nps_common.xml index c04ef309a4..fa9f776b96 100644 --- a/conf/modules/targets/nps_common.xml +++ b/conf/modules/targets/nps_common.xml @@ -12,7 +12,7 @@ math - + diff --git a/conf/modules/targets/sim.xml b/conf/modules/targets/sim.xml index 4cde12cf74..d7a2b450f9 100644 --- a/conf/modules/targets/sim.xml +++ b/conf/modules/targets/sim.xml @@ -11,26 +11,9 @@ - system_core,electrical,settings,actuators_dummy,@gps,@ins,@ahrs - gps_sim,imu_sim,ins_sim,ahrs_sim,baro_sim,telemetry_sim + targets/nps,fdm_fixedwing_sim - - - - - - - - - -UNAME = $(shell uname -s) -ifeq ("$(UNAME)","Darwin") - sim.CFLAGS += $(shell if test -d /opt/paparazzi/include; then echo "-I/opt/paparazzi/include"; elif test -d /opt/local/include; then echo "-I/opt/local/include"; fi) -endif - -sim.CFLAGS += $(CPPFLAGS) - diff --git a/conf/modules/telemetry_nps.xml b/conf/modules/telemetry_nps.xml index 9d29933ed2..56f055d9a0 100644 --- a/conf/modules/telemetry_nps.xml +++ b/conf/modules/telemetry_nps.xml @@ -22,7 +22,7 @@ - + diff --git a/conf/modules/udp.xml b/conf/modules/udp.xml index 638593d53b..1adcafe72f 100644 --- a/conf/modules/udp.xml +++ b/conf/modules/udp.xml @@ -19,7 +19,7 @@ - + diff --git a/sw/airborne/arch/sim/led_hw.c b/sw/airborne/arch/sim/led_hw.c deleted file mode 100644 index 008ea79d59..0000000000 --- a/sw/airborne/arch/sim/led_hw.c +++ /dev/null @@ -1,20 +0,0 @@ -#include "led_hw.h" - -#include -#include -#include -#include - -value *leds_closure = 0; -bool led_disabled = false; - -value register_leds_cb(value cb_name) -{ - leds_closure = (value *)caml_named_value(String_val(cb_name)); - return Val_unit; -} - -void _led_on(int i) { if (leds_closure && !led_disabled) callback2(*leds_closure, Val_int(i), Val_int(1)); } -void _led_off(int i) { if (leds_closure && !led_disabled) callback2(*leds_closure, Val_int(i), Val_int(0)); } -void _led_toggle(int i) { if (leds_closure && !led_disabled) callback2(*leds_closure, Val_int(i), Val_int(2)); } - diff --git a/sw/airborne/arch/sim/led_hw.h b/sw/airborne/arch/sim/led_hw.h deleted file mode 100644 index 50bd754603..0000000000 --- a/sw/airborne/arch/sim/led_hw.h +++ /dev/null @@ -1,19 +0,0 @@ -#ifndef LED_HW_H -#define LED_HW_H - -#include - -extern bool led_disabled; -extern void _led_on(int i); -extern void _led_off(int i); -extern void _led_toggle(int i); - -#define LED_INIT(i) { led_disabled = false; } -#define LED_ON(i) _led_on(i); -#define LED_OFF(i) _led_off(i); -#define LED_TOGGLE(i) _led_toggle(i); -#define LED_DISABLE(i) { LED_OFF(i); led_disabled = true; } - -#define LED_PERIODIC() {} - -#endif /* LED_HW_H */ diff --git a/sw/airborne/arch/sim/modules/radio_control/ppm_arch.c b/sw/airborne/arch/sim/modules/radio_control/ppm_arch.c index b0eb98ef5d..865bb86d4d 100644 --- a/sw/airborne/arch/sim/modules/radio_control/ppm_arch.c +++ b/sw/airborne/arch/sim/modules/radio_control/ppm_arch.c @@ -27,21 +27,14 @@ #include "modules/radio_control/radio_control.h" #include "modules/radio_control/ppm.h" - -#include - -#if USE_NPS #include "nps_radio_control.h" -#else -#include -#endif +#include void ppm_arch_init(void) { } -#if USE_NPS #ifdef RADIO_CONTROL #define PPM_OF_NPS(_nps, _neutral, _min, _max) \ ((_nps) >= 0 ? (_neutral) + (_nps) * ((_max)-(_neutral)) : (_neutral) + (_nps) * ((_neutral)- (_min))) @@ -74,24 +67,3 @@ void radio_control_feed(void) void radio_control_feed(void) {} #endif //RADIO_CONTROL -#elif !USE_JSBSIM // not NPS and not JSBSIM -> simple ocaml sim -#ifdef RADIO_CONTROL -value update_rc_channel(value c, value v) -{ - ppm_pulses[Int_val(c)] = Double_val(v); - return Val_unit; -} - -value send_ppm(value unit) -{ - ppm_frame_available = true; - return unit; -} -#else // RADIO_CONTROL -value update_rc_channel(value c __attribute__((unused)), value v __attribute__((unused))) -{ - return Val_unit; -} -value send_ppm(value unit) {return unit;} -#endif // RADIO_CONTROL -#endif // USE_NPS diff --git a/sw/airborne/arch/sim/modules/radio_control/rc_datalink.c b/sw/airborne/arch/sim/modules/radio_control/rc_datalink.c deleted file mode 100644 index e87ff0833d..0000000000 --- a/sw/airborne/arch/sim/modules/radio_control/rc_datalink.c +++ /dev/null @@ -1,35 +0,0 @@ -/* - * Copyright (C) 2010 The Paparazzi Team - * - * 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. - */ - -#include "modules/radio_control/radio_control.h" - -#include -#include - -value update_rc_channel(value c __attribute__((unused)), value v __attribute__((unused))) -{ - return Val_unit; -} - -value send_ppm(value unit) -{ - return unit; -} diff --git a/sw/airborne/arch/sim/modules/radio_control/spektrum_arch.c b/sw/airborne/arch/sim/modules/radio_control/spektrum_arch.c index 673a751d97..80961ec937 100644 --- a/sw/airborne/arch/sim/modules/radio_control/spektrum_arch.c +++ b/sw/airborne/arch/sim/modules/radio_control/spektrum_arch.c @@ -32,11 +32,7 @@ #include "std.h" #include -#if USE_NPS #include "nps_radio_control.h" -#else -#include -#endif static bool spektrum_available; @@ -58,7 +54,6 @@ void spektrum_event(void) void spektrum_try_bind(void) {} -#if USE_NPS #ifdef RADIO_CONTROL void radio_control_feed(void) { @@ -73,32 +68,3 @@ void radio_control_feed(void) void radio_control_feed(void) {} #endif //RADIO_CONTROL -#else // not NPS -> simple ocaml sim -#ifdef RADIO_CONTROL -value update_rc_channel(value c, value v) -{ - // OCaml sim sends ppm values read from radio xml - //assume "ppm" value range from 1000 to 2000 for now.. like in fake spektrum.xml - if (Int_val(c) == 0) { - // throttle channel has neutral at 1000 - radio_control.values[Int_val(c)] = (Double_val(v) - 1000.0) / 1000 * MAX_PPRZ; - } else { - // all other channels at 1500 - radio_control.values[Int_val(c)] = (Double_val(v) - 1500.0) / 500 * MAX_PPRZ; - } - return Val_unit; -} - -value send_ppm(value unit) -{ - spektrum_available = true; - return unit; -} -#else // RADIO_CONTROL -value update_rc_channel(value c __attribute__((unused)), value v __attribute__((unused))) -{ - return Val_unit; -} -value send_ppm(value unit) {return unit;} -#endif // RADIO_CONTROL -#endif // USE_NPS diff --git a/sw/airborne/arch/sim/sim_adc_generic.c b/sw/airborne/arch/sim/sim_adc_generic.c deleted file mode 100644 index 5e77335e35..0000000000 --- a/sw/airborne/arch/sim/sim_adc_generic.c +++ /dev/null @@ -1,19 +0,0 @@ -#include -#include - -uint16_t adc_generic_val1; -uint16_t adc_generic_val2; - -void adc_generic_init(void) -{ -} - -void adc_generic_periodic(void) -{ -} - -value update_adc1(value adc1) -{ - adc_generic_val1 = Int_val(adc1); - return Val_unit; -} diff --git a/sw/airborne/arch/sim/sim_ahrs.c b/sw/airborne/arch/sim/sim_ahrs.c deleted file mode 100644 index 1750099d15..0000000000 --- a/sw/airborne/arch/sim/sim_ahrs.c +++ /dev/null @@ -1,38 +0,0 @@ -/** \file sim_ahrs.c - * \brief Regroup functions to simulate an ahrs - * - * Ahrs soft simulation. OCaml binding. - */ - - -#include -#include -#include "std.h" - -float sim_phi; ///< in radians -float sim_theta; ///< in radians -float sim_psi; ///< in radians -float sim_p; ///< in radians/s -float sim_q; ///< in radians/s -float sim_r; ///< in radians/s - -// Updates from Ocaml sim - -value provide_attitude(value phi, value theta, value psi) -{ - sim_phi = Double_val(phi); - sim_theta = Double_val(theta); - sim_psi = - Double_val(psi) + M_PI / 2.; - - return Val_unit; -} - -value provide_rates(value p, value q, value r) -{ - sim_p = Double_val(p); - sim_q = Double_val(q); - sim_r = Double_val(r); - - return Val_unit; -} - diff --git a/sw/airborne/arch/sim/sim_airspeed.c b/sw/airborne/arch/sim/sim_airspeed.c deleted file mode 100644 index 674b216439..0000000000 --- a/sw/airborne/arch/sim/sim_airspeed.c +++ /dev/null @@ -1,17 +0,0 @@ -/** \file sim_airspeed.c - */ - - -#include -#include "generated/airframe.h" - -#include - -float sim_air_speed; - -value set_airspeed(value air_speed) -{ - sim_air_speed = Double_val(air_speed); - return Val_unit; -} - diff --git a/sw/airborne/arch/sim/sim_ap.c b/sw/airborne/arch/sim/sim_ap.c deleted file mode 100644 index 12a356dcb7..0000000000 --- a/sw/airborne/arch/sim/sim_ap.c +++ /dev/null @@ -1,123 +0,0 @@ -/* Definitions and declarations required to compile autopilot code on a - i386 architecture. Bindings for OCaml. */ - -#include -#include -#include -#include -#include -#include -#include "std.h" -#include "main_ap.h" -#include "autopilot.h" -#include "modules/gps/gps.h" -#include "generated/settings.h" -#include "firmwares/fixedwing/nav.h" -#include "firmwares/fixedwing/stabilization/stabilization_attitude.h" -#include "firmwares/fixedwing/guidance/guidance_v.h" -#include "modules/core/commands.h" -#include "modules/datalink/datalink.h" -#include "modules/datalink/telemetry.h" -#include "generated/flight_plan.h" - -#include "generated/modules.h" - -#include -#include - - -/* Dummy definitions to replace the ones from the files not compiled in the - simulator */ -uint8_t ir_estim_mode; -uint8_t vertical_mode; -uint8_t inflight_calib_mode; -bool rc_event_1, rc_event_2; -uint8_t gps_nb_ovrn, link_fbw_fbw_nb_err, link_fbw_nb_err; -float alt_roll_pgain; -float roll_rate_pgain; - -#ifndef SIM_UPDATE_DL -#define SIM_UPDATE_DL TRUE -#endif - -uint8_t ac_id; - -#if PERIODIC_FREQUENCY != 60 -#warning "Simple OCaml sim can currently only handle a PERIODIC_FREQUENCY of 60Hz" -#endif - -#if SYS_TIME_FREQUENCY != 120 -#warning "Simple OCaml sim can currently only handle a SYS_TIME_FREQUENCY of 120Hz" -#endif - -/** needs to be called at SYS_TIME_FREQUENCY */ -value sim_sys_time_task(value unit) -{ - sys_tick_handler(); - return unit; -} - -value sim_periodic_task(value unit) -{ - main_ap_periodic(); - main_ap_event(); - return unit; -} - -float ftimeofday(void) -{ - struct timeval t; - struct timezone z; - gettimeofday(&t, &z); - return (t.tv_sec + t.tv_usec / 1e6); -} - -value sim_init(value unit) -{ - modules_mcu_init(); - main_ap_init(); - - return unit; -} - -value update_bat(value bat) -{ - electrical.vsupply = (float)Int_val(bat) / 10.; - return Val_unit; -} - -value update_dl_status(value dl_enabled) -{ - ivy_tp.ivy_dl_enabled = Int_val(dl_enabled); - return Val_unit; -} - - -value get_commands(value val_commands) -{ - int i; - - for (i = 0; i < COMMANDS_NB; i++) { - Store_field(val_commands, i, Val_int(commands[i])); - } - - return Val_int(commands[COMMAND_THROTTLE]); -} - -value set_datalink_message(value s) -{ - int n = string_length(s); - char *ss = (char *)String_val(s); - assert(n <= MSG_SIZE); - - int i; - for (i = 0; i < n; i++) { - dl_buffer[i] = ss[i]; - } - - dl_msg_available = true; - DlCheckAndParse(&(DOWNLINK_DEVICE).device, &ivy_tp.trans_tx, dl_buffer, &dl_msg_available, SIM_UPDATE_DL); - - return Val_unit; -} - diff --git a/sw/airborne/arch/sim/sim_gps.c b/sw/airborne/arch/sim/sim_gps.c deleted file mode 100644 index e1dafc6914..0000000000 --- a/sw/airborne/arch/sim/sim_gps.c +++ /dev/null @@ -1,82 +0,0 @@ -/* OCaml binding to link the simulator to autopilot functions. */ - -#include -#include -#include - -/** From airborne/autopilot/ */ -#include "generated/airframe.h" -#include "generated/flight_plan.h" -#include "modules/gps/gps_sim.h" -#include "math/pprz_geodetic_float.h" -#include "math/pprz_geodetic_int.h" - -// currently needed for nav_utm_zone0 -#include "modules/nav/common_nav.h" - -#include - - -value sim_use_gps_pos(value x, value y, value z, value c, value a, value s, value cl, value t, value m, value lat, - value lon) -{ - gps.fix = (Bool_val(m) ? 3 : 0); - gps.course = Double_val(c) * 1e7; - SetBit(gps.valid_fields, GPS_VALID_COURSE_BIT); - gps.hmsl = Double_val(a) * 1000.; - SetBit(gps.valid_fields, GPS_VALID_HMSL_BIT); - gps.gspeed = Double_val(s) * 100.; - gps.ned_vel.x = gps.gspeed * cos(Double_val(c)); - gps.ned_vel.y = gps.gspeed * sin(Double_val(c)); - gps.ned_vel.z = -Double_val(cl) * 100.; - SetBit(gps.valid_fields, GPS_VALID_VEL_NED_BIT); - gps.week = 0; // FIXME - gps.tow = Double_val(t) * 1000.; - - struct LlaCoor_f lla_f; - lla_f.lat = Double_val(lat); - lla_f.lon = Double_val(lon); - //TODO set alt above ellipsoid, use hmsl for now - lla_f.alt = Double_val(a); - LLA_BFP_OF_REAL(gps.lla_pos, lla_f); - SetBit(gps.valid_fields, GPS_VALID_POS_LLA_BIT); - - gps.utm_pos.east = Int_val(x); - gps.utm_pos.north = Int_val(y); - gps.utm_pos.alt = gps.hmsl; - gps.utm_pos.zone = Int_val(z); - SetBit(gps.valid_fields, GPS_VALID_POS_UTM_BIT); - - /** Space vehicle info simulation */ - gps.nb_channels = 7; - int i; - static int time; - time++; - for (i = 0; i < gps.nb_channels; i++) { - gps.svinfos[i].svid = 7 + i; - gps.svinfos[i].elev = (cos(((100 * i) + time) / 100.) + 1) * 45; - gps.svinfos[i].azim = (time / gps.nb_channels + 50 * i) % 360; - gps.svinfos[i].cno = 40 + sin((time + i * 10) / 100.) * 10.; - gps.svinfos[i].flags = ((time / 10) % (i + 1) == 0 ? 0x00 : 0x01); - gps.svinfos[i].qi = (int)((time / 1000.) + i) % 8; - } - gps.pdop = gps.sacc = gps.pacc = 500 + 200 * sin(time / 100.); - gps.num_sv = 7; - - gps_sim_publish(); - - return Val_unit; -} - -/* Second binding required because number of args > 5 */ -value sim_use_gps_pos_bytecode(value *a, int argn) -{ - assert(argn == 11); - return sim_use_gps_pos(a[0], a[1], a[2], a[3], a[4], a[5], a[6], a[7], a[8], a[9], a[10]); -} - -void ubxsend_cfg_rst(uint16_t a __attribute__((unused)), uint8_t b __attribute__((unused))) -{ - return; -} - diff --git a/sw/airborne/arch/sim/sim_uart_hw.c b/sw/airborne/arch/sim/sim_uart_hw.c deleted file mode 100644 index 32b0e7057f..0000000000 --- a/sw/airborne/arch/sim/sim_uart_hw.c +++ /dev/null @@ -1,5 +0,0 @@ -#include "uart_hw.h" - -char stdinout_buffer[STDINOUT_BUFFER_SIZE]; -uint8_t stdinout_rx_insert_idx = 0; -uint8_t stdinout_rx_extract_idx = 0; diff --git a/sw/simulator/Makefile b/sw/simulator/Makefile index 293dfd187a..7669193452 100644 --- a/sw/simulator/Makefile +++ b/sw/simulator/Makefile @@ -31,38 +31,18 @@ INCLUDES = PKG = -package glibivy,pprz LINKPKG = $(PKG) -linkpkg -dllpath-pkg pprz,pprzlink -SIMML = simlib.ml data.ml flightModel.ml gps.ml -SIMHML = $(SIMML) hitl.ml sim.ml -SIMHCMO=$(SIMHML:%.ml=%.cmo) -SIMSML = $(SIMML) sitl.ml sim.ml -SIMSCMO=$(SIMSML:%.ml=%.cmo) -SIMSCMX=$(SIMSML:%.ml=%.cmx) - AIRBORNE = ../airborne VARINCLUDE=$(PAPARAZZI_HOME)/var/include ACINCLUDE = $(PAPARAZZI_HOME)/var/$(AIRCRAFT) CAML_CFLAGS = -I $(shell $(OCAMLC) -where) - -all : gaia sitl.cma - -sitl.cma : fg.o $(SIMSCMO) $(LIBPPRZCMA) $(LIBPPRZLINKCMA) - @echo OL $@ - $(Q)$(OCAMLMKLIB) -o sitl $^ - -sitl.cmxa : $(SIMSCMX) $(LIBPPRZCMXA) $(LIBPPRZLINKCMXA) - @echo OC $@ - $(Q)$(OCAMLOPT) -o $@ -a $^ +all : gaia gaia : gaia.cmo $(LIBPPRZCMA) $(LIBPPRZLINKCMA) @echo OL $@ $(Q)$(OCAMLC) $(INCLUDES) -o $@ $(LINKPKG) gtkInit.cmo $< -diffusion : simlib.cmo diffusion.cmo - @echo OL $@ - $(Q)$(OCAMLC) $(INCLUDES) -o $@ $(LINKPKG) gtkInit.cmo $^ - %.cmo : %.ml @echo OC $< $(Q)$(OCAMLC) $(INCLUDES) -c $(PKG) $< @@ -80,7 +60,7 @@ diffusion : simlib.cmo diffusion.cmo $(Q)$(OCAMLC) $(INCLUDES) -c $(PKG) $< clean : - $(Q)rm -f *.cm* *~ *.out .depend *.o *.a *.so gaia simhitl diffusion + $(Q)rm -f *.cm* *~ *.out .depend *.o *.a *.so gaia .PHONY: all clean diff --git a/sw/simulator/data.ml b/sw/simulator/data.ml deleted file mode 100644 index a149896a23..0000000000 --- a/sw/simulator/data.ml +++ /dev/null @@ -1,73 +0,0 @@ -(* - * Usefull data for simulation - * - * Copyright (C) 2004 Pascal Brisset, Antoine Drouin - * - * 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. - * - *) - -open Printf - -let (//) = Filename.concat - -let user_conf_path = Env.paparazzi_home // "conf" -let user_var_path = Env.paparazzi_home // "var" - -let conf_xml = ExtXml.parse_file (user_conf_path // "conf.xml") - -let messages_ap = - let xml = ExtXml.parse_file (user_var_path // "messages.xml") in - try - ExtXml.child xml ~select:(fun x -> Xml.attrib x "name" = "telemetry") "msg_class" - with - Not_found -> failwith "'telemetry' msg_class missing in messages.xml" - -type aircraft = { - name : string; - id : int; - airframe : Xml.xml; - flight_plan : Xml.xml; - radio: Xml.xml - } - - -let aircraft = fun name -> - let aircraft_xml, id = - let rec loop = function - [] -> failwith ("Aircraft not found : "^name) - | x::_ when Xml.tag x = "aircraft" && Xml.attrib x "name" = name -> - begin - try - (x, int_of_string (Xml.attrib x "ac_id")) - with - _ -> - failwith (sprintf "Int value expected for 'ac_id' in %s" (Xml.to_string x)) - end - | _x::xs -> loop xs in - loop (Xml.children conf_xml) in - - let airframe_file = user_conf_path // ExtXml.attrib aircraft_xml "airframe" in - - { id = id; name = name; - airframe = Airframe.expand_includes (string_of_int id) (ExtXml.parse_file airframe_file); - flight_plan = ExtXml.parse_file (user_conf_path // ExtXml.attrib aircraft_xml "flight_plan"); - radio = ExtXml.parse_file (user_conf_path // ExtXml.attrib aircraft_xml "radio") - } - -module type MISSION = sig val ac : aircraft end diff --git a/sw/simulator/data.mli b/sw/simulator/data.mli deleted file mode 100644 index aa4734fc64..0000000000 --- a/sw/simulator/data.mli +++ /dev/null @@ -1,36 +0,0 @@ -(* - * Usefull data for simulation - * - * Copyright (C) 2004 Pascal Brisset, Antoine Drouin - * - * 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. - * - *) - -val user_conf_path : string -val conf_xml : Xml.xml -val messages_ap : Xml.xml -type aircraft = { - name : string; - id : int; - airframe : Xml.xml; - flight_plan : Xml.xml; - radio : Xml.xml; -} -val aircraft : string -> aircraft -module type MISSION = sig val ac : aircraft end diff --git a/sw/simulator/diffusion.ml b/sw/simulator/diffusion.ml deleted file mode 100644 index da2b033b6a..0000000000 --- a/sw/simulator/diffusion.ml +++ /dev/null @@ -1,138 +0,0 @@ - -open Printf - -module Ground_Pprz = PprzLink.Messages(struct let name = "ground" end) -module LL = Latlong -open LL - -type plume = { mutable utm_x : float; mutable utm_y : float; mutable value : int; utm_zone : int } - -(* NW of Muret ref *) -let muret = utm_of WGS84 {LL.posn_lat=(Deg>>Rad)43.4624; posn_long=(Deg>>Rad)1.2727} -let royan = utm_of WGS84 {LL.posn_lat=(Deg>>Rad)45.7122; posn_long=(Deg>>Rad)(-1.2037)} -let source = fun () -> { utm_x = muret.LL.utm_x -. 300.; utm_y = muret.LL.utm_y -. 300.; value = 255; utm_zone = muret.LL.utm_zone} - -let available_ids = ref [] -let gen_id = - let x = ref 0 in - fun () -> - match !available_ids with - [] -> - incr x; !x - | x::xs -> - available_ids := xs; - x - -let dt = 1. (* s *) -let mixing_length = 5. (* m/s *) -let wind_x = ref 0. -let wind_y = ref 0. - - -let ivy_bus = Defivybus.default_ivy_bus - -let plumes = Hashtbl.create 97 -let t = ref 0 - -let one_step = fun () -> - incr t; - - (* New plume *) - if !t mod 5 = 0 then - Hashtbl.add plumes (gen_id ()) (source ()); - - (* Eddy + wind *) - Hashtbl.iter (fun id plume -> - let a = Random.float (2.*.LL.pi) in - plume.utm_x <- plume.utm_x +. (mixing_length*.cos a +. !wind_x)*. dt; - plume.utm_y <- plume.utm_y +. (mixing_length*.sin a +. !wind_y)*. dt; - plume.value <- plume.value - 1; - if plume.value <= 0 then begin - Hashtbl.remove plumes id; - available_ids := id :: !available_ids; - end) - plumes - - -let my_id = "diffusion" -let send_on_ivy = fun () -> - if Hashtbl.length plumes > 0 then - let ids = ref [] - and xs= ref [] - and ys = ref [] - and vs = ref [] in - Hashtbl.iter (fun id plume -> - ids := string_of_int id :: !ids; - let wgs84 = LL.of_utm WGS84 {LL.utm_zone = plume.utm_zone ; utm_x = plume.utm_x; utm_y = plume.utm_y } in - xs := sprintf "%.6f" ((Rad>>Deg)wgs84.posn_lat) :: !xs; - ys := sprintf "%.6f" ((Rad>>Deg)wgs84.posn_long) :: !ys; - vs := sprintf "%d" plume.value :: !vs) - plumes ; - let ids = Bytes.concat "," !ids - and xs = Bytes.concat "," !xs - and ys = Bytes.concat "," !ys - and vs = Bytes.concat "," !vs in - Ground_Pprz.message_send my_id "PLUMES" - [ "ids", PprzLink.String ids; - "lats", PprzLink.String xs; - "longs", PprzLink.String ys; - "values", PprzLink.String vs ] - -let debug = let i = ref 0 in fun () -> - incr i; - let f = open_out (sprintf "plumes_%d.txt" !i) in - Hashtbl.iter (fun id plume -> - fprintf f "%.1f %.1f\n" plume.utm_x plume.utm_y) - plumes; - close_out f - -let detect_distance = 20. - - -let flight_param_msg = fun _sender vs -> - let lat = PprzLink.float_assoc "lat" vs - and long = PprzLink.float_assoc "long" vs in - let utm_ac = utm_of WGS84 {LL.posn_lat=(Deg>>Rad)lat; posn_long=(Deg>>Rad)long} in - Hashtbl.iter (fun id plume -> - let utm_plume = {LL.utm_zone = plume.utm_zone; utm_x = plume.utm_x; utm_y = plume.utm_y } in - let d = utm_distance utm_ac utm_plume in - if d < detect_distance then begin - let ac_id = PprzLink.string_assoc "ac_id" vs in - for i = 0 to 2 do - Ground_Pprz.message_send my_id "DL_SETTING" - ["ac_id", PprzLink.String ac_id; "index", PprzLink.Int i(***FIXME***); "value", PprzLink.Float (float plume.value)] - done - end) - plumes - - - -let safe_bind = fun msg cb -> - let safe_cb = fun sender vs -> - try cb sender vs with x -> prerr_endline (Printexc.to_string x) in - ignore (Ground_Pprz.message_bind msg safe_cb) - -let gaia = fun time_scale _sender vs -> - time_scale#set_value (PprzLink.float_assoc "time_scale" vs); - wind_x := (PprzLink.float_assoc "wind_east" vs); - wind_y := (PprzLink.float_assoc "wind_north" vs) - - -let _ = - Ivy.init "Paparazzi gaia" "READY" (fun _ _ -> ()); - Ivy.start !ivy_bus; - - let periodic = fun () -> - one_step (); - send_on_ivy (); - (*** debug ();***) - true in - - let time_scale = object val mutable v = 1. method value = v method set_value x = v <- x end in - - Simlib.timer ~scale:time_scale dt periodic; - - safe_bind "FLIGHT_PARAM" flight_param_msg; - safe_bind "WORLD_ENV" (gaia time_scale); - - Unix.handle_unix_error GMain.Main.main () diff --git a/sw/simulator/fg.c b/sw/simulator/fg.c deleted file mode 100644 index f043a516f0..0000000000 --- a/sw/simulator/fg.c +++ /dev/null @@ -1,57 +0,0 @@ -/** Values boxing for Flight Gear */ - -#include -#include -#include -#include - -#include -#include - -#include "flight_gear.h" - -value fg_sizeof(value unit) { - return Val_int(sizeof(struct FGNetGUI)); -} - -value fg_msg_native(value s, value lat, value lon, value z, value phi, value theta, value psi); - -value fg_msg_bytecode(value *argv, int argc) { - return fg_msg_native(argv[0], argv[1], argv[2], argv[3], argv[4], argv[5], argv[6]); - -} - - -value fg_msg_native(value s, value lat, value lon, value z, value phi, value theta, value psi) { - - struct FGNetGUI msg = {0}; - msg.version = FG_NET_GUI_VERSION; - - msg.longitude = Double_val(lon); - msg.latitude = Double_val(lat); - msg.altitude = Double_val(z) + 50; - msg.agl = 0.; - msg.phi = Double_val(phi); - msg.theta = Double_val(theta); - msg.psi = - Double_val(psi) + M_PI_2; - - msg.vcas = 0.; - msg.climb_rate = 0.; - - msg.num_tanks = 1; - msg.fuel_quantity[0] = 10.; - - msg.cur_time = 3213092700ul+((uint32_t)((msg.longitude)*13578)); //time(NULL); - msg.warp = 0; - msg.ground_elev = 0.; - - msg.tuned_freq = 123.45; - msg.nav_radial = 123.; - msg.in_range = 1; - msg.course_deviation_deg = 12.; - msg.gs_deviation_deg = 123.; - - memcpy(String_val(s), (char*)&msg, sizeof(msg)); - - return Val_unit; -} diff --git a/sw/simulator/fg_environment.xml b/sw/simulator/fg_environment.xml deleted file mode 100644 index f845afcb2b..0000000000 --- a/sw/simulator/fg_environment.xml +++ /dev/null @@ -1,53 +0,0 @@ - - - - - - true - host - - - time (s) - double - /sim/time/elapsed-sec - - - - wind-from-north (m/s) - float - /environment/wind-from-north-fps - 0.3048 - - - - wind-from-east (m/s) - float - /environment/wind-from-east-fps - 0.3048 - - - - wind-from-down (m/s) - float - /environment/wind-from-down-fps - 0.3048 - - - - wind-from-heading (deg) - float - /environment/wind-from-heading-deg - - - - wind-speed (m/s) - float - 0.514444444 - /environment/wind-speed-kt - - - magic,0x12345678 - - - - diff --git a/sw/simulator/flightModel.ml b/sw/simulator/flightModel.ml deleted file mode 100644 index 2482a3fbe0..0000000000 --- a/sw/simulator/flightModel.ml +++ /dev/null @@ -1,280 +0,0 @@ -(* - * Basic flight model for simulation - * - * Copyright (C) 2004-2006 Pascal Brisset, Antoine Drouin - * - * 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. - * - *) - -open Simlib -open Printf - -let ios = fun x -> - try int_of_string x with _ -> failwith (Printf.sprintf "int_of_string: '%s'" x) - -type meter = float -type meter_s = float -type radian = float -type radian_s = float -type state = { - start : float; - mutable t : float; - mutable x : meter; - mutable y : meter; - mutable z : meter; - mutable z_dot : meter_s; - mutable psi : radian; (* Trigonometric *) - mutable phi : radian; - mutable theta : radian; - mutable theta_dot : radian_s; - mutable phi_dot : radian_s; - mutable psi_dot : radian_s; - mutable delta_a : float; - mutable delta_b : float; - mutable thrust : float; - mutable air_speed : meter_s - } - -module type SIG = - sig - val init : radian -> state - val do_commands : state -> Simlib.pprz_t array -> unit - val nb_commands : int - val nominal_airspeed : float (* m/s *) - val max_bat_level : float (* V *) - val roll_neutral_default : float (* rad *) - val pitch_neutral_default : float (* rad *) - val state_update : state -> float -> float * float * float -> float -> float -> unit - (** [state_update nom_airspeed state (wind_x, wind_y, wind_z) on_ground dt] With m/s for wind and s for - dt *) - end - -let get_xyz state = (state.x, state.y, state.z) -let get_time state = state.t -let get_attitude state = (state.phi, state.theta, state.psi) -let get_pqr state = (state.phi_dot, state.theta_dot, state.psi_dot) - -let get_air_speed state = state.air_speed -let set_air_speed state = fun s -> state.air_speed <- s - -let g = 9.81 - -module Make(A:Data.MISSION) = struct - open Data - - let section = fun name -> - try - ExtXml.child A.ac.airframe ~select:(fun x -> ExtXml.attrib x "name" = name) "section" - with - Not_found -> - failwith (Printf.sprintf "Child 'section' with 'name=%s' expected in '%s'\n" name (Xml.to_string A.ac.airframe)) - - let tag_define = fun sect name -> - try - (ExtXml.child sect ~select:(fun x -> ExtXml.attrib x "name" = name) "define") - with - Not_found -> - failwith (Printf.sprintf "Child 'define' with 'name=%s' expected in '%s'\n" name (Xml.to_string sect)) - - let defined_value = fun sect name -> - try - (Xml.attrib (tag_define sect name) "value") - with - Not_found -> - failwith (Printf.sprintf "Child 'define' with 'name=%s' in '%s' has no value\n" name (Xml.to_string sect)) - - let float_value = fun section s -> - let x = (defined_value section s) in - try float_of_string x with Failure _ -> - failwith (sprintf "float_of_string: %s" x) - - (* FIXME: refactor code_unit_scale of tag to pprz.ml *) - let code_unit_scale_of_tag = function t -> - (* if unit attribute is not specified don't even attempt to convert the units *) - let u = try ExtXml.attrib t "unit" with _ -> failwith "Unit conversion error" in - let cu = try ExtXml.attrib t "code_unit" with _ -> "" in - (* default value for code_unit is rad[/s] when unit is deg[/s] *) - try match (u, cu) with - ("deg", "") -> PprzLink.scale_of_units u "rad" (* implicit conversion to rad *) - | ("deg/s", "") -> PprzLink.scale_of_units u "rad/s" (* implicit conversion to rad/s *) - | (_, "") -> failwith "Unit conversion error" (* code unit is not defined and no implicit conversion *) - | (_,_) -> PprzLink.scale_of_units u cu (* try to convert *) - with - PprzLink.Unit_conversion_error s -> prerr_endline (sprintf "Unit conversion error: %s" s); flush stderr; exit 1 - | PprzLink.Unknown_conversion (su, scu) -> prerr_endline (sprintf "Warning: unknown unit conversion: from %s to %s" su scu); flush stderr; failwith "Unknown unit conversion" - | _ -> failwith "Unit conversion error" - - let code_value = fun section s -> - let t = (tag_define section s) in - try - let coef = try (code_unit_scale_of_tag t) with _ -> 1. in - (ExtXml.float_attrib t "value") *. coef - with - _ -> - failwith (Printf.sprintf "Can't convert 'define' with 'name=%s' in '%s' to floating point value\n" s (Xml.to_string section)) - - let simu_section = - try section "SIMU" with _ -> Xml.Element("", [], []) - - let roll_response_factor = - try float_value simu_section "ROLL_RESPONSE_FACTOR" with _ -> 15. - - let pitch_response_factor = - try float_value simu_section "PITCH_RESPONSE_FACTOR" with _ -> 1. - - let yaw_response_factor = - try float_value simu_section "YAW_RESPONSE_FACTOR" with _ -> 1. - - let weight = - try float_value simu_section "WEIGHT" with _ -> 1. - - let max_bat_level = - try float_value (section "BAT") "MAX_BAT_LEVEL" with _ -> 12.5 - - let h_ctrl_section = - try section "HORIZONTAL CONTROL" with _ -> Xml.Element("",[],[]) - - let max_phi = - try code_value h_ctrl_section "ROLL_MAX_SETPOINT" with _ -> 0.7 (* rad *) - let max_phi_dot = 0.25 (* rad/s *) - let bound = fun x mi ma -> if x > ma then ma else if x < mi then mi else x - - - - let commands = - try - let l = ExtXml.child A.ac.airframe "commands" in - let rec loop i = function - [] -> [] - | x::xs -> (ExtXml.attrib x "name", i)::loop (i+1) xs in - loop 0 (Xml.children l) - with - Not_found -> - failwith (Printf.sprintf "Child 'commands' expected in '%s'\n" (Xml.to_string A.ac.airframe)) - - let command = fun n -> - try List.assoc n commands with - Not_found -> failwith (sprintf "Unknown command '%s'" n) - - let misc_section = section "MISC" - - let infrared_section = try section "INFRARED" with _ -> Xml.Element("",[],[]) - - let nominal_airspeed = code_value misc_section "NOMINAL_AIRSPEED" - let maximum_airspeed = try code_value misc_section "MAXIMUM_AIRSPEED" with _ -> nominal_airspeed *. 1.5 - let max_power = try code_value misc_section "MAXIMUM_POWER" with _ -> 5. *. maximum_airspeed *. weight - - let roll_neutral_default = try code_value infrared_section "ROLL_NEUTRAL_DEFAULT" with _ -> 0. - let pitch_neutral_default = try code_value infrared_section "PITCH_NEUTRAL_DEFAULT" with _ -> 0. - - - let vert_ctrl_section = try section "VERTICAL CONTROL" with _ -> Xml.Element("",[],[]) - let cruise_thrust = try float_value vert_ctrl_section "AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE" with _ -> 0.45 - - let min_thrust = 0 - let max_thrust = max_pprz - - let command_throttle = command "THROTTLE" - let command_roll = command "ROLL" - let command_pitch = command "PITCH" - - let float_attrib = fun x a -> float_of_string (ExtXml.attrib x a) - let int_attrib = fun x a -> int_of_string (ExtXml.attrib x a) - - - let do_commands = fun state commands -> - let c_lda = 4e-5 in (* FIXME *) - state.delta_a <- c_lda *. float commands.(command_roll); - state.delta_b <- float commands.(command_pitch); - state.thrust <- (float (commands.(command_throttle) - min_thrust) /. float (max_thrust - min_thrust)) - - let nb_commands = 10 (* FIXME *) - - let init route = { - start = Unix.gettimeofday (); t = 0.; x = 0.; y = 0. ; z = 0.; - psi = route; phi = 0.; phi_dot = 0.; theta_dot = 0.; psi_dot = 0.; - delta_a = 0.; delta_b = 0.; thrust = 0.; air_speed = 0.; - theta = 0.; z_dot = 0. - } - -(* Minimum complexity *) -(* - Johnson, E.N., Fontaine, S.G., and Kahn, A.D., - “Minimum Complexity Uninhabited Air Vehicle Guidance And Flight Control System,” - Proceedings of the 20th Digital Avionics Systems Conference, 2001. - http://www.ae.gatech.edu/~ejohnson/JohnsonFontaineKahn.pdf - - Johnson, E.N. and Fontaine, S.G., - “Use Of Flight Simulation To Complement Flight Testing Of Low-Cost UAVs,” - Proceedings of the AIAA Modeling and Simulation Technology Conference, 2001. - http://www.ae.gatech.edu/~ejohnson/AIAA%202001-4059.pdf - *) - let state_update = fun state nominal_airspeed (wx, wy, wz) agl dt -> - let now = state.t +. dt in - if state.air_speed = 0. && state.thrust > 0. then - state.air_speed <- nominal_airspeed; - - if agl >= -3. && state.air_speed > 0. then begin - let v2 = state.air_speed**2. - and vn2 = nominal_airspeed ** 2. in - - let phi_dot_dot = roll_response_factor *. state.delta_a *. v2 /. vn2 -. state.phi_dot in - state.phi_dot <- state.phi_dot +. phi_dot_dot *. dt; - state.phi_dot <- bound state.phi_dot (-.max_phi_dot) max_phi_dot; - state.phi <- norm_angle (state.phi +. state.phi_dot *. dt); - state.phi <- bound state.phi (-.max_phi) max_phi; - - state.psi_dot <- -. g /. state.air_speed *. tan (yaw_response_factor *. state.phi); - state.psi <- norm_angle (state.psi +. state.psi_dot *. dt); - - (* Aerodynamic pitching moment coeff, proportional to elevator; - No Thrust moment, so null (0) for steady flight *) - let c_m = 5e-7 *.state.delta_b in - let theta_dot_dot = pitch_response_factor *. c_m *. v2 -. state.theta_dot in - state.theta_dot <- state.theta_dot +. theta_dot_dot *. dt; - state.theta <- state.theta +. state.theta_dot *. dt; - - (* Flight path angle *) - let gamma = atan2 state.z_dot state.air_speed in - - (* Cz proportional to angle of attack *) - let alpha = state.theta -. gamma in - let c_z = 0.2 *. alpha +. g /. vn2 in - - (* Lift *) - let lift = c_z *. state.air_speed**2. in - let z_dot_dot = lift /. weight *. cos state.theta *. cos state.phi -. g in - state.z_dot <- state.z_dot +. z_dot_dot *.dt; - state.z <- state.z +. state.z_dot *. dt; - - (* Constant Cx, drag to get expected cruise and maximum throttle *) - let drag = cruise_thrust +. (v2 -. vn2)*.(1.-. cruise_thrust)/.(maximum_airspeed ** 2. -. vn2) in - let air_speed_dot = max_power /. state.air_speed *. (state.thrust -. drag) /. weight -. g *. sin gamma in - state.air_speed <- state.air_speed +. air_speed_dot *. dt; - state.air_speed <- max state.air_speed 10.; (* Avoid stall *) - - (* FIXME: wind effect should be in the forces *) - let x_dot = state.air_speed *. cos state.psi +. wx - and y_dot = state.air_speed *. sin state.psi +. wy in - state.x <- state.x +. x_dot *. dt; - state.y <- state.y +. y_dot *. dt; - state.z <- state.z +. wz *. dt - end; - state.t <- now -end (* Make functor *) diff --git a/sw/simulator/flightModel.mli b/sw/simulator/flightModel.mli deleted file mode 100644 index 289924243f..0000000000 --- a/sw/simulator/flightModel.mli +++ /dev/null @@ -1,53 +0,0 @@ -(* - * Basic flight model for simulation - * - * Copyright (C) 2004-2006 Pascal Brisset, Antoine Drouin - * - * 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. - * - *) - -type meter = float -type meter_s = float -type radian = float -type radian_s = float -type state - -val get_xyz : state -> meter * meter * meter -val get_time : state -> float -val get_attitude : state -> radian * radian * radian -val get_pqr : state -> radian_s * radian_s * radian_s - -val set_air_speed : state -> meter_s -> unit -val get_air_speed : state -> meter_s - -module type SIG = - sig - val init : radian -> state - val do_commands : state -> Simlib.pprz_t array -> unit - val nb_commands : int - val nominal_airspeed : float (* m/s *) - val max_bat_level : float (* V *) - val roll_neutral_default : float (* rad *) - val pitch_neutral_default : float (* rad *) - val state_update : state -> float -> float * float *float -> float -> float -> unit - (** [state_update nom_airspeed state (wind_x, wind_y, wind_z) on_ground dt] With m/s for wind and s for - dt *) - end - -module Make : functor (A : Data.MISSION) -> SIG diff --git a/sw/simulator/gps.ml b/sw/simulator/gps.ml deleted file mode 100644 index d2ecfd11e8..0000000000 --- a/sw/simulator/gps.ml +++ /dev/null @@ -1,81 +0,0 @@ -(* - * Basic GPS parameters simulation - * - * Copyright (C) 2004 Pascal Brisset, Antoine Drouin - * - * 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. - * - *) - -open Simlib -open Latlong - -type state = { - mutable availability : bool; - wgs84 : Latlong.geographic; - alt : float; - time : float; - climb : float; - gspeed : float; - course : float - } - - -let climb_noise = - let ng = Ocaml_tools.make_1st_order_noise_generator 0.9 0.1 in - fun c -> c +. ng () - -let state = fun pos0 alt0 -> - let last_x = ref 0. and last_y = ref 0. and last_z = ref 0. - and last_gspeed = ref 0. and last_course = ref 0. and last_climb = ref 0. - and last_t = ref 0. - and tow = float (Latlong.get_gps_tow ()) in - - fun (x, y, z) t -> - let dt = t -. !last_t in - - if dt > 0. then begin (** Compute derivatives *) - let dx = x -. !last_x - and dy = y -. !last_y in - last_gspeed := sqrt (dx*.dx +. dy*.dy) /. dt; - last_course := norm_angle (pi/.2. -. atan2 dy dx); - last_climb := (z -. !last_z) /. dt - end; (** Else use previous derivatives *) - - let utm0 = utm_of WGS84 !pos0 in - let utm = utm_add utm0 (x, y) in - let wgs84 = of_utm WGS84 utm - and alt = !alt0 +. z in - - last_x := x; - last_y := y; - last_z := z; - last_t := t; - - let course = if !last_course < 0. then !last_course +. 2. *. pi else !last_course in - - { - wgs84 = wgs84; - alt = alt; - time = t +. tow; - climb = climb_noise !last_climb; - gspeed = !last_gspeed; - course = course; - availability = true; - } - diff --git a/sw/simulator/gps.mli b/sw/simulator/gps.mli deleted file mode 100644 index 66ca9ba030..0000000000 --- a/sw/simulator/gps.mli +++ /dev/null @@ -1,36 +0,0 @@ -(* - * Basic GPS parameters simulation - * - * Copyright (C) 2004 Pascal Brisset, Antoine Drouin - * - * 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. - * - *) - -type state = { - mutable availability : bool; - wgs84 : Latlong.geographic; - alt : float; - time : float; - climb : float; - gspeed : float; - course : float; (** North = 0 *) - } -val state : Latlong.geographic ref -> float ref -> (float * float * float -> float -> state) -(** [state pos0 alt0] Returns a function which must be called with -an updated position [xyz] and time [t] *) diff --git a/sw/simulator/flight_gear.h b/sw/simulator/nps/flight_gear.h similarity index 100% rename from sw/simulator/flight_gear.h rename to sw/simulator/nps/flight_gear.h diff --git a/sw/simulator/nps/nps_fdm.h b/sw/simulator/nps/nps_fdm.h index fbd4e218b6..196c32ec1a 100644 --- a/sw/simulator/nps/nps_fdm.h +++ b/sw/simulator/nps/nps_fdm.h @@ -27,7 +27,7 @@ extern "C" { #endif #include "std.h" -#include "../flight_gear.h" +#include "flight_gear.h" #include "math/pprz_geodetic_double.h" #include "math/pprz_algebra_double.h" diff --git a/sw/simulator/nps/nps_fdm_fixedwing_sim.c b/sw/simulator/nps/nps_fdm_fixedwing_sim.c new file mode 100644 index 0000000000..b7d54e57a4 --- /dev/null +++ b/sw/simulator/nps/nps_fdm_fixedwing_sim.c @@ -0,0 +1,309 @@ +/* + * Copyright (C) 2023 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 + * . + */ + +#include "nps_fdm.h" + +#include +#include + +#include "std.h" +#include "math/pprz_geodetic.h" +#include "math/pprz_geodetic_double.h" +#include "math/pprz_geodetic_float.h" +#include "math/pprz_algebra.h" +#include "math/pprz_algebra_float.h" +#include "math/pprz_isa.h" + +#include "generated/airframe.h" +#include "generated/flight_plan.h" + +#include "state.h" + +// Check if rover firmware +#ifndef FIXEDWING_FIRMWARE +#error "The module nps_fdm_fixedwing_sim is a basic flight model for fixedwing only" +#endif + +#ifndef ROLL_RESPONSE_FACTOR +#define ROLL_RESPONSE_FACTOR 15. +#endif + +#ifndef PITCH_RESPONSE_FACTOR +#define PITCH_RESPONSE_FACTOR 1. +#endif + +#ifndef YAW_RESPONSE_FACTOR +#define YAW_RESPONSE_FACTOR 1. +#endif + +#ifndef WEIGHT +#define WEIGHT 1. +#endif + +#ifndef ROLL_MAX_SETPOINT +#define ROLL_MAX_SETPOINT RadOfDeg(40.) +#endif + +#ifndef ROLL_RATE_MAX_SETPOINT +#define ROLL_RATE_MAX_SETPOINT RadOfDeg(15.) +#endif + +#ifndef NOMINAL_AIRSPEED +#error "Please define NOMINAL_AIRSPEED in your airframe file" +#endif + +#ifndef MAXIMUM_AIRSPEED +#define MAXIMUM_AIRSPEED (NOMINAL_AIRSPEED * 1.5) +#endif + +#ifndef MAXIMUM_POWER +#define MAXIMUM_POWER (5. * MAXIMUM_AIRSPEED * WEIGHT) +#endif + +#ifndef AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE +#define AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE 0.45 +#endif + +// NpsFdm structure +struct NpsFdm fdm; + +// Reference point +static struct LtpDef_d ltpdef; + +// Static functions declaration +static void init_ltp(void); + +struct fixedwing_sim_state { + struct EnuCoor_d pos; + struct EnuCoor_d speed; + struct DoubleEulers attitude; + struct DoubleEulers rates; + double airspeed; + double delta_roll; + double delta_pitch; + double delta_thrust; + struct EnuCoor_d wind; +}; + +static struct fixedwing_sim_state sim_state; + + +/** NPS FDM rover init ***************************/ +void nps_fdm_init(double dt) +{ + fdm.init_dt = dt; // (1 / simulation freq) + fdm.curr_dt = dt; + fdm.time = dt; + + fdm.on_ground = TRUE; + + fdm.nan_count = 0; + fdm.pressure = -1; + fdm.pressure_sl = PPRZ_ISA_SEA_LEVEL_PRESSURE; + fdm.total_pressure = -1; + fdm.dynamic_pressure = -1; + fdm.temperature = -1; + + fdm.ltpprz_to_body_eulers.phi = 0.0; + fdm.ltpprz_to_body_eulers.theta = 0.0; + fdm.ltpprz_to_body_eulers.psi = 0.0; + + VECT3_ASSIGN(sim_state.pos, 0., 0., 0.); + VECT3_ASSIGN(sim_state.speed, 0., 0., 0.); + EULERS_ASSIGN(sim_state.attitude, 0., 0., 0.); + EULERS_ASSIGN(sim_state.rates, 0., 0., 0.); + VECT3_ASSIGN(sim_state.wind, 0., 0., 0.); + sim_state.airspeed = 0.; + sim_state.delta_roll = 0.; + sim_state.delta_pitch = 0.; + sim_state.delta_thrust = 0.; + + init_ltp(); +} + + +/** Minimum complexity flight dynamic model + * In legacy Paparazzi simulator, was implemented in OCaml + * and correspond to the 'sim' target + * + * Johnson, E.N., Fontaine, S.G., and Kahn, A.D., + * “Minimum Complexity Uninhabited Air Vehicle Guidance And Flight Control System,” + * Proceedings of the 20th Digital Avionics Systems Conference, 2001. + * http://www.ae.gatech.edu/~ejohnson/JohnsonFontaineKahn.pdf + + * Johnson, E.N. and Fontaine, S.G., + * “Use Of Flight Simulation To Complement Flight Testing Of Low-Cost UAVs,” + * Proceedings of the AIAA Modeling and Simulation Technology Conference, 2001. + * http://www.ae.gatech.edu/~ejohnson/AIAA%202001-4059.pdf + */ +void nps_fdm_run_step(bool launch, double *commands, int commands_nb __attribute__((unused))) +{ + + // commands + sim_state.delta_roll = commands[COMMAND_ROLL] * MAX_PPRZ; + sim_state.delta_pitch = -commands[COMMAND_PITCH] * MAX_PPRZ; // invert back to correct sign + sim_state.delta_thrust = commands[COMMAND_THROTTLE]; + + static bool already_launched = false; + if (launch && !already_launched) { + sim_state.airspeed = NOMINAL_AIRSPEED; + already_launched = true; + } + if (sim_state.airspeed == 0. && sim_state.delta_thrust > 0.) { + sim_state.airspeed = NOMINAL_AIRSPEED; + } + + if (sim_state.pos.z >= -3. && sim_state.airspeed > 0.) { + double v2 = sim_state.airspeed * sim_state.airspeed; + double vn2 = NOMINAL_AIRSPEED * NOMINAL_AIRSPEED; + + // roll dynamic + double c_l = 4e-5 * sim_state.delta_roll; + double phi_dot_dot = ROLL_RESPONSE_FACTOR * c_l * v2 / vn2 - sim_state.rates.phi; + sim_state.rates.phi = sim_state.rates.phi + phi_dot_dot * fdm.curr_dt; + BoundAbs(sim_state.rates.phi, ROLL_RATE_MAX_SETPOINT); + sim_state.attitude.phi = sim_state.attitude.phi + sim_state.rates.phi * fdm.curr_dt; + NormRadAngle(sim_state.attitude.phi); + BoundAbs(sim_state.attitude.phi, ROLL_MAX_SETPOINT); + + // yaw dynamic + sim_state.rates.psi = PPRZ_ISA_GRAVITY / sim_state.airspeed * tan(YAW_RESPONSE_FACTOR * sim_state.attitude.phi); + sim_state.attitude.psi = sim_state.attitude.psi + sim_state.rates.psi * fdm.curr_dt; + NormRadAngle(sim_state.attitude.psi); + + // Aerodynamic pitching moment coeff, proportional to elevator + // No Thrust moment, so null (0) for steady flight + double c_m = 5e-7 * sim_state.delta_pitch; + double theta_dot_dot = PITCH_RESPONSE_FACTOR * c_m * v2 - sim_state.rates.theta; + sim_state.rates.theta = sim_state.rates.theta + theta_dot_dot * fdm.curr_dt; + sim_state.attitude.theta = sim_state.attitude.theta + sim_state.rates.theta * fdm.curr_dt; + + // Flight path angle + double gamma = atan2(sim_state.speed.z, sim_state.airspeed); + + // Cz proportional to angle of attack + double alpha = sim_state.attitude.theta - gamma; + double c_z = 0.2 * alpha + PPRZ_ISA_GRAVITY / vn2; + + // Lift + double lift = c_z * v2; + double z_dot_dot = lift / WEIGHT * cos(sim_state.attitude.theta) * cos(sim_state.attitude.phi) - PPRZ_ISA_GRAVITY; + sim_state.speed.z = sim_state.speed.z + z_dot_dot * fdm.curr_dt; + sim_state.pos.z = sim_state.pos.z + sim_state.speed.z * fdm.curr_dt; + + // Constant Cx, drag to get expected cruise and maximum throttle + double cruise_th = AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE; + double drag = cruise_th + (v2 - vn2) * (1. - cruise_th) / (MAXIMUM_AIRSPEED*MAXIMUM_AIRSPEED - vn2); + double airspeed_dot = MAXIMUM_POWER / sim_state.airspeed * (sim_state.delta_thrust - drag) / WEIGHT - PPRZ_ISA_GRAVITY * sin(gamma); + sim_state.airspeed = sim_state.airspeed + airspeed_dot * fdm.curr_dt; + sim_state.airspeed = Max(sim_state.airspeed, 10.); // avoid stall + + // Wind effect (FIXME apply to forces ?) + sim_state.speed.x = sim_state.airspeed * sin(sim_state.attitude.psi) + sim_state.wind.x; + sim_state.speed.y = sim_state.airspeed * cos(sim_state.attitude.psi) + sim_state.wind.y; + sim_state.pos.x = sim_state.pos.x + sim_state.speed.x * fdm.curr_dt; + sim_state.pos.y = sim_state.pos.y + sim_state.speed.y * fdm.curr_dt; + sim_state.pos.z = sim_state.pos.z + sim_state.wind.z * fdm.curr_dt; + } + + /****************************************************************************/ + // Coordenates transformations | + // ----------------------------| + + /* in ECEF */ + ecef_of_enu_point_d(&fdm.ecef_pos, <pdef, &sim_state.pos); + lla_of_ecef_d(&fdm.lla_pos, &fdm.ecef_pos); + ecef_of_enu_vect_d(&fdm.ecef_ecef_vel, <pdef, &sim_state.speed); + //ecef_of_enu_vect_d(&fdm.ecef_ecef_accel, <pdef, &rover_acc); + + /* in NED */ + ned_of_ecef_point_d(&fdm.ltpprz_pos, <pdef, &fdm.ecef_pos); + ned_of_ecef_vect_d(&fdm.ltpprz_ecef_vel, <pdef, &fdm.ecef_ecef_vel); + ned_of_ecef_vect_d(&fdm.ltpprz_ecef_accel, <pdef, &fdm.ecef_ecef_accel); + fdm.hmsl = -fdm.ltpprz_pos.z + NAV_ALT0 / 1000.; + + /* Eulers */ + fdm.ltp_to_body_eulers = sim_state.attitude; + // Exporting Eulers to AHRS (in quaternions) + double_quat_of_eulers(&fdm.ltp_to_body_quat, &fdm.ltp_to_body_eulers); + + // Angular vel & acc + fdm.body_ecef_rotvel.p = sim_state.rates.phi; + fdm.body_ecef_rotvel.q = sim_state.rates.theta; + fdm.body_ecef_rotvel.r = sim_state.rates.psi; +} + + +/************************** + ** Generating LTP plane ** + **************************/ + +static void init_ltp(void) +{ + + struct LlaCoor_d llh_nav0; /* Height above the ellipsoid */ + llh_nav0.lat = RadOfDeg((double)NAV_LAT0 / 1e7); + llh_nav0.lon = RadOfDeg((double)NAV_LON0 / 1e7); + llh_nav0.alt = (double)(NAV_ALT0) / 1000.0; + + struct EcefCoor_d ecef_nav0; + + ecef_of_lla_d(&ecef_nav0, &llh_nav0); + + ltp_def_from_ecef_d(<pdef, &ecef_nav0); + fdm.ecef_pos = ecef_nav0; + + // accel and mag data should not be used + // AHRS and INS are bypassed + fdm.ltp_g.x = 0.; + fdm.ltp_g.y = 0.; + fdm.ltp_g.z = 0.; + + fdm.ltp_h.x = 1.; + fdm.ltp_h.y = 0.; + fdm.ltp_h.z = 0.; + +} + + +void nps_fdm_set_wind(double speed, double dir) +{ + sim_state.wind.x = speed * sin(dir); + sim_state.wind.y = speed * cos(dir); + sim_state.wind.z = 0.; +} + +void nps_fdm_set_wind_ned(double wind_north, double wind_east, double wind_down) +{ + sim_state.wind.x = wind_east; + sim_state.wind.y = wind_north; + sim_state.wind.z = -wind_down; +} + +void nps_fdm_set_turbulence(double wind_speed __attribute__((unused)), + int turbulence_severity __attribute__((unused))) +{ +} + +void nps_fdm_set_temperature(double temp __attribute__((unused)), + double h __attribute__((unused))) +{ +} + diff --git a/sw/simulator/nps/nps_flightgear.c b/sw/simulator/nps/nps_flightgear.c index 2e60cee3dd..4de8f41472 100644 --- a/sw/simulator/nps/nps_flightgear.c +++ b/sw/simulator/nps/nps_flightgear.c @@ -13,7 +13,7 @@ #include #include "std.h" -#include "../flight_gear.h" +#include "flight_gear.h" #include "nps_main.h" #include "nps_fdm.h" #include "nps_atmosphere.h" diff --git a/sw/simulator/pprzsim-launch b/sw/simulator/pprzsim-launch index cfcd8e6f1e..45335a6490 100755 --- a/sw/simulator/pprzsim-launch +++ b/sw/simulator/pprzsim-launch @@ -50,46 +50,28 @@ def main(): action="store_true", dest="verbose") parser.add_option("--norc", dest="norc", action="store_true", help="Hide the simulated RC") - - # special options for the fixedwing OCaml sim - ocamlsim_opts = OptionGroup(parser, "Simple fixedwing OCaml Simulator Options", - "These are only relevant to the simple fixedwing OCaml sim") - ocamlsim_opts.add_option("--boot", dest="boot", action="store_true", - help="Boot the aircraft on start") - #ocamlsim_opts.add_option("--norc", dest="norc", action="store_true", - # help="Hide the simulated RC") - ocamlsim_opts.add_option("--noground", dest="noground", action="store_true", - help="Disable ground detection") - ocamlsim_opts.add_option("--launch", dest="launch", action="store_true", - help="Launch the aircraft on startup") - - # special options for NPS - nps_opts = OptionGroup(parser, "NPS Options", "These are only relevant to the NPS sim") - nps_opts.add_option("-p", "--fg_port", dest="fg_port", metavar="PORT", - type="int", default=5501, action="store", - help="Port on FlightGear host to connect to (Default: %default)") - nps_opts.add_option("-P", "--fg_port_in", dest="fg_port_in", metavar="PORT_IN", - type="int", default=5502, action="store", - help="Port to receive from FlightGear host (Default: %default)") - nps_opts.add_option("--fg_time_offset", type="int", action="store", metavar="SEC", - help="FlightGear time offset in seconds (e.g. 21600 for 6h)") - nps_opts.add_option("-j", "--js_dev", dest="js_dev", metavar="IDX", - type="int", default=-1, action="store", - help="Use joystick with specified index (e.g. 0)") - nps_opts.add_option("--spektrum_dev", type="string", action="callback", - callback=spektrum_callback, metavar="DEV", - help="Spektrum device to use (e.g. /dev/ttyUSB0)") - nps_opts.add_option("--rc_script", type="int", action="store", metavar="NO", - help="Number of RC script to use") - nps_opts.add_option("--time_factor", type="float", action="store", metavar="factor", - help="Time factor (default 1.0)") - nps_opts.add_option("--fg_fdm", action="store_true", - help="Use FlightGear native-fdm protocol instead of native-gui") - nps_opts.add_option("--nodisplay", dest="nodisplay", action="store_true", - help="Don't send NPS Ivy messages") - - parser.add_option_group(ocamlsim_opts) - parser.add_option_group(nps_opts) + parser.add_option("-p", "--fg_port", dest="fg_port", metavar="PORT", + type="int", default=5501, action="store", + help="Port on FlightGear host to connect to (Default: %default)") + parser.add_option("-P", "--fg_port_in", dest="fg_port_in", metavar="PORT_IN", + type="int", default=5502, action="store", + help="Port to receive from FlightGear host (Default: %default)") + parser.add_option("--fg_time_offset", type="int", action="store", metavar="SEC", + help="FlightGear time offset in seconds (e.g. 21600 for 6h)") + parser.add_option("-j", "--js_dev", dest="js_dev", metavar="IDX", + type="int", default=-1, action="store", + help="Use joystick with specified index (e.g. 0)") + parser.add_option("--spektrum_dev", type="string", action="callback", + callback=spektrum_callback, metavar="DEV", + help="Spektrum device to use (e.g. /dev/ttyUSB0)") + parser.add_option("--rc_script", type="int", action="store", metavar="NO", + help="Number of RC script to use") + parser.add_option("--time_factor", type="float", action="store", metavar="factor", + help="Time factor (default 1.0)") + parser.add_option("--fg_fdm", action="store_true", + help="Use FlightGear native-fdm protocol instead of native-gui") + parser.add_option("--nodisplay", dest="nodisplay", action="store_true", + help="Don't send NPS Ivy messages") (options, args) = parser.parse_args() @@ -121,22 +103,7 @@ def main(): simargs = [] - if options.simtype == "sim": - if options.boot: - simargs.append("-boot") - if options.norc: - simargs.append("-norc") - if options.noground: - simargs.append("-noground") - if options.launch: - simargs.append("-launch") - if options.ivy_bus: - simargs.append("-b") - simargs.append(options.ivy_bus) - if options.fg_host: - simargs.append("-fg") - simargs.append(options.fg_host) - elif (options.simtype == "nps") or (options.simtype == "hitl"): + if options.simtype in ["sim", "nps", "hitl"]: if options.fg_host: simargs.append("--fg_host") simargs.append(options.fg_host) diff --git a/sw/simulator/sim.ml b/sw/simulator/sim.ml deleted file mode 100644 index 09d634f8c5..0000000000 --- a/sw/simulator/sim.ml +++ /dev/null @@ -1,325 +0,0 @@ -(* - * Hardware in the loop basic simulator (handling GPS, infrared and commands) - * - * Copyright (C) 2004-2006 Pascal Brisset, Antoine Drouin - * - * 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. - * - *) - - -open Printf -open Simlib -open Latlong - -module Ground_Pprz = PprzLink.Messages(struct let name = "ground" end) - -let float_attrib xml a = float_of_string (ExtXml.attrib xml a) - -(* From mapFP.ml to avoid complex linking *) -let georef_of_xml = fun xml -> - let lat0 = Latlong.deg_of_string (ExtXml.attrib xml "lat0") - and lon0 = Latlong.deg_of_string (ExtXml.attrib xml "lon0") in - { posn_lat = (Deg>>Rad)lat0; posn_long = (Deg>>Rad)lon0 } - - -(* Frequencies for perdiodic tasks are expressed in s *) -let airspeed_period = 1./.20. -let fm_period = 1./.25. -let fg_period = 1./.25. -let ahrs_period = 1./.20. - -let gensym = let n = ref 0 in fun p -> incr n; p ^ string_of_int !n -let cb_register = fun closure -> - let s = gensym "sim_callback_" in - Callback.register s closure; - s - - -module type AIRCRAFT = -sig - val init : int -> GPack.box -> unit - (** [init ac_id box] *) - val boot : Simlib.value -> unit - (** [boot time_acceleration] *) - - val commands : pprz_t array -> unit - (** Called once at init *) - - val airspeed : float -> unit - (** [air_speed] Called on timer *) - - val attitude_and_rates : float -> float -> float -> float -> float -> float ->unit - (** [ahrs phi theta psi p q r] Called on timer *) - - val gps : Gps.state -> unit - (** [gps state] Called on timer *) -end - - -module type AIRCRAFT_ITL = - functor (A : Data.MISSION) -> functor (FM: FlightModel.SIG) -> AIRCRAFT - -external fg_sizeof : unit -> int = "fg_sizeof" -external fg_msg : bytes -> float -> float -> float -> float -> float -> float -> unit = "fg_msg_bytecode" "fg_msg_native" - -let ac_name = ref "A/C not set" - -let ivy_bus = ref Defivybus.default_ivy_bus - -let fg_client = ref "" - -let autoboot = ref false -let autolaunch = ref false -let noground = ref false - -let common_options = [ - "-b", Arg.Set_string ivy_bus, (sprintf " Default is %s" !ivy_bus); - "-boot", Arg.Set autoboot, "Boot the A/C on start"; - "-launch", Arg.Set autolaunch, "Launch the A/C on start"; - "-noground", Arg.Set noground, "Disable ground detection"; - "-fg", Arg.Set_string fg_client, "Flight gear client address" -] - -module Make(AircraftItl : AIRCRAFT_ITL) = struct - - module A = struct - let ac = Data.aircraft !ac_name - end - - module FM = FlightModel.Make(A) - - module Aircraft = AircraftItl(A)(FM) - - let flight_plan = A.ac.Data.flight_plan - - let pos0 = ref (georef_of_xml flight_plan) - let qfu = try float_attrib flight_plan "qfu" with _ -> 0. - - (* Try to get the ground alt from the SRTM data, default to flight plan *) - - let alt0 = - let ground_alt = - try - float (Srtm.of_wgs84 !pos0) - with Srtm.Tile_not_found x -> - float_attrib flight_plan "ground_alt" in - ref ground_alt - - let main () = - let icon = GdkPixbuf.from_file Env.icon_sim_file in - let window = GWindow.window ~type_hint:`DIALOG ~icon ~title: !ac_name () in - let quit = fun () -> GMain.Main.quit (); exit 0 in - ignore (window#connect#destroy ~callback:quit); - let vbox = GPack.vbox ~packing:window#add () in - - Aircraft.init A.ac.Data.id vbox; - - let gps_period = 0.25 in - - let compute_gps_state = Gps.state pos0 alt0 in - - let initial_state = FM.init (pi/.2. -. qfu/.180.*.pi) in - - let state = ref initial_state in - - let _reset = fun () -> state := initial_state in - - let commands = Array.make FM.nb_commands 0 in - - Aircraft.commands commands; - - let north_label = GMisc.label ~text:"000" () - and east_label = GMisc.label ~text:"000" () - and alt_label = GMisc.label ~text:"000" () in - - let last_gps_state = ref None in - let _run = ref false in - - let wind_x = ref 0. - and wind_y = ref 0. - and wind_z = ref 0. in - let time_scale = object val mutable v = 1. method value = v method set_value x = v <- x end - and gps_availability = ref 1 in - - let world_update = fun _ vs -> - gps_availability := PprzLink.int_assoc "gps_availability" vs; - wind_x := PprzLink.float_assoc "wind_east" vs; - wind_y := PprzLink.float_assoc "wind_north" vs; - wind_z := PprzLink.float_assoc "wind_up" vs; - time_scale#set_value (PprzLink.float_assoc "time_scale" vs) - in - - let ask_for_world_env = fun () -> - try - let (x, y, z) = FlightModel.get_xyz !state in - - let gps_sol = compute_gps_state (x,y,z) (FlightModel.get_time !state) in - - let float = fun f -> PprzLink.Float f in - let values = ["east", float x; "north", float y; "up", float z; - "lat", float ((Rad>>Deg)gps_sol.Gps.wgs84.posn_lat); - "long", float ((Rad>>Deg)gps_sol.Gps.wgs84.posn_long); - "alt", float gps_sol.Gps.alt ] in - let (b, flag) = Ground_Pprz.message_req "sim" "WORLD_ENV" values world_update in - (* unbind manually after 1s if no message received *) - ignore (GMain.Timeout.add ~ms:1000 ~callback:(fun () -> if !flag then Ivy.unbind b; false)) - with - exc -> fprintf stderr "Error in sim: %s\n%!" (Printexc.to_string exc) - in - - ignore (GMain.Timeout.add ~ms:1000 ~callback:(fun () -> ask_for_world_env (); true)); - - - let fm_task = fun () -> - FM.do_commands !state commands; - let agl = - if !noground then max_float - else - match !last_gps_state with - Some s -> - begin - try s.Gps.alt -. float (Srtm.of_wgs84 s.Gps.wgs84) with - _ -> s.Gps.alt -. !alt0 - end - | None -> 0. in - FM.state_update !state FM.nominal_airspeed (!wind_x, !wind_y, !wind_z) agl fm_period - - and airspeed_task = fun () -> - Aircraft.airspeed (FlightModel.get_air_speed !state) - - and gps_task = fun () -> - let (x,y,z) = FlightModel.get_xyz !state in - east_label#set_text (Printf.sprintf "%.0f" x); - north_label#set_text (Printf.sprintf "%.0f" y); - alt_label#set_text (Printf.sprintf "%.0f" z); - let s = compute_gps_state (x,y,z) (FlightModel.get_time !state) in - s.Gps.availability <- not (!gps_availability = 0); - last_gps_state := Some s; - Aircraft.gps s - - and ahrs_task = fun () -> - let (phi, theta, psi) = FlightModel.get_attitude !state - and p, q, r = FlightModel.get_pqr !state in - Aircraft.attitude_and_rates phi theta psi p q r in - - (** Sending to Flight Gear *) - let fg_task = fun socket buffer sockaddr () -> - match !last_gps_state with - None -> () - | Some s -> - let lat = s.Gps.wgs84.Latlong.posn_lat - and lon = s.Gps.wgs84.Latlong.posn_long - and alt = s.Gps.alt - (* and theta_ = s.Gps.course *) - and (phi, theta, psi) = FlightModel.get_attitude !state in - fg_msg buffer lat lon alt phi theta psi; - (** for i = 0 to Bytes.length buffer - 1 do fprintf stderr "%x " (Char.code buffer.[i]) done; fprintf stderr "\n"; **) - try - ignore (Unix.sendto socket buffer 0 (Bytes.length buffer) [] sockaddr) - with - Unix.Unix_error (e,f,a) -> Printf.fprintf stderr "Error sending to FlightGear: %s (%s(%s))\n" (Unix.error_message e) f a; flush stderr - in - - let set_pos = fun _ -> - let current_pos = Latlong.string_of !pos0 in - begin - match GToolbox.input_string ~title:"Setting geographic position" ~text:current_pos "Geographic position" with - Some s -> pos0 := Latlong.of_string s - | _ -> () - end; - begin - let text = string_of_float !alt0 in - match GToolbox.input_string ~title:"Setting initial altitude" ~text "Geographic altitude" with - Some s -> alt0 := float_of_string s - | _ -> () - end - in - - let boot = fun () -> - Aircraft.boot (time_scale:>value); - Simlib.timer ~scale:time_scale fm_period fm_task; - Simlib.timer ~scale:time_scale airspeed_period airspeed_task; - Simlib.timer ~scale:time_scale gps_period gps_task; - Simlib.timer ~scale:time_scale ahrs_period ahrs_task; - - (** Connection to Flight Gear client *) - if !fg_client <> "" then - try - let inet_addr = Unix.inet_addr_of_string !fg_client in - let socket = Unix.socket Unix.PF_INET Unix.SOCK_DGRAM 0 in - (* Unix.connect socket (Unix.ADDR_INET (inet_addr, 5501)); *) - let buffer = Bytes.create (fg_sizeof ()) in - let sockaddr = (Unix.ADDR_INET (inet_addr, 5501)) in - Simlib.timer ~scale:time_scale fg_period (fg_task socket buffer sockaddr); - fprintf stdout "Sending to FlightGear at %s\n" !fg_client; flush stdout - with - e -> fprintf stderr "Error setting up FlightGear viz: %s\n" (Printexc.to_string e); flush stderr - in - - let take_off = fun () -> FlightModel.set_air_speed !state FM.nominal_airspeed in - - let hbox = GPack.hbox ~spacing:4 ~packing:vbox#pack () in - let s = GButton.button ~label:"Set Pos" ~packing:hbox#pack () in - ignore (s#connect#clicked ~callback:set_pos); - - let set_pos_and_boot = fun () -> - s#misc#set_sensitive false; - boot () in - - autoboot := !autoboot || !autolaunch; - - if not !autoboot then begin - let s = GButton.button ~label:"Boot" ~packing:(hbox#pack) () in - let callback = fun () -> - set_pos_and_boot (); - s#misc#set_sensitive false in - ignore (s#connect#clicked ~callback) - end else - set_pos_and_boot (); - - if not !autolaunch then begin - let t = GButton.button ~label:"Launch" ~packing:hbox#pack () in - let callback = fun () -> - take_off (); - t#misc#set_sensitive false in - ignore (t#connect#clicked ~callback); - - (* Monitor an AUTO2 launch to disable the button *) - let monitor = fun () -> - if FlightModel.get_air_speed !state > 0. then begin - t#misc#set_sensitive false; - false - end else - true in - ignore (GMain.Timeout.add ~ms:1000 ~callback:monitor) - end else - take_off (); - - let hbox = GPack.hbox ~packing:vbox#pack () in - let l = fun s -> ignore(GMisc.label ~text:s ~packing:hbox#pack ()) in - l "East:"; hbox#pack east_label#coerce; - l " North:"; hbox#pack north_label#coerce; - l " Height:"; hbox#pack alt_label#coerce; - - Ivy.init (sprintf "Paparazzi sim %d" A.ac.Data.id) "READY" (fun _ _ -> ()); - Ivy.start !ivy_bus; - - window#show (); - Unix.handle_unix_error GMain.Main.main () -end diff --git a/sw/simulator/sim.mli b/sw/simulator/sim.mli deleted file mode 100644 index 5393572cbc..0000000000 --- a/sw/simulator/sim.mli +++ /dev/null @@ -1,26 +0,0 @@ -(** Options for HITL and SITL simulators *) -val common_options : (string * Arg.spec * string) list - -val ac_name : string ref - -(** A complete aircraft with it mission *) -module type AIRCRAFT = - sig - val init : int -> GPack.box -> unit - val boot : Simlib.value -> unit - val commands : Simlib.pprz_t array -> unit - val airspeed : float -> unit - val attitude_and_rates : float -> float -> float -> float -> float -> float -> unit - val gps : Gps.state -> unit - end - -(** A simulated aircraft, without its conf *) -module type AIRCRAFT_ITL = - functor (A : Data.MISSION) -> functor (FM: FlightModel.SIG) -> AIRCRAFT - -(** Functor to build the simulator *) -module Make : - functor (AircraftItl : AIRCRAFT_ITL) -> - sig - val main : unit -> unit - end diff --git a/sw/simulator/simlib.ml b/sw/simulator/simlib.ml deleted file mode 100644 index 66693f4f66..0000000000 --- a/sw/simulator/simlib.ml +++ /dev/null @@ -1,65 +0,0 @@ -(* - * Utilities for the simulators - * - * Copyright (C) 2004 Pascal Brisset, Antoine Drouin - * - * 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. - * - *) - -type pprz_t = int - -let max_pprz = 9600 - -let pi = 4. *. atan 1. -let rec norm_angle = fun x -> - if x > pi then norm_angle (x-.2.*.pi) - else if x < -.pi then norm_angle (x+.2.*.pi) - else x - -let deg_of_rad = fun rad -> rad /. pi *. 180. - -let rad_of_deg = fun x -> x /. 180. *. pi - -let set_float = fun option var name -> - (option, Arg.Set_float var, Printf.sprintf "%s (%f)" name !var) -let set_string = fun option var name -> - (option, Arg.Set_string var, Printf.sprintf "%s (%s)" name !var) - -let ms x = max 0 (truncate (1000.*.x)) -(* Non derivating timer *) -class type value = object method value : float end - -let timer ?scale p f = - let scale = - match scale with - None -> object method value = 1. end - | Some s -> (s :> value) in - let rec loop = fun expected -> - let next = expected +. p /. scale#value in - let dt = ms (next -. Unix.gettimeofday()) in - if dt < 1 then begin (* No timer needed, simply loop *) - f (); loop next - end else - GMain.Timeout.add - ~ms:dt - ~callback:(fun () -> - ignore (loop next); - f (); - false) in - ignore (loop (Unix.gettimeofday())) diff --git a/sw/simulator/simlib.mli b/sw/simulator/simlib.mli deleted file mode 100644 index 00651f2ec7..0000000000 --- a/sw/simulator/simlib.mli +++ /dev/null @@ -1,37 +0,0 @@ -(* - * Utilities for the simulators - * - * Copyright (C) 2004 Pascal Brisset, Antoine Drouin - * - * 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. - * - *) - -type pprz_t = int -val max_pprz : pprz_t -val pi : float -val norm_angle : float -> float -val deg_of_rad : float -> float -val rad_of_deg : float -> float -val set_float : string -> float ref -> string -> string * Arg.spec * string -val set_string : string -> string ref -> string -> string * Arg.spec * string - -class type value = object method value : float end - -val timer : ?scale:#value -> float -> (unit -> 'a) -> unit -(** [timer ?time_accel period_s callback] Non derivating periodic timer *) diff --git a/sw/simulator/simsitl.ml b/sw/simulator/simsitl.ml deleted file mode 100644 index 2913e2c360..0000000000 --- a/sw/simulator/simsitl.ml +++ /dev/null @@ -1,14 +0,0 @@ - -(* don't set locale to avoid float_of_string problem *) -let locale = GtkMain.Main.init ~setlocale:false () - -let _ = - Arg.parse (Sim.common_options@Sitl.options) - (fun x -> Printf.fprintf stderr "Warning: Don't do anything with %s\n" x) - "Usage: " - - -module M = Sim.Make(Sitl.Make) - -let _ = - M.main () diff --git a/sw/simulator/sitl.ml b/sw/simulator/sitl.ml deleted file mode 100644 index 2a4947d5a9..0000000000 --- a/sw/simulator/sitl.ml +++ /dev/null @@ -1,221 +0,0 @@ -(* - * Software in the loop basic simulator (handling GPS, infrared and commands) - * - * Copyright (C) 2004 Pascal Brisset, Antoine Drouin - * - * 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. - * - *) - -open Printf - -module Ground_Pprz = PprzLink.Messages(struct let name = "ground" end) -module Dl_Pprz = PprzLink.Messages(struct let name = "datalink" end) - -let ground_id = 0 (* cf tmtc/link.ml *) - -let ios = int_of_string -let fos = float_of_string - -let raw_datalink_msg_separator = Str.regexp ";" - -let norc = ref false -let adc1 = ref false - -module Make (A:Data.MISSION) (FM: FlightModel.SIG) = struct - - let servos_period = 1./.40. (* s *) - let periodic_period = 1./.60. (* s *) - let rc_period = 1./.40. (* s *) - let sys_time_period = 1./.120. (* s *) - - (* Commands handling (rcommands is the intermediate storage) *) - let rc_channels = Array.of_list (Xml.children A.ac.Data.radio) - let nb_channels = Array.length rc_channels - let rc_channel_no = fun x -> - List.assoc x (Array.to_list (Array.mapi (fun i c -> Xml.attrib c "function", i) rc_channels)) - - let rcommands = ref [||] - let adj_bat = GData.adjustment ~value:FM.max_bat_level ~lower:0. ~upper:(FM.max_bat_level+.2.) ~step_incr:0.1 ~page_size:0. () - - external get_commands : Simlib.pprz_t array -> int = "get_commands" -(** Returns gaz servo value (us) *) - - let energy = ref 0. - - (** Get the commands from the autopilot, store them - (to be used by the flight model) - Computes an energy consumption from throttle level *) - let update_servos = - let accu = ref 0. in - fun bat_button () -> - let gaz = get_commands !rcommands in - (* 100% = 1W *) - if bat_button#active then - let energy = float (gaz-1000) /. 1000. *. servos_period in - accu := !accu +. energy *. 0.00259259259259259252; (* To be improved !!! *) - if !accu >= 0.1 then begin - let b = adj_bat#value in - adj_bat#set_value (b -. !accu); - accu := 0. - end - -(* Radio command handling *) - external update_channel : int -> float -> unit = "update_rc_channel" - external send_ppm : unit -> unit = "send_ppm" - - let inverted = ["ROLL"; "PITCH"; "YAW"; "GAIN1"; "GAIN2"] - - let rc = fun () -> - let name = Xml.attrib A.ac.Data.radio "name" ^ " " ^ A.ac.Data.name in - let icon = GdkPixbuf.from_file Env.icon_file in - let window = GWindow.window ~icon ~title:name ~border_width:0 ~width:200 ~height:400 () in - let quit = fun () -> GMain.Main.quit (); exit 0 in - ignore (window#connect#destroy ~callback:quit); - let vbox = GPack.vbox ~height:10 ~spacing: 1 ~border_width: 1 ~packing:window#add () in - let on_off = GButton.check_button ~label:"On" ~active:true ~packing:vbox#pack () in - let sliders = GPack.vbox ~packing:vbox#add () in - let float_attrib = fun a x -> float_of_string (ExtXml.attrib x a) in - Array.iteri - (fun i c -> - let mi = float_attrib "min" c - and ma = float_attrib "max" c - and value = float_attrib "neutral" c in - let lower = min mi ma - and upper = max mi ma in - let adj = GData.adjustment ~value ~lower ~upper ~step_incr:1.0 () in - let hbox = GPack.hbox ~packing:sliders#add () in - let f = (ExtXml.attrib c "function") in - let _l = GMisc.label ~width:75 ~text:f ~packing:hbox#pack () in - let inv = not ((List.mem f inverted) == (ma < mi)) in - let _scale = GRange.scale `HORIZONTAL ~inverted:inv ~adjustment:adj ~packing:hbox#add () in - let update = fun () -> update_channel i adj#value in - - ignore (adj#connect#value_changed ~callback:update); - update ()) - rc_channels; - (* github issue #821: people seems to want sliders always sensitive, even if RC is OFF *) - (* ignore (on_off#connect#toggled ~callback:(fun () -> sliders#coerce#misc#set_sensitive on_off#active)); *) - - on_off#set_active false; - - let send_ppm = fun () -> - if on_off#active then send_ppm () in - - Simlib.timer rc_period send_ppm; (** FIXME: should use time_scale *) - window#show () - - external sys_time_task : unit -> unit = "sim_sys_time_task" - external periodic_task : unit -> unit = "sim_periodic_task" - external sim_init : unit -> unit = "sim_init" - external update_bat : int -> unit = "update_bat" - external update_adc1 : int -> unit = "update_adc1" - external update_dl_status : int -> unit = "update_dl_status" - - let bat_button = GButton.check_button ~label:"Auto" ~active:false () - let dl_button = GButton.check_button ~label:"Enable Datalink/Telemetry" ~active:true () - - let my_id = ref (-1) - let init = fun id vbox -> - if not !norc then - rc (); - my_id := id; - sim_init (); - - (* Bat level *) - let hbox = GPack.hbox ~spacing:4 ~packing:vbox#add () in - let _label = GMisc.label ~text:"Bat (V) " ~packing:hbox#pack () in - let _scale = GRange.scale `HORIZONTAL ~adjustment:adj_bat ~packing:hbox#add () in - let update = fun () -> update_bat (truncate (adj_bat#value *. 10.)) in - hbox#pack bat_button#coerce; - let tips = GData.tooltips () in - tips#set_tip bat_button#coerce ~text:"Select for auto-decreasing voltage"; - ignore (adj_bat#connect#value_changed ~callback:update); - update (); - - (* Datalink status *) - let hbox = GPack.hbox ~spacing:4 ~packing:vbox#add () in - let update = fun () -> update_dl_status (if dl_button#active then 1 else 0) in - hbox#pack dl_button#coerce; - let tips = GData.tooltips () in - tips#set_tip dl_button#coerce ~text:"Enable/disable communication with the aircraft"; - ignore (dl_button#connect#toggled ~callback:update); - update(); - - (* ADC1 *) - if !adc1 then - let hbox = GPack.hbox ~spacing:4 ~packing:vbox#add () in - let _label = GMisc.label ~text:"Generic ADC 1 " ~packing:hbox#pack () in - let adj_adc1 = GData.adjustment ~value:512. ~lower:0. ~upper:1033. ~step_incr:1. () in - let _scale = GRange.scale `HORIZONTAL ~adjustment:adj_adc1 ~packing:hbox#add () in - let update = fun () -> update_adc1 (truncate adj_adc1#value) in - ignore (adj_adc1#connect#value_changed ~callback:update); - update () - - open Latlong - - external set_message : string -> unit = "set_datalink_message" - let get_message = fun name link_mode _sender vs -> - let set = fun rcv_id -> - let msg_id, _ = Dl_Pprz.message_of_name name in - let s = Dl_Pprz.payload_of_values msg_id ground_id rcv_id vs in - set_message (Protocol.string_of_payload s) in - let ac_id = try Some (PprzLink.int_assoc "ac_id" vs) with _ -> None in - match link_mode, ac_id with - PprzLink.Forwarded, Some x when x = !my_id -> if dl_button#active then set x - | PprzLink.Broadcasted, _ -> if dl_button#active then set PprzLink.broadcast_id - | _ -> () - - let message_bind = fun name link_mode -> - ignore (Dl_Pprz.message_bind name (get_message name link_mode)) - - let boot = fun time_scale -> - Simlib.timer ~scale:time_scale servos_period (update_servos bat_button); - Simlib.timer ~scale:time_scale periodic_period periodic_task; - Simlib.timer ~scale:time_scale sys_time_period sys_time_task; - - (* Forward or broacast messages according to "link" mode *) - Hashtbl.iter - (fun _m_id msg -> - match msg.PprzLink.link with - Some x -> message_bind msg.PprzLink.name x - | _ -> ()) - Dl_Pprz.messages;; - -(* Functions called by the simulator *) - let commands = fun s -> rcommands := s - - external set_airspeed : float -> unit = "set_airspeed" - let airspeed = fun air_speed -> - set_airspeed air_speed - - external provide_attitude : float -> float -> float -> unit = "provide_attitude" - external provide_rates : float -> float -> float -> unit = "provide_rates" - let attitude_and_rates = fun phi theta psi p q r -> - provide_attitude phi theta psi; - provide_rates p q r - - external use_gps_pos: int -> int -> int -> float -> float -> float -> float -> float -> bool -> float -> float -> unit = "sim_use_gps_pos_bytecode" "sim_use_gps_pos" - let gps = fun gps -> - let utm = utm_of WGS84 gps.Gps.wgs84 in - let cm = fun f -> truncate (f *. 100.) in - use_gps_pos (cm utm.utm_x) (cm utm.utm_y) utm.utm_zone gps.Gps.course gps.Gps.alt gps.Gps.gspeed gps.Gps.climb gps.Gps.time gps.Gps.availability gps.Gps.wgs84.Latlong.posn_lat gps.Gps.wgs84.Latlong.posn_long - -end -let options = ["-norc", Arg.Set norc, "Hide the simulated RC"; - "-adc1", Arg.Set adc1, "Enable the generic adc 1"] diff --git a/sw/simulator/sitl.mli b/sw/simulator/sitl.mli deleted file mode 100644 index 00f25de0dc..0000000000 --- a/sw/simulator/sitl.mli +++ /dev/null @@ -1,6 +0,0 @@ -(* Software In The Loop *) - -module Make : Sim.AIRCRAFT_ITL -val options : (string * Arg.spec * string) list -(** Arg options specific to Sitl *) - diff --git a/sw/supervision/python/session_widget.py b/sw/supervision/python/session_widget.py index d944379e53..0f8c0a9812 100644 --- a/sw/supervision/python/session_widget.py +++ b/sw/supervision/python/session_widget.py @@ -131,20 +131,12 @@ class SessionWidget(QWidget, Ui_Session): # simulator is "sim" simulator = "sim" - if simulator == "nps": - t = self.tools["Simulator"] - simu = Program.from_tool(t) - simu.args.append(Arg("-t", "nps")) - self.launch_program(simu) - datalink = Program.from_tool(self.tools["Data Link"]) - datalink.args = [Arg("-udp", None), Arg("-udp_broadcast", None)] - self.launch_program(datalink) - else: - # simulator is "sim" - sim = Program.from_tool(self.tools["Simulator"]) - sim.args.extend([Arg("-t", "sim"), Arg("--boot", None), Arg("--norc", None)]) - self.launch_program(sim) - + sim = Program.from_tool(self.tools["Simulator"]) + sim.args.append(Arg("-t", simulator)) + self.launch_program(sim) + datalink = Program.from_tool(self.tools["Data Link"]) + datalink.args = [Arg("-udp", None), Arg("-udp_broadcast", None)] + self.launch_program(datalink) server = Program.from_tool(self.tools["Server"]) server.args.append(Arg("-n", None)) gcs = Program.from_tool(self.tools["PprzGCS"]) diff --git a/tests/sim/01_Microjet.t b/tests/sim/01_Microjet.t deleted file mode 100644 index a993920d71..0000000000 --- a/tests/sim/01_Microjet.t +++ /dev/null @@ -1,87 +0,0 @@ -#!/usr/bin/perl -w - -use Test::More tests => 7; -use lib "$ENV{'PAPARAZZI_SRC'}/tests/lib"; -use Program; -use Proc::Background; - -$|++; - -#################### -# Make the airframe -my $make_compile_options = "AIRCRAFT=Microjet clean_ac sim"; -my $compile_output = run_program( - "Attempting to build the sim firmware.", - $ENV{'PAPARAZZI_SRC'}, - "make $make_compile_options", - 0,1); -unlike($compile_output, '/Aircraft \'Microjet\' not found in/', "The compile output does not contain the message \"Aircraft \'Microjet\' not found in\""); -unlike($compile_output, '/\bError\b/i', "The compile output does not contain the word \"Error\""); - -# Start the server process -my $server_command = "$ENV{'PAPARAZZI_HOME'}/sw/ground_segment/tmtc/server"; -my @server_options = qw(-n); -my $server = Proc::Background->new($server_command, @server_options); -sleep 2; # The service should die in this time if there's an error -ok($server->alive(), "The server process started successfully"); - -# Start the pprzsim-launch process -my $pprzsim_command = "$ENV{'PAPARAZZI_HOME'}/sw/simulator/pprzsim-launch"; -my @pprzsim_options = qw(-a Microjet --boot --norc); -sleep 2; # The service should die in this time if there's an error -my $pprzsim = Proc::Background->new($pprzsim_command, @pprzsim_options); -ok($pprzsim->alive(), "The pprzsim-launch process started successfully"); - -# Open the Ivy bus and read from it... -SKIP : { - skip "Skipping testing of the simulator since we can't load the Ivy module. Please install IO::Socket::Multicast", 1 unless eval("use Ivy; 1"); - ok(1, "We can load the Ivy module."); - # TODO: learn how to read and write to the Ivy bus -} - -# Shutdown the server and pprzsim-launch processes -ok($server->die(), "The server process shutdown successfully."); -ok($pprzsim->die(), "The pprzsim-launch process shutdown successfully."); - -################################################################################ -# functions used by this test script. -sub run_program -{ - my $message = shift; - my $dir = shift; - my $command = shift; - my $verbose = shift; - my $dont_fail_on_error = shift; - - warn "$message\n" if $verbose; - if (defined $dir) - { - $command = "cd $dir;" . $command; - } - my $prog = new Program("bash"); - my $fh = $prog->open("-c \"$command\""); - warn "Running command: \"". $prog->last_command() ."\"\n" if $verbose; - $fh->autoflush(1); - my @output; - while (<$fh>) - { - warn $_ if $verbose; - chomp $_; - push @output, $_; - } - $fh->close; - my $exit_status = $?/256; - unless ($exit_status == 0) - { - if ($dont_fail_on_error) - { - warn "Error: The command \"". $prog->last_command() ."\" failed to complete successfully. Exit status: $exit_status\n" if $verbose; - } - else - { - die "Error: The command \"". $prog->last_command() ."\" failed to complete successfully. Exit status: $exit_status\n"; - } - } - return wantarray ? @output : join "\n", @output; -} -