Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-29 19:17:28 +08:00
Code Issues Actions 30 Packages Projects Releases Wiki Activity

sim is back !

Browse Source
This commit is contained in:
Pascal Brisset
2006-04-21 22:42:48 +00:00
parent 767fb02b61
commit c63cbbcb9b
20 changed files with 75 additions and 291 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/twinstar2.xml
+2
View File
@@ -113,6 +113,8 @@ 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
ap.CFLAGS += -DMOBILE_CAM -DDATALINK
ap.EXTRA_SRCS += traffic_info.c datalink.c
sim.CFLAGS += -DDATALINK -DMOBILE_CAM
sim.EXTRA_SRCS += traffic_info.c datalink.c
#ap.CFLAGS += -DWAVECARD
conf/autopilot/sitl.makefile
+2 -4
View File
@@ -2,8 +2,6 @@ sim.ARCHDIR = $(ARCHI)
sim.ARCH = sitl
sim.TARGET = autopilot
sim.TARGETDIR = autopilot
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.CFLAGS += -DSITL -DAP -DFBW -DRADIO_CONTROL -DINTER_MCU -DDOWNLINK -DDOWNLINK_TRANSPORT=IvyTransport -DINFRARED
# ap.CFLAGS += -DGPS -DUBX
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
conf/autopilot/twin_mcu.makefile
+1 -1
View File
@@ -1,4 +1,4 @@
ap.srcs += main_ap.c sys_time.c main.c inter_mcu.c link_mcu.c gps_ubx.c gps.c infrared.c pid.c nav.c estimator.c cam.c
ap.srcs += main_ap.c sys_time.c main.c inter_mcu.c link_mcu.c gps_ubx.c gps.c infrared.c pid.c nav.c estimator.c cam.c downlink.c
ap.CFLAGS += -DMCU_SPI_LINK -DGPS -DUBX -DINFRARED -DRADIO_CONTROL -DINTER_MCU
fbw.srcs += sys_time.c main_fbw.c main.c commands.c radio_control.c pprz_transport.c downlink.c inter_mcu.c
sw/airborne/ap_downlink.h
+1 -1
View File
@@ -74,7 +74,7 @@ extern uint8_t telemetry_mode_Ap;
#define PERIODIC_SEND_NAVIGATION_REF() DOWNLINK_SEND_NAVIGATION_REF(&nav_utm_east0, &nav_utm_north0, &nav_utm_zone0);
#ifdef DATALINK
#if defined DOWNLINK && defined DATALINK
#define PERIODIC_SEND_ACINFO() { \
struct ac_info_ *s=get_ac_info(3); \
DOWNLINK_SEND_ACINFO(&s->east, &s->north, &s->course, &s->alt, &s->gspeed); \
sw/airborne/downlink.h
+3
View File
@@ -53,6 +53,9 @@ extern uint8_t downlink_nb_ovrn;
#define DownlinkPutUint32ByAddr(_x) Transport(PutUint32ByAddr(_x))
#define DownlinkPutFloatByAddr(_x) Transport(PutFloatByAddr(_x))
#define DownlinkPutInt16Array(_n, _x) Transport(PutInt16Array(_n, _x))
#define DownlinkPutUint16Array(_n, _x) Transport(PutUint16Array(_n, _x))
#define DonwlinkOverrun() downlink_nb_ovrn++;
#define DownlinkStartMessage(_name, msg_id, payload_len) { \
sw/airborne/inter_mcu.h
+10
View File
@@ -49,6 +49,10 @@
#include "radio_control.h"
#include "main_fbw.h"
#ifdef SITL
#include <stdio.h>
#endif
/*
* System clock in MHz.
*/
@@ -152,9 +156,15 @@ static inline void to_autopilot_from_rc_values (void) {
static inline void inter_mcu_event_task( void) {
if (from_ap_receive_valid) {
from_ap_receive_valid = FALSE;
// printf("inter_mcu valid\n");
time_since_last_ap = 0;
ap_ok = TRUE;
to_autopilot_from_rc_values();
#if defined AP
/**Directly set the flag indicating to AP that shared buffer is available*/
from_fbw_receive_valid = TRUE;
#endif
}
}
sw/airborne/main_ap.c
+4 -1
View File
@@ -462,8 +462,11 @@ inline void periodic_task_ap( void ) {
from_ap.from_ap.channels[COMMAND_ROLL] = desired_aileron;
from_ap.from_ap.channels[COMMAND_PITCH] = desired_elevator;
#if defined MCU_SPI_LINK && !defined SITL
#if defined MCU_SPI_LINK
link_fbw_send();
#elif defined INTER_MCU && defined FBW
/**Directly set the flag indicating to FBW that shared buffer is available*/
from_ap_receive_valid = TRUE;
#endif
break;
default:
sw/airborne/pprz_transport.h
+11
View File
@@ -86,6 +86,17 @@ extern uint8_t ck_a, ck_b;
#define PprzTransportPutUint32ByAddr(_x) PprzTransportPut4ByteByAddr(_x)
#define PprzTransportPutFloatByAddr(_x) PprzTransportPut4ByteByAddr(_x)
#define PprzTransportPut(_put, _n, _x) { \
int i; \
PprzTransportPutUint8(_n); \
for(i = 0; i < _n; i++) { \
_put(&_x[i]); \
} \
}
#define PprzTransportPutInt16Array(_n, _x) PprzTransportPut(PprzTransportPutInt16ByAddr, _n, _x)
#define PprzTransportPutUint16Array(_n, _x) PprzTransportPut(PprzTransportPutUint16ByAddr, _n, _x)
#endif /* PPRZ_TRANSPORT_H */
sw/airborne/sim/ivy_transport.h
+24 -8
View File
@@ -13,13 +13,29 @@ extern char* ivy_p;
#define IvyTransportTrailer() { *ivy_p = '\0'; IvySendMsg(ivy_buf); }
#define IvyTransportPutUint8(x) { ivy_p += sprintf(ivy_p, "%u ", x); }
#define IvyTransportPutNamedUint8(_name, _x) { ivy_p += sprintf(ivy_p, "%s", _name); }
#define IvyTransportPutNamedUint8(_name, _x) { ivy_p += sprintf(ivy_p, "%s ", _name); }
#define IvyTransportPutUint8ByAddr(x) { ivy_p += sprintf(ivy_p, " %u", *x); }
#define IvyTransportPutUint16ByAddr(x) { ivy_p += sprintf(ivy_p, " %u", *x); }
#define IvyTransportPutUint32ByAddr(x) { ivy_p += sprintf(ivy_p, " %u", *x);}
#define Space() ivy_p += sprintf(ivy_p, " ");
#define Comma() ivy_p += sprintf(ivy_p, ",");
#define IvyTransportPutInt8ByAddr(x) { ivy_p += sprintf(ivy_p, " %d", *x); }
#define IvyTransportPutInt16ByAddr(x) { ivy_p += sprintf(ivy_p, " %d", *x); }
#define IvyTransportPutInt32ByAddr(x) { ivy_p += sprintf(ivy_p, " %d", *x); }
#define IvyTransportPutFloatByAddr(x) { ivy_p += sprintf(ivy_p, " %f", *x); }
#define IvyTransportPutUintByAddr(x) ivy_p += sprintf(ivy_p, "%u", *x);
#define IvyTransportPutUint8ByAddr(x) IvyTransportPutUintByAddr(x) Space()
#define IvyTransportPutUint16ByAddr(x) IvyTransportPutUintByAddr(x) Space()
#define IvyTransportPutUint32ByAddr(x) IvyTransportPutUintByAddr(x) Space()
#define IvyTransportPutIntByAddr(x) ivy_p += sprintf(ivy_p, "%d", *x);
#define IvyTransportPutInt8ByAddr(x) IvyTransportPutIntByAddr(x) Space()
#define IvyTransportPutInt16ByAddr(x) IvyTransportPutIntByAddr(x) Space()
#define IvyTransportPutInt32ByAddr(x) IvyTransportPutIntByAddr(x) Space()
#define IvyTransportPutFloatByAddr(x) ivy_p += sprintf(ivy_p, "%f ", *x);
#define IvyTransportPutArray(_put, _n, _x) { \
int i; \
for(i = 0; i < _n; i++) { \
_put(&_x[i]); \
Comma(); \
} \
}
#define IvyTransportPutInt16Array(_n, _x) IvyTransportPutArray(IvyTransportPutIntByAddr, _n, _x)
#define IvyTransportPutUint16Array(_n, _x) IvyTransportPutArray(IvyTransportPutUintByAddr, _n, _x)
sw/airborne/sim/sim_ap.c
+6 -3
View File
@@ -34,6 +34,9 @@ uint8_t ac_id;
value sim_periodic_task(value _unit __attribute__ ((unused))) {
periodic_task_ap();
periodic_task_fbw();
event_task_ap();
event_task_fbw();
return Val_unit;
}
@@ -45,10 +48,10 @@ float ftimeofday(void) {
return (t.tv_sec + t.tv_usec/1e6);
}
value sim_init(value id) {
value sim_init(value unit) {
init_fbw();
init_ap();
ac_id = Int_val(id);
return Val_unit;
return unit;
}
value update_bat(value bat) {
sw/ground_segment/tmtc/messages.ml
+5 -2
View File
@@ -37,7 +37,6 @@ let xml_file = Env.paparazzi_src // "conf" // "messages.xml"
let _ =
let ivy_bus = ref "127.255.255.255:2010" in
(* let classes = ref ["telemetry_ap";"ground"] in *)
let classes = ref [] in
Arg.parse
[ "-b", Arg.String (fun x -> ivy_bus := x), "Bus\tDefault is 127.255.255.255:2010";
@@ -116,7 +115,11 @@ let _ =
let xml_classes =
let class_of = fun n -> List.find (fun x -> ExtXml.attrib x "name" = n) (Xml.children xml) in
let class_of = fun n ->
try
List.find (fun x -> ExtXml.attrib x "name" = n) (Xml.children xml)
with Not_found -> failwith (sprintf "Unknown messages class: %s" n) in
List.map (fun x ->
match Str.split (Str.regexp ":") x with
[cl; s] -> (cl, class_of cl, Some s)
sw/simulator/flightModel.ml
+2 -2
View File
@@ -159,8 +159,8 @@ module Make(A:Data.MISSION) = struct
let do_commands = fun state commands ->
let c_lda = 1e-4 in (* FIXME *)
state.delta_a <- c_lda *. float commands.(command_roll);
let c_lda = 4e-5 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 nb_commands = 10 (* FIXME *)
sw/simulator/ppm_hw.h
View File
sw/simulator/sim_ap.c
-170
View File
@@ -1,170 +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 <time.h>
#include "std.h"
#include "inter_mcu.h"
#include "autopilot.h"
#include "estimator.h"
#include "gps.h"
#include "traffic_info.h"
#include "flight_plan.h"
#include "nav.h"
#include "pid.h"
#include "infrared.h"
#include "cam.h"
#include <caml/mlvalues.h>
#include <caml/memory.h>
uint8_t ir_estim_mode;
uint8_t vertical_mode;
uint8_t inflight_calib_mode;
bool_t rc_event_1, rc_event_2;
bool_t launch;
bool_t link_fbw_receive_valid;
uint8_t gps_nb_ovrn, modem_nb_ovrn, link_fbw_fbw_nb_err, link_fbw_nb_err;
uint8_t ac_id;
inter_mcu_msg from_fbw, from_ap;
static int16_t values_from_ap[RADIO_CTL_NB];
uint16_t ppm_pulses[ PPM_NB_PULSES ]; /** From ppm_hw.c */
value sim_periodic_task(value _unit __attribute__ ((unused))) {
periodic_task();
return Val_unit;
}
static int radio_status = 1;
static int radio_really_lost;
value set_radio_status(value on) {
radio_status = Int_val(on);
if (! radio_status) radio_really_lost = FALSE;
return Val_unit;
}
value set_really_lost(value on) {
radio_really_lost = Int_val(on);
return Val_unit;
}
value sim_rc_task(value _unit __attribute__ ((unused))) {
NormalizePpm(); /** -> rc_values */
/*** printf("sim_rc_task ppm=%d rc_val=%d\n", ppm_pulses[RADIO_MODE], rc_values[RADIO_MODE]); ***/
int i;
for(i = 0; i < RADIO_CTL_NB; i++)
from_fbw.from_fbw.channels[i] = rc_values[i];
from_fbw.from_fbw.status = (radio_status << STATUS_RADIO_OK) | (radio_really_lost << RADIO_REALLY_LOST) | (1 << AVERAGED_CHANNELS_SENT);
link_fbw_receive_valid = TRUE;
telecommand_task();
return Val_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 id) {
pprz_mode = PPRZ_MODE_MANUAL;
estimator_init();
ir_init();
ac_id = Int_val(id);
return Val_unit;
}
value update_bat(value bat) {
from_fbw.from_fbw.vsupply = Int_val(bat);
return Val_unit;
}
value update_rc_channel(value c, value v) {
ppm_pulses[Int_val(c)] = Double_val(v);
return Val_unit;
}
// Defined in servo.c
#define _4017_NB_CHANNELS 10
#define SERVO_NEUTRAL_(i) SERVOS_NEUTRALS_ ## i
#define SERVO_NEUTRAL(i) (SERVO_NEUTRAL_(i))
#define SERVO_MIN (SERVO_MIN_US)
#define SERVO_MAX (SERVO_MAX_US)
#define ChopServo(x, min, max) ((x) < min ? min : ((x) > max ? max : (x)))
#ifdef SERVO_MOTOR_RIGHT
#define SERVO_GAZ SERVO_MOTOR_RIGHT
#endif
#define Actuator(i) servo_widths[i]
value set_servos(value servos) {
int i;
/** Get values computed by the autopilot */
for(i = 0; i < RADIO_CTL_NB; i++) {
values_from_ap[i] = US_OF_CLOCK(from_ap.from_ap.channels[i]);
/***printf("%d:%d\n", i, values_from_ap[i]); ***/
}
uint16_t servo_widths[_4017_NB_CHANNELS];
SetActuatorsFromCommands(values_from_ap);
for(i=0; i < _4017_NB_CHANNELS; i++)
Store_field(servos, i, Val_int(servo_widths[i]));
return Val_int(servo_widths[SERVO_GAZ]);
}
value set_ac_info_native(value ac_id, value ux, value uy, value course, value alt, value gspeed) {
SetAcInfo(Int_val(ac_id), Double_val(ux), Double_val(uy),
Double_val(course), Double_val(alt), Double_val(gspeed));
return Val_unit;
}
value set_ac_info(value * argv, int argn) {
assert (argn == 6);
return set_ac_info_native(argv[0], argv[1], argv[2], argv[3],argv[4], argv[5]);
}
value move_waypoint(value wp_id, value ux, value uy, value a) {
MoveWaypoint(Int_val(wp_id), Double_val(ux), Double_val(uy), Double_val(a));
return Val_unit;
}
value goto_block(value block_id) {
nav_goto_block(Int_val(block_id));
return Val_unit;
}
value send_event(value event_id) {
uint8_t event = Int_val(event_id);
switch (event) {
case 1 : rc_event_1 = TRUE; break; // FIXME !
case 2 : rc_event_2 = TRUE; break;
default: ;
}
return Val_unit;
}
value dl_setting(value index __attribute__ ((unused)),
value val __attribute__ ((unused))) {
/** DlSetting macro may be empty: unused attr to get rif of the warning */
DlSetting(Int_val(index), Double_val(val));
return Val_unit;
}
sw/simulator/sim_gps.c
-59
View File
@@ -1,59 +0,0 @@
/* OCaml binding to link the simulator to autopilot functions. */
#include <assert.h>
#include <math.h>
#include <inttypes.h>
/** From airborne/autopilot/ */
#include "airframe.h"
#include "flight_plan.h"
#include "autopilot.h"
#include "gps.h"
#include <caml/mlvalues.h>
uint8_t gps_mode;
uint32_t gps_itow; /* ms */
int32_t gps_alt; /* m */
uint16_t gps_gspeed; /* cm/s */
int16_t gps_climb; /* cm/s */
int16_t gps_course; /* decideg */
int32_t gps_utm_east, gps_utm_north;
uint8_t gps_utm_zone;
struct svinfo gps_svinfos[GPS_NB_CHANNELS];
uint8_t gps_nb_channels = 0;
value sim_use_gps_pos(value x, value y, value z, value c, value a, value s, value cl, value t, value m) {
gps_mode = (Bool_val(m) ? 3 : 0);
gps_utm_east = Int_val(x);
gps_utm_north = Int_val(y);
gps_utm_zone = Int_val(z);
gps_course = DegOfRad(Double_val(c)) * 10.;
gps_alt = Double_val(a) * 100.;
gps_gspeed = Double_val(s) * 100.;
gps_climb = Double_val(cl) * 100.;
gps_itow = Double_val(t) * 1000.;
/** 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/100.) * 10.;
gps_svinfos[i].flags = 0x01;
gps_svinfos[i].qi = (int)((time / 1000.) + i) % 8;
}
use_gps_pos(); /* From main.c */
return Val_unit;
}
/* Second binding required because number of args > 5 */
value sim_use_gps_pos_bytecode(value *a, int argn) {
assert(argn == 9);
return sim_use_gps_pos(a[0],a[1],a[2],a[3],a[4],a[5],a[6],a[7], a[8]);
}
sw/simulator/sim_ir.c
-27
View File
@@ -1,27 +0,0 @@
/** \file sim_ir.c
* \brief Regroup functions to simulate autopilot/infrared.c
*
* Infrared soft simulation. OCaml binding.
*/
#include <inttypes.h>
#include "infrared.h"
#include "airframe.h"
#include <caml/mlvalues.h>
void ir_gain_calib(void) {
}
value set_ir(value roll, value top) {
ir_roll = Int_val(roll);
ir_top = Int_val(top);
return Val_unit;
}
/** Required by infrared.c:ir_init() */
void adc_buf_channel(void* a __attribute__ ((unused)),
void* b __attribute__ ((unused)),
void* c __attribute__ ((unused))) {
}
sw/simulator/sitl.ml
+3 -4
View File
@@ -66,7 +66,6 @@ module Make(A:Data.MISSION) = struct
if bat_button#active then
let energy = float (gaz-1000) /. 1000. *. servos_period in
accu := !accu +. energy *. 0.00259259259259259252; (* To be improved !!! *)
printf "\b\b\b\b\b%.3f%!" !accu;
if !accu >= 0.1 then begin
let b = adj_bat#value in
adj_bat#set_value (b -. !accu);
@@ -108,11 +107,11 @@ module Make(A:Data.MISSION) = struct
rc_channels;
ignore (on_off#connect#toggled (fun () -> sliders#coerce#misc#set_sensitive on_off#active));
Stdlib.timer 0.1 send_ppm; (** FIXME: should use time_scale *)
Stdlib.timer rc_period send_ppm; (** FIXME: should use time_scale *)
window#show ()
external periodic_task : unit -> unit = "sim_periodic_task"
external sim_init : int -> unit = "sim_init"
external sim_init : unit -> unit = "sim_init"
external update_bat : int -> unit = "update_bat"
let bat_button = GButton.toggle_button ~label:"Bat" ()
@@ -121,7 +120,7 @@ module Make(A:Data.MISSION) = struct
let init = fun id vbox ->
rc ();
my_id := id;
sim_init id;
sim_init ();
let hbox = GPack.hbox ~packing:vbox#add () in
hbox#pack bat_button#coerce;
sw/simulator/sys_time_hw.h
-4
View File
@@ -1,4 +0,0 @@
#define SYS_TICS_OF_USEC(x) ((x))
#define SIGNED_SYS_TICS_OF_USEC(x) ((x))
#define US_OF_CLOCK(x) ((x))
#define SERVOS_TICS_OF_USEC(s) SYS_TICS_OF_USEC(s)
sw/simulator/uart_hw.h
View File
sw/tools/gen_messages.ml
+1 -5
View File
@@ -132,11 +132,7 @@ module Gen_onboard = struct
fprintf avr_h "\t DownlinkPut%sByAddr((%s)); \\\n" (nameof t) name
| Array (t, varname) ->
let s = sizeof (Basic t) in
fprintf avr_h "\t DownlinkPutUint8(%s);\\\n" (length_name varname);
fprintf avr_h "\t {\\\n\t int i;\\\n\t for(i = 0; i < %s; i++) {\\\n" (length_name varname);
fprintf avr_h "\t DownlinkPut%sByAddr((uint8_t*)(&%s[i])); \\\n" (nameof (Basic t)) name;
fprintf avr_h "\t }\\\n";
fprintf avr_h "\t }\\\n"
fprintf avr_h "\t DownlinkPut%sArray(%s, %s); \\\n" (nameof (Basic t)) (length_name varname) name
let print_one avr_h = function
(Array _, s, _) -> fprintf avr_h "%s, %s" (length_name s) s