mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-26 16:30:07 +08:00
[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
This commit is contained in:
Gautier Hattenberger
committed by
GitHub
GitHub
parent
dfb08fa733
commit
41451d5422
@@ -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
|
||||
|
||||
+1
-158
@@ -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
|
||||
|
||||
@@ -10,7 +10,7 @@
|
||||
<depends>actuators</depends>
|
||||
<provides>actuators</provides>
|
||||
</dep>
|
||||
<makefile target="nps">
|
||||
<makefile target="sim|nps">
|
||||
<file_arch name="actuators_pwm_arch.c"/>
|
||||
</makefile>
|
||||
</module>
|
||||
|
||||
@@ -0,0 +1,19 @@
|
||||
<!DOCTYPE module SYSTEM "module.dtd">
|
||||
|
||||
<module name="fdm_fixedwing_sim" dir="fdm">
|
||||
<doc>
|
||||
<description>
|
||||
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.
|
||||
</description>
|
||||
</doc>
|
||||
<header/>
|
||||
<makefile target="sim|nps|hitl">
|
||||
<define name="NPS_BYPASS_AHRS" value="TRUE"/>
|
||||
<define name="NPS_BYPASS_INS" value="TRUE"/>
|
||||
<file name="nps_fdm_fixedwing_sim.c" dir="nps"/>
|
||||
</makefile>
|
||||
</module>
|
||||
|
||||
@@ -28,7 +28,7 @@
|
||||
<makefile target="!fbw">
|
||||
<file name="main_ap.c" dir="."/>
|
||||
</makefile>
|
||||
<makefile target="nps">
|
||||
<makefile target="sim|nps">
|
||||
<define name="AP"/>
|
||||
<file name="nps_autopilot_fixedwing.c" dir="nps"/>
|
||||
</makefile>
|
||||
|
||||
@@ -17,7 +17,7 @@
|
||||
</header>
|
||||
<init fun="gps_nps_init()"/>
|
||||
<periodic fun="gps_nps_periodic_check()" freq="1." autorun="TRUE"/>
|
||||
<makefile target="nps">
|
||||
<makefile target="sim|nps">
|
||||
<file name="gps_sim_nps.c"/>
|
||||
<define name="GPS_TYPE_H" value="modules/gps/gps_sim_nps.h" type="string"/>
|
||||
<test>
|
||||
|
||||
@@ -40,7 +40,7 @@
|
||||
</header>
|
||||
<init fun="imu_init()"/>
|
||||
|
||||
<makefile target="!sim|fbw">
|
||||
<makefile target="!fbw">
|
||||
<define name="USE_IMU"/>
|
||||
<file name="imu.c"/>
|
||||
<test/>
|
||||
|
||||
@@ -16,7 +16,7 @@
|
||||
<init fun="imu_nps_init()"/>
|
||||
<event fun="imu_nps_event()"/>
|
||||
|
||||
<makefile target="nps">
|
||||
<makefile target="sim|nps">
|
||||
<file name="imu_nps.c"/>
|
||||
</makefile>
|
||||
</module>
|
||||
|
||||
@@ -17,8 +17,5 @@
|
||||
<makefile target="ap">
|
||||
<file name="ins_arduimu.c"/>
|
||||
</makefile>
|
||||
<makefile target="sim">
|
||||
<file_arch name="ins_arduimu.c"/>
|
||||
</makefile>
|
||||
</module>
|
||||
|
||||
|
||||
@@ -29,8 +29,5 @@
|
||||
<makefile target="ap">
|
||||
<file name="ins_arduimu_basic.c"/>
|
||||
</makefile>
|
||||
<makefile target="sim">
|
||||
<file_arch name="ins_arduimu_basic.c"/>
|
||||
</makefile>
|
||||
</module>
|
||||
|
||||
|
||||
@@ -14,7 +14,7 @@
|
||||
<file name="ins_gps_passthrough.h"/>
|
||||
</header>
|
||||
<init fun="ins_gps_passthrough_init()"/>
|
||||
<makefile target="nps">
|
||||
<makefile target="sim|nps">
|
||||
<file name="imu.c" dir="modules/imu"/>
|
||||
<file name="imu_nps.c" dir="modules/imu"/>
|
||||
<define name="USE_IMU"/>
|
||||
@@ -34,7 +34,7 @@
|
||||
<file name="ins.c"/>
|
||||
<file name="ins_gps_passthrough.c"/>
|
||||
</makefile>
|
||||
<makefile target="nps" firmware="fixedwing">
|
||||
<makefile target="sim|nps" firmware="fixedwing">
|
||||
<define name="INS_TYPE_H" value="modules/ins/ins_gps_passthrough.h" type="string"/>
|
||||
<file name="ins.c"/>
|
||||
<file name="ins_gps_passthrough_utm.c"/>
|
||||
|
||||
@@ -31,7 +31,6 @@
|
||||
<define name="USE_COMMANDS"/>
|
||||
<file_arch name="sys_time_arch.c" dir="mcu_periph"/>
|
||||
<file_arch name="led_hw.c" dir="." cond="ifeq ($(ARCH), stm32)"/>
|
||||
<file_arch name="led_hw.c" dir="." cond="ifeq ($(TARGET), sim)"/>
|
||||
<flag name="LDFLAGS" value="lrt" cond="ifeq ($(ARCH), linux)"/>
|
||||
<raw>
|
||||
VPATH += $(PAPARAZZI_HOME)/var/share
|
||||
|
||||
@@ -15,7 +15,7 @@
|
||||
<provides>baro,airspeed,sonar,incidence,temperature</provides>
|
||||
<suggests>gps_nps,imu_nps,ins_nps,actuators_nps,telemetry_nps</suggests>
|
||||
</dep>
|
||||
<makefile target="nps">
|
||||
<makefile target="sim|nps">
|
||||
<define name="AP"/>
|
||||
<define name="SITL"/>
|
||||
<file name="nps_electrical.c" dir="$(NPS_DIR)"/>
|
||||
|
||||
@@ -12,7 +12,7 @@
|
||||
<dep>
|
||||
<depends>math</depends>
|
||||
</dep>
|
||||
<makefile target="nps|hitl">
|
||||
<makefile target="sim|nps|hitl">
|
||||
<configure name="BARO_PERIODIC_FREQUENCY" default="50"/>
|
||||
<configure name="NPS_DIR" value="nps"/>
|
||||
<raw>
|
||||
|
||||
@@ -11,26 +11,9 @@
|
||||
</description>
|
||||
</doc>
|
||||
<dep>
|
||||
<depends>system_core,electrical,settings,actuators_dummy,@gps,@ins,@ahrs</depends>
|
||||
<suggests>gps_sim,imu_sim,ins_sim,ahrs_sim,baro_sim,telemetry_sim</suggests>
|
||||
<depends>targets/nps,fdm_fixedwing_sim</depends>
|
||||
</dep>
|
||||
<makefile target="sim">
|
||||
<define name="SITL"/>
|
||||
<define name="AP"/>
|
||||
<file name="main_bare.c" dir="."/>
|
||||
<file_arch name="sim_ap.c" dir="."/>
|
||||
<file_arch name="sim_gps.c" dir="."/>
|
||||
<file_arch name="sim_adc_generic.c" dir="."/>
|
||||
<file_arch name="sim_ahrs.c" dir="."/>
|
||||
<file_arch name="sim_airspeed.c" dir="."/>
|
||||
<raw>
|
||||
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)
|
||||
</raw>
|
||||
</makefile>
|
||||
</module>
|
||||
|
||||
|
||||
@@ -22,7 +22,7 @@
|
||||
</header>
|
||||
<init fun="pprz_dl_init()"/>
|
||||
<event fun="pprz_dl_event()"/>
|
||||
<makefile target="nps">
|
||||
<makefile target="sim|nps">
|
||||
<configure name="MODEM_DEV" default="UDP0" case="upper|lower"/>
|
||||
<configure name="MODEM_PORT_OUT" default="4242"/>
|
||||
<configure name="MODEM_PORT_IN" default="4243"/>
|
||||
|
||||
@@ -19,7 +19,7 @@
|
||||
<file_arch name="udp_arch.c" dir="mcu_periph"/>
|
||||
<file name="udp_socket.c" dir="arch/linux" cond="ifeq ($(ARCH), linux)"/>
|
||||
</makefile>
|
||||
<makefile target="nps">
|
||||
<makefile target="sim|nps">
|
||||
<include name="arch/linux"/>
|
||||
<file name="udp_socket.c" dir="arch/linux"/>
|
||||
</makefile>
|
||||
|
||||
@@ -1,20 +0,0 @@
|
||||
#include "led_hw.h"
|
||||
|
||||
#include <stdio.h>
|
||||
#include <caml/mlvalues.h>
|
||||
#include <caml/memory.h>
|
||||
#include <caml/callback.h>
|
||||
|
||||
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)); }
|
||||
|
||||
@@ -1,19 +0,0 @@
|
||||
#ifndef LED_HW_H
|
||||
#define LED_HW_H
|
||||
|
||||
#include <stdbool.h>
|
||||
|
||||
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 */
|
||||
@@ -27,21 +27,14 @@
|
||||
|
||||
#include "modules/radio_control/radio_control.h"
|
||||
#include "modules/radio_control/ppm.h"
|
||||
|
||||
#include <inttypes.h>
|
||||
|
||||
#if USE_NPS
|
||||
#include "nps_radio_control.h"
|
||||
#else
|
||||
#include <caml/mlvalues.h>
|
||||
#endif
|
||||
#include <inttypes.h>
|
||||
|
||||
|
||||
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
|
||||
|
||||
@@ -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 <inttypes.h>
|
||||
#include <caml/mlvalues.h>
|
||||
|
||||
value update_rc_channel(value c __attribute__((unused)), value v __attribute__((unused)))
|
||||
{
|
||||
return Val_unit;
|
||||
}
|
||||
|
||||
value send_ppm(value unit)
|
||||
{
|
||||
return unit;
|
||||
}
|
||||
@@ -32,11 +32,7 @@
|
||||
#include "std.h"
|
||||
#include <inttypes.h>
|
||||
|
||||
#if USE_NPS
|
||||
#include "nps_radio_control.h"
|
||||
#else
|
||||
#include <caml/mlvalues.h>
|
||||
#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
|
||||
|
||||
@@ -1,19 +0,0 @@
|
||||
#include <caml/mlvalues.h>
|
||||
#include <inttypes.h>
|
||||
|
||||
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;
|
||||
}
|
||||
@@ -1,38 +0,0 @@
|
||||
/** \file sim_ahrs.c
|
||||
* \brief Regroup functions to simulate an ahrs
|
||||
*
|
||||
* Ahrs soft simulation. OCaml binding.
|
||||
*/
|
||||
|
||||
|
||||
#include <inttypes.h>
|
||||
#include <caml/mlvalues.h>
|
||||
#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;
|
||||
}
|
||||
|
||||
@@ -1,17 +0,0 @@
|
||||
/** \file sim_airspeed.c
|
||||
*/
|
||||
|
||||
|
||||
#include <inttypes.h>
|
||||
#include "generated/airframe.h"
|
||||
|
||||
#include <caml/mlvalues.h>
|
||||
|
||||
float sim_air_speed;
|
||||
|
||||
value set_airspeed(value air_speed)
|
||||
{
|
||||
sim_air_speed = Double_val(air_speed);
|
||||
return Val_unit;
|
||||
}
|
||||
|
||||
@@ -1,123 +0,0 @@
|
||||
/* Definitions and declarations required to compile autopilot code on a
|
||||
i386 architecture. Bindings for OCaml. */
|
||||
|
||||
#include <stdio.h>
|
||||
#include <assert.h>
|
||||
#include <sys/time.h>
|
||||
#include <sys/stat.h>
|
||||
#include <time.h>
|
||||
#include <string.h>
|
||||
#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 <caml/mlvalues.h>
|
||||
#include <caml/memory.h>
|
||||
|
||||
|
||||
/* 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;
|
||||
}
|
||||
|
||||
@@ -1,82 +0,0 @@
|
||||
/* OCaml binding to link the simulator to autopilot functions. */
|
||||
|
||||
#include <assert.h>
|
||||
#include <math.h>
|
||||
#include <inttypes.h>
|
||||
|
||||
/** 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 <caml/mlvalues.h>
|
||||
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
@@ -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;
|
||||
+2
-22
@@ -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
|
||||
|
||||
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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 ()
|
||||
@@ -1,57 +0,0 @@
|
||||
/** Values boxing for Flight Gear */
|
||||
|
||||
#include <string.h>
|
||||
#include <caml/alloc.h>
|
||||
#include <caml/mlvalues.h>
|
||||
#include <caml/memory.h>
|
||||
|
||||
#include <math.h>
|
||||
#include <time.h>
|
||||
|
||||
#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;
|
||||
}
|
||||
@@ -1,53 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
|
||||
<PropertyList>
|
||||
<generic>
|
||||
<output>
|
||||
<binary_mode>true</binary_mode>
|
||||
<byte_order>host</byte_order>
|
||||
|
||||
<chunk>
|
||||
<name>time (s)</name>
|
||||
<type>double</type>
|
||||
<node>/sim/time/elapsed-sec</node>
|
||||
</chunk>
|
||||
|
||||
<chunk>
|
||||
<name>wind-from-north (m/s)</name>
|
||||
<type>float</type>
|
||||
<node>/environment/wind-from-north-fps</node>
|
||||
<factor>0.3048</factor>
|
||||
</chunk>
|
||||
|
||||
<chunk>
|
||||
<name>wind-from-east (m/s)</name>
|
||||
<type>float</type>
|
||||
<node>/environment/wind-from-east-fps</node>
|
||||
<factor>0.3048</factor>
|
||||
</chunk>
|
||||
|
||||
<chunk>
|
||||
<name>wind-from-down (m/s)</name>
|
||||
<type>float</type>
|
||||
<node>/environment/wind-from-down-fps</node>
|
||||
<factor>0.3048</factor>
|
||||
</chunk>
|
||||
|
||||
<chunk>
|
||||
<name>wind-from-heading (deg)</name>
|
||||
<type>float</type>
|
||||
<node>/environment/wind-from-heading-deg</node>
|
||||
</chunk>
|
||||
|
||||
<chunk>
|
||||
<name>wind-speed (m/s)</name>
|
||||
<type>float</type>
|
||||
<factor>0.514444444</factor>
|
||||
<node>/environment/wind-speed-kt</node>
|
||||
</chunk>
|
||||
|
||||
<binary_footer>magic,0x12345678</binary_footer>
|
||||
|
||||
</output>
|
||||
</generic>
|
||||
</PropertyList>
|
||||
@@ -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 *)
|
||||
@@ -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
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
@@ -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] *)
|
||||
@@ -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"
|
||||
|
||||
|
||||
@@ -0,0 +1,309 @@
|
||||
/*
|
||||
* Copyright (C) 2023 Gautier Hattenberger <gautier.hattenberger@enac.fr>
|
||||
*
|
||||
* This file is part of paparazzi.
|
||||
*
|
||||
* paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with paparazzi; see the file COPYING. If not, see
|
||||
* <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include "nps_fdm.h"
|
||||
|
||||
#include <stdlib.h>
|
||||
#include <stdio.h>
|
||||
|
||||
#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)))
|
||||
{
|
||||
}
|
||||
|
||||
@@ -13,7 +13,7 @@
|
||||
#include <pthread.h>
|
||||
|
||||
#include "std.h"
|
||||
#include "../flight_gear.h"
|
||||
#include "flight_gear.h"
|
||||
#include "nps_main.h"
|
||||
#include "nps_fdm.h"
|
||||
#include "nps_atmosphere.h"
|
||||
|
||||
+23
-56
@@ -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)
|
||||
|
||||
@@ -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 "<ivy bus> 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
|
||||
@@ -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
|
||||
@@ -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()))
|
||||
@@ -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 *)
|
||||
@@ -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 ()
|
||||
@@ -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"]
|
||||
@@ -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 *)
|
||||
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user