mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-26 16:30:07 +08:00
simulator starts !
This commit is contained in:
Pascal Brisset
@@ -49,9 +49,9 @@ test: static ac1 ac2
|
||||
PAPARAZZI_HOME=$(PAPARAZZI_SRC) PAPARAZZI_SRC=$(PAPARAZZI_SRC) $(SUPERVISION)
|
||||
|
||||
ac1 : conf sim_static
|
||||
make AIRCRAFT=Twin1 PAPARAZZI_HOME=$(PAPARAZZI_SRC) sim_ac
|
||||
make AIRCRAFT=Twin1 PAPARAZZI_HOME=$(PAPARAZZI_SRC) sim
|
||||
ac2 : conf sim_static
|
||||
make AIRCRAFT=Twin2 PAPARAZZI_HOME=$(PAPARAZZI_SRC) sim_ac
|
||||
make AIRCRAFT=Twin2 PAPARAZZI_HOME=$(PAPARAZZI_SRC) sim
|
||||
|
||||
lib:
|
||||
cd $(LIB)/ocaml; $(MAKE)
|
||||
@@ -69,9 +69,6 @@ configurator: lib
|
||||
sim_static :
|
||||
cd $(SIMULATOR); $(MAKE) PAPARAZZI_SRC=$(PAPARAZZI_SRC)
|
||||
|
||||
sim_sitl :
|
||||
cd $(SIMULATOR); $(MAKE) sim_sitl PAPARAZZI_HOME=$(PAPARAZZI_SRC) PAPARAZZI_SRC=$(PAPARAZZI_SRC) AIRCRAFT=$(AIRCRAFT)
|
||||
|
||||
fbw fly_by_wire: ac_h
|
||||
cd $(AIRBORNE); $(MAKE) TARGET=fbw all
|
||||
|
||||
@@ -136,7 +133,6 @@ static_h :
|
||||
ac_h : tools static_h
|
||||
PAPARAZZI_HOME=`pwd` PAPARAZZI_SRC=`pwd` $(TOOLS)/gen_aircraft.out $(AIRCRAFT)
|
||||
|
||||
sim_ac: ac_h sim_sitl
|
||||
hard_ac: ac_h fbw ap
|
||||
ac: hard_ac
|
||||
|
||||
|
||||
+12
-5
@@ -31,8 +31,10 @@ SRC_ARCH = $(PAPARAZZI_SRC)/sw/airborne/sim
|
||||
|
||||
CC = gcc
|
||||
OCAMLC = ocamlc
|
||||
CAMLINCLUDES = -I +lablgtk2 -I $(PAPARAZZI_SRC)/sw/lib/ocaml -I $(PAPARAZZI_SRC)/sw/simulator
|
||||
SITLCMA = $(PAPARAZZI_SRC)/sw/simulator/sitl.cma
|
||||
SIMDIR = $(PAPARAZZI_SRC)/sw/simulator
|
||||
CAMLINCLUDES = -I +lablgtk2 -I $(PAPARAZZI_SRC)/sw/lib/ocaml -I $(SIMDIR)
|
||||
SIMSITLML = $(OBJDIR)/simsitl.ml
|
||||
SITLCMA = $(SIMDIR)/sitl.cma
|
||||
|
||||
|
||||
#
|
||||
@@ -41,7 +43,7 @@ SITLCMA = $(PAPARAZZI_SRC)/sw/simulator/sitl.cma
|
||||
|
||||
|
||||
CFLAGS = \
|
||||
-W -Wall \
|
||||
-W -Wall -Werror \
|
||||
$(INCLUDES) \
|
||||
-Wstrict-prototypes \
|
||||
$($(TARGET).CFLAGS) \
|
||||
@@ -60,10 +62,15 @@ $(TARGET).objs = $($(TARGET).objso:%.S=$(OBJDIR)/%.o)
|
||||
|
||||
all compile: $(OBJDIR)/simsitl
|
||||
|
||||
$(OBJDIR)/simsitl : $($(TARGET).objs) $(SITLCMA)
|
||||
$(OCAMLC) -custom $(CAMLINCLUDES) -o $@ glibivy-ocaml.cma lib-pprz.cma lablgtk.cma gtkInit.cmo $^
|
||||
$(OBJDIR)/simsitl : $($(TARGET).objs) $(SITLCMA) $(SIMSITLML)
|
||||
$(OCAMLC) -custom $(CAMLINCLUDES) -o $@ glibivy-ocaml.cma lib-pprz.cma lablgtk.cma $($(TARGET).objs) gtkInit.cmo $(SITLCMA) $(SIMSITLML)
|
||||
|
||||
|
||||
# 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 $@ $<
|
||||
|
||||
|
||||
@@ -106,11 +106,6 @@
|
||||
<makefile>
|
||||
include $(PAPARAZZI_SRC)/conf/autopilot/v1_2_1.makefile
|
||||
include $(PAPARAZZI_SRC)/conf/autopilot/twin_mcu_avr.makefile
|
||||
|
||||
include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
|
||||
sim.CFLAGS += -Werror
|
||||
|
||||
# ap.CFLAGS += -DDATALINK
|
||||
# ap.EXTRA_SRCS += traffic_info.c datalink.c
|
||||
</makefile>
|
||||
</airframe>
|
||||
|
||||
@@ -107,40 +107,17 @@
|
||||
<define name="PHI0" value="-30" unit="deg"/>
|
||||
</section>
|
||||
|
||||
<section name="DOWNLINK" prefix="DOWNLINK_">
|
||||
<define name="FBW_DEVICE" value="uart0"/>
|
||||
<define name="AP_DEVICE" value="uart0"/>
|
||||
</section>
|
||||
|
||||
<makefile>
|
||||
|
||||
include $(PAPARAZZI_SRC)/conf/autopilot/v1_2_1.makefile
|
||||
include $(PAPARAZZI_SRC)/conf/autopilot/twin_mcu_avr.makefile
|
||||
include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
|
||||
|
||||
fbw.srcs += commands.c
|
||||
|
||||
fbw.CFLAGS += -DACTUATORS=\"servos_4017.h\" -DSERVOS_4017
|
||||
fbw.srcs += $(SRC_ARCH)/servos_4017.c
|
||||
|
||||
fbw.CFLAGS += -DRADIO_CONTROL
|
||||
fbw.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
|
||||
|
||||
fbw.CFLAGS += -DDOWNLINK -DUART0 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=uart0
|
||||
fbw.srcs += fbw_downlink.c $(SRC_ARCH)/uart_hw.c
|
||||
|
||||
fbw.CFLAGS += -DINTER_MCU -DMCU_SPI_LINK
|
||||
fbw.srcs += inter_mcu.c $(SRC_ARCH)/spi_hw.c
|
||||
|
||||
fbw.CFLAGS += -Werror
|
||||
|
||||
ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_AP_DEVICE=uart0
|
||||
ap.srcs += downlink.c $(SRC_ARCH)/uart_hw.c
|
||||
|
||||
ap.CFLAGS += -DGPS -DUBX -DINFRARED -DRADIO_CONTROL
|
||||
ap.CFLAGS += -DDATALINK
|
||||
ap.EXTRA_SRCS += traffic_info.c datalink.c
|
||||
ap.CFLAGS += -DMOBILE_CAM
|
||||
# ap.CFLAGS += -DWAVECARD
|
||||
# ap.EXTRA_SRCS += wavecard.c
|
||||
ap.CFLAGS += -DMAXSTREAM
|
||||
ap.EXTRA_SRCS += maxstream.c
|
||||
sim.CFLAGS += -DDATALINK -DMOBILE_CAM
|
||||
sim.EXTRA_SRCS += traffic_info.c datalink.c
|
||||
#ap.CFLAGS += -DWAVECARD
|
||||
#ap.EXTRA_SRCS += wavecard.c
|
||||
#ap.CFLAGS += -DMAXSTREAM
|
||||
#ap.EXTRA_SRCS += maxstream.c
|
||||
</makefile>
|
||||
</airframe>
|
||||
|
||||
@@ -2,8 +2,8 @@ sim.ARCHDIR = $(ARCHI)
|
||||
sim.ARCH = sitl
|
||||
sim.TARGET = autopilot
|
||||
sim.TARGETDIR = autopilot
|
||||
sim.CFLAGS += -DSITL -DAP -DFBW -DRADIO_CONTROL -DINTER_MCU
|
||||
sim.CFLAGS += -DSITL -DAP -DFBW -DRADIO_CONTROL -DINTER_MCU -DDOWNLINK -DDOWNLINK_TRANSPORT=IvyTransport
|
||||
# ap.CFLAGS += -DGPS -DUBX -DINFRARED -DDOWNLINK
|
||||
#ap.CFLAGS += -DACTUATORS=\"servos_4017.h\" -DSERVOS_4017
|
||||
#ap.srcs += $(SRC_ARCH)/servos_4017.c
|
||||
sim.srcs = radio_control.c downlink.c commands.c gps.c inter_mcu.c link_mcu.c infrared.c pid.c nav.c estimator.c cam.c sys_time.c main_fbw.c main_ap.c $(SRC_ARCH)/ppm_hw.c $(SRC_ARCH)/sim_gps.c $(SRC_ARCH)/sim_ir.c $(SRC_ARCH)/sim_ap.c
|
||||
sim.srcs = radio_control.c downlink.c commands.c gps.c inter_mcu.c link_mcu.c infrared.c pid.c nav.c estimator.c cam.c sys_time.c main_fbw.c main_ap.c $(SRC_ARCH)/ppm_hw.c $(SRC_ARCH)/sim_gps.c $(SRC_ARCH)/sim_ir.c $(SRC_ARCH)/sim_ap.c $(SRC_ARCH)/ivy_transport.c
|
||||
|
||||
@@ -26,7 +26,12 @@
|
||||
#define DOWNLINK_H
|
||||
|
||||
#include <inttypes.h>
|
||||
|
||||
#ifdef SITL
|
||||
#include "ivy_transport.h"
|
||||
#else
|
||||
#include "pprz_transport.h"
|
||||
#endif
|
||||
|
||||
extern uint8_t downlink_nb_ovrn;
|
||||
|
||||
@@ -50,10 +55,10 @@ extern uint8_t downlink_nb_ovrn;
|
||||
|
||||
#define DonwlinkOverrun() downlink_nb_ovrn++;
|
||||
|
||||
#define DownlinkStartMessage(msg_id, payload_len) { \
|
||||
#define DownlinkStartMessage(_name, msg_id, payload_len) { \
|
||||
Transport(Header(payload_len)); \
|
||||
Transport(PutUint8(AC_ID)); \
|
||||
Transport(PutUint8(msg_id)); \
|
||||
Transport(PutNamedUint8(_name, msg_id)); \
|
||||
}
|
||||
|
||||
#define DownlinkEndMessage() Transport(Trailer())
|
||||
|
||||
@@ -40,6 +40,7 @@
|
||||
#include "link_mcu.h"
|
||||
#include "sys_time.h"
|
||||
#include "flight_plan.h"
|
||||
#include "datalink.h"
|
||||
|
||||
#ifdef LED
|
||||
#include "led.h"
|
||||
|
||||
@@ -60,6 +60,8 @@ extern uint8_t ck_a, ck_b;
|
||||
PprzTransportPut1Byte(_byte); \
|
||||
}
|
||||
|
||||
#define PprzTransportPutNamedUint8(_name, _byte) PprzTransportPutUint8(_byte)
|
||||
|
||||
#define PprzTransportPut1ByteByAddr(_byte) { \
|
||||
uint8_t _x = *(_byte); \
|
||||
PprzTransportPutUint8(_x); \
|
||||
|
||||
@@ -28,11 +28,11 @@ exception Error of string
|
||||
val child : Xml.xml -> ?select:(Xml.xml -> bool) -> string -> Xml.xml
|
||||
(** [child xml ?p i] If [i] is an integer, returns the [i]'th child of [xml].
|
||||
Else returns the child of [xml] with tag [i] (the first one satisfying [p]
|
||||
if specified *)
|
||||
if specified). Else raises [Not_found]. *)
|
||||
|
||||
val get : Xml.xml -> string -> Xml.xml
|
||||
(** [get xml path] Returns the son of [xml] specified by [path] (where
|
||||
separator is [.] *)
|
||||
separator is [.]). May raise [Not_found]. *)
|
||||
|
||||
val get_attrib : Xml.xml -> string -> string -> string
|
||||
(** [get_attrib xml path attrib_name] *)
|
||||
|
||||
+12
-16
@@ -57,8 +57,8 @@ sim_sitl : $(OBJDIR)/simsitl
|
||||
simhitl.out : $(SIMHCMO) simhitl.cmo
|
||||
$(OCAMLC) $(INCLUDES) -o $@ str.cma xml-light.cma unix.cma lib.cma lablgtk.cma gtkInit.cmo $^
|
||||
|
||||
sitl.cma : $(SIMSCMO)
|
||||
ocamlc -o $@ -a $^
|
||||
sitl.cma : fg.o $(SIMSCMO)
|
||||
ocamlmklib -o sitl $^
|
||||
|
||||
fg.o : fg.c
|
||||
$(OCAMLC) -c fg.c
|
||||
@@ -97,10 +97,6 @@ $(OBJDIR)/simsitl.cmo : $(OBJDIR)/simsitl.ml
|
||||
$(OBJDIR)/simsitl.cmx : $(OBJDIR)/simsitl.ml
|
||||
$(OCAMLOPT) $(INCLUDES) -c -o $@ $<
|
||||
|
||||
$(OBJDIR)/simsitl.ml : simsitl.ml
|
||||
@echo "Sim.ac_name := \"$(AIRCRAFT)\"" > $@
|
||||
@cat $< >> $@
|
||||
|
||||
%.cmo : %.ml
|
||||
$(OCAMLC) $(INCLUDES) -c $<
|
||||
|
||||
@@ -116,13 +112,13 @@ $(OBJDIR)/simsitl.ml : simsitl.ml
|
||||
clean :
|
||||
\rm -f *.cm* *~ *.out .depend *.o *.a *.so gaia
|
||||
|
||||
flightModel.cmo: flightModel.cmi
|
||||
hitl.cmo: hitl.cmi
|
||||
sim.cmo: flightModel.cmi sim.cmi
|
||||
simhitl.cmo: hitl.cmi sim.cmi
|
||||
simsitl.cmo: sim.cmi sitl.cmi
|
||||
sitl.cmo: sitl.cmi
|
||||
sitl.cmi: sim.cmi
|
||||
data.cmo : data.cmi
|
||||
stdlib.cmo : stdlib.cmi
|
||||
gps.cmo : gps.cmi
|
||||
#
|
||||
# Dependencies
|
||||
#
|
||||
|
||||
.depend:
|
||||
ocamldep *.ml* > .depend
|
||||
|
||||
ifneq ($(MAKECMDGOALS),clean)
|
||||
-include .depend
|
||||
endif
|
||||
|
||||
@@ -38,9 +38,9 @@ let messages_ap =
|
||||
(* let xml = Xml.parse_file (pprz_conf_path // "messages.xml") in *)
|
||||
let xml = Xml.parse_file (user_conf_path // "messages.xml") in
|
||||
try
|
||||
ExtXml.child xml ~select:(fun x -> Xml.attrib x "name" = "telemetry_ap") "class"
|
||||
ExtXml.child xml ~select:(fun x -> Xml.attrib x "name" = "telemetry") "class"
|
||||
with
|
||||
Not_found -> failwith "'telemetry_ap' class missing in messages.xml"
|
||||
Not_found -> failwith "'telemetry' class missing in messages.xml"
|
||||
|
||||
type aircraft = {
|
||||
name : string;
|
||||
|
||||
+23
-64
@@ -3,7 +3,7 @@
|
||||
*
|
||||
* Basic flight model for simulation
|
||||
*
|
||||
* Copyright (C) 2004-2005 Pascal Brisset, Antoine Drouin
|
||||
* Copyright (C) 2004-2006 Pascal Brisset, Antoine Drouin
|
||||
*
|
||||
* This file is part of paparazzi.
|
||||
*
|
||||
@@ -25,6 +25,7 @@
|
||||
*)
|
||||
|
||||
open Stdlib
|
||||
open Printf
|
||||
|
||||
let ios = fun x ->
|
||||
try int_of_string x with _ -> failwith (Printf.sprintf "int_of_string: '%s'" x)
|
||||
@@ -123,12 +124,20 @@ module Make(A:Data.MISSION) = struct
|
||||
|
||||
|
||||
|
||||
let servos =
|
||||
let commands =
|
||||
try
|
||||
ExtXml.child A.ac.airframe "servos"
|
||||
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 'servos' expected in '%s'\n" (Xml.to_string A.ac.airframe))
|
||||
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"
|
||||
|
||||
@@ -139,70 +148,20 @@ module Make(A:Data.MISSION) = struct
|
||||
let adc_roll_neutral = ios (defined_value infrared_section "ADC_ROLL_NEUTRAL")
|
||||
let roll_neutral_default = rad_of_deg (float_value infrared_section "ROLL_NEUTRAL_DEFAULT")
|
||||
|
||||
let get_servo name =
|
||||
try
|
||||
ExtXml.child servos ~select:(fun x -> ExtXml.attrib x "name" = name) "servo"
|
||||
with
|
||||
Not_found ->
|
||||
failwith (Printf.sprintf "Child 'servo' with name='%s' expected in '%s'\n" name (Xml.to_string servos))
|
||||
|
||||
let us_attrib = fun x a -> int_of_string (ExtXml.attrib x a)
|
||||
|
||||
let gaz = try get_servo "GAZ" with _ -> get_servo "MOTOR_RIGHT"
|
||||
let min_thrust = us_attrib gaz "min"
|
||||
let max_thrust = us_attrib gaz "max"
|
||||
let min_thrust = 0
|
||||
let max_thrust = max_pprz
|
||||
|
||||
type servo_id = int
|
||||
type ppm = int
|
||||
|
||||
let no_thrust = int_of_string (ExtXml.attrib gaz "no")
|
||||
|
||||
|
||||
|
||||
let some_aileron_left = try Some (get_servo "AILERON_LEFT") with _ -> None
|
||||
let some_ailevon_left = try Some (get_servo "AILEVON_LEFT") with _ -> None
|
||||
let some_ailevon_right = try Some (get_servo "AILEVON_RIGHT") with _ -> None
|
||||
|
||||
let command_throttle = command "THROTTLE"
|
||||
let command_roll = command "ROLL"
|
||||
|
||||
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 sign = fun x ->
|
||||
if float_attrib x "min" < float_attrib x "max" then 1 else -1
|
||||
|
||||
let do_thrust = fun state servo ->
|
||||
state.thrust <- (float (servo.(no_thrust) - min_thrust) /. float (max_thrust - min_thrust))
|
||||
let do_commands = fun state commands ->
|
||||
let c_lda = 1e-4 in (* FIXME *)
|
||||
state.delta_a <- c_lda *. float commands.(command_roll);
|
||||
state.thrust <- (float (commands.(command_throttle) - min_thrust) /. float (max_thrust - min_thrust))
|
||||
|
||||
let do_servos =
|
||||
match some_aileron_left, some_ailevon_left, some_ailevon_right with
|
||||
Some aileron_left, None, None ->
|
||||
let c_lda = 16. *. 9e-5 in (* phi_dot_dot from aileron *)
|
||||
|
||||
let sign_aileron_left = sign aileron_left
|
||||
and n_delta_a = us_attrib aileron_left "neutral"
|
||||
and no_aileron_left = int_attrib aileron_left "no" in
|
||||
fun state servo ->
|
||||
let left = - sign_aileron_left * (servo.(no_aileron_left) - n_delta_a) in
|
||||
(** if left <> 0 then Printf.printf "left=%d\n" (servo.(no_aileron_left) - n_delta_a); flush stdout; **)
|
||||
state.delta_a <- c_lda *. float left;
|
||||
do_thrust state servo
|
||||
| None, Some ailevon_left, Some ailevon_right ->
|
||||
let c_lda = 2.5e-4 in (* phi_dot_dot from aileron *)
|
||||
|
||||
let sign_ailevon_left = sign ailevon_left
|
||||
and sign_ailevon_right = sign ailevon_right
|
||||
and left_neutral = us_attrib ailevon_left "neutral"
|
||||
and right_neutral = us_attrib ailevon_right "neutral"
|
||||
and left_travel = float (us_attrib ailevon_left "max" - us_attrib ailevon_left "min") /. 1200.
|
||||
and right_travel = float (us_attrib ailevon_right "max" - us_attrib ailevon_right "min") /. 1200.
|
||||
and no_ailevon_left = int_attrib ailevon_left "no"
|
||||
and no_ailevon_right = int_attrib ailevon_right "no" in
|
||||
fun state servo ->
|
||||
do_thrust state servo;
|
||||
let sum = (float (servo.(no_ailevon_left) - left_neutral) /. left_travel +.
|
||||
float (servo.(no_ailevon_right) - right_neutral) /. right_travel) /. 2. in
|
||||
(* Printf.printf "%d %f\n" (servo no_ailevon_left - left_neutral) sum; flush stdout; *)
|
||||
state.delta_a <- c_lda *. (-. sum)
|
||||
| _ -> failwith "Aileron or Ailevon left and right PLEASE"
|
||||
|
||||
let nb_servos = 10 (* 4017 *)
|
||||
let nb_commands = 10 (* FIXME *)
|
||||
end
|
||||
|
||||
@@ -3,7 +3,7 @@
|
||||
*
|
||||
* Basic flight model for simulation
|
||||
*
|
||||
* Copyright (C) 2004-2005 Pascal Brisset, Antoine Drouin
|
||||
* Copyright (C) 2004-2006 Pascal Brisset, Antoine Drouin
|
||||
*
|
||||
* This file is part of paparazzi.
|
||||
*
|
||||
@@ -42,8 +42,8 @@ val set_air_speed : state -> meter_s -> unit
|
||||
module Make :
|
||||
functor (A : Data.MISSION) ->
|
||||
sig
|
||||
val do_servos : state -> Stdlib.us array -> unit
|
||||
val nb_servos : int
|
||||
val do_commands : state -> Stdlib.pprz_t array -> unit
|
||||
val nb_commands : int
|
||||
val nominal_airspeed : float (* m/s *)
|
||||
val roll_neutral_default : float (* rad *)
|
||||
val state_update : state -> float * float -> float -> unit
|
||||
|
||||
+6
-6
@@ -1,9 +1,9 @@
|
||||
(*
|
||||
* $Id$
|
||||
*
|
||||
* Hardware in the loop basic simulator (handling GPS, infrared and servos)
|
||||
* Hardware in the loop basic simulator (handling GPS, infrared and commands)
|
||||
*
|
||||
* Copyright (C) 2004 Pascal Brisset, Antoine Drouin
|
||||
* Copyright (C) 2004-2006 Pascal Brisset, Antoine Drouin
|
||||
*
|
||||
* This file is part of paparazzi.
|
||||
*
|
||||
@@ -43,7 +43,7 @@ module type AIRCRAFT =
|
||||
val boot : Stdlib.value -> unit
|
||||
(** [boot time_acceleration] *)
|
||||
|
||||
val servos : us array -> unit
|
||||
val commands : pprz_t array -> unit
|
||||
(** Called once at init *)
|
||||
|
||||
val infrared : float -> float -> float -> unit
|
||||
@@ -109,9 +109,9 @@ module Make(AircraftItl : AIRCRAFT_ITL) = struct
|
||||
|
||||
let reset = fun () -> state := initial_state in
|
||||
|
||||
let servos = Array.create FM.nb_servos 0 in
|
||||
let commands = Array.create FM.nb_commands 0 in
|
||||
|
||||
Aircraft.servos servos;
|
||||
Aircraft.commands commands;
|
||||
|
||||
let north_label = GMisc.label ~text:"000" ()
|
||||
and east_label = GMisc.label ~text:"000" ()
|
||||
@@ -140,7 +140,7 @@ module Make(AircraftItl : AIRCRAFT_ITL) = struct
|
||||
|
||||
|
||||
let fm_task = fun () ->
|
||||
FM.do_servos !state servos;
|
||||
FM.do_commands !state commands;
|
||||
FM.state_update !state (!wind_x, !wind_y) fm_period
|
||||
|
||||
and ir_task = fun () ->
|
||||
|
||||
@@ -8,7 +8,7 @@ module type AIRCRAFT =
|
||||
sig
|
||||
val init : int -> GPack.box -> unit
|
||||
val boot : Stdlib.value -> unit
|
||||
val servos : Stdlib.us array -> unit
|
||||
val commands : Stdlib.pprz_t array -> unit
|
||||
val infrared : float -> float -> float -> unit
|
||||
val gps : Gps.state -> unit
|
||||
end
|
||||
|
||||
@@ -1,7 +1,7 @@
|
||||
(*
|
||||
* $Id$
|
||||
*
|
||||
* Software in the loop basic simulator (handling GPS, infrared and servos)
|
||||
* Software in the loop basic simulator (handling GPS, infrared and commands)
|
||||
*
|
||||
* Copyright (C) 2004 Pascal Brisset, Antoine Drouin
|
||||
*
|
||||
@@ -41,24 +41,27 @@ module Make(A:Data.MISSION) = struct
|
||||
ExtXml.child Data.messages_ap ~select:(fun x -> ExtXml.attrib x "name" = name) "message"
|
||||
|
||||
|
||||
(* Servos handling (rservos is the intermediate storage) *)
|
||||
(* 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 rservos = ref [||]
|
||||
let rcommands = ref [||]
|
||||
let adj_bat = GData.adjustment ~value:12.5 ~lower:0. ~upper:23. ~step_incr:0.1 ()
|
||||
|
||||
external get_commands : Stdlib.us array -> int = "get_commands"
|
||||
external get_commands : Stdlib.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 !rservos in
|
||||
let gaz = get_commands !rcommands in
|
||||
(* 100% = 1W *)
|
||||
if bat_button#active then
|
||||
let energy = float (gaz-1000) /. 1000. *. servos_period in
|
||||
@@ -109,7 +112,6 @@ module Make(A:Data.MISSION) = struct
|
||||
window#show ()
|
||||
|
||||
external periodic_task : unit -> unit = "sim_periodic_task"
|
||||
external rc_task : unit -> unit = "sim_rc_task"
|
||||
external sim_init : int -> unit = "sim_init"
|
||||
external update_bat : int -> unit = "update_bat"
|
||||
|
||||
@@ -182,7 +184,6 @@ module Make(A:Data.MISSION) = struct
|
||||
let boot = fun time_scale ->
|
||||
Stdlib.timer ~scale:time_scale servos_period (update_servos bat_button);
|
||||
Stdlib.timer ~scale:time_scale periodic_period periodic_task;
|
||||
Stdlib.timer ~scale:time_scale rc_period rc_task;
|
||||
ignore (Ground_Pprz.message_bind "FLIGHT_PARAM" get_flight_param);
|
||||
ignore (Ground_Pprz.message_bind "MOVE_WAYPOINT" get_move_waypoint);
|
||||
ignore (Ground_Pprz.message_bind "SEND_EVENT" get_send_event);
|
||||
@@ -190,7 +191,7 @@ module Make(A:Data.MISSION) = struct
|
||||
ignore (Ground_Pprz.message_bind "DL_SETTING" get_dl_setting)
|
||||
|
||||
(* Functions called by the simulator *)
|
||||
let servos = fun s -> rservos := s
|
||||
let commands = fun s -> rcommands := s
|
||||
|
||||
external set_ir : int -> int -> unit = "set_ir"
|
||||
let infrared = fun ir_left ir_front ir_top ->
|
||||
|
||||
@@ -24,7 +24,9 @@
|
||||
*
|
||||
*)
|
||||
|
||||
type us = int
|
||||
type pprz_t = int
|
||||
|
||||
let max_pprz = 9600
|
||||
|
||||
let pi = 4. *. atan 1.
|
||||
let rec norm_angle = fun x ->
|
||||
|
||||
@@ -24,7 +24,8 @@
|
||||
*
|
||||
*)
|
||||
|
||||
type us = int
|
||||
type pprz_t = int
|
||||
val max_pprz : pprz_t
|
||||
val pi : float
|
||||
val norm_angle : float -> float
|
||||
val deg_of_rad : float -> float
|
||||
|
||||
@@ -161,7 +161,7 @@ module Gen_onboard = struct
|
||||
fprintf avr_h "){ \\\n";
|
||||
let size = (size_fields fields "0") in
|
||||
fprintf avr_h "\tif (DownlinkCheckFreeSpace(DownlinkSizeOf(%s))) {\\\n" size;
|
||||
fprintf avr_h "\t DownlinkStartMessage(DL_%s,%s) \\\n" s size;
|
||||
fprintf avr_h "\t DownlinkStartMessage(\"%s\", DL_%s, %s) \\\n" s s size;
|
||||
List.iter (print_avr_field avr_h) fields;
|
||||
fprintf avr_h "\t DownlinkEndMessage() \\\n";
|
||||
fprintf avr_h "\t} else \\\n";
|
||||
|
||||
Reference in New Issue
Block a user