Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-22 12:28:03 +08:00
Code Issues Actions 25 Packages Projects Releases Wiki Activity

[telemetry] add support for ivy_transport

Browse Source
This commit is contained in:
Gautier Hattenberger
2014-11-05 18:25:27 +01:00
parent e5cdfa1ce1
commit 96445e9ffe
16 changed files with 342 additions and 103 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/firmwares/subsystems/fixedwing/autopilot.makefile
+4 -4
View File
@@ -183,8 +183,8 @@ sim.srcs += $(fbw_srcs) $(ap_srcs)
sim.CFLAGS += -DSITL
sim.srcs += $(SRC_ARCH)/sim_ap.c
sim.CFLAGS += -DDOWNLINK -DPERIODIC_TELEMETRY -DDOWNLINK_TRANSPORT=IvyTransport
sim.srcs += subsystems/datalink/downlink.c $(SRC_FIRMWARE)/datalink.c $(SRC_ARCH)/ivy_transport.c
sim.CFLAGS += -DDOWNLINK -DPERIODIC_TELEMETRY -DDOWNLINK_TRANSPORT=ivy_tp -DDOWNLINK_DEVICE=ivy_tp
sim.srcs += subsystems/datalink/downlink.c $(SRC_FIRMWARE)/datalink.c subsystems/datalink/ivy_transport.c
sim.srcs += $(SRC_ARCH)/sim_gps.c $(SRC_ARCH)/sim_adc_generic.c
@@ -223,8 +223,8 @@ jsbsim.srcs += $(SIMDIR)/sim_ac_jsbsim.c $(SIMDIR)/sim_ac_fw.c $(SIMDIR)/sim_a
jsbsim.CFLAGS += -I/usr/include $(shell pkg-config glib-2.0 --cflags)
jsbsim.LDFLAGS += $(shell pkg-config glib-2.0 --libs) -lglibivy -lm $(shell pcre-config --libs)
jsbsim.CFLAGS += -DDOWNLINK -DPERIODIC_TELEMETRY -DDOWNLINK_TRANSPORT=IvyTransport
jsbsim.srcs += subsystems/datalink/downlink.c $(SRC_FIRMWARE)/datalink.c $(SRC_ARCH)/ivy_transport.c
jsbsim.CFLAGS += -DDOWNLINK -DPERIODIC_TELEMETRY -DDOWNLINK_TRANSPORT=ivy_tp -DDOWNLINK_DEVICE=ivy_tp
jsbsim.srcs += subsystems/datalink/downlink.c $(SRC_FIRMWARE)/datalink.c subsystems/datalink/ivy_transport.c
jsbsim.srcs += $(SRC_ARCH)/jsbsim_hw.c $(SRC_ARCH)/jsbsim_ir.c $(SRC_ARCH)/jsbsim_gps.c $(SRC_ARCH)/jsbsim_ahrs.c $(SRC_ARCH)/jsbsim_transport.c
conf/firmwares/subsystems/fixedwing/fdm_crrcsim.makefile
+2 -2
View File
@@ -47,6 +47,6 @@ nps.srcs += $(NPSDIR)/nps_main.c \
$(NPSDIR)/nps_flightgear.c \
nps.CFLAGS += -DDOWNLINK -DPERIODIC_TELEMETRY -DDOWNLINK_TRANSPORT=IvyTransport
nps.srcs += subsystems/datalink/downlink.c $(SRC_FIRMWARE)/datalink.c $(SRC_ARCH)/ivy_transport.c
nps.CFLAGS += -DDOWNLINK -DPERIODIC_TELEMETRY -DDOWNLINK_TRANSPORT=ivy_tp -DDOWNLINK_DEVICE=ivy_tp
nps.srcs += subsystems/datalink/downlink.c $(SRC_FIRMWARE)/datalink.c subsystems/datalink/ivy_transport.c
conf/firmwares/subsystems/fixedwing/fdm_jsbsim.makefile
+2 -2
View File
@@ -65,6 +65,6 @@ nps.srcs += $(NPSDIR)/nps_main.c \
nps.srcs += math/pprz_geodetic_wmm2010.c
nps.CFLAGS += -DDOWNLINK -DPERIODIC_TELEMETRY -DDOWNLINK_TRANSPORT=IvyTransport
nps.srcs += subsystems/datalink/downlink.c $(SRC_FIRMWARE)/datalink.c $(SRC_ARCH)/ivy_transport.c
nps.CFLAGS += -DDOWNLINK -DPERIODIC_TELEMETRY -DDOWNLINK_TRANSPORT=ivy_tp -DDOWNLINK_DEVICE=ivy_tp
nps.srcs += subsystems/datalink/downlink.c $(SRC_FIRMWARE)/datalink.c subsystems/datalink/ivy_transport.c
conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile
+2 -2
View File
@@ -62,8 +62,8 @@ nps.srcs += $(NPSDIR)/nps_main.c \
# for geo mag calculation
nps.srcs += math/pprz_geodetic_wmm2010.c
nps.CFLAGS += -DDOWNLINK -DPERIODIC_TELEMETRY -DDOWNLINK_TRANSPORT=IvyTransport -DDefaultPeriodic='&telemetry_Main'
nps.srcs += $(SRC_ARCH)/ivy_transport.c
nps.CFLAGS += -DDOWNLINK -DPERIODIC_TELEMETRY -DDOWNLINK_TRANSPORT=ivy_tp -DDOWNLINK_DEVICE=ivy_tp -DDefaultPeriodic='&telemetry_Main'
nps.srcs += subsystems/datalink/ivy_transport.c
nps.srcs += subsystems/datalink/downlink.c subsystems/datalink/telemetry.c
nps.srcs += $(SRC_FIRMWARE)/rotorcraft_telemetry.c
nps.srcs += $(SRC_FIRMWARE)/datalink.c
sw/airborne/arch/sim/ivy_transport.c
-3
View File
@@ -1,3 +0,0 @@
char ivy_buf[256];
char *ivy_p = ivy_buf;
int ivy_dl_enabled = 1;
sw/airborne/arch/sim/ivy_transport.h
-73
View File
@@ -1,73 +0,0 @@
#include <stdio.h>
#include <Ivy/ivy.h>
extern char ivy_buf[];
extern char* ivy_p;
extern int ivy_dl_enabled;
#define IvyTransportCheckFreeSpace(_dev,_) TRUE
#define IvyTransportSizeOf(_dev, x) (x)
#define IvyTransportHeader(_dev,len) ivy_p=ivy_buf;
#define IvyTransportTrailer(_dev) { *(--ivy_p) = '\0'; if (ivy_dl_enabled) { IvySendMsg("%s",ivy_buf); } }
#define IvyTransportPutUint8(_dev,x) { ivy_p += sprintf(ivy_p, "%u ", x); }
#define IvyTransportPutNamedUint8(_dev,_name, _x) { ivy_p += sprintf(ivy_p, "%s ", _name); }
#define Space() ivy_p += sprintf(ivy_p, " ");
#define Comma() ivy_p += sprintf(ivy_p, ",");
#define DelimStart() ivy_p += sprintf(ivy_p, "|");
#define DelimEnd() ivy_p += sprintf(ivy_p, "|");
#define IvyTransportPutcByAddr(_dev,x) ivy_p += sprintf(ivy_p, "%c", *x);
#define IvyTransportPutCharByAddr(_dev,x) IvyTransportPutcByAddr(_dev,x) Space()
#define IvyTransportPutUintByAddr(_dev,x) ivy_p += sprintf(ivy_p, "%u", *x);
#define IvyTransportPutUint8ByAddr(_dev,x) IvyTransportPutUintByAddr(_dev,x) Space()
#define IvyTransportPutUint16ByAddr(_dev,x) IvyTransportPutUintByAddr(_dev,x) Space()
#define IvyTransportPutUint32ByAddr(_dev,x) IvyTransportPutUintByAddr(_dev,x) Space()
#define IvyTransportPutUint64ByAddr(_dev,x) ivy_p += sprintf(ivy_p, "%llu", *x); Space()
#define IvyTransportPutIntByAddr(_dev,x) ivy_p += sprintf(ivy_p, "%d", *x);
#define IvyTransportPutInt8ByAddr(_dev,x) IvyTransportPutIntByAddr(_dev,x) Space()
#define IvyTransportPutInt16ByAddr(_dev,x) IvyTransportPutIntByAddr(_dev,x) Space()
#define IvyTransportPutInt32ByAddr(_dev,x) IvyTransportPutIntByAddr(_dev,x) Space()
#define IvyTransportPutInt64ByAddr(_dev,x) ivy_p += sprintf(ivy_p, "%lld", *x); Space()
#define IvyTransportPutOneFloatByAddr(_dev,x) ivy_p += sprintf(ivy_p, "%f", *x);
#define IvyTransportPutFloatByAddr(_dev,x) IvyTransportPutOneFloatByAddr(_dev,x) Space()
#define IvyTransportPutDoubleByAddr(_dev,x) IvyTransportPutOneFloatByAddr(_dev,x) Space()
#define IvyTransportPutArray(_dev,_put, _n, _x) { \
int __i; \
DelimStart(); \
for(__i = 0; __i < _n; __i++) { \
_put(_dev,&_x[__i]); \
Comma(); \
} DelimEnd(); Space(); \
}
#define IvyTransportPutInt8Array(_dev,_n, _x) IvyTransportPutArray(_dev,IvyTransportPutIntByAddr, _n, _x)
#define IvyTransportPutUint8Array(_dev,_n, _x) IvyTransportPutArray(_dev,IvyTransportPutUintByAddr, _n, _x)
#define IvyTransportPutCharArray(_dev,_n, _x) IvyTransportPutArray(_dev,IvyTransportPutcByAddr, _n, _x)
#define IvyTransportPutInt16Array(_dev,_n, _x) IvyTransportPutArray(_dev,IvyTransportPutIntByAddr, _n, _x)
#define IvyTransportPutUint16Array(_dev,_n, _x) IvyTransportPutArray(_dev,IvyTransportPutUintByAddr, _n, _x)
#define IvyTransportPutUint32Array(_dev,_n, _x) IvyTransportPutArray(_dev,IvyTransportPutUintByAddr, _n, _x)
#define IvyTransportPutInt32Array(_dev,_n, _x) IvyTransportPutArray(_dev,IvyTransportPutIntByAddr, _n, _x)
#define IvyTransportPutUint64Array(_dev,_n, _x) IvyTransportPutArray(_dev,IvyTransportPutUintByAddr, _n, _x)
#define IvyTransportPutInt64Array(_dev,_n, _x) IvyTransportPutArray(_dev,IvyTransportPutIntByAddr, _n, _x)
#define IvyTransportPutFloatArray(_dev,_n, _x) IvyTransportPutArray(_dev,IvyTransportPutOneFloatByAddr, _n, _x)
#define IvyTransportPutDoubleArray(_dev,_n, _x) IvyTransportPutFloatArray(_dev,_n, _x)
#define IvyTransportPutInt8FixedArray(_dev,_n, _x) IvyTransportPutArray(_dev,IvyTransportPutIntByAddr, _n, _x)
#define IvyTransportPutUint8FixedArray(_dev,_n, _x) IvyTransportPutArray(_dev,IvyTransportPutUintByAddr, _n, _x)
#define IvyTransportPutCharFixedArray(_dev,_n, _x) IvyTransportPutArray(_dev,IvyTransportPutcByAddr, _n, _x)
#define IvyTransportPutInt16FixedArray(_dev,_n, _x) IvyTransportPutArray(_dev,IvyTransportPutIntByAddr, _n, _x)
#define IvyTransportPutUint16FixedArray(_dev,_n, _x) IvyTransportPutArray(_dev,IvyTransportPutUintByAddr, _n, _x)
#define IvyTransportPutUint32FixedArray(_dev,_n, _x) IvyTransportPutArray(_dev,IvyTransportPutUintByAddr, _n, _x)
#define IvyTransportPutInt32FixedArray(_dev,_n, _x) IvyTransportPutArray(_dev,IvyTransportPutIntByAddr, _n, _x)
#define IvyTransportPutUint64FixedArray(_dev,_n, _x) IvyTransportPutArray(_dev,IvyTransportPutUintByAddr, _n, _x)
#define IvyTransportPutInt64FixedArray(_dev,_n, _x) IvyTransportPutArray(_dev,IvyTransportPutIntByAddr, _n, _x)
#define IvyTransportPutFloatFixedArray(_dev,_n, _x) IvyTransportPutArray(_dev,IvyTransportPutOneFloatByAddr, _n, _x)
#define IvyTransportPutDoubleFixedArray(_dev,_n, _x) IvyTransportPutFloatArray(_dev,_n, _x)
sw/airborne/arch/sim/sim_ap.c
+1 -1
View File
@@ -110,7 +110,7 @@ value update_bat(value bat) {
}
value update_dl_status(value dl_enabled) {
ivy_dl_enabled = Int_val(dl_enabled);
ivy_tp.ivy_dl_enabled = Int_val(dl_enabled);
return Val_unit;
}
sw/airborne/firmwares/fixedwing/main_ap.c
+4
View File
@@ -78,6 +78,7 @@ PRINT_CONFIG_MSG_VALUE("USE_BARO_BOARD is TRUE, reading onboard baro: ", BARO_BO
#include "subsystems/datalink/pprz_transport.h"
#include "subsystems/datalink/xbee.h"
#include "subsystems/datalink/w5100.h"
#include "subsystems/datalink/ivy_transport.h"
// modules & settings
#include "generated/modules.h"
@@ -254,6 +255,9 @@ void init_ap( void ) {
w5100_init();
#endif
#endif /* DATALINK */
#if SITL
ivy_transport_init();
#endif
#if defined AEROCOMM_DATA_PIN
IO0DIR |= _BV(AEROCOMM_DATA_PIN);
sw/airborne/firmwares/rotorcraft/main.c
+4
View File
@@ -44,6 +44,7 @@
#include "subsystems/settings.h"
#include "subsystems/datalink/pprz_transport.h"
#include "subsystems/datalink/xbee.h"
#include "subsystems/datalink/ivy_transport.h"
#include "subsystems/commands.h"
#include "subsystems/actuators.h"
@@ -178,6 +179,9 @@ STATIC_INLINE void main_init( void ) {
#if DATALINK == XBEE
xbee_init();
#endif
#if SITL
ivy_transport_init();
#endif
// register the timers for the periodic functions
main_periodic_tid = sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL);
sw/airborne/subsystems/datalink/ivy_transport.c
+178
View File
@@ -0,0 +1,178 @@
/*
* Copyright (C) 2003 Pascal Brisset, Antoine Drouin
* Copyright (C) 2014 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 subsystems/datalink/ivy_transport.c
*
* Building Paparazzi frames over IVY.
*
*/
#include "std.h"
#include "subsystems/datalink/ivy_transport.h"
#include "subsystems/datalink/downlink.h"
#include "subsystems/datalink/transport.h"
#include <stdio.h>
#include <Ivy/ivy.h>
struct ivy_transport ivy_tp;
static void put_bytes(struct ivy_transport *trans, struct device *dev __attribute__((unused)), enum TransportDataType type __attribute__((unused)), enum TransportDataFormat format __attribute__((unused)), uint8_t len, const void *bytes)
{
const uint8_t *b = (const uint8_t *) bytes;
// Start delimiter for arrays
if (format == DL_FORMAT_ARRAY) {
trans->ivy_p += sprintf(trans->ivy_p, "|");
}
int i = 0;
while (i < len) {
// print data with correct type
switch (type) {
case DL_TYPE_CHAR:
trans->ivy_p += sprintf(trans->ivy_p, "%c", (char)(*((char*)(b+i))));
i++;
break;
case DL_TYPE_UINT8:
trans->ivy_p += sprintf(trans->ivy_p, "%u", b[i]);
i++;
break;
case DL_TYPE_UINT16:
trans->ivy_p += sprintf(trans->ivy_p, "%u", (uint16_t)(*((uint16_t*)(b+i))));
i += 2;
break;
case DL_TYPE_UINT32:
case DL_TYPE_TIMESTAMP:
trans->ivy_p += sprintf(trans->ivy_p, "%u", (uint32_t)(*((uint32_t*)(b+i))));
i += 4;
break;
case DL_TYPE_UINT64:
trans->ivy_p += sprintf(trans->ivy_p, "%llu", (uint64_t)(*((uint64_t*)(b+i))));
i += 8;
break;
case DL_TYPE_INT8:
trans->ivy_p += sprintf(trans->ivy_p, "%d", (int8_t)(*((int8_t*)(b+i))));
i++;
break;
case DL_TYPE_INT16:
trans->ivy_p += sprintf(trans->ivy_p, "%d", (int16_t)(*((int16_t*)(b+i))));
i += 2;
break;
case DL_TYPE_INT32:
trans->ivy_p += sprintf(trans->ivy_p, "%d", (int32_t)(*((int32_t*)(b+i))));
i += 4;
break;
case DL_TYPE_INT64:
trans->ivy_p += sprintf(trans->ivy_p, "%lld", (int64_t)(*((int64_t*)(b+i))));
i += 8;
break;
case DL_TYPE_FLOAT:
trans->ivy_p += sprintf(trans->ivy_p, "%f", (float)(*((float*)(b+i))));
i += 4;
break;
case DL_TYPE_DOUBLE:
trans->ivy_p += sprintf(trans->ivy_p, "%f", (double)(*((double*)(b+i))));
i += 8;
break;
case DL_TYPE_ARRAY_LENGTH:
default:
// Don't print array length but increment index
i++;
break;
}
// Coma delimiter for array, space otherwise
if (format == DL_FORMAT_ARRAY) {
trans->ivy_p += sprintf(trans->ivy_p, ",");
} else {
trans->ivy_p += sprintf(trans->ivy_p, " ");
}
}
// End delimiter for arrays
if (format == DL_FORMAT_ARRAY) {
trans->ivy_p += sprintf(trans->ivy_p, "| ");
}
}
static void put_named_byte(struct ivy_transport *trans, struct device *dev __attribute__((unused)), enum TransportDataType type __attribute__((unused)), enum TransportDataFormat format __attribute__((unused)), uint8_t byte __attribute__((unused)), const char * name __attribute__((unused)))
{
trans->ivy_p += sprintf(trans->ivy_p, "%s ", name);
}
static uint8_t size_of(struct ivy_transport *trans __attribute__((unused)), uint8_t len)
{
return len;
}
static void start_message(struct ivy_transport *trans, struct device *dev __attribute__((unused)), uint8_t payload_len __attribute__((unused)))
{
trans->ivy_p = trans->ivy_buf;
}
static void end_message(struct ivy_transport *trans, struct device *dev __attribute__((unused)))
{
*(--trans->ivy_p) = '\0';
if (trans->ivy_dl_enabled) {
IvySendMsg("%s", trans->ivy_buf);
}
}
static void overrun(struct ivy_transport *trans __attribute__((unused)), struct device *dev __attribute__((unused)))
{
downlink_nb_ovrn++;
}
static void count_bytes(struct ivy_transport *trans __attribute__((unused)), struct device *dev __attribute__((unused)), uint8_t bytes)
{
downlink_nb_bytes += bytes;
}
static int check_available_space(struct ivy_transport *trans __attribute__((unused)), struct device *dev __attribute__((unused)), uint8_t bytes __attribute__((unused)))
{
return TRUE;
}
static int check_free_space(struct ivy_transport* p __attribute__((unused)), uint8_t len __attribute__((unused))) { return TRUE; }
static void transmit(struct ivy_transport* p __attribute__((unused)), uint8_t byte __attribute__((unused))) {}
static void send_message(struct ivy_transport* p __attribute__((unused))) {}
void ivy_transport_init(void)
{
ivy_tp.ivy_p = ivy_tp.ivy_buf;
ivy_tp.ivy_dl_enabled = TRUE;
ivy_tp.trans_tx.size_of = (size_of_t) size_of;
ivy_tp.trans_tx.check_available_space = (check_available_space_t) check_available_space;
ivy_tp.trans_tx.put_bytes = (put_bytes_t) put_bytes;
ivy_tp.trans_tx.put_named_byte = (put_named_byte_t) put_named_byte;
ivy_tp.trans_tx.start_message = (start_message_t) start_message;
ivy_tp.trans_tx.end_message = (end_message_t) end_message;
ivy_tp.trans_tx.overrun = (overrun_t) overrun;
ivy_tp.trans_tx.count_bytes = (count_bytes_t) count_bytes;
ivy_tp.trans_tx.impl = (void *)(&ivy_tp);
ivy_tp.device.check_free_space = (check_free_space_t) check_free_space;
ivy_tp.device.transmit = (transmit_t) transmit;
ivy_tp.device.send_message = (send_message_t) send_message;
ivy_tp.device.periph = (void *)(&ivy_tp);
}
sw/airborne/subsystems/datalink/ivy_transport.h
+53
View File
@@ -0,0 +1,53 @@
/*
* Copyright (C) 2003 Pascal Brisset, Antoine Drouin
* Copyright (C) 2014 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 subsystems/datalink/ivy_transport.h
*
* Building Paparazzi frames over IVY.
*
*/
#ifndef IVY_TRANSPORT_H
#define IVY_TRANSPORT_H
#include "subsystems/datalink/transport.h"
#include "mcu_periph/device.h"
// IVY transport
struct ivy_transport {
char ivy_buf[256];
char* ivy_p;
int ivy_dl_enabled;
// generic transmission interface
struct transport_tx trans_tx;
// generic (dummy) device
struct device device;
};
extern struct ivy_transport ivy_tp;
// Init function
extern void ivy_transport_init(void);
#endif // IVY_TRANSPORT_H
sw/airborne/subsystems/datalink/pprz_transport.c
+29 -5
View File
@@ -1,5 +1,6 @@
/*
* Copyright (C) 2006 Pascal Brisset, Antoine Drouin
* Copyright (C) 2014 Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* This file is part of paparazzi.
*
@@ -14,12 +15,29 @@
* 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.
* along with paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*
*/
/**
* @file subsystems/datalink/pprz_transport.c
*
* Building and parsing Paparazzi frames.
*
* Pprz frame:
*
* |STX|length|... payload=(length-4) bytes ...|Checksum A|Checksum B|
*
* where checksum is computed over length and payload:
* @code
* ck_A = ck_B = length
* for each byte b in payload
* ck_A += b;
* ck_b += ck_A;
* @endcode
*/
#include <inttypes.h>
#include "subsystems/datalink/downlink.h"
#ifndef PPRZ_DATALINK_EXPORT
@@ -37,7 +55,7 @@ static void put_1byte(struct pprz_transport *trans, struct device *dev, const ui
dev->transmit(dev->periph, byte);
}
static void put_bytes(struct pprz_transport *trans, struct device *dev, uint8_t len, const void *bytes)
static void put_bytes(struct pprz_transport *trans, struct device *dev, enum TransportDataType type __attribute__((unused)), enum TransportDataFormat format __attribute__((unused)), uint8_t len, const void *bytes)
{
const uint8_t *b = (const uint8_t *) bytes;
int i;
@@ -46,10 +64,15 @@ static void put_bytes(struct pprz_transport *trans, struct device *dev, uint8_t
}
}
static void put_named_byte(struct pprz_transport *trans, struct device *dev, enum TransportDataType type __attribute__((unused)), enum TransportDataFormat format __attribute__((unused)), uint8_t byte, const char * name __attribute__((unused)))
{
put_1byte(trans, dev, byte);
}
static uint8_t size_of(struct pprz_transport *trans __attribute__((unused)), uint8_t len)
{
// message length: payload + protocol overhead (STX + len + ck_a + ck_b = 4)
return len + 4;;
return len + 4;
}
static void start_message(struct pprz_transport *trans, struct device *dev, uint8_t payload_len)
@@ -90,6 +113,7 @@ void pprz_transport_init(void)
pprz_tp.trans_tx.size_of = (size_of_t) size_of;
pprz_tp.trans_tx.check_available_space = (check_available_space_t) check_available_space;
pprz_tp.trans_tx.put_bytes = (put_bytes_t) put_bytes;
pprz_tp.trans_tx.put_named_byte = (put_named_byte_t) put_named_byte;
pprz_tp.trans_tx.start_message = (start_message_t) start_message;
pprz_tp.trans_tx.end_message = (end_message_t) end_message;
pprz_tp.trans_tx.overrun = (overrun_t) overrun;
sw/airborne/subsystems/datalink/pprzlog_transport.c
+8 -2
View File
@@ -54,7 +54,7 @@ static void put_1byte(struct pprzlog_transport *trans, struct device *dev, const
dev->transmit(dev->periph, byte);
}
static void put_bytes(struct pprzlog_transport *trans, struct device *dev, uint8_t len, const void *bytes)
static void put_bytes(struct pprzlog_transport *trans, enum TransportDataType type __attribute__((unused)), enum TransportDataFormat format __attribute__((unused)), struct device *dev, uint8_t len, const void *bytes)
{
const uint8_t *b = (const uint8_t *) bytes;
int i;
@@ -63,6 +63,11 @@ static void put_bytes(struct pprzlog_transport *trans, struct device *dev, uint8
}
}
static void put_named_byte(struct pprzlog_transport *trans, struct device *dev, enum TransportDataType type __attribute__((unused)), enum TransportDataFormat format __attribute__((unused)), uint8_t byte, const char * name __attribute__((unused)))
{
put_1byte(trans, dev, byte);
}
static uint8_t size_of(struct pprzlog_transport *trans __attribute__((unused)), uint8_t len)
{
return len;
@@ -75,7 +80,7 @@ static void start_message(struct pprzlog_transport *trans, struct device *dev, u
trans->ck = 0;
put_1byte(trans, dev, msg_len);
uint32_t ts = get_sys_time_usec()/100;
put_bytes(trans, dev, 4, (uint8_t*)(&ts));
put_bytes(trans, dev, DL_TYPE_TIMESTAMP, DL_FORMAT_SCALAR, 4, (uint8_t*)(&ts));
}
static void end_message(struct pprzlog_transport *trans, struct device *dev)
@@ -102,6 +107,7 @@ void pprzlog_transport_init(void)
pprzlog_tp.trans_tx.size_of = (size_of_t) size_of;
pprzlog_tp.trans_tx.check_available_space = (check_available_space_t) check_available_space;
pprzlog_tp.trans_tx.put_bytes = (put_bytes_t) put_bytes;
pprzlog_tp.trans_tx.put_named_byte = (put_named_byte_t) put_named_byte;
pprzlog_tp.trans_tx.start_message = (start_message_t) start_message;
pprzlog_tp.trans_tx.end_message = (end_message_t) end_message;
pprzlog_tp.trans_tx.overrun = (overrun_t) overrun;
sw/airborne/subsystems/datalink/transport.h
+28 -1
View File
@@ -53,6 +53,31 @@ struct transport_rx {
#define TransportLink(_dev, _x) _TransportLink(_dev, _x)
/** Data type
*/
enum TransportDataType {
DL_TYPE_ARRAY_LENGTH,
DL_TYPE_CHAR,
DL_TYPE_UINT8,
DL_TYPE_INT8,
DL_TYPE_UINT16,
DL_TYPE_INT16,
DL_TYPE_UINT32,
DL_TYPE_INT32,
DL_TYPE_UINT64,
DL_TYPE_INT64,
DL_TYPE_FLOAT,
DL_TYPE_DOUBLE,
DL_TYPE_TIMESTAMP
};
/** Data format (scalar or array)
*/
enum TransportDataFormat {
DL_FORMAT_SCALAR,
DL_FORMAT_ARRAY
};
/** Function pointers definition
*
* they are used to cast the real functions with the correct type
@@ -60,7 +85,8 @@ struct transport_rx {
*/
typedef uint8_t (*size_of_t)(void *, uint8_t);
typedef int (*check_available_space_t)(void *, struct device *, uint8_t);
typedef void (*put_bytes_t)(void *, struct device *, uint8_t, const void *);
typedef void (*put_bytes_t)(void *, struct device *, enum TransportDataType, enum TransportDataFormat, uint8_t, const void *);
typedef void (*put_named_byte_t)(void *, struct device *, enum TransportDataType, enum TransportDataFormat, uint8_t, const char *);
typedef void (*start_message_t)(void *, struct device *, uint8_t);
typedef void (*end_message_t)(void *, struct device *);
typedef void (*overrun_t)(void *, struct device *);
@@ -72,6 +98,7 @@ struct transport_tx {
size_of_t size_of; ///< get size of payload with transport header and trailer
check_available_space_t check_available_space; ///< check if transmit buffer is not full
put_bytes_t put_bytes; ///< send bytes
put_named_byte_t put_named_byte; ///< send a single byte or its name
start_message_t start_message; ///< transport header
end_message_t end_message; ///< transport trailer
overrun_t overrun; ///< overrun
sw/airborne/subsystems/datalink/xbee.c
+8 -2
View File
@@ -59,7 +59,7 @@ static void put_1byte(struct xbee_transport *trans, struct device *dev, const ui
dev->transmit(dev->periph, byte);
}
static void put_bytes(struct xbee_transport *trans, struct device *dev, uint8_t len, const void *bytes)
static void put_bytes(struct xbee_transport *trans, struct device *dev, enum TransportDataType type __attribute__((unused)), enum TransportDataFormat format __attribute__((unused)), uint8_t len, const void *bytes)
{
const uint8_t *b = (const uint8_t *) bytes;
int i;
@@ -68,6 +68,11 @@ static void put_bytes(struct xbee_transport *trans, struct device *dev, uint8_t
}
}
static void put_named_byte(struct xbee_transport *trans, struct device *dev, enum TransportDataType type __attribute__((unused)), enum TransportDataFormat format __attribute__((unused)), uint8_t byte, const char * name __attribute__((unused)))
{
put_1byte(trans, dev, byte);
}
static uint8_t size_of(struct xbee_transport *trans __attribute__((unused)), uint8_t len)
{
// message length: payload + API overhead + XBEE TX overhead (868 or 2.4)
@@ -82,7 +87,7 @@ static void start_message(struct xbee_transport *trans, struct device *dev, uint
dev->transmit(dev->periph, (len & 0xff));
trans->cs_tx = 0;
const uint8_t header[] = XBEE_TX_HEADER;
put_bytes(trans, dev, XBEE_TX_OVERHEAD + 1, header);
put_bytes(trans, dev, DL_TYPE_UINT8, DL_FORMAT_SCALAR, XBEE_TX_OVERHEAD + 1, header);
}
static void end_message(struct xbee_transport *trans, struct device *dev)
@@ -159,6 +164,7 @@ void xbee_init( void ) {
xbee_tp.trans_tx.size_of = (size_of_t) size_of;
xbee_tp.trans_tx.check_available_space = (check_available_space_t) check_available_space;
xbee_tp.trans_tx.put_bytes = (put_bytes_t) put_bytes;
xbee_tp.trans_tx.put_named_byte = (put_named_byte_t) put_named_byte;
xbee_tp.trans_tx.start_message = (start_message_t) start_message;
xbee_tp.trans_tx.end_message = (end_message_t) end_message;
xbee_tp.trans_tx.overrun = (overrun_t) overrun;
sw/tools/generators/gen_messages.ml
+19 -6
View File
@@ -41,8 +41,21 @@ let c_type = fun format ->
| "Uint32" -> "uint32_t"
| "Uint16" -> "uint16_t"
| "Uint8" -> "uint8_t"
| "Char" -> "char"
| _ -> failwith (sprintf "gen_messages.c_type: unknown format '%s'" format)
let dl_type = fun format ->
match format with
"Float" -> "DL_TYPE_FLOAT"
| "Double" -> "DL_TYPE_DOUBLE"
| "Int32" -> "DL_TYPE_INT32"
| "Int16" -> "DL_TYPE_INT16"
| "Int8" -> "DL_TYPE_INT8"
| "Uint32" -> "DL_TYPE_UINT32"
| "Uint16" -> "DL_TYPE_UINT16"
| "Uint8" -> "DL_TYPE_UINT8"
| "Char" -> "DL_TYPE_CHAR"
| _ -> failwith (sprintf "gen_messages.dl_type: unknown format '%s'" format)
type field = _type * string * format option
@@ -132,14 +145,14 @@ module Gen_onboard = struct
let print_field = fun h (t, name, (_f: format option)) ->
match t with
Basic _ ->
fprintf h "\t trans->put_bytes(trans->impl, dev, %s, (void *) _%s);\n" (Syntax.sizeof t) name
fprintf h "\t trans->put_bytes(trans->impl, dev, %s, DL_FORMAT_SCALAR, %s, (void *) _%s);\n" (dl_type (Syntax.nameof t)) (Syntax.sizeof t) name
| Array (t, varname) ->
let _s = Syntax.sizeof (Basic t) in
fprintf h "\t trans->put_bytes(trans->impl, dev, 1, (void *) &%s);\n" (Syntax.length_name varname);
fprintf h "\t trans->put_bytes(trans->impl, dev, %s * %s, (void *) _%s);\n" (Syntax.sizeof (Basic t)) (Syntax.length_name varname) name
fprintf h "\t trans->put_bytes(trans->impl, dev, DL_TYPE_ARRAY_LENGTH, DL_FORMAT_SCALAR, 1, (void *) &%s);\n" (Syntax.length_name varname);
fprintf h "\t trans->put_bytes(trans->impl, dev, %s, DL_FORMAT_ARRAY, %s * %s, (void *) _%s);\n" (dl_type (Syntax.nameof (Basic t))) (Syntax.sizeof (Basic t)) (Syntax.length_name varname) name
| FixedArray (t, varname, len) ->
let _s = Syntax.sizeof (Basic t) in
fprintf h "\t trans->put_bytes(trans->impl, dev, %s * %d, (void *) _%s);\n" (Syntax.sizeof (Basic t)) len name
fprintf h "\t trans->put_bytes(trans->impl, dev, %s, DL_FORMAT_ARRAY, %s * %d, (void *) _%s);\n" (dl_type (Syntax.nameof (Basic t))) (Syntax.sizeof (Basic t)) len name
let print_macro_param h = function
(Array _, s, _) -> fprintf h "%s, %s" (Syntax.length_name s) s
@@ -197,8 +210,8 @@ module Gen_onboard = struct
fprintf h "\tif (trans->check_available_space(trans->impl, dev, trans->size_of(trans->impl, %s +2 /* msg header overhead */))) {\n" size;
fprintf h "\t trans->count_bytes(trans->impl, dev, trans->size_of(trans->impl, %s +2 /* msg header overhead */));\n" size;
fprintf h "\t trans->start_message(trans->impl, dev, %s +2 /* msg header overhead */);\n" size;
fprintf h "\t uint8_t msg_header[] = { ac_id, DL_%s };\n" s;
fprintf h "\t trans->put_bytes(trans->impl, dev, 2, (void *) msg_header);\n";
fprintf h "\t trans->put_bytes(trans->impl, dev, DL_TYPE_UINT8, DL_FORMAT_SCALAR, 1, &ac_id);\n";
fprintf h "\t trans->put_named_byte(trans->impl, dev, DL_TYPE_UINT8, DL_FORMAT_SCALAR, DL_%s, \"%s\");\n" s s;
List.iter (print_field h) fields;
fprintf h "\t trans->end_message(trans->impl, dev);\n";
fprintf h "\t} else\n";