From 7709dcfa2ed789cd9f04494fb08e3ace697b9a72 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Fri, 11 Mar 2022 15:48:51 +0100 Subject: [PATCH] [datalink] move all hardcoded message parsing to modules datalink events (#2838) --- conf/modules/air_data.xml | 1 + conf/modules/autopilot_gnc.xml | 6 +- conf/modules/autopilot_guided.xml | 22 +++ conf/modules/datalink_common.xml | 4 +- conf/modules/gps.xml | 2 + conf/modules/gps_ublox.xml | 3 +- conf/modules/nav_basic_fw.xml | 2 + conf/modules/nav_basic_rotorcraft.xml | 2 + conf/modules/nav_rover_base.xml | 2 + conf/modules/radio_control_datalink.xml | 2 + conf/modules/settings.xml | 2 + .../firmwares/fixedwing/fixedwing_datalink.c | 137 ------------------ sw/airborne/firmwares/fixedwing/nav.c | 28 ++++ sw/airborne/firmwares/fixedwing/nav.h | 4 + .../firmwares/rotorcraft/autopilot_guided.c | 14 ++ .../firmwares/rotorcraft/autopilot_guided.h | 4 + sw/airborne/firmwares/rotorcraft/navigation.c | 24 +++ sw/airborne/firmwares/rotorcraft/navigation.h | 2 + .../rotorcraft/rotorcraft_datalink.c | 90 ------------ sw/airborne/firmwares/rover/navigation.c | 23 +++ sw/airborne/firmwares/rover/navigation.h | 2 + sw/airborne/firmwares/rover/rover_datalink.c | 74 ---------- sw/airborne/modules/air_data/air_data.c | 30 ++++ sw/airborne/modules/air_data/air_data.h | 6 + sw/airborne/modules/core/settings.c | 49 +++++++ sw/airborne/modules/core/settings.h | 4 + sw/airborne/modules/datalink/datalink.c | 127 ++-------------- sw/airborne/modules/datalink/datalink.h | 5 +- sw/airborne/modules/gps/gps.c | 21 +++ sw/airborne/modules/gps/gps.h | 3 + sw/airborne/modules/gps/gps_ubx.c | 17 +++ sw/airborne/modules/gps/gps_ubx.h | 1 + .../modules/radio_control/rc_datalink.c | 23 +++ .../modules/radio_control/rc_datalink.h | 3 + 34 files changed, 312 insertions(+), 427 deletions(-) create mode 100644 conf/modules/autopilot_guided.xml delete mode 100644 sw/airborne/firmwares/fixedwing/fixedwing_datalink.c delete mode 100644 sw/airborne/firmwares/rotorcraft/rotorcraft_datalink.c delete mode 100644 sw/airborne/firmwares/rover/rover_datalink.c diff --git a/conf/modules/air_data.xml b/conf/modules/air_data.xml index 67adf2dd90..529f34a6a3 100644 --- a/conf/modules/air_data.xml +++ b/conf/modules/air_data.xml @@ -36,6 +36,7 @@ + diff --git a/conf/modules/autopilot_gnc.xml b/conf/modules/autopilot_gnc.xml index 56298af4b7..b325b194f3 100644 --- a/conf/modules/autopilot_gnc.xml +++ b/conf/modules/autopilot_gnc.xml @@ -7,7 +7,7 @@ - system_core,@guidance,@navigation,@commands + system_core,@guidance,@navigation,@commands,autopilot_guided autopilot
@@ -16,6 +16,7 @@ + @@ -24,8 +25,5 @@ - - - diff --git a/conf/modules/autopilot_guided.xml b/conf/modules/autopilot_guided.xml new file mode 100644 index 0000000000..40703a281a --- /dev/null +++ b/conf/modules/autopilot_guided.xml @@ -0,0 +1,22 @@ + + + + + + Guided mode interface for autpilote + + only rotorcraft for now + + + + @guidance + +
+ +
+ + + + + + diff --git a/conf/modules/datalink_common.xml b/conf/modules/datalink_common.xml index 9010d4db1d..792491c190 100644 --- a/conf/modules/datalink_common.xml +++ b/conf/modules/datalink_common.xml @@ -14,6 +14,7 @@ + @@ -27,7 +28,6 @@ - @@ -39,7 +39,6 @@ - @@ -48,7 +47,6 @@ - diff --git a/conf/modules/gps.xml b/conf/modules/gps.xml index ad06b14c08..ce443c47e0 100644 --- a/conf/modules/gps.xml +++ b/conf/modules/gps.xml @@ -28,6 +28,8 @@
+ + diff --git a/conf/modules/gps_ublox.xml b/conf/modules/gps_ublox.xml index 41e51e8883..51438807e0 100644 --- a/conf/modules/gps_ublox.xml +++ b/conf/modules/gps_ublox.xml @@ -21,7 +21,8 @@ - + + diff --git a/conf/modules/nav_basic_fw.xml b/conf/modules/nav_basic_fw.xml index 1a0750ec46..c3e6f7e64c 100644 --- a/conf/modules/nav_basic_fw.xml +++ b/conf/modules/nav_basic_fw.xml @@ -32,6 +32,8 @@ + + diff --git a/conf/modules/nav_basic_rotorcraft.xml b/conf/modules/nav_basic_rotorcraft.xml index dda7a0b2f1..b68b1ec978 100644 --- a/conf/modules/nav_basic_rotorcraft.xml +++ b/conf/modules/nav_basic_rotorcraft.xml @@ -24,6 +24,8 @@ + + diff --git a/conf/modules/nav_rover_base.xml b/conf/modules/nav_rover_base.xml index d67d36d3ce..0e523f6cd1 100644 --- a/conf/modules/nav_rover_base.xml +++ b/conf/modules/nav_rover_base.xml @@ -23,6 +23,8 @@ + + diff --git a/conf/modules/radio_control_datalink.xml b/conf/modules/radio_control_datalink.xml index 0d9e708309..bcfcf2d6a6 100644 --- a/conf/modules/radio_control_datalink.xml +++ b/conf/modules/radio_control_datalink.xml @@ -16,6 +16,8 @@ + + diff --git a/conf/modules/settings.xml b/conf/modules/settings.xml index 947b38aed3..da5052ef5e 100644 --- a/conf/modules/settings.xml +++ b/conf/modules/settings.xml @@ -13,6 +13,8 @@ + + diff --git a/sw/airborne/firmwares/fixedwing/fixedwing_datalink.c b/sw/airborne/firmwares/fixedwing/fixedwing_datalink.c deleted file mode 100644 index f827996ce0..0000000000 --- a/sw/airborne/firmwares/fixedwing/fixedwing_datalink.c +++ /dev/null @@ -1,137 +0,0 @@ -/* - * Copyright (C) 2005 Pascal Brisset, Antoine Drouin - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - */ - -/** - * @file firmwares/fixedwing/fixedwing_datalink.c - * Handling of messages coming from ground and other A/Cs. - * - */ - -#include "modules/datalink/datalink.h" -#include "modules/datalink/downlink.h" - -#include "pprzlink/messages.h" -#include "pprzlink/dl_protocol.h" - -#if defined NAV || defined WIND_INFO -#include "state.h" -#endif - -#ifdef NAV -#include "firmwares/fixedwing/nav.h" -#include "modules/nav/common_nav.h" -#include "math/pprz_geodetic_float.h" -#endif - -#ifdef HITL -#include "modules/gps/gps.h" -#endif - -#define MOfMM(_x) (((float)(_x))/1000.) - -void firmware_parse_msg(struct link_device *dev __attribute__((unused)), struct transport_tx *trans __attribute__((unused)), uint8_t *buf) -{ - uint8_t msg_id = IdOfPprzMsg(buf); - - /* parse telemetry messages coming from ground station */ - switch (msg_id) { - -#ifdef NAV - case DL_BLOCK: { - if (DL_BLOCK_ac_id(buf) != AC_ID) { break; } - nav_goto_block(DL_BLOCK_block_id(buf)); - SEND_NAVIGATION(trans, dev); - } - break; - - case DL_MOVE_WP: { - if (DL_MOVE_WP_ac_id(buf) != AC_ID) { break; } - uint8_t wp_id = DL_MOVE_WP_wp_id(buf); - - /* Computes from (lat, long) in the referenced UTM zone */ - struct LlaCoor_f lla; - lla.lat = RadOfDeg((float)(DL_MOVE_WP_lat(buf) / 1e7)); - lla.lon = RadOfDeg((float)(DL_MOVE_WP_lon(buf) / 1e7)); - lla.alt = MOfMM(DL_MOVE_WP_alt(buf)); - struct UtmCoor_f utm; - utm.zone = nav_utm_zone0; - utm_of_lla_f(&utm, &lla); - nav_move_waypoint(wp_id, utm.east, utm.north, utm.alt); - - /* Waypoint range is limited. Computes the UTM pos back from the relative - coordinates */ - utm.east = waypoints[wp_id].x + nav_utm_east0; - utm.north = waypoints[wp_id].y + nav_utm_north0; - pprz_msg_send_WP_MOVED(trans, dev, AC_ID, &wp_id, &utm.east, &utm.north, &utm.alt, &nav_utm_zone0); - } - break; -#endif /** NAV */ - -#ifdef WIND_INFO - case DL_WIND_INFO: { - if (DL_WIND_INFO_ac_id(buf) != AC_ID) { break; } - uint8_t flags = DL_WIND_INFO_flags(buf); - struct FloatVect2 wind = { 0.f, 0.f }; - float upwind = 0.f; - if (bit_is_set(flags, 0)) { - wind.x = DL_WIND_INFO_north(buf); - wind.y = DL_WIND_INFO_east(buf); - stateSetHorizontalWindspeed_f(&wind); - } - if (bit_is_set(flags, 1)) { - upwind = DL_WIND_INFO_up(buf); - stateSetVerticalWindspeed_f(upwind); - } -#if !USE_AIRSPEED - if (bit_is_set(flags, 2)) { - stateSetAirspeed_f(DL_WIND_INFO_airspeed(buf)); - } -#endif -#ifdef WIND_INFO_RET - float airspeed = stateGetAirspeed_f(); - pprz_msg_send_WIND_INFO_RET(trans, dev, AC_ID, &flags, &wind.y, &wind.x, &upwind, &airspeed); -#endif - } - break; -#endif /** WIND_INFO */ - -#ifdef HITL - /** GPS sensors are replaced by messages on the datalink */ - case DL_HITL_UBX: { - /** This code simulates gps_ubx.c:parse_ubx() */ - if (gps_msg_received) { - gps_nb_ovrn++; - } else { - ubx_class = DL_HITL_UBX_class(buf); - ubx_id = DL_HITL_UBX_id(buf); - uint8_t l = DL_HITL_UBX_ubx_payload_length(buf); - uint8_t *ubx_payload = DL_HITL_UBX_ubx_payload(buf); - memcpy(ubx_msg_buf, ubx_payload, l); - gps_msg_received = true; - } - } - break; -#endif /* HITL */ - - default: - break; - } -} diff --git a/sw/airborne/firmwares/fixedwing/nav.c b/sw/airborne/firmwares/fixedwing/nav.c index 5dc07cc33d..e2831275bf 100644 --- a/sw/airborne/firmwares/fixedwing/nav.c +++ b/sw/airborne/firmwares/fixedwing/nav.c @@ -584,6 +584,34 @@ void nav_without_gps(void) #endif } +void nav_parse_BLOCK(struct link_device *dev, struct transport_tx *trans, uint8_t *buf) +{ + if (DL_BLOCK_ac_id(buf) != AC_ID) { return; } + nav_goto_block(DL_BLOCK_block_id(buf)); + SEND_NAVIGATION(trans, dev); +} + +void nav_parse_MOVE_WP(struct link_device *dev, struct transport_tx *trans, uint8_t *buf) +{ + if (DL_MOVE_WP_ac_id(buf) != AC_ID) { return; } + uint8_t wp_id = DL_MOVE_WP_wp_id(buf); + + /* Computes from (lat, long) in the referenced UTM zone */ + struct LlaCoor_f lla; + lla.lat = RadOfDeg((float)(DL_MOVE_WP_lat(buf) / 1e7f)); + lla.lon = RadOfDeg((float)(DL_MOVE_WP_lon(buf) / 1e7f)); + lla.alt = (float)(DL_MOVE_WP_alt(buf) / 1000.f); // mm to m + struct UtmCoor_f utm; + utm.zone = nav_utm_zone0; + utm_of_lla_f(&utm, &lla); + nav_move_waypoint(wp_id, utm.east, utm.north, utm.alt); + + /* Waypoint range is limited. Computes the UTM pos back from the relative + coordinates */ + utm.east = waypoints[wp_id].x + nav_utm_east0; + utm.north = waypoints[wp_id].y + nav_utm_north0; + pprz_msg_send_WP_MOVED(trans, dev, AC_ID, &wp_id, &utm.east, &utm.north, &utm.alt, &nav_utm_zone0); +} /**************** 8 Navigation **********************************************/ diff --git a/sw/airborne/firmwares/fixedwing/nav.h b/sw/airborne/firmwares/fixedwing/nav.h index 436084bf3e..5a13375a3a 100644 --- a/sw/airborne/firmwares/fixedwing/nav.h +++ b/sw/airborne/firmwares/fixedwing/nav.h @@ -41,6 +41,8 @@ #include "modules/nav/common_flight_plan.h" #include "modules/nav/common_nav.h" #include "autopilot.h" +#include "pprzlink/pprzlink_device.h" +#include "pprzlink/pprzlink_transport.h" /** Default fixedwing navigation frequency */ #ifndef NAVIGATION_FREQUENCY @@ -124,6 +126,8 @@ extern void nav_periodic_task(void); extern void nav_home(void); extern void nav_init(void); extern void nav_without_gps(void); +extern void nav_parse_BLOCK(struct link_device *dev, struct transport_tx *trans, uint8_t *buf); +extern void nav_parse_MOVE_WP(struct link_device *dev, struct transport_tx *trans, uint8_t *buf); extern float nav_circle_trigo_qdr; /** Angle from center to mobile */ extern void nav_circle_XY(float x, float y, float radius); diff --git a/sw/airborne/firmwares/rotorcraft/autopilot_guided.c b/sw/airborne/firmwares/rotorcraft/autopilot_guided.c index 5d9475b1bb..0388b3f07e 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot_guided.c +++ b/sw/airborne/firmwares/rotorcraft/autopilot_guided.c @@ -30,6 +30,7 @@ #include "autopilot.h" #include "firmwares/rotorcraft/guidance.h" #include "state.h" +#include "pprzlink/dl_protocol.h" bool autopilot_guided_goto_ned(float x, float y, float z, float heading) @@ -151,3 +152,16 @@ void autopilot_guided_update(uint8_t flags, float x, float y, float z, float yaw } } +/** Parse GUIDED_SETPOINT_NED messages from datalink + */ +void autopilot_guided_parse_GUIDED(uint8_t *buf) { + if (DL_GUIDED_SETPOINT_NED_ac_id(buf) != AC_ID) { return; } + + autopilot_guided_update( + DL_GUIDED_SETPOINT_NED_flags(buf), + DL_GUIDED_SETPOINT_NED_x(buf), + DL_GUIDED_SETPOINT_NED_y(buf), + DL_GUIDED_SETPOINT_NED_z(buf), + DL_GUIDED_SETPOINT_NED_yaw(buf)); +} + diff --git a/sw/airborne/firmwares/rotorcraft/autopilot_guided.h b/sw/airborne/firmwares/rotorcraft/autopilot_guided.h index 0abf307648..8cfc77cefb 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot_guided.h +++ b/sw/airborne/firmwares/rotorcraft/autopilot_guided.h @@ -88,6 +88,10 @@ extern bool autopilot_guided_move_ned(float vx, float vy, float vz, float headin */ extern void autopilot_guided_update(uint8_t flags, float x, float y, float z, float yaw); +/** Parse GUIDED_SETPOINT_NED messages from datalink + */ +extern void autopilot_guided_parse_GUIDED(uint8_t *buf); + /** Bitmask for setting the flags attribute in autopilot_guided_update function * See function description for more details */ diff --git a/sw/airborne/firmwares/rotorcraft/navigation.c b/sw/airborne/firmwares/rotorcraft/navigation.c index 2412a538c8..9371a56fcf 100644 --- a/sw/airborne/firmwares/rotorcraft/navigation.c +++ b/sw/airborne/firmwares/rotorcraft/navigation.c @@ -224,6 +224,30 @@ void nav_init(void) auto_nav_init(); } +void nav_parse_BLOCK(uint8_t *buf) +{ + if (DL_BLOCK_ac_id(buf) != AC_ID) { return; } + nav_goto_block(DL_BLOCK_block_id(buf)); +} + +void nav_parse_MOVE_WP(uint8_t *buf) +{ + uint8_t ac_id = DL_MOVE_WP_ac_id(buf); + if (ac_id != AC_ID) { return; } + if (stateIsLocalCoordinateValid()) { + uint8_t wp_id = DL_MOVE_WP_wp_id(buf); + struct LlaCoor_i lla; + lla.lat = DL_MOVE_WP_lat(buf); + lla.lon = DL_MOVE_WP_lon(buf); + /* WP_alt from message is alt above MSL in mm + * lla.alt is above ellipsoid in mm + */ + lla.alt = DL_MOVE_WP_alt(buf) - state.ned_origin_i.hmsl + + state.ned_origin_i.lla.alt; + waypoint_move_lla(wp_id, &lla); + } +} + static inline void UNUSED nav_advance_carrot(void) { struct EnuCoor_i *pos = stateGetPositionEnu_i(); diff --git a/sw/airborne/firmwares/rotorcraft/navigation.h b/sw/airborne/firmwares/rotorcraft/navigation.h index 6bd5a7221a..49e26a812b 100644 --- a/sw/airborne/firmwares/rotorcraft/navigation.h +++ b/sw/airborne/firmwares/rotorcraft/navigation.h @@ -119,6 +119,8 @@ extern bool exception_flag[10]; extern void nav_init(void); extern void nav_run(void); +extern void nav_parse_BLOCK(uint8_t *buf); +extern void nav_parse_MOVE_WP(uint8_t *buf); extern void set_exception_flag(uint8_t flag_num); diff --git a/sw/airborne/firmwares/rotorcraft/rotorcraft_datalink.c b/sw/airborne/firmwares/rotorcraft/rotorcraft_datalink.c deleted file mode 100644 index e7b3e5a127..0000000000 --- a/sw/airborne/firmwares/rotorcraft/rotorcraft_datalink.c +++ /dev/null @@ -1,90 +0,0 @@ -/* - * Copyright (C) 2008-2009 Antoine Drouin - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - */ - -/** - * @file firmwares/rotorcraft/rotorcraft_datalink.c - * Handling of messages coming from ground and other A/Cs. - * - */ - -#include "modules/datalink/datalink.h" - -#include "pprzlink/dl_protocol.h" - -#ifdef USE_NAVIGATION -#include "firmwares/rotorcraft/navigation.h" -#include "math/pprz_geodetic_int.h" -#endif - -#include "autopilot.h" -#include "firmwares/rotorcraft/autopilot_guided.h" - -void firmware_parse_msg(struct link_device *dev __attribute__((unused)), struct transport_tx *trans __attribute__((unused)), uint8_t *buf) -{ - uint8_t msg_id = IdOfPprzMsg(buf); - - /* parse telemetry messages coming from ground station */ - switch (msg_id) { -#ifndef INTERMCU_FBW - -#ifdef USE_NAVIGATION - case DL_BLOCK : { - if (DL_BLOCK_ac_id(buf) != AC_ID) { break; } - nav_goto_block(DL_BLOCK_block_id(buf)); - } - break; - - case DL_MOVE_WP : { - uint8_t ac_id = DL_MOVE_WP_ac_id(buf); - if (ac_id != AC_ID) { break; } - if (stateIsLocalCoordinateValid()) { - uint8_t wp_id = DL_MOVE_WP_wp_id(buf); - struct LlaCoor_i lla; - lla.lat = DL_MOVE_WP_lat(buf); - lla.lon = DL_MOVE_WP_lon(buf); - /* WP_alt from message is alt above MSL in mm - * lla.alt is above ellipsoid in mm - */ - lla.alt = DL_MOVE_WP_alt(buf) - state.ned_origin_i.hmsl + - state.ned_origin_i.lla.alt; - waypoint_move_lla(wp_id, &lla); - } - } - break; -#endif /* USE_NAVIGATION */ - -#ifdef AP_MODE_GUIDED - case DL_GUIDED_SETPOINT_NED: - if (DL_GUIDED_SETPOINT_NED_ac_id(buf) != AC_ID) { break; } - - autopilot_guided_update(DL_GUIDED_SETPOINT_NED_flags(buf), - DL_GUIDED_SETPOINT_NED_x(buf), - DL_GUIDED_SETPOINT_NED_y(buf), - DL_GUIDED_SETPOINT_NED_z(buf), - DL_GUIDED_SETPOINT_NED_yaw(buf)); - break; -#endif - -#endif // INTERMCU_FBW - default: - break; - } -} diff --git a/sw/airborne/firmwares/rover/navigation.c b/sw/airborne/firmwares/rover/navigation.c index fa535f470e..ef36015c2b 100644 --- a/sw/airborne/firmwares/rover/navigation.c +++ b/sw/airborne/firmwares/rover/navigation.c @@ -112,6 +112,29 @@ void nav_run(void) VECT2_COPY(nav.carrot, nav.target); } +void nav_parse_BLOCK(uint8_t *buf) +{ + if (DL_BLOCK_ac_id(buf) != AC_ID) { return; } + nav_goto_block(DL_BLOCK_block_id(buf)); +} + +void nav_parse_MOVE_WP(uint8_t *buf) +{ + uint8_t ac_id = DL_MOVE_WP_ac_id(buf); + if (ac_id != AC_ID) { return; } + if (stateIsLocalCoordinateValid()) { + uint8_t wp_id = DL_MOVE_WP_wp_id(buf); + struct LlaCoor_i lla; + lla.lat = DL_MOVE_WP_lat(buf); + lla.lon = DL_MOVE_WP_lon(buf); + /* WP_alt from message is alt above MSL in mm + * lla.alt is above ellipsoid in mm + */ + lla.alt = DL_MOVE_WP_alt(buf) - state.ned_origin_i.hmsl + + state.ned_origin_i.lla.alt; + waypoint_move_lla(wp_id, &lla); + } +} bool nav_check_wp_time(struct EnuCoor_f *wp, float stay_time) { diff --git a/sw/airborne/firmwares/rover/navigation.h b/sw/airborne/firmwares/rover/navigation.h index d704509605..538f9474b9 100644 --- a/sw/airborne/firmwares/rover/navigation.h +++ b/sw/airborne/firmwares/rover/navigation.h @@ -162,6 +162,8 @@ extern uint8_t last_wp __attribute__((unused)); extern void nav_init(void); extern void nav_run(void); +extern void nav_parse_BLOCK(uint8_t *buf); +extern void nav_parse_MOVE_WP(uint8_t *buf); extern void set_exception_flag(uint8_t flag_num); diff --git a/sw/airborne/firmwares/rover/rover_datalink.c b/sw/airborne/firmwares/rover/rover_datalink.c deleted file mode 100644 index e0a6b67285..0000000000 --- a/sw/airborne/firmwares/rover/rover_datalink.c +++ /dev/null @@ -1,74 +0,0 @@ -/* - * Copyright (C) 2018 Gautier Hattenberger - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, see - * . - */ - -/** - * @file firmwares/rover/rover_datalink.c - * Handling of messages coming from ground and other A/Cs. - * - */ - -#include "modules/datalink/datalink.h" - -#include "pprzlink/dl_protocol.h" - -#ifdef USE_NAVIGATION -#include "firmwares/rover/navigation.h" -#include "math/pprz_geodetic_int.h" -#endif - -#include "autopilot.h" - -void firmware_parse_msg(struct link_device *dev __attribute__((unused)), struct transport_tx *trans __attribute__((unused)), uint8_t *buf) -{ - uint8_t msg_id = IdOfPprzMsg(buf); - - /* parse telemetry messages coming from ground station */ - switch (msg_id) { - -#ifdef USE_NAVIGATION - case DL_BLOCK : { - if (DL_BLOCK_ac_id(buf) != AC_ID) { break; } - nav_goto_block(DL_BLOCK_block_id(buf)); - } - break; - - case DL_MOVE_WP : { - uint8_t ac_id = DL_MOVE_WP_ac_id(buf); - if (ac_id != AC_ID) { break; } - if (stateIsLocalCoordinateValid()) { - uint8_t wp_id = DL_MOVE_WP_wp_id(buf); - struct LlaCoor_i lla; - lla.lat = DL_MOVE_WP_lat(buf); - lla.lon = DL_MOVE_WP_lon(buf); - /* WP_alt from message is alt above MSL in mm - * lla.alt is above ellipsoid in mm - */ - lla.alt = DL_MOVE_WP_alt(buf) - state.ned_origin_i.hmsl + - state.ned_origin_i.lla.alt; - waypoint_move_lla(wp_id, &lla); - } - } - break; -#endif /* USE_NAVIGATION */ - - default: - break; - } -} diff --git a/sw/airborne/modules/air_data/air_data.c b/sw/airborne/modules/air_data/air_data.c index 9166cba70a..b11cd71370 100644 --- a/sw/airborne/modules/air_data/air_data.c +++ b/sw/airborne/modules/air_data/air_data.c @@ -33,6 +33,7 @@ #include "math/pprz_isa.h" #include "state.h" #include "generated/airframe.h" +#include "pprzlink/dl_protocol.h" /** global AirData state */ @@ -274,6 +275,35 @@ void air_data_periodic(void) } } +void air_data_parse_WIND_INFO(struct link_device *dev __attribute__((unused)), struct transport_tx *trans __attribute__((unused)), uint8_t *buf) +{ + if (DL_WIND_INFO_ac_id(buf) != AC_ID) { return; } + uint8_t flags = DL_WIND_INFO_flags(buf); + struct FloatVect2 wind = { 0.f, 0.f }; + float upwind = 0.f; + if (bit_is_set(flags, 0)) { + wind.x = DL_WIND_INFO_north(buf); + wind.y = DL_WIND_INFO_east(buf); + stateSetHorizontalWindspeed_f(&wind); + air_data.wind_speed = float_vect2_norm(&wind); + air_data.wind_dir = atan2f(wind.y, wind.x); + } + if (bit_is_set(flags, 1)) { + upwind = DL_WIND_INFO_up(buf); + stateSetVerticalWindspeed_f(upwind); + } +#if !USE_AIRSPEED + if (bit_is_set(flags, 2)) { + air_data.tas = DL_WIND_INFO_airspeed(buf); + air_data.airspeed = eas_from_tas(air_data.tas); + stateSetAirspeed_f(air_data.airspeed); + } +#endif +#ifdef WIND_INFO_RET + float airspeed = stateGetAirspeed_f(); + pprz_msg_send_WIND_INFO_RET(trans, dev, AC_ID, &flags, &wind.y, &wind.x, &upwind, &airspeed); +#endif +} /** * Calculate equivalent airspeed from dynamic pressure. diff --git a/sw/airborne/modules/air_data/air_data.h b/sw/airborne/modules/air_data/air_data.h index f03b0795f8..d0630524cd 100644 --- a/sw/airborne/modules/air_data/air_data.h +++ b/sw/airborne/modules/air_data/air_data.h @@ -32,6 +32,8 @@ #define AIR_DATA_H #include "std.h" +#include "pprzlink/pprzlink_device.h" +#include "pprzlink/pprzlink_transport.h" /** Air Data strucute */ struct AirData { @@ -68,6 +70,10 @@ extern void air_data_init(void); */ extern void air_data_periodic(void); +/** Parse datalink wind info message + */ +extern void air_data_parse_WIND_INFO(struct link_device *dev, struct transport_tx *trans, uint8_t *buf); + /** Return AMSL (altitude AboveSeaLevel). * If AMSL from baro is valid, return that, otherwise from gps. */ diff --git a/sw/airborne/modules/core/settings.c b/sw/airborne/modules/core/settings.c index ba9bb378a2..5486b392b4 100644 --- a/sw/airborne/modules/core/settings.c +++ b/sw/airborne/modules/core/settings.c @@ -27,6 +27,9 @@ #include "modules/core/settings.h" #include "generated/settings.h" +#include "generated/airframe.h" +#include "pprzlink/messages.h" +#include "pprzlink/dl_protocol.h" struct PersistentSettings pers_settings; @@ -88,3 +91,49 @@ int32_t settings_clear(void) settings_clear_flag = false; return -1; } + +void settings_parse_msg_SETTING(struct link_device *dev, struct transport_tx *trans, uint8_t *buf) +{ +#ifndef INTERMCU_FBW + if (DL_SETTING_ac_id(buf) != AC_ID) { return; } + uint8_t sender_id = pprzlink_get_msg_sender_id(buf); + uint8_t i = DL_SETTING_index(buf); + float var = DL_SETTING_value(buf); + DlSetting(i, var); + // Reply to the sender of the message + struct pprzlink_msg msg; + msg.trans = trans; + msg.dev = dev; + msg.sender_id = AC_ID; + msg.receiver_id = sender_id; + msg.component_id = 0; + pprzlink_msg_send_DL_VALUE(&msg, &i, &var); +#else + (void)dev; + (void)trans; + (void)buf; +#endif +} + +void settings_parse_msg_GET_SETTING(struct link_device *dev, struct transport_tx *trans, uint8_t *buf) +{ +#ifndef INTERMCU_FBW + if (DL_GET_SETTING_ac_id(buf) != AC_ID) { return; } + uint8_t sender_id = pprzlink_get_msg_sender_id(buf); + uint8_t i = DL_GET_SETTING_index(buf); + float val = settings_get_value(i); + // Reply to the sender of the message + struct pprzlink_msg msg; + msg.trans = trans; + msg.dev = dev; + msg.sender_id = AC_ID; + msg.receiver_id = sender_id; + msg.component_id = 0; + pprzlink_msg_send_DL_VALUE(&msg, &i, &val); +#else + (void)dev; + (void)trans; + (void)buf; +#endif +} + diff --git a/sw/airborne/modules/core/settings.h b/sw/airborne/modules/core/settings.h index 2e19f73026..cfb24c8640 100644 --- a/sw/airborne/modules/core/settings.h +++ b/sw/airborne/modules/core/settings.h @@ -29,10 +29,14 @@ #define CORE_SETTINGS_H #include "std.h" +#include "pprzlink/pprzlink_device.h" +#include "pprzlink/pprzlink_transport.h" extern void settings_init(void); extern int32_t settings_store(void); extern int32_t settings_clear(void); +extern void settings_parse_msg_SETTING(struct link_device *dev, struct transport_tx *trans, uint8_t *buf); +extern void settings_parse_msg_GET_SETTING(struct link_device *dev, struct transport_tx *trans, uint8_t *buf); extern bool settings_store_flag; extern bool settings_clear_flag; diff --git a/sw/airborne/modules/datalink/datalink.c b/sw/airborne/modules/datalink/datalink.c index cd196289cb..6d1dbcfa79 100644 --- a/sw/airborne/modules/datalink/datalink.c +++ b/sw/airborne/modules/datalink/datalink.c @@ -57,126 +57,23 @@ void datalink_periodic(void) datalink_time++; // called at 1Hz } +void datalink_parse_PING(struct link_device *dev, struct transport_tx *trans, uint8_t *buf) +{ + // Reply to the sender of the message + struct pprzlink_msg msg; + msg.trans = trans; + msg.dev = dev; + msg.sender_id = AC_ID; + msg.receiver_id = pprzlink_get_msg_sender_id(buf); + msg.component_id = 0; + pprzlink_msg_send_PONG(&msg); +} + void dl_parse_msg(struct link_device *dev, struct transport_tx *trans, uint8_t *buf) { uint8_t msg_id = pprzlink_get_msg_id(buf); uint8_t class_id = pprzlink_get_msg_class_id(buf); - uint8_t sender_id = pprzlink_get_msg_sender_id(buf); - - // Check that the message is a datalink message - if (class_id == DL_datalink_CLASS_ID) { - /* parse datalink messages coming from ground station */ - switch (msg_id) { - case DL_PING: - { - // Reply to the sender of the message - struct pprzlink_msg msg; - msg.trans = trans; - msg.dev = dev; - msg.sender_id = AC_ID; - msg.receiver_id = sender_id; - msg.component_id = 0; - pprzlink_msg_send_PONG(&msg); - } - break; - -#ifndef INTERMCU_FBW - case DL_SETTING : - { - if (DL_SETTING_ac_id(buf) != AC_ID) { break; } - uint8_t i = DL_SETTING_index(buf); - float var = DL_SETTING_value(buf); - DlSetting(i, var); - // Reply to the sender of the message - struct pprzlink_msg msg; - msg.trans = trans; - msg.dev = dev; - msg.sender_id = AC_ID; - msg.receiver_id = sender_id; - msg.component_id = 0; - pprzlink_msg_send_DL_VALUE(&msg, &i, &var); - } - break; - - case DL_GET_SETTING : - { - if (DL_GET_SETTING_ac_id(buf) != AC_ID) { break; } - uint8_t i = DL_GET_SETTING_index(buf); - float val = settings_get_value(i); - // Reply to the sender of the message - struct pprzlink_msg msg; - msg.trans = trans; - msg.dev = dev; - msg.sender_id = AC_ID; - msg.receiver_id = sender_id; - msg.component_id = 0; - pprzlink_msg_send_DL_VALUE(&msg, &i, &val); - } - break; -#endif - -#ifdef RADIO_CONTROL_TYPE_DATALINK - case DL_RC_3CH : -#ifdef RADIO_CONTROL_DATALINK_LED - LED_TOGGLE(RADIO_CONTROL_DATALINK_LED); -#endif - parse_rc_3ch_datalink( - DL_RC_3CH_throttle_mode(buf), - DL_RC_3CH_roll(buf), - DL_RC_3CH_pitch(buf)); - break; - case DL_RC_4CH : - if (DL_RC_4CH_ac_id(buf) == AC_ID) { -#ifdef RADIO_CONTROL_DATALINK_LED - LED_TOGGLE(RADIO_CONTROL_DATALINK_LED); -#endif - parse_rc_4ch_datalink(DL_RC_4CH_mode(buf), - DL_RC_4CH_throttle(buf), - DL_RC_4CH_roll(buf), - DL_RC_4CH_pitch(buf), - DL_RC_4CH_yaw(buf)); - } - break; -#endif // RADIO_CONTROL_TYPE_DATALINK - -#if USE_GPS - case DL_GPS_INJECT : - { - // Check if the GPS is for this AC - if (DL_GPS_INJECT_ac_id(buf) != AC_ID) { break; } - - // GPS parse data - gps_inject_data( - DL_GPS_INJECT_packet_id(buf), - DL_GPS_INJECT_data_length(buf), - DL_GPS_INJECT_data(buf) - ); - } - break; -#if USE_GPS_UBX_RTCM - case DL_RTCM_INJECT : - { - // GPS parse data - gps_inject_data(DL_RTCM_INJECT_packet_id(buf), - DL_RTCM_INJECT_data_length(buf), - DL_RTCM_INJECT_data(buf)); - } - break; -#endif // USE_GPS_UBX_RTCM -#endif // USE_GPS - - default: - break; - } - } - /* Parse firmware specific datalink */ - firmware_parse_msg(dev, trans, buf); - /* Parse modules datalink */ modules_parse_datalink(msg_id, class_id, dev, trans, buf); } -/* default empty WEAK implementation for firmwares without an extra firmware_parse_msg */ -WEAK void firmware_parse_msg(struct link_device *dev __attribute__((unused)), struct transport_tx *trans __attribute__((unused)), uint8_t *buf __attribute__((unused))) -{ -} diff --git a/sw/airborne/modules/datalink/datalink.h b/sw/airborne/modules/datalink/datalink.h index 7e5ab02140..9de4e9a285 100644 --- a/sw/airborne/modules/datalink/datalink.h +++ b/sw/airborne/modules/datalink/datalink.h @@ -58,9 +58,6 @@ EXTERN uint8_t dl_buffer[MSG_SIZE] __attribute__((aligned)); /** Should be called when chars are available in dl_buffer */ EXTERN void dl_parse_msg(struct link_device *dev, struct transport_tx *trans, uint8_t *buf); -/** Firmware specfic msg handler */ -EXTERN void firmware_parse_msg(struct link_device *dev, struct transport_tx *trans, uint8_t *buf); - #if USE_NPS EXTERN bool datalink_enabled; #endif @@ -78,6 +75,8 @@ EXTERN bool datalink_enabled; /** periodic function, should be called at 1Hz */ extern void datalink_periodic(void); +extern void datalink_parse_PING(struct link_device *dev, struct transport_tx *trans, uint8_t *buf); + /** Check for new message and parse */ static inline void DlCheckAndParse(struct link_device *dev, struct transport_tx *trans, uint8_t *buf, bool *msg_available, bool update_dl) { diff --git a/sw/airborne/modules/gps/gps.c b/sw/airborne/modules/gps/gps.c index e2ab543ada..a98b03d07a 100644 --- a/sw/airborne/modules/gps/gps.c +++ b/sw/airborne/modules/gps/gps.c @@ -383,6 +383,27 @@ void WEAK gps_inject_data(uint8_t packet_id __attribute__((unused)), uint8_t len } +void gps_parse_GPS_INJECT(uint8_t *buf) +{ + // Check if the GPS is for this AC + if (DL_GPS_INJECT_ac_id(buf) != AC_ID) { return; } + + // GPS parse data + gps_inject_data( + DL_GPS_INJECT_packet_id(buf), + DL_GPS_INJECT_data_length(buf), + DL_GPS_INJECT_data(buf) + ); +} + +void gps_parse_RTCM_INJECT(uint8_t *buf) +{ + // GPS parse data + gps_inject_data(DL_RTCM_INJECT_packet_id(buf), + DL_RTCM_INJECT_data_length(buf), + DL_RTCM_INJECT_data(buf)); +} + /** * Convenience functions to get utm position from GPS state */ diff --git a/sw/airborne/modules/gps/gps.h b/sw/airborne/modules/gps/gps.h index 8b36eeb152..ccd0384b02 100644 --- a/sw/airborne/modules/gps/gps.h +++ b/sw/airborne/modules/gps/gps.h @@ -171,6 +171,9 @@ extern void gps_init(void); /** GPS packet injection (default empty) */ extern void gps_inject_data(uint8_t packet_id, uint8_t length, uint8_t *data); +extern void gps_parse_GPS_INJECT(uint8_t *buf); +extern void gps_parse_RTCM_INJECT(uint8_t *buf); + /** GPS timeout in seconds */ #ifndef GPS_TIMEOUT #define GPS_TIMEOUT 2 diff --git a/sw/airborne/modules/gps/gps_ubx.c b/sw/airborne/modules/gps/gps_ubx.c index bbd7c13691..5cd43e9913 100644 --- a/sw/airborne/modules/gps/gps_ubx.c +++ b/sw/airborne/modules/gps/gps_ubx.c @@ -22,6 +22,7 @@ #include "modules/gps/gps_ubx.h" #include "modules/core/abi.h" +#include "pprzlink/dl_protocol.h" #include "led.h" #ifndef USE_GPS_UBX_RTCM @@ -117,6 +118,22 @@ void gps_ubx_event(void) } } +void gps_ubx_parse_HITL_UBX(uint8_t *buf) +{ + /** This code simulates gps_ubx.c:parse_ubx() */ + if (gps_ubx.msg_available) { + gps_ubx.error_cnt++; + gps_ubx.error_last = GPS_UBX_ERR_OVERRUN; + } else { + gps_ubx.msg_class = DL_HITL_UBX_class(buf); + gps_ubx.msg_id = DL_HITL_UBX_id(buf); + uint8_t l = DL_HITL_UBX_ubx_payload_length(buf); + uint8_t *ubx_payload = DL_HITL_UBX_ubx_payload(buf); + memcpy(gps_ubx.msg_buf, ubx_payload, l); + gps_ubx.msg_available = true; + } +} + static void gps_ubx_parse_nav_pvt(void) { uint8_t flags = UBX_NAV_PVT_flags(gps_ubx.msg_buf); diff --git a/sw/airborne/modules/gps/gps_ubx.h b/sw/airborne/modules/gps/gps_ubx.h index 93eadfdccf..3395d8bd87 100644 --- a/sw/airborne/modules/gps/gps_ubx.h +++ b/sw/airborne/modules/gps/gps_ubx.h @@ -45,6 +45,7 @@ extern void gps_ubx_init(void); extern void gps_ubx_event(void); +extern void gps_ubx_parse_HITL_UBX(uint8_t *buf); #define gps_ubx_periodic_check() gps_periodic_check(&gps_ubx.state) diff --git a/sw/airborne/modules/radio_control/rc_datalink.c b/sw/airborne/modules/radio_control/rc_datalink.c index 8bff9ee43d..708a59e03b 100644 --- a/sw/airborne/modules/radio_control/rc_datalink.c +++ b/sw/airborne/modules/radio_control/rc_datalink.c @@ -27,6 +27,7 @@ #include "modules/radio_control/rc_datalink.h" #include "modules/radio_control/radio_control.h" #include "modules/core/abi.h" +#include "pprzlink/dl_protocol.h" int8_t rc_dl_values[ RC_DL_NB_CHANNEL ]; volatile bool rc_dl_frame_available; @@ -38,6 +39,28 @@ void rc_datalink_init(void) rc_dl_frame_available = false; } +void rc_datalink_parse_RC_3CH(uint8_t *buf) +{ +#ifdef RADIO_CONTROL_DATALINK_LED + LED_TOGGLE(RADIO_CONTROL_DATALINK_LED); +#endif + parse_rc_3ch_datalink( + DL_RC_3CH_throttle_mode(buf), + DL_RC_3CH_roll(buf), + DL_RC_3CH_pitch(buf)); +} + +void rc_datalink_parse_RC_4CH(uint8_t *buf) +{ +#ifdef RADIO_CONTROL_DATALINK_LED + LED_TOGGLE(RADIO_CONTROL_DATALINK_LED); +#endif + parse_rc_4ch_datalink(DL_RC_4CH_mode(buf), + DL_RC_4CH_throttle(buf), + DL_RC_4CH_roll(buf), + DL_RC_4CH_pitch(buf), + DL_RC_4CH_yaw(buf)); +} void parse_rc_3ch_datalink(uint8_t throttle_mode, int8_t roll, diff --git a/sw/airborne/modules/radio_control/rc_datalink.h b/sw/airborne/modules/radio_control/rc_datalink.h index ff047cdecd..0aea35df83 100644 --- a/sw/airborne/modules/radio_control/rc_datalink.h +++ b/sw/airborne/modules/radio_control/rc_datalink.h @@ -63,6 +63,9 @@ extern void parse_rc_4ch_datalink( int8_t pitch, int8_t yaw); +extern void rc_datalink_parse_RC_3CH(uint8_t *buf); +extern void rc_datalink_parse_RC_4CH(uint8_t *buf); + /** * RC init function. */