Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-10 06:59:54 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

[datalink] move all hardcoded message parsing to modules datalink events (#2838)

Browse Source
This commit is contained in:
Gautier Hattenberger
2022-03-11 15:48:51 +01:00
committed by GitHub
parent e436d78c84
commit 7709dcfa2e
34 changed files with 312 additions and 427 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/modules/air_data.xml
+1
View File
@@ -36,6 +36,7 @@
</header>
<init fun="air_data_init()"/>
<periodic fun="air_data_periodic()" freq="10"/>
<datalink message="WIND_INFO" fun="air_data_parse_WIND_INFO(dev,trans,buf)"/>
<makefile target="!fbw">
<file name="air_data.c"/>
</makefile>
conf/modules/autopilot_gnc.xml
+2 -4
View File
@@ -7,7 +7,7 @@
</description>
</doc>
<dep>
<depends>system_core,@guidance,@navigation,@commands</depends>
<depends>system_core,@guidance,@navigation,@commands,autopilot_guided</depends>
<provides>autopilot</provides>
</dep>
<header>
@@ -16,6 +16,7 @@
<init fun="autopilot_init()"/>
<periodic fun="autopilot_periodic()"/>
<event fun="autopilot_event()"/>
<datalink message="GUIDED_SETPOINT_NED" fun="autopilot_guided_parse_GUIDED(buf)" cond="AP_MODE_GUIDED"/>
<makefile target="!fbw">
<file name="autopilot.c" dir="."/>
<file name="autopilot_firmware.c" dir="$(SRC_FIRMWARE)"/>
@@ -24,8 +25,5 @@
<file name="autopilot_generated.c" dir="$(SRC_FIRMWARE)" cond="ifeq ($(USE_GENERATED_AUTOPILOT), TRUE)"/>
<define name="USE_GENERATED_AUTOPILOT" value="1" cond="ifeq ($(USE_GENERATED_AUTOPILOT), TRUE)"/>
</makefile>
<makefile target="!fbw" firmware="rotorcraft">
<file name="autopilot_guided.c" dir="$(SRC_FIRMWARE)"/>
</makefile>
</module>
conf/modules/autopilot_guided.xml
+22
View File
@@ -0,0 +1,22 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="autopilot_guided" task="control">
<doc>
<description>
Guided mode interface for autpilote
only rotorcraft for now
</description>
</doc>
<dep>
<depends>@guidance</depends>
</dep>
<header>
<file name="autopilot_guided.h" dir="."/>
</header>
<datalink message="GUIDED_SETPOINT_NED" fun="autopilot_guided_parse_GUIDED(buf)" cond="AP_MODE_GUIDED"/>
<makefile target="!fbw" firmware="rotorcraft">
<file name="autopilot_guided.c" dir="$(SRC_FIRMWARE)"/>
</makefile>
</module>
conf/modules/datalink_common.xml
+1 -3
View File
@@ -14,6 +14,7 @@
<init fun="downlink_init()"/>
<periodic fun="telemetry_reporting_task()"/>
<periodic fun="datalink_periodic()" freq="1"/>
<datalink message="PING" fun="datalink_parse_PING(dev, trans, buf)"/>
<makefile>
<define name="DOWNLINK"/>
<define name="PERIODIC_TELEMETRY"/>
@@ -27,7 +28,6 @@
</test>
</makefile>
<makefile target="!fbw" firmware="fixedwing">
<file name="fixedwing_datalink.c" dir="$(SRC_FIRMWARE)"/>
<file name="ap_downlink.c" dir="$(SRC_FIRMWARE)"/>
<test firmware="fixedwing">
<define name="DOWNLINK_TRANSPORT" value="pprz_tp"/>
@@ -39,7 +39,6 @@
<file name="fbw_downlink.c" dir="$(SRC_FIRMWARE)"/>
</makefile>
<makefile firmware="rotorcraft">
<file name="rotorcraft_datalink.c" dir="$(SRC_FIRMWARE)"/>
<file name="rotorcraft_telemetry.c" dir="$(SRC_FIRMWARE)"/>
<test firmware="rotorcraft">
<define name="DOWNLINK_TRANSPORT" value="pprz_tp"/>
@@ -48,7 +47,6 @@
</test>
</makefile>
<makefile firmware="rover">
<file name="rover_datalink.c" dir="$(SRC_FIRMWARE)"/>
<file name="rover_telemetry.c" dir="$(SRC_FIRMWARE)"/>
<test firmware="rover">
<define name="DOWNLINK_TRANSPORT" value="pprz_tp"/>
conf/modules/gps.xml
+2
View File
@@ -28,6 +28,8 @@
<file name="gps.h"/>
</header>
<init fun="gps_init()"/>
<datalink message="GPS_INJECT" fun="gps_parse_GPS_INJECT(buf)"/>
<datalink message="RTCM_INJECT" fun="gps_parse_RTCM_INJECT(buf)"/>
<makefile target="fbw|ap|sim|nps|hitl">
<configure name="GPS_LED" default="none"/>
conf/modules/gps_ublox.xml
+2 -1
View File
@@ -21,7 +21,8 @@
<init fun="gps_ubx_init()"/>
<periodic fun="gps_ubx_periodic_check()" freq="1." autorun="TRUE"/>
<event fun="gps_ubx_event()"/>
<makefile target="ap|fbw">
<datalink message="HITL_UBX" fun="gps_ubx_parse_HITL_UBX(buf)"/>
<makefile target="ap|fbw|hitl">
<configure name="UBX_GPS_PORT" default="$(GPS_PORT)" case="upper|lower"/>
<configure name="UBX_GPS_BAUD" default="$(GPS_BAUD)"/>
conf/modules/nav_basic_fw.xml
+2
View File
@@ -32,6 +32,8 @@
<file name="nav.h" dir="firmwares/fixedwing"/>
</header>
<init fun="nav_init()"/>
<datalink message="BLOCK" fun="nav_parse_BLOCK(dev,trans,buf)"/>
<datalink message="MOVE_WP" fun="nav_parse_MOVE_WP(dev,trans,buf)"/>
<makefile target="ap|sim|nps|hitl" firmware="fixedwing">
<file name="nav.c" dir="$(SRC_FIRMWARE)"/>
<file name="common_flight_plan.c"/>
conf/modules/nav_basic_rotorcraft.xml
+2
View File
@@ -24,6 +24,8 @@
<file name="navigation.h" dir="firmwares/rotorcraft"/>
</header>
<init fun="nav_init()"/>
<datalink message="BLOCK" fun="nav_parse_BLOCK(buf)"/>
<datalink message="MOVE_WP" fun="nav_parse_MOVE_WP(buf)"/>
<makefile target="ap|nps|hitl" firmware="rotorcraft">
<file name="navigation.c" dir="$(SRC_FIRMWARE)"/>
<file name="common_flight_plan.c"/>
conf/modules/nav_rover_base.xml
+2
View File
@@ -23,6 +23,8 @@
</header>
<init fun="nav_init()"/>
<init fun="nav_rover_init()"/>
<datalink message="BLOCK" fun="nav_parse_BLOCK(buf)"/>
<datalink message="MOVE_WP" fun="nav_parse_MOVE_WP(buf)"/>
<makefile>
<file name="nav_rover_base.c"/>
<file name="navigation.c" dir="firmwares/rover"/>
conf/modules/radio_control_datalink.xml
+2
View File
@@ -16,6 +16,8 @@
</header>
<init fun="rc_datalink_init()"/>
<event fun="rc_datalink_event()"/>
<datalink message="RC_3CH" fun="rc_datalink_parse_RC_3CH(buf)"/>
<datalink message="RC_4CH" fun="rc_datalink_parse_RC_4CH(buf)"/>
<makefile target="ap|fbw|sim|nps">
<configure name="RADIO_CONTROL_DATALINK_LED" default="none"/>
<define name="RADIO_CONTROL_DATALINK_LED" value="$(RADIO_CONTROL_DATALINK_LED)" cond="ifneq ($(RADIO_CONTROL_DATALINK_LED),none)"/>
conf/modules/settings.xml
+2
View File
@@ -13,6 +13,8 @@
<file name="settings.h"/>
</header>
<init fun="settings_init()"/>
<datalink message="SETTING" fun="settings_parse_msg_SETTING(dev,trans,buf)"/>
<datalink message="GET_SETTING" fun="settings_parse_msg_GET_SETTING(dev,trans,buf)"/>
<makefile target="!fbw">
<file name="settings.c"/>
<file_arch name="settings_arch.c"/>
sw/airborne/firmwares/fixedwing/fixedwing_datalink.c
-137
View File
@@ -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;
}
}
sw/airborne/firmwares/fixedwing/nav.c
+28
View File
@@ -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 **********************************************/
sw/airborne/firmwares/fixedwing/nav.h
+4
View File
@@ -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);
sw/airborne/firmwares/rotorcraft/autopilot_guided.c
+14
View File
@@ -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));
}
sw/airborne/firmwares/rotorcraft/autopilot_guided.h
+4
View File
@@ -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
*/
sw/airborne/firmwares/rotorcraft/navigation.c
+24
View File
@@ -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();
sw/airborne/firmwares/rotorcraft/navigation.h
+2
View File
@@ -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);
sw/airborne/firmwares/rotorcraft/rotorcraft_datalink.c
-90
View File
@@ -1,90 +0,0 @@
/*
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
*
* 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;
}
}
sw/airborne/firmwares/rover/navigation.c
+23
View File
@@ -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)
{
sw/airborne/firmwares/rover/navigation.h
+2
View File
@@ -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);
sw/airborne/firmwares/rover/rover_datalink.c
-74
View File
@@ -1,74 +0,0 @@
/*
* Copyright (C) 2018 Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @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;
}
}
sw/airborne/modules/air_data/air_data.c
+30
View File
@@ -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.
sw/airborne/modules/air_data/air_data.h
+6
View File
@@ -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.
*/
sw/airborne/modules/core/settings.c
+49
View File
@@ -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
}
sw/airborne/modules/core/settings.h
+4
View File
@@ -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;
sw/airborne/modules/datalink/datalink.c
+12 -115
View File
@@ -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)))
{
}
sw/airborne/modules/datalink/datalink.h
+2 -3
View File
@@ -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)
{
sw/airborne/modules/gps/gps.c
+21
View File
@@ -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
*/
sw/airborne/modules/gps/gps.h
+3
View File
@@ -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
sw/airborne/modules/gps/gps_ubx.c
+17
View File
@@ -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);
sw/airborne/modules/gps/gps_ubx.h
+1
View File
@@ -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)
sw/airborne/modules/radio_control/rc_datalink.c
+23
View File
@@ -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,
sw/airborne/modules/radio_control/rc_datalink.h
+3
View File
@@ -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.
*/