From b3c88fd995c9c6db0c2ed106a7e2958fbdb80ce6 Mon Sep 17 00:00:00 2001 From: Xavier Gibert Date: Wed, 4 Apr 2012 16:38:08 +0200 Subject: [PATCH] 2 modules to load messages (by class_type or by class_name). Some message names are changed. --- conf/messages/custom_downlink.xml | 269 ++- conf/messages/downlink.xml | 2062 +++++++---------- conf/messages/sensors.xml | 173 ++ conf/messages_conf.xml.example | 15 +- conf/telemetry/aerocomm.xml | 2 +- conf/telemetry/booz_minimal.xml | 4 +- conf/telemetry/default.xml | 4 +- conf/telemetry/default_fixedwing_imu.xml | 4 +- conf/telemetry/default_fixedwing_imu_9k6.xml | 4 +- conf/telemetry/fw_h_ctl_a.xml | 2 +- conf/telemetry/hitl.xml | 2 +- conf/telemetry/iridium.xml | 2 +- conf/telemetry/minimal.xml | 2 +- conf/telemetry/osam_imu.xml | 2 +- conf/telemetry/telemetry_booz2.xml | 8 +- conf/telemetry/telemetry_jtm.xml | 8 +- conf/telemetry/tl.xml | 2 +- conf/telemetry/ugv.xml | 2 +- conf/telemetry/xbee868.xml | 2 +- sw/airborne/arch/sim/jsbsim_transport.c | 2 +- sw/airborne/booz/test/booz2_test_crista.c | 4 +- sw/airborne/firmwares/beth/main_overo.c | 4 +- sw/airborne/firmwares/beth/main_stm32.c | 6 +- sw/airborne/firmwares/fixedwing/ap_downlink.h | 4 +- sw/airborne/firmwares/fixedwing/datalink.c | 2 +- sw/airborne/firmwares/rotorcraft/telemetry.h | 14 +- sw/airborne/test/subsystems/test_ahrs.c | 6 +- sw/airborne/test/subsystems/test_imu.c | 6 +- sw/ground_segment/cockpit/live.ml | 6 +- sw/ground_segment/tmtc/airprox.ml | 2 +- sw/ground_segment/tmtc/dia.ml | 6 +- sw/ground_segment/tmtc/diadec.ml | 4 +- sw/ground_segment/tmtc/fw_server.ml | 4 +- sw/ground_segment/tmtc/ivy2udp.ml | 6 +- sw/ground_segment/tmtc/ivy_tcp_aircraft.ml | 6 +- sw/ground_segment/tmtc/ivy_tcp_controller.ml | 6 +- sw/ground_segment/tmtc/link.ml | 12 +- sw/ground_segment/tmtc/messages.ml | 2 +- sw/ground_segment/tmtc/rotorcraft_server.ml | 2 +- sw/ground_segment/tmtc/server.ml | 17 +- sw/ground_segment/tmtc/settings.ml | 4 +- sw/lib/ocaml/papget.ml | 4 +- sw/lib/ocaml/pprz.ml | 238 +- sw/lib/ocaml/pprz.mli | 102 +- sw/logalizer/play_core.ml | 6 +- sw/logalizer/plot.ml | 7 +- sw/logalizer/plotter.ml | 2 +- sw/logalizer/sd2log.ml | 4 +- sw/simulator/gaia.ml | 2 +- sw/simulator/hitl.ml | 6 +- sw/simulator/sim.ml | 2 +- sw/simulator/sitl.ml | 6 +- 52 files changed, 1675 insertions(+), 1398 deletions(-) create mode 100644 conf/messages/sensors.xml diff --git a/conf/messages/custom_downlink.xml b/conf/messages/custom_downlink.xml index 1c3483a3462..afd6402605a 100644 --- a/conf/messages/custom_downlink.xml +++ b/conf/messages/custom_downlink.xml @@ -1,8 +1,29 @@ + + + + + + - + + + + + + + + + + + + + + + + @@ -56,12 +77,6 @@ - - - - - - @@ -123,6 +138,11 @@ + + + + + @@ -172,11 +192,6 @@ - - - - - @@ -294,4 +309,234 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/messages/downlink.xml b/conf/messages/downlink.xml index 14b48bb7361..b61cefdcc6f 100644 --- a/conf/messages/downlink.xml +++ b/conf/messages/downlink.xml @@ -1,103 +1,96 @@ - - - - - - - - - + + - - + - + - - - - - - - - - - - - - - + + + + - - - - - + + + - - - + + + + + + + + + + + + + + + + - + - + - - - - - - - - - - - + - + + + + + + + + + + + - - - - - - + + + + - - + + + - + + @@ -108,328 +101,223 @@ - + - - + + - - - - - - + + + - - - - - - - - + + + - - - - - - - - - - + + + + - - - - - - - - - - - - - - - - - - - - + + + + + - - - - - - - - - - - - - - - - - + + + + - - - - - - - - - - - - - - - - - - - - - - + + - - - - - - - - - - - - - - - - - - - - - - - + + + + - - - - - - - - - - - - - + + + + + + + + - - - - - - - - - - - - - + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + - - - - - - - - - + + + + + + + + + + + + + + + + + - - - - + - - - - - - - + + + + + + + + - - - - - + + + + + - - - - - - - - - - - - + + + + + + + - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - + + + + + + - - - - + + + + - - - + + + - - - + + + + + + + - - + + + + + + - - - + + + + - - + - + @@ -438,55 +326,37 @@ - - - - - - - - - - - - - - - - - - - + - + - + - + - + - + @@ -506,7 +376,7 @@ - + @@ -515,12 +385,10 @@ - - - + - + @@ -537,6 +405,8 @@ + + @@ -615,163 +485,56 @@ - - - - - - - - - - - - - - - - - - - + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + - - - - - - - - - + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - + + + + - - - - + @@ -804,61 +567,19 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + - - + + + + + + + - - + + @@ -884,13 +605,12 @@ - - - - - - - + + + + + + @@ -902,587 +622,713 @@ - - - - - - - - + + + + + - + - - - + + + - - - + + - - - - - - - - + + + + - - + + + + + - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - + - - + + - - - - - + + + + + + + - - - - + + + - - - - + + + - - + + - - + + + + - - - - - - + - - - - - - + + + + - - - + - - - - - - - - + + + + + + + - - - - + + + + + + + + + + - - - - - - - + + + + + - - - - - + + + + + + + + + + - - - - - - - + + + + + + + - - - + + + + - - - + + - - + + + + - - - + + + + + - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - + + + + + + + - - - + + + + + + + + + - - - - - - - - - + + + + + - - - - - - - - - - - - - - - - - + + + + + + + + - - - - - - - - + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - + - - - - - - - - - - - + + + - - - - - - - - - - - - - - - - - - - + + + + + + + + - - - - - - - - - - - - - - - - - + + + + + - - - + + + + - - - - - - - - - - - - - - - - - - - - - - + + - - - - - - - + + + - + + + - - - - - - - - - - - - - + + + - - - - - - - + + + - - - + + - - - - - - - + - - - - + + + + + + + - - - - - - - - + + + + + + + - - - - - - + + + + + + + - - - - - + + + - + + + + - - - - - - - - - - - - - - - - + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - @@ -1504,53 +1350,9 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - @@ -1558,102 +1360,16 @@ + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - @@ -1670,32 +1386,6 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/conf/messages/sensors.xml b/conf/messages/sensors.xml new file mode 100644 index 00000000000..46d0cad24a7 --- /dev/null +++ b/conf/messages/sensors.xml @@ -0,0 +1,173 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/messages_conf.xml.example b/conf/messages_conf.xml.example index e542f3f5aac..62d0978a107 100644 --- a/conf/messages_conf.xml.example +++ b/conf/messages_conf.xml.example @@ -4,12 +4,13 @@ - - - - + + + + + - - - + + + diff --git a/conf/telemetry/aerocomm.xml b/conf/telemetry/aerocomm.xml index 7870513dfbd..f0791e1bed2 100644 --- a/conf/telemetry/aerocomm.xml +++ b/conf/telemetry/aerocomm.xml @@ -6,7 +6,7 @@ - + diff --git a/conf/telemetry/booz_minimal.xml b/conf/telemetry/booz_minimal.xml index 281730aa574..c4675301bec 100644 --- a/conf/telemetry/booz_minimal.xml +++ b/conf/telemetry/booz_minimal.xml @@ -12,7 +12,7 @@ - + @@ -22,7 +22,7 @@ - + diff --git a/conf/telemetry/default.xml b/conf/telemetry/default.xml index 89fee5251a9..733c758e682 100644 --- a/conf/telemetry/default.xml +++ b/conf/telemetry/default.xml @@ -10,7 +10,7 @@ - + @@ -34,7 +34,7 @@ - + diff --git a/conf/telemetry/default_fixedwing_imu.xml b/conf/telemetry/default_fixedwing_imu.xml index f7f832e34d8..6f6c00e73be 100644 --- a/conf/telemetry/default_fixedwing_imu.xml +++ b/conf/telemetry/default_fixedwing_imu.xml @@ -10,7 +10,7 @@ - + @@ -36,7 +36,7 @@ - + diff --git a/conf/telemetry/default_fixedwing_imu_9k6.xml b/conf/telemetry/default_fixedwing_imu_9k6.xml index e97b9046a99..b5f0eff3c4b 100644 --- a/conf/telemetry/default_fixedwing_imu_9k6.xml +++ b/conf/telemetry/default_fixedwing_imu_9k6.xml @@ -10,7 +10,7 @@ - + @@ -36,7 +36,7 @@ - + diff --git a/conf/telemetry/fw_h_ctl_a.xml b/conf/telemetry/fw_h_ctl_a.xml index d9bf211906c..1f306588b5b 100644 --- a/conf/telemetry/fw_h_ctl_a.xml +++ b/conf/telemetry/fw_h_ctl_a.xml @@ -8,7 +8,7 @@ - + diff --git a/conf/telemetry/hitl.xml b/conf/telemetry/hitl.xml index 034ed1b6532..36f504f426e 100644 --- a/conf/telemetry/hitl.xml +++ b/conf/telemetry/hitl.xml @@ -6,7 +6,7 @@ - + diff --git a/conf/telemetry/iridium.xml b/conf/telemetry/iridium.xml index 6ca164367e4..27042bc9feb 100644 --- a/conf/telemetry/iridium.xml +++ b/conf/telemetry/iridium.xml @@ -9,7 +9,7 @@ - + diff --git a/conf/telemetry/minimal.xml b/conf/telemetry/minimal.xml index 3789235f917..420735b8767 100644 --- a/conf/telemetry/minimal.xml +++ b/conf/telemetry/minimal.xml @@ -6,7 +6,7 @@ - + diff --git a/conf/telemetry/osam_imu.xml b/conf/telemetry/osam_imu.xml index 22efef2abca..95e6b74cf26 100644 --- a/conf/telemetry/osam_imu.xml +++ b/conf/telemetry/osam_imu.xml @@ -6,7 +6,7 @@ - + diff --git a/conf/telemetry/telemetry_booz2.xml b/conf/telemetry/telemetry_booz2.xml index 5340d21ec26..ab868c2c5b2 100644 --- a/conf/telemetry/telemetry_booz2.xml +++ b/conf/telemetry/telemetry_booz2.xml @@ -12,7 +12,7 @@ - + @@ -40,9 +40,9 @@ - - - + + + diff --git a/conf/telemetry/telemetry_jtm.xml b/conf/telemetry/telemetry_jtm.xml index 2b1d290955d..3767ec78428 100644 --- a/conf/telemetry/telemetry_jtm.xml +++ b/conf/telemetry/telemetry_jtm.xml @@ -12,7 +12,7 @@ - + @@ -40,9 +40,9 @@ - - - + + + diff --git a/conf/telemetry/tl.xml b/conf/telemetry/tl.xml index f82332f4c92..30272b19cc3 100644 --- a/conf/telemetry/tl.xml +++ b/conf/telemetry/tl.xml @@ -15,7 +15,7 @@ - + diff --git a/conf/telemetry/ugv.xml b/conf/telemetry/ugv.xml index bb153041b8b..fea3f094e91 100644 --- a/conf/telemetry/ugv.xml +++ b/conf/telemetry/ugv.xml @@ -4,7 +4,7 @@ - + diff --git a/conf/telemetry/xbee868.xml b/conf/telemetry/xbee868.xml index 95e73ffaa28..e4d4b491866 100755 --- a/conf/telemetry/xbee868.xml +++ b/conf/telemetry/xbee868.xml @@ -9,7 +9,7 @@ - + diff --git a/sw/airborne/arch/sim/jsbsim_transport.c b/sw/airborne/arch/sim/jsbsim_transport.c index 24137fec0d8..f376f56d453 100644 --- a/sw/airborne/arch/sim/jsbsim_transport.c +++ b/sw/airborne/arch/sim/jsbsim_transport.c @@ -61,6 +61,6 @@ void parse_dl_move_wp(char* argv[]) { coordinates */ utm.east = waypoints[wp_id].x + nav_utm_east0; utm.north = waypoints[wp_id].y + nav_utm_north0; - DOWNLINK_SEND_WP_MOVED(DefaultChannel, DefaultDevice, &wp_id, &utm.east, &utm.north, &a, &nav_utm_zone0); + DOWNLINK_SEND_WP_MOVED_UTM(DefaultChannel, DefaultDevice, &wp_id, &utm.east, &utm.north, &a, &nav_utm_zone0); } diff --git a/sw/airborne/booz/test/booz2_test_crista.c b/sw/airborne/booz/test/booz2_test_crista.c index 2859408924f..c6f5f0b3c78 100644 --- a/sw/airborne/booz/test/booz2_test_crista.c +++ b/sw/airborne/booz/test/booz2_test_crista.c @@ -108,11 +108,11 @@ static inline void on_imu_event(void) { &imu_accel_unscaled.z); } else if (cnt == 7) { - DOWNLINK_SEND_IMU_GYRO_SCALED(&imu_gyro.x, + DOWNLINK_SEND_IMU_GYRO_INT(&imu_gyro.x, &imu_gyro.y, &imu_gyro.z); - DOWNLINK_SEND_IMU_ACCEL_SCALED(&imu_accel.x, + DOWNLINK_SEND_IMU_ACCEL_INT(&imu_accel.x, &imu_accel.y, &imu_accel.z); } diff --git a/sw/airborne/firmwares/beth/main_overo.c b/sw/airborne/firmwares/beth/main_overo.c index b24958321f2..2571d2b151e 100644 --- a/sw/airborne/firmwares/beth/main_overo.c +++ b/sw/airborne/firmwares/beth/main_overo.c @@ -159,13 +159,13 @@ static void main_periodic(int my_sig_num) { RunOnceEvery(10, {DOWNLINK_SEND_IMU_ACCEL_RAW(gcs_com.udp_transport, //&msg_in.payload.msg_up.accel.x,&msg_in.payload.msg_up.accel.y,&msg_in.payload.msg_up.accel.z &imu.accel_unscaled.x,&imu.accel_unscaled.y,&imu.accel_unscaled.z);}) - RunOnceEvery(50, {DOWNLINK_SEND_IMU_GYRO_SCALED(gcs_com.udp_transport, + RunOnceEvery(50, {DOWNLINK_SEND_IMU_GYRO_INT(gcs_com.udp_transport, //&msg_in.payload.msg_up.gyro.p,&msg_in.payload.msg_up.gyro.q,&msg_in.payload.msg_up.gyro.r) &imu.gyro.p,&imu.gyro.q,&imu.gyro.r);}); RunOnceEvery(50, {DOWNLINK_SEND_AHRS_EULER(gcs_com.udp_transport, &estimator.tilt, &estimator.elevation, &estimator.azimuth );}); - RunOnceEvery(50, {DOWNLINK_SEND_IMU_ACCEL_SCALED(DefaultChannel, + RunOnceEvery(50, {DOWNLINK_SEND_IMU_ACCEL_INT(DefaultChannel, //&msg_in.payload.msg_up.accel.x,&msg_in.payload.msg_up.accel.y,&msg_in.payload.msg_up.accel.z &imu.accel.x,&imu.accel.y,&imu.accel.z);});*/ diff --git a/sw/airborne/firmwares/beth/main_stm32.c b/sw/airborne/firmwares/beth/main_stm32.c index af702db5ee0..b5a68241518 100644 --- a/sw/airborne/firmwares/beth/main_stm32.c +++ b/sw/airborne/firmwares/beth/main_stm32.c @@ -176,12 +176,12 @@ static inline void on_gyro_accel_event(void) { &imu.accel_unscaled.z); } else if (cnt == 7) { - DOWNLINK_SEND_IMU_GYRO_SCALED(DefaultChannel, + DOWNLINK_SEND_IMU_GYRO_INT(DefaultChannel, &imu.gyro.p, &imu.gyro.q, &imu.gyro.r); - DOWNLINK_SEND_IMU_ACCEL_SCALED(DefaultChannel, + DOWNLINK_SEND_IMU_ACCEL_INT(DefaultChannel, &imu.accel.x, &imu.accel.y, &imu.accel.z); @@ -196,7 +196,7 @@ static inline void on_mag_event(void) { if (cnt > 1) cnt = 0; if (cnt%2) { - DOWNLINK_SEND_IMU_MAG_SCALED(DefaultChannel, + DOWNLINK_SEND_IMU_MAG_INT(DefaultChannel, &imu.mag.x, &imu.mag.y, &imu.mag.z); diff --git a/sw/airborne/firmwares/fixedwing/ap_downlink.h b/sw/airborne/firmwares/fixedwing/ap_downlink.h index 70c62f362ef..b47a1ff05a1 100644 --- a/sw/airborne/firmwares/fixedwing/ap_downlink.h +++ b/sw/airborne/firmwares/fixedwing/ap_downlink.h @@ -113,11 +113,11 @@ extern uint8_t telemetry_mode_Ap_DefaultChannel; #define DownlinkSendWp(_trans, _dev, i) { \ float x = nav_utm_east0 + waypoints[i].x; \ float y = nav_utm_north0 + waypoints[i].y; \ - DOWNLINK_SEND_WP_MOVED(_trans, _dev, &i, &x, &y, &(waypoints[i].a),&nav_utm_zone0); \ + DOWNLINK_SEND_WP_MOVED_UTM(_trans, _dev, &i, &x, &y, &(waypoints[i].a),&nav_utm_zone0); \ } -#define PERIODIC_SEND_WP_MOVED(_trans, _dev) { \ +#define PERIODIC_SEND_WP_MOVED_UTM(_trans, _dev) { \ static uint8_t i; \ i++; if (i >= nb_waypoint) i = 0; \ DownlinkSendWp(_trans, _dev, i); \ diff --git a/sw/airborne/firmwares/fixedwing/datalink.c b/sw/airborne/firmwares/fixedwing/datalink.c index 73e6ee55d00..985d48a55c3 100644 --- a/sw/airborne/firmwares/fixedwing/datalink.c +++ b/sw/airborne/firmwares/fixedwing/datalink.c @@ -128,7 +128,7 @@ void dl_parse_msg(void) { coordinates */ utm.east = waypoints[wp_id].x + nav_utm_east0; utm.north = waypoints[wp_id].y + nav_utm_north0; - DOWNLINK_SEND_WP_MOVED(DefaultChannel, DefaultDevice, &wp_id, &utm.east, &utm.north, &a, &nav_utm_zone0); + DOWNLINK_SEND_WP_MOVED_UTM(DefaultChannel, DefaultDevice, &wp_id, &utm.east, &utm.north, &a, &nav_utm_zone0); } else if (msg_id == DL_BLOCK && DL_BLOCK_ac_id(dl_buffer) == AC_ID) { nav_goto_block(DL_BLOCK_block_id(dl_buffer)); SEND_NAVIGATION(DefaultChannel, DefaultDevice); diff --git a/sw/airborne/firmwares/rotorcraft/telemetry.h b/sw/airborne/firmwares/rotorcraft/telemetry.h index deba1abf423..ef56a6b8925 100644 --- a/sw/airborne/firmwares/rotorcraft/telemetry.h +++ b/sw/airborne/firmwares/rotorcraft/telemetry.h @@ -134,22 +134,22 @@ extern uint8_t telemetry_mode_Main_DefaultChannel; #define PERIODIC_SEND_PPM(_trans, _dev) {} #endif -#define PERIODIC_SEND_IMU_GYRO_SCALED(_trans, _dev) { \ - DOWNLINK_SEND_IMU_GYRO_SCALED(_trans, _dev, \ +#define PERIODIC_SEND_IMU_GYRO_INT(_trans, _dev) { \ + DOWNLINK_SEND_IMU_GYRO_INT(_trans, _dev, \ &imu.gyro.p, \ &imu.gyro.q, \ &imu.gyro.r); \ } -#define PERIODIC_SEND_IMU_ACCEL_SCALED(_trans, _dev) { \ - DOWNLINK_SEND_IMU_ACCEL_SCALED(_trans, _dev, \ +#define PERIODIC_SEND_IMU_ACCEL_INT(_trans, _dev) { \ + DOWNLINK_SEND_IMU_ACCEL_INT(_trans, _dev, \ &imu.accel.x, \ &imu.accel.y, \ &imu.accel.z); \ } -#define PERIODIC_SEND_IMU_MAG_SCALED(_trans, _dev) { \ - DOWNLINK_SEND_IMU_MAG_SCALED(_trans, _dev, \ +#define PERIODIC_SEND_IMU_MAG_INT(_trans, _dev) { \ + DOWNLINK_SEND_IMU_MAG_INT(_trans, _dev, \ &imu.mag.x, \ &imu.mag.y, \ &imu.mag.z); \ @@ -703,7 +703,7 @@ extern uint8_t telemetry_mode_Main_DefaultChannel; } \ } -#define PERIODIC_SEND_WP_MOVED(_trans, _dev) { \ +#define PERIODIC_SEND_WP_MOVED_UTM(_trans, _dev) { \ static uint8_t i; \ i++; if (i >= nb_waypoint) i = 0; \ DOWNLINK_SEND_WP_MOVED_ENU(_trans, _dev, \ diff --git a/sw/airborne/test/subsystems/test_ahrs.c b/sw/airborne/test/subsystems/test_ahrs.c index a117dd55e3d..0b10e8f118d 100644 --- a/sw/airborne/test/subsystems/test_ahrs.c +++ b/sw/airborne/test/subsystems/test_ahrs.c @@ -136,20 +136,20 @@ static inline void main_report(void) { &imu.mag_unscaled.z); }, { - DOWNLINK_SEND_IMU_ACCEL_SCALED(DefaultChannel, DefaultDevice, + DOWNLINK_SEND_IMU_ACCEL_INT(DefaultChannel, DefaultDevice, &imu.accel.x, &imu.accel.y, &imu.accel.z); }, { - DOWNLINK_SEND_IMU_GYRO_SCALED(DefaultChannel, DefaultDevice, + DOWNLINK_SEND_IMU_GYRO_INT(DefaultChannel, DefaultDevice, &imu.gyro.p, &imu.gyro.q, &imu.gyro.r); }, { - DOWNLINK_SEND_IMU_MAG_SCALED(DefaultChannel, DefaultDevice, + DOWNLINK_SEND_IMU_MAG_INT(DefaultChannel, DefaultDevice, &imu.mag.x, &imu.mag.y, &imu.mag.z); diff --git a/sw/airborne/test/subsystems/test_imu.c b/sw/airborne/test/subsystems/test_imu.c index ed7c389ff7b..d5e5466acd2 100644 --- a/sw/airborne/test/subsystems/test_imu.c +++ b/sw/airborne/test/subsystems/test_imu.c @@ -118,7 +118,7 @@ static inline void on_accel_event(void) { &imu.accel_unscaled.z); } else if (cnt == 7) { - DOWNLINK_SEND_IMU_ACCEL_SCALED(DefaultChannel, DefaultDevice, + DOWNLINK_SEND_IMU_ACCEL_INT(DefaultChannel, DefaultDevice, &imu.accel.x, &imu.accel.y, &imu.accel.z); @@ -140,7 +140,7 @@ static inline void on_gyro_accel_event(void) { &imu.gyro_unscaled.r); } else if (cnt == 7) { - DOWNLINK_SEND_IMU_GYRO_SCALED(DefaultChannel, DefaultDevice, + DOWNLINK_SEND_IMU_GYRO_INT(DefaultChannel, DefaultDevice, &imu.gyro.p, &imu.gyro.q, &imu.gyro.r); @@ -155,7 +155,7 @@ static inline void on_mag_event(void) { if (cnt > 10) cnt = 0; if (cnt == 0) { - DOWNLINK_SEND_IMU_MAG_SCALED(DefaultChannel, DefaultDevice, + DOWNLINK_SEND_IMU_MAG_INT(DefaultChannel, DefaultDevice, &imu.mag.x, &imu.mag.y, &imu.mag.z); diff --git a/sw/ground_segment/cockpit/live.ml b/sw/ground_segment/cockpit/live.ml index 569cbe246e6..0954188c11c 100644 --- a/sw/ground_segment/cockpit/live.ml +++ b/sw/ground_segment/cockpit/live.ml @@ -29,9 +29,9 @@ open Latlong module LL = Latlong open Printf -module Tele_Pprz = Pprz.Messages(struct let _type = "downlink" and single_class = "" end) -module Ground_Pprz = Pprz.Messages(struct let _type = "ground" and single_class = "" end) -module Alert_Pprz = Pprz.Messages(struct let _type = "" and single_class = "alert" end) +module Tele_Pprz = Pprz.Messages_of_type(struct let class_type = "downlink" end) +module Ground_Pprz = Pprz.Messages_of_type(struct let class_type = "ground" end) +module Alert_Pprz = Pprz.Messages_of_name(struct let class_name = "alert" end) let (//) = Filename.concat diff --git a/sw/ground_segment/tmtc/airprox.ml b/sw/ground_segment/tmtc/airprox.ml index 69c04e54e2a..69ff9fdc6da 100644 --- a/sw/ground_segment/tmtc/airprox.ml +++ b/sw/ground_segment/tmtc/airprox.ml @@ -27,7 +27,7 @@ open Aircraft open Latlong -module Alerts_Pprz = Pprz.Messages(struct let _type = "" and single_class = "alert" end) +module Alerts_Pprz = Pprz.Messages_of_name(struct let class_name = "alert" end) (** computes distance between 2d points *) let distance = fun (x1, y1) (x2, y2) -> diff --git a/sw/ground_segment/tmtc/dia.ml b/sw/ground_segment/tmtc/dia.ml index 39da1d5da7b..45a310e5975 100644 --- a/sw/ground_segment/tmtc/dia.ml +++ b/sw/ground_segment/tmtc/dia.ml @@ -32,8 +32,8 @@ open Printf let msg_period = 500 (* ms *) let ac_id = ref 1 -module Ground_Pprz = Pprz.Messages(struct let _type = "ground" and single_class = "" end) -module Sub_Pprz = Pprz.Messages(struct let _type = "" and single_class = "DIA" end) +module Ground_Pprz = Pprz.Messages_of_type(struct let class_type = "ground" end) +module Sub_Pprz = Pprz.Messages_of_name(struct let class_name = "DIA" end) type state = { mutable lat : float; @@ -69,7 +69,7 @@ let send_msg = fun () -> "cam_roll", Pprz.Int state.cam_roll; "cam_pitch", Pprz.Int state.cam_pitch ] in - let s = Sub_Pprz.payload_of_values !ac_id (Sub_Pprz.class_id_of_msg "NAV_INFO") msg_id vs in + let s = Sub_Pprz.payload_of_values !ac_id msg_id vs in Debug.call 'l' (fun f -> fprintf f "sending: %s\n" (Debug.xprint (Serial.string_of_payload s))); Hdlc.write_data (Serial.string_of_payload s) diff --git a/sw/ground_segment/tmtc/diadec.ml b/sw/ground_segment/tmtc/diadec.ml index f492a191abf..8d981d1955b 100644 --- a/sw/ground_segment/tmtc/diadec.ml +++ b/sw/ground_segment/tmtc/diadec.ml @@ -26,7 +26,7 @@ open Printf -module Sub_Pprz = Pprz.Messages(struct let _type = "" and single_class = "DIA" end) +module Sub_Pprz = Pprz.Messages_of_name(struct let class_name = "DIA" end) module PprzTransport = Serial.Transport(Pprz.Transport) @@ -35,7 +35,7 @@ let use_tele_message = fun buf -> Debug.call 'l' (fun f -> fprintf f "pprz receiving: %s\n" (Debug.xprint buf)); try let (packet_seq, ac_id, class_id, msg_id, values) = Sub_Pprz.values_of_payload payload in - let msg = Sub_Pprz.message_of_id class_id msg_id in + let msg = Sub_Pprz.message_of_id msg_id in printf "%d %s\n%!" ac_id (Sub_Pprz.string_of_message msg values) with _ -> diff --git a/sw/ground_segment/tmtc/fw_server.ml b/sw/ground_segment/tmtc/fw_server.ml index e9faa8696d0..d3172814d35 100644 --- a/sw/ground_segment/tmtc/fw_server.ml +++ b/sw/ground_segment/tmtc/fw_server.ml @@ -30,7 +30,7 @@ open Aircraft open Latlong module LL = Latlong module U = Unix -module Dl_Pprz = Pprz.Messages (struct let _type = "uplink" and single_class = "" end) +module Dl_Pprz = Pprz.Messages_of_type (struct let class_type = "uplink" end) (* FIXME: bound the loop *) @@ -286,7 +286,7 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values -> a.nb_dl_setting_values <- max a.nb_dl_setting_values (i+1) end else failwith "Too much dl_setting values !!!" - | "WP_MOVED" -> + | "WP_MOVED_UTM" -> begin match a.nav_ref with Some Utm nav_ref -> diff --git a/sw/ground_segment/tmtc/ivy2udp.ml b/sw/ground_segment/tmtc/ivy2udp.ml index f7a30c49d0f..f1940282a82 100644 --- a/sw/ground_segment/tmtc/ivy2udp.ml +++ b/sw/ground_segment/tmtc/ivy2udp.ml @@ -26,8 +26,8 @@ let my_id = 0 -module Tm_Pprz = Pprz.Messages(struct let _type = "donwlink" and single_class = "" end) -module Dl_Pprz = Pprz.Messages(struct let _type = "uplink" and single_class = "" end) +module Tm_Pprz = Pprz.Messages_of_type(struct let class_type = "donwlink" end) +module Dl_Pprz = Pprz.Messages_of_type(struct let class_type = "uplink" end) module PprzTransport = Serial.Transport(Pprz.Transport) open Printf @@ -61,7 +61,7 @@ let () = let get_ivy_message = fun _ args -> try let (msg_id, vs) = Tm_Pprz.values_of_string args.(0) in - let payload = Tm_Pprz.payload_of_values (int_of_string !id) (Tm_Pprz.class_id_of_msg_args args.(0)) msg_id vs in + let payload = Tm_Pprz.payload_of_values (int_of_string !id) (Pprz.class_id_of_msg_args args.(0)) msg_id vs in let buf = Pprz.Transport.packet payload in let n = String.length buf in let n' = Unix.sendto socket buf 0 n [] sockaddr in diff --git a/sw/ground_segment/tmtc/ivy_tcp_aircraft.ml b/sw/ground_segment/tmtc/ivy_tcp_aircraft.ml index 2762fe15b83..415219cca13 100644 --- a/sw/ground_segment/tmtc/ivy_tcp_aircraft.ml +++ b/sw/ground_segment/tmtc/ivy_tcp_aircraft.ml @@ -1,6 +1,6 @@ let my_id = 0 -module Tm_Pprz = Pprz.Messages(struct let _type = "donwlink" and single_class = "" end) -module Dl_Pprz = Pprz.Messages(struct let _type = "uplink" and single_class = "" end) +module Tm_Pprz = Pprz.Messages_of_type(struct let class_type = "donwlink" end) +module Dl_Pprz = Pprz.Messages_of_type(struct let class_type = "uplink" end) module PprzTransport = Serial.Transport(Pprz.Transport) open Printf @@ -32,7 +32,7 @@ let () = let get_ivy_message = fun _ args -> try let (msg_id, vs) = Tm_Pprz.values_of_string args.(0) in - let payload = Tm_Pprz.payload_of_values (int_of_string !id) (Tm_Pprz.class_id_of_msg_args args.(0)) msg_id vs in + let payload = Tm_Pprz.payload_of_values (int_of_string !id) (Pprz.class_id_of_msg_args args.(0)) msg_id vs in let buf = Pprz.Transport.packet payload in fprintf o "%s%!" buf with _ -> () in diff --git a/sw/ground_segment/tmtc/ivy_tcp_controller.ml b/sw/ground_segment/tmtc/ivy_tcp_controller.ml index fc3ac1334f9..793c572fd7d 100644 --- a/sw/ground_segment/tmtc/ivy_tcp_controller.ml +++ b/sw/ground_segment/tmtc/ivy_tcp_controller.ml @@ -1,7 +1,7 @@ open Printf -module Tm_Pprz = Pprz.Messages(struct let _type = "donwlink" and single_class = "" end) -module Dl_Pprz = Pprz.Messages(struct let _type = "uplink" and single_class = "" end) +module Tm_Pprz = Pprz.Messages_of_type(struct let class_type = "donwlink" end) +module Dl_Pprz = Pprz.Messages_of_type(struct let class_type = "uplink" end) module PprzTransport = Serial.Transport(Pprz.Transport) let () = @@ -57,7 +57,7 @@ let () = try let (msg_id, vs) = Dl_Pprz.values_of_string args.(0) in let ac_id = Pprz.int_assoc "ac_id" vs in - let payload = Dl_Pprz.payload_of_values ac_id (Dl_Pprz.class_id_of_msg_args args.(0)) msg_id vs in + let payload = Dl_Pprz.payload_of_values ac_id (Pprz.class_id_of_msg_args args.(0)) msg_id vs in let buf = Pprz.Transport.packet payload in fprintf o "%s%!" buf with exc -> prerr_endline (Printexc.to_string exc) in diff --git a/sw/ground_segment/tmtc/link.ml b/sw/ground_segment/tmtc/link.ml index c2a6d35eda5..2939b261aa0 100644 --- a/sw/ground_segment/tmtc/link.ml +++ b/sw/ground_segment/tmtc/link.ml @@ -30,9 +30,9 @@ open Latlong open Printf (* Handlers for the modem and Ivy messages *) -module Tm_Pprz = Pprz.Messages (struct let _type = "downlink" and single_class = "" end) -module Ground_Pprz = Pprz.Messages (struct let _type = "ground" and single_class = "" end) -module Dl_Pprz = Pprz.Messages (struct let _type = "uplink" and single_class = "" end) +module Tm_Pprz = Pprz.Messages_of_type (struct let class_type = "downlink" end) +module Ground_Pprz = Pprz.Messages_of_type (struct let class_type = "ground" end) +module Dl_Pprz = Pprz.Messages_of_type (struct let class_type = "uplink" end) module PprzTransport = Serial.Transport (Pprz.Transport) module PprzTransportExtended = Serial.Transport (Pprz.TransportExtended) @@ -428,7 +428,7 @@ let message_uplink = fun device -> let forwarder = fun name _sender vs -> Debug.call 'f' (fun f -> fprintf f "forward %s\n" name); let ac_id = Pprz.int_assoc "ac_id" vs in - let class_id = Dl_Pprz.class_id_of_msg name in + let class_id = Pprz.class_id_of_msg name in let msg_id, _ = Dl_Pprz.message_of_name name in let gen_packet_seq = get_up_packet_sequence ac_id in let s = Dl_Pprz.payload_of_values ~gen_packet_seq:gen_packet_seq my_id class_id msg_id vs in @@ -438,7 +438,7 @@ let message_uplink = fun device -> let broadcaster = fun name _sender vs -> Debug.call 'f' (fun f -> fprintf f "broadcast %s\n" name); - let class_id = Dl_Pprz.class_id_of_msg name in + let class_id = Pprz.class_id_of_msg name in let msg_id, _ = Dl_Pprz.message_of_name name in let payload = Dl_Pprz.payload_of_values ~gen_packet_seq:(get_up_packet_sequence 0) my_id class_id msg_id vs in broadcast device payload Low in @@ -457,7 +457,7 @@ let message_uplink = fun device -> let send_ping_msg = fun device -> Hashtbl.iter (fun ac_id status -> - let class_id = Dl_Pprz.class_id_of_msg "PING" in + let class_id = Pprz.class_id_of_msg "PING" in let msg_id, _ = Dl_Pprz.message_of_name "PING" in let s = Dl_Pprz.payload_of_values ~gen_packet_seq:(get_up_packet_sequence ac_id) my_id class_id msg_id [] in send ac_id device s High; diff --git a/sw/ground_segment/tmtc/messages.ml b/sw/ground_segment/tmtc/messages.ml index d2b99c8b093..2accaf3950d 100644 --- a/sw/ground_segment/tmtc/messages.ml +++ b/sw/ground_segment/tmtc/messages.ml @@ -156,7 +156,7 @@ let one_page = fun sender class_name (notebook:GPack.notebook) bind m -> let rec one_class = fun (notebook:GPack.notebook) (ident, xml_class, sender) -> let class_name = (Xml.attrib xml_class "name") in let messages = Xml.children xml_class in - let module P = Pprz.Messages (struct let _type = "" and single_class = class_name end) in + let module P = Pprz.Messages_of_name (struct let class_name = class_name end) in let senders = Hashtbl.create 5 in match sender with | Some "*" -> diff --git a/sw/ground_segment/tmtc/rotorcraft_server.ml b/sw/ground_segment/tmtc/rotorcraft_server.ml index bd1e17e58b5..346a6d58827 100644 --- a/sw/ground_segment/tmtc/rotorcraft_server.ml +++ b/sw/ground_segment/tmtc/rotorcraft_server.ml @@ -28,7 +28,7 @@ open Aircraft open Latlong module LL = Latlong module U = Unix -module Dl_Pprz = Pprz.Messages (struct let _type = "uplink" and single_class = "" end) +module Dl_Pprz = Pprz.Messages_of_type (struct let class_type = "uplink" end) let nav_ref_alt = ref 0. let nav_ref_hmsl = ref 0. diff --git a/sw/ground_segment/tmtc/server.ml b/sw/ground_segment/tmtc/server.ml index 8c0ed9c1a9b..27727a5afba 100644 --- a/sw/ground_segment/tmtc/server.ml +++ b/sw/ground_segment/tmtc/server.ml @@ -36,11 +36,11 @@ open Aircraft module U = Unix module LL = Latlong -module Ground = struct let _type = "ground" and single_class = "" end -module Ground_Pprz = Pprz.Messages(Ground) -module Tm_Pprz = Pprz.Messages (struct let _type = "downlink" and single_class = "" end) -module Alerts_Pprz = Pprz.Messages(struct let _type = "" and single_class = "alert" end) -module Dl_Pprz = Pprz.Messages (struct let _type = "uplink" and single_class = "" end) +module Ground = struct let class_type = "ground" end +module Ground_Pprz = Pprz.Messages_of_type(Ground) +module Tm_Pprz = Pprz.Messages_of_type (struct let class_type = "downlink" end) +module Alerts_Pprz = Pprz.Messages_of_name(struct let class_name = "alert" end) +module Dl_Pprz = Pprz.Messages_of_type (struct let class_type = "uplink" end) @@ -136,12 +136,13 @@ let log = fun ?timestamp logging ac_name msg_name values -> (** Callback for a message from a registered A/C *) let ac_msg = fun messages_xml logging ac_name ac -> - let module Tele_Pprz = Pprz.MessagesOfXml(struct let xml = messages_xml let _type="downlink" and single_class = "" end) in + (*let module Tele_Pprz = Pprz.MessagesOfXml(struct let xml = messages_xml let selection = "downlink" and mode = "type" end) in*) (* XGGDEBUG:DYNMOD: What is doing this? I NEED THE FULL MODULE TO USE MESSAGE_OF_ID *) + let module Tele_Pprz = Pprz.Messages_of_type (struct let class_type = "downlink" end) in fun ts m -> try let timestamp = try Some (float_of_string ts) with _ -> None in let (msg_id, values) = Tele_Pprz.values_of_string m in - let cls_id = Tele_Pprz.class_id_of_msg_args m in + let cls_id = Pprz.class_id_of_msg_args m in let msg = Tele_Pprz.message_of_id cls_id msg_id in log ?timestamp logging ac_name msg.Pprz.name values; Fw_server.log_and_parse ac_name ac msg values; @@ -679,7 +680,7 @@ let raw_datalink = fun logging _sender vs -> let ac_id = Pprz.string_assoc "ac_id" vs and m = Pprz.string_assoc "message" vs in let msg_id, vs = Dl_Pprz.values_of_string_unsorted m in - let cls_id = Dl_Pprz.class_id_of_msg_args_unsorted m in + let cls_id = Pprz.class_id_of_msg_args_unsorted m in let msg = Dl_Pprz.message_of_id cls_id msg_id in Dl_Pprz.message_send dl_id msg.Pprz.name vs; log logging ac_id msg.Pprz.name vs diff --git a/sw/ground_segment/tmtc/settings.ml b/sw/ground_segment/tmtc/settings.ml index 2abe47a55c1..407a5081ac1 100644 --- a/sw/ground_segment/tmtc/settings.ml +++ b/sw/ground_segment/tmtc/settings.ml @@ -25,8 +25,8 @@ *) open Printf -module Ground_Pprz = Pprz.Messages(struct let _type = "ground" and single_class = "" end) -module Tele_Pprz = Pprz.Messages(struct let _type = "downlink" and single_class = ""end) +module Ground_Pprz = Pprz.Messages_of_type(struct let class_type = "ground" end) +module Tele_Pprz = Pprz.Messages_of_type(struct let class_type = "downlink" end) let (//) = Filename.concat let conf_dir = Env.paparazzi_home // "conf" diff --git a/sw/lib/ocaml/papget.ml b/sw/lib/ocaml/papget.ml index 0a08ee1b19a..0f15b3175d3 100644 --- a/sw/lib/ocaml/papget.ml +++ b/sw/lib/ocaml/papget.ml @@ -56,7 +56,7 @@ let base_and_index = (field_descr, 0) -class message_field = fun ?sender ?(class_name="telemetry") msg_name field_descr -> +class message_field = fun ?sender ?(class_name="telemetry") msg_name field_descr -> (* XGGDEBUG:DYNMOD: if there's a change of 'telemetry' to 'downlink', change here the class_name *) object val mutable callbacks = [] val mutable last_value = "0." @@ -70,7 +70,7 @@ class message_field = fun ?sender ?(class_name="telemetry") msg_name field_descr method type_ = "message_field" initializer - let module P = Pprz.Messages (struct let _type = "" and single_class = class_name end) in + let module P = Pprz.Messages_of_name (struct let class_name = class_name end) in let process_message = fun _sender values -> let (field_name, index) = base_and_index field_descr in let value = diff --git a/sw/lib/ocaml/pprz.ml b/sw/lib/ocaml/pprz.ml index 757151dae80..24a6e258fd9 100644 --- a/sw/lib/ocaml/pprz.ml +++ b/sw/lib/ocaml/pprz.ml @@ -469,13 +469,16 @@ let offset_fields = 4 module type CLASS_Xml = sig val xml : Xml.xml - val _type : string - val single_class : string + val selection : string + val mode : string end -module type CLASS = sig - val _type : string - val single_class : string +module type CLASS_NAME = sig + val class_name : string +end + +module type CLASS_TYPE = sig + val class_type : string end type msg_and_class_id = { @@ -483,19 +486,91 @@ type msg_and_class_id = { cls_id : int; } -module type MESSAGES = sig +let msg_xml = Xml.parse_file messages_file + +let space = Str.regexp "[ \t]+" +let semicolon = Str.regexp "[;\t]+" +let coma = Str.regexp "[,\t]+" + +let is_message_inside = fun pattern message -> + let msg_name = Xml.attrib message "name" in + if msg_name = pattern then true else false + +let get_id_and_messages = fun msg clas -> + let id = ExtXml.attrib clas "id" + and messages = Xml.children clas in + let contains = List.map (is_message_inside msg) messages in + let is_inside = List.mem true contains in + if is_inside = true then int_of_string(id) else -1 + +exception Msg_Duplicated of string +exception Msg_Not_found of string + +let class_id_of_msg = fun msg -> + try + let classes = Xml.children msg_xml in + let classes_with = List.map (get_id_and_messages msg) classes in + let result = List.filter (fun x -> if(x>(-1))then true else false) classes_with in + match result with + | [] -> raise (Msg_Not_found msg) + | [x] -> x + | _ -> raise (Msg_Duplicated msg) +with + | Msg_Duplicated s -> failwith (sprintf "No class containing message %s" s) + | Msg_Not_found s -> failwith (sprintf "More than one class containing message %s" s) + | Not_found -> failwith (sprintf "No class attribute found for message '%s'" msg) + | e -> failwith (sprintf "Unhandled exception (Exception: %s)" (Printexc.to_string e)) + + let class_id_of_msg_args = fun s -> + match Str.split space s with + | [] -> failwith (sprintf "Cannot obtain msg_name from arguments string '%s'" s) + | msg_name::args -> class_id_of_msg msg_name + +(** For raw_datalink messages *) +let class_id_of_msg_args_unsorted = fun s -> +match Str.split semicolon s with + | [] -> failwith (sprintf "Cannot obtain msg_name from unsorted arguments string '%s'" s) + | msg_name::args -> class_id_of_msg msg_name + +module type MESSAGES = sig val messages : (msg_and_class_id, message) Hashtbl.t - val message_of_id : class_id -> message_id -> message val message_of_name : string -> message_id * message - val class_id_of_msg : message_name -> class_id - (** [class_id_of_msg msg_name] returns the class id containing the given message *) + val values_of_payload : Serial.payload -> packet_seq * sender_id * class_id * message_id * values + (** [values_of_bin payload] Parses a raw payload, returns the + the A/C id, class id, message id and the list of (field_name, value) *) + + val values_of_string : string -> message_id * values + (** May raise [(Unknown_msg_name msg_name)] *) + + val values_of_string_unsorted : string -> message_id * values + (** May raise [(Unknown_msg_name msg_name)] *) + + val string_of_message : ?sep:string -> message -> values -> string + (** [string_of_message ?sep msg values] Default [sep] is space *) - val class_id_of_msg_args : string -> class_id - (** [class_id_of_msg_args args.(0)] returns the class id containing the given message when string with spaces is the parameter *) + val message_send : ?timestamp:float -> string -> string -> values -> unit + (** [message_send sender msg_name values] *) - val class_id_of_msg_args_unsorted : string -> class_id - (** [class_id_of_msg_args_unsorted args.(0)] returns the class id containing the given message when string with semicolons is the parameter *) + val message_bind : ?sender:string ->string -> (string -> values -> unit) -> Ivy.binding + (** [message_bind ?sender msg_name callback] *) + + val message_answerer : string -> string -> (string -> values -> values) -> Ivy.binding + (** [message_answerer sender msg_name callback] Set a handler for a + [message_req] (which will send a [msg_name]_REQ message). + [callback asker args] must return the list of attributes of the answer. *) + + val message_req : string -> string -> values -> (string -> values -> unit) -> unit + (** [message_req sender msg_name values receiver] Sends a request on the Ivy + bus for the specified message. A [msg_name]_REQ message is send and a + [msg_name] message is expected for the reply. On reception, [receiver] + will be applied on [sender_name] and attribute values of the values. *) +end + +module type MESSAGES_TYPE = sig + val messages : (msg_and_class_id, message) Hashtbl.t + val message_of_id : class_id -> message_id -> message + val message_of_name : string -> message_id * message val values_of_payload : Serial.payload -> packet_seq * sender_id * class_id * message_id * values (** [values_of_bin payload] Parses a raw payload, returns the @@ -526,11 +601,42 @@ module type MESSAGES = sig (** [message_answerer sender msg_name values receiver] Sends a request on the Ivy bus for the specified message. On reception, [receiver] will be applied on [sender_name] and expected values. *) end +module type MESSAGES_NAME = sig + val messages : (msg_and_class_id, message) Hashtbl.t + val message_of_id : message_id -> message + val message_of_name : string -> message_id * message + + val values_of_payload : Serial.payload -> packet_seq * sender_id * class_id * message_id * values + (** [values_of_bin payload] Parses a raw payload, returns the + the Sender id, class id, message id and the list of (field_name, value) *) + + val payload_of_values : ?gen_packet_seq:int -> sender_id -> message_id -> values -> Serial.payload + (** [payload_of_values ?gen_packet_seq sender_id class_id id vs] Returns a payload *) + + val values_of_string : string -> message_id * values + (** May raise [(Unknown_msg_name msg_name)] *) + + val values_of_string_unsorted : string -> message_id * values + (** May raise [(Unknown_msg_name msg_name)] *) + + val string_of_message : ?sep:string -> message -> values -> string + (** [string_of_message ?sep msg values] Default [sep] is space *) + val message_send : ?timestamp:float -> string -> string -> values -> unit + (** [message_send sender msg_name values] *) + + val message_bind : ?sender:string ->string -> (string -> values -> unit) -> Ivy.binding + (** [message_bind ?sender msg_name callback] *) + + val message_answerer : string -> string -> (string -> values -> values) -> Ivy.binding + (** [message_answerer sender msg_name callback] *) + + val message_req : string -> string -> values -> (string -> values -> unit) -> unit + (** [message_answerer sender msg_name values receiver] Sends a request on the Ivy bus for the specified message. On reception, [receiver] will be applied on [sender_name] and expected values. *) +end module MessagesOfXml(Class:CLASS_Xml) = struct let max_length = 256 - let msg_xml = Xml.parse_file messages_file type _class = { class_id : int; @@ -545,10 +651,6 @@ module MessagesOfXml(Class:CLASS_Xml) = struct (** messages_by_name: Hastbl{(message_id,class_id),message} *) let messages = Hashtbl.create 256 - let space = Str.regexp "[ \t]+" - let semicolon = Str.regexp "[;\t]+" - let coma = Str.regexp "[,\t]+" - let parse_class = fun struc -> let xml_class = struc.class_xml in let by_id = Hashtbl.create 13 in @@ -577,39 +679,39 @@ module MessagesOfXml(Class:CLASS_Xml) = struct let messages_loaded = try - match Class._type,Class.single_class with - | _,"" -> + match Class.mode with + | "type" -> let class_structs = List.map (fun _class -> {class_id = int_of_string (ExtXml.attrib _class "id") ; class_name = ExtXml.attrib _class "name" ; class_type = ExtXml.attrib _class "type" ; class_xml = _class } ) (Xml.children msg_xml) in let selected_classes = ref [] in ignore (List.map (fun struc -> match struc.class_type with - | "datalink" -> if (Class._type = "uplink"||Class._type = "downlink") then selected_classes := List.append !selected_classes [struc]; - | other_type -> if Class._type = other_type then selected_classes := List.append !selected_classes [struc]; + | "datalink" -> if (Class.selection = "uplink"||Class.selection = "downlink") then selected_classes := List.append !selected_classes [struc]; + | other_type -> if Class.selection = other_type then selected_classes := List.append !selected_classes [struc]; ) class_structs); ignore (List.map (fun sel -> let by_id = parse_class sel in Hashtbl.add classes_by_id sel.class_id by_id ) !selected_classes); true - | "",_ -> + | "name" -> let class_structs = List.map (fun _class -> {class_id = int_of_string (ExtXml.attrib _class "id") ; class_name = ExtXml.attrib _class "name" ; class_type = ExtXml.attrib _class "type" ; class_xml = _class } ) (Xml.children msg_xml) in let selected_classes = ref [] in - ignore (List.map (fun struc -> if struc.class_name = Class.single_class then selected_classes := List.append !selected_classes [struc]; + ignore (List.map (fun struc -> if struc.class_name = Class.selection then selected_classes := List.append !selected_classes [struc]; ) class_structs); ignore (List.map (fun sel -> let by_id = parse_class sel in Hashtbl.add classes_by_id sel.class_id by_id ) !selected_classes); true - | _,_ -> failwith ("Pprz->messages: cannot initialize with both _type and single_class ") + | _ -> failwith ("Pprz->messages: cannot initialize with mode different than 'name' or 'type' ") with | Not_found -> failwith ("Pprz->messages: error loading classes and messages (Not_found) ") | e -> failwith (sprintf "Pprz->messages: error loading classes and messages (Exception: %s) " (Printexc.to_string e)) - let message_of_id = fun class_id id -> + let message_of_id_ = fun class_id id -> try let sel_class = Hashtbl.find classes_by_id class_id in Hashtbl.find sel_class id @@ -620,47 +722,7 @@ module MessagesOfXml(Class:CLASS_Xml) = struct try Hashtbl.find messages_by_name name with - Not_found -> raise (Unknown_msg_name (name, Class._type)) - - let is_message_inside = fun pattern message -> - let msg_name = Xml.attrib message "name" in - if msg_name = pattern then true else false - - let get_id_and_messages = fun msg clas -> - let id = ExtXml.attrib clas "id" - and messages = Xml.children clas in - let contains = List.map (is_message_inside msg) messages in - let is_inside = List.mem true contains in - if is_inside = true then int_of_string(id) else -1 - - exception Msg_Duplicated of string - exception Msg_Not_found of string - - let class_id_of_msg = fun msg -> - try - let classes = Xml.children msg_xml in - let classes_with = List.map (get_id_and_messages msg) classes in - let result = List.filter (fun x -> if(x>(-1))then true else false) classes_with in - match result with - | [] -> raise (Msg_Not_found msg) - | [x] -> x - | _ -> raise (Msg_Duplicated msg) - with - | Msg_Duplicated s -> failwith (sprintf "No class containing message %s" s) - | Msg_Not_found s -> failwith (sprintf "More than one class containing message %s" s) - | Not_found -> failwith (sprintf "No class attribute found for message '%s'" msg) - | e -> failwith (sprintf "Unhandled exception (Exception: %s)" (Printexc.to_string e)) - - let class_id_of_msg_args = fun s -> - match Str.split space s with - | [] -> failwith (sprintf "Cannot obtain msg_name from arguments string '%s'" s) - | msg_name::args -> class_id_of_msg msg_name - - (** For raw_datalink messages *) - let class_id_of_msg_args_unsorted = fun s -> - match Str.split semicolon s with - | [] -> failwith (sprintf "Cannot obtain msg_name from unsorted arguments string '%s'" s) - | msg_name::args -> class_id_of_msg msg_name + Not_found -> raise (Unknown_msg_name (name, Class.selection)) let values_of_payload = fun buffer -> let buffer = Serial.string_of_payload buffer in @@ -669,7 +731,7 @@ module MessagesOfXml(Class:CLASS_Xml) = struct let sender_id = Char.code buffer.[offset_sender_id] in let packet_seq = Char.code buffer.[offset_packet_seq] in let class_id = Char.code buffer.[offset_class_id] in - let message = message_of_id class_id id in + let message = message_of_id_ class_id id in Debug.call 'T' (fun f -> fprintf f "Pprz.values id=%d\n" id); let rec loop = fun index fields -> match fields with @@ -686,8 +748,8 @@ module MessagesOfXml(Class:CLASS_Xml) = struct Invalid_argument("index out of bounds") -> failwith (sprintf "Pprz.values_of_payload, wrong argument: %s" (Debug.xprint buffer)) - let payload_of_values = fun ?gen_packet_seq sender_id class_id id values -> - let message = message_of_id class_id id in + let payload_of_values_ = fun ?gen_packet_seq sender_id class_id id values -> + let message = message_of_id_ class_id id in (** The actual length is computed from the values *) let p = String.make max_length '#' in @@ -823,10 +885,40 @@ module MessagesOfXml(Class:CLASS_Xml) = struct end -module Messages(Class:CLASS) = struct +module Messages_of_type(Class:CLASS_TYPE) = struct include MessagesOfXml(struct let xml = messages_xml () - let _type = Class._type - let single_class = Class.single_class + let selection = Class.class_type + let mode = "type" end) + let message_of_id = fun class_id message_id -> message_of_id_ class_id message_id + let payload_of_values = fun ?gen_packet_seq sender_id class_id id values -> payload_of_values_ ?gen_packet_seq sender_id class_id id values end + +type class_id_name = { + class_id : int; + class_name : string; +} +let class_id_of_name = fun name -> + try + let messages_xml = Xml.parse_file messages_file in + let classes = List.map (fun i_class -> {class_id = int_of_string (ExtXml.attrib i_class "id") ; class_name = ExtXml.attrib i_class "name"}) (Xml.children messages_xml) in + let sel_class = List.find (fun x -> if x.class_name = name then true else false ) classes in + sel_class.class_id + with + | Not_found -> failwith (sprintf "Pprz.Messages_of_name initialization error. No class with name %s" name) + +module Messages_of_name(Class:CLASS_NAME) = struct + include MessagesOfXml(struct + let xml = messages_xml () + let selection = Class.class_name + let mode = "name" + end) + let sel_class_id = class_id_of_name Class.class_name + let message_of_id = fun message_id -> message_of_id_ sel_class_id message_id + let payload_of_values = fun ?gen_packet_seq sender_id id values -> payload_of_values_ ?gen_packet_seq sender_id sel_class_id id values +end + + + + diff --git a/sw/lib/ocaml/pprz.mli b/sw/lib/ocaml/pprz.mli index 1c5e24e1126..404f620a74b 100644 --- a/sw/lib/ocaml/pprz.mli +++ b/sw/lib/ocaml/pprz.mli @@ -138,15 +138,18 @@ module TransportExtended : Serial.PROTOCOL val offset_fields : int -module type CLASS = sig - val _type : string - val single_class : string +module type CLASS_NAME = sig + val class_name : string +end + +module type CLASS_TYPE = sig + val class_type : string end module type CLASS_Xml = sig val xml : Xml.xml - val _type : string - val single_class : string + val selection : string + val mode : string end type msg_and_class_id = { @@ -154,19 +157,54 @@ type msg_and_class_id = { cls_id : int; } +val class_id_of_msg : message_name -> class_id +(** [class_id_of_msg msg_name] returns the class id containing the given message *) + +val class_id_of_msg_args : string -> class_id +(** [class_id_of_msg_args args.(0)] returns the class id containing the given message when args.(0) is the parameter *) + +val class_id_of_msg_args_unsorted : string -> class_id +(** [class_id_of_msg_args_unsorted args.(0)] returns the class id containing the given message when string with semicolons is the parameter *) + module type MESSAGES = sig val messages : (msg_and_class_id, message) Hashtbl.t - val message_of_id : class_id -> message_id -> message val message_of_name : string -> message_id * message - - val class_id_of_msg : message_name -> class_id - (** [class_id_of_msg msg_name] returns the class id containing the given message *) - val class_id_of_msg_args : string -> class_id - (** [class_id_of_msg_args args.(0)] returns the class id containing the given message when args.(0) is the parameter *) + val values_of_payload : Serial.payload -> packet_seq * sender_id * class_id * message_id * values + (** [values_of_bin payload] Parses a raw payload, returns the + the A/C id, class id, message id and the list of (field_name, value) *) + + val values_of_string : string -> message_id * values + (** May raise [(Unknown_msg_name msg_name)] *) + + val values_of_string_unsorted : string -> message_id * values + (** May raise [(Unknown_msg_name msg_name)] *) + + val string_of_message : ?sep:string -> message -> values -> string + (** [string_of_message ?sep msg values] Default [sep] is space *) + + val message_send : ?timestamp:float -> string -> string -> values -> unit + (** [message_send sender msg_name values] *) + + val message_bind : ?sender:string ->string -> (string -> values -> unit) -> Ivy.binding + (** [message_bind ?sender msg_name callback] *) + + val message_answerer : string -> string -> (string -> values -> values) -> Ivy.binding + (** [message_answerer sender msg_name callback] Set a handler for a + [message_req] (which will send a [msg_name]_REQ message). + [callback asker args] must return the list of attributes of the answer. *) + + val message_req : string -> string -> values -> (string -> values -> unit) -> unit + (** [message_req sender msg_name values receiver] Sends a request on the Ivy + bus for the specified message. A [msg_name]_REQ message is send and a + [msg_name] message is expected for the reply. On reception, [receiver] + will be applied on [sender_name] and attribute values of the values. *) +end - val class_id_of_msg_args_unsorted : string -> class_id - (** [class_id_of_msg_args_unsorted args.(0)] returns the class id containing the given message when string with semicolons is the parameter *) +module type MESSAGES_TYPE = sig + val messages : (msg_and_class_id, message) Hashtbl.t + val message_of_id : class_id -> message_id -> message + val message_of_name : string -> message_id * message val values_of_payload : Serial.payload -> packet_seq * sender_id * class_id * message_id * values (** [values_of_bin payload] Parses a raw payload, returns the @@ -202,5 +240,41 @@ module type MESSAGES = sig will be applied on [sender_name] and attribute values of the values. *) end -module Messages : functor (Class : CLASS) -> MESSAGES +module type MESSAGES_NAME = sig + val messages : (msg_and_class_id, message) Hashtbl.t + val message_of_id : message_id -> message + val message_of_name : string -> message_id * message + + val values_of_payload : Serial.payload -> packet_seq * sender_id * class_id * message_id * values + (** [values_of_bin payload] Parses a raw payload, returns the + the Sender id, class id, message id and the list of (field_name, value) *) + + val payload_of_values : ?gen_packet_seq:int -> sender_id -> message_id -> values -> Serial.payload + (** [payload_of_values ?gen_packet_seq sender_id class_id id vs] Returns a payload *) + + val values_of_string : string -> message_id * values + (** May raise [(Unknown_msg_name msg_name)] *) + + val values_of_string_unsorted : string -> message_id * values + (** May raise [(Unknown_msg_name msg_name)] *) + + val string_of_message : ?sep:string -> message -> values -> string + (** [string_of_message ?sep msg values] Default [sep] is space *) + + val message_send : ?timestamp:float -> string -> string -> values -> unit + (** [message_send sender msg_name values] *) + + val message_bind : ?sender:string ->string -> (string -> values -> unit) -> Ivy.binding + (** [message_bind ?sender msg_name callback] *) + + val message_answerer : string -> string -> (string -> values -> values) -> Ivy.binding + (** [message_answerer sender msg_name callback] *) + + val message_req : string -> string -> values -> (string -> values -> unit) -> unit + (** [message_answerer sender msg_name values receiver] Sends a request on the Ivy bus for the specified message. On reception, [receiver] will be applied on [sender_name] and expected values. *) +end + +module Messages_of_type : functor (Class : CLASS_TYPE) -> MESSAGES_TYPE +module Messages_of_name : functor (Class : CLASS_NAME) -> MESSAGES_NAME + module MessagesOfXml : functor (Class : CLASS_Xml) -> MESSAGES diff --git a/sw/logalizer/play_core.ml b/sw/logalizer/play_core.ml index 0b3fceab818..e55ae96affd 100644 --- a/sw/logalizer/play_core.ml +++ b/sw/logalizer/play_core.ml @@ -26,8 +26,8 @@ open Printf -module Ground_Pprz = Pprz.Messages(struct let _type = "ground" and single_class = "" end) -module Tm_Pprz = Pprz.Messages(struct let _type = "downlink" and single_class = "" end) +module Ground_Pprz = Pprz.Messages_of_type(struct let class_type = "ground" end) +module Tm_Pprz = Pprz.Messages_of_type(struct let class_type = "downlink" end) let (//) = Filename.concat let replay_dir = Env.paparazzi_home // "var" // "replay" @@ -149,7 +149,7 @@ let run = fun serial_port log adj i0 speed no_gui -> | Some channel -> try let msg_id, vs = Tm_Pprz.values_of_string m in - let payload = Tm_Pprz.payload_of_values (int_of_string ac) (Tm_Pprz.class_id_of_msg_args m) msg_id vs in + let payload = Tm_Pprz.payload_of_values (int_of_string ac) (Pprz.class_id_of_msg_args m) msg_id vs in let buf = Pprz.Transport.packet payload in Debug.call 'o' (fun f -> fprintf f "%s\n" (Debug.xprint buf)); fprintf channel "%s%!" buf diff --git a/sw/logalizer/plot.ml b/sw/logalizer/plot.ml index 62cebede754..360ce28718b 100644 --- a/sw/logalizer/plot.ml +++ b/sw/logalizer/plot.ml @@ -679,8 +679,9 @@ let load_log = fun ?export ?factor (plot:plot) (menubar:GMenu.menu_shell GMenu.f let class_type = "downlink" in Debug.call 'p' (fun f -> fprintf f "class_type: %s\n" class_type); - let module M = struct let _type = class_type and single_class = "" let xml = protocol end in - let module P = Pprz.MessagesOfXml(M) in + (*let module M = struct let selection = class_type and mode = "type" let xml = protocol end in *)(* XGGDEBUG:DYNMOD: Why only MessagesOfXml and not the full module? I need to use message_of_id and it's in the full module *) + (*let module P = Pprz.MessagesOfXml(M) in *) + let module P = Pprz.Messages_of_type (struct let class_type = class_type end) in let f = try @@ -709,7 +710,7 @@ let load_log = fun ?export ?factor (plot:plot) (menubar:GMenu.menu_shell GMenu.f (*Elements of [acs] are assoc lists of [fields] indexed by msg id*) let msg_id, vs = P.values_of_string m in - let cls_id = P.class_id_of_msg_args m in + let cls_id = Pprz.class_id_of_msg_args m in let ids_of_msg = { msg_id = msg_id; cls_id = cls_id} in if not (Hashtbl.mem msgs ids_of_msg) then diff --git a/sw/logalizer/plotter.ml b/sw/logalizer/plotter.ml index 13605f70ca2..ad3832eae37 100644 --- a/sw/logalizer/plotter.ml +++ b/sw/logalizer/plotter.ml @@ -497,7 +497,7 @@ let rec plot_window = fun window -> let v = float *. a +. b in plot#add_value name v in - let module P = Pprz.Messages (struct let _type = "" and single_class = class_name end) in + let module P = Pprz.Messages_of_name (struct let class_name = class_name end) in let binding = if sender = "*" then P.message_bind msg_name cb diff --git a/sw/logalizer/sd2log.ml b/sw/logalizer/sd2log.ml index 337f19c0e25..95c6492a33d 100644 --- a/sw/logalizer/sd2log.ml +++ b/sw/logalizer/sd2log.ml @@ -32,8 +32,8 @@ let logs_path = var_path // "logs" let conf_xml = Xml.parse_file (Env.paparazzi_home // "conf" // "conf.xml") -module Tm_Pprz = Pprz.Messages (struct let _type = "downlink" and single_class = "" end) -module Dl_Pprz = Pprz.Messages (struct let _type = "uplink" and single_class = "" end) +module Tm_Pprz = Pprz.Messages_of_type (struct let class_type = "downlink" end) +module Dl_Pprz = Pprz.Messages_of_type (struct let class_type = "uplink" end) module Parser = Serial.Transport(Logpprz.Transport) diff --git a/sw/simulator/gaia.ml b/sw/simulator/gaia.ml index 7136601c42b..fca04063dcb 100644 --- a/sw/simulator/gaia.ml +++ b/sw/simulator/gaia.ml @@ -30,7 +30,7 @@ open Latlong let my_id = "gaia" let sending_period = 5000 (* ms *) -module Ground_Pprz = Pprz.Messages(struct let _type = "ground" and single_class = "" end) +module Ground_Pprz = Pprz.Messages_of_type(struct let class_type = "ground" end) let ivy_bus = ref Defivybus.default_ivy_bus diff --git a/sw/simulator/hitl.ml b/sw/simulator/hitl.ml index 756d87a3f57..4cdc0e78343 100644 --- a/sw/simulator/hitl.ml +++ b/sw/simulator/hitl.ml @@ -29,9 +29,9 @@ open Stdlib module LL = Latlong open LL -module TelePprz = Pprz.Messages(struct let _type = "downlink" and single_class = "" end) -module DatalinkPprz = Pprz.Messages(struct let _type = "uplink" and single_class = "" end) -module GroundPprz = Pprz.Messages(struct let _type = "ground" and single_class = "" end) +module TelePprz = Pprz.Messages_of_type(struct let class_type = "downlink" end) +module DatalinkPprz = Pprz.Messages_of_type(struct let class_type = "uplink" end) +module GroundPprz = Pprz.Messages_of_type(struct let class_type = "ground" end) module Make (A:Data.MISSION) (FM: FlightModel.SIG) = struct let my_id = ref (-1) diff --git a/sw/simulator/sim.ml b/sw/simulator/sim.ml index 5abfc153800..4cadf776072 100644 --- a/sw/simulator/sim.ml +++ b/sw/simulator/sim.ml @@ -28,7 +28,7 @@ open Printf open Stdlib open Latlong -module Ground_Pprz = Pprz.Messages(struct let _type = "ground" and single_class = "" end) +module Ground_Pprz = Pprz.Messages_of_type(struct let class_type = "ground" end) let float_attrib xml a = float_of_string (ExtXml.attrib xml a) diff --git a/sw/simulator/sitl.ml b/sw/simulator/sitl.ml index 148a97dba5a..76b9ae29a16 100644 --- a/sw/simulator/sitl.ml +++ b/sw/simulator/sitl.ml @@ -26,8 +26,8 @@ open Printf -module Ground_Pprz = Pprz.Messages(struct let _type = "ground" and single_class = "" end) -module Dl_Pprz = Pprz.Messages(struct let _type = "uplink" and single_class = "" end) +module Ground_Pprz = Pprz.Messages_of_type(struct let class_type = "ground" end) +module Dl_Pprz = Pprz.Messages_of_type(struct let class_type = "uplink" end) let ground_id = 0 (* cf tmtc/link.ml *) @@ -167,7 +167,7 @@ module Make (A:Data.MISSION) (FM: FlightModel.SIG) = struct let get_message = fun name link_mode _sender vs -> let set = fun () -> let msg_id, _ = Dl_Pprz.message_of_name name in - let s = Dl_Pprz.payload_of_values ground_id (Dl_Pprz.class_id_of_msg name) msg_id vs in + let s = Dl_Pprz.payload_of_values ground_id (Pprz.class_id_of_msg name) msg_id vs in set_message (Serial.string_of_payload s) in let ac_id = Pprz.int_assoc "ac_id" vs in match link_mode with