diff --git a/conf/airframes/TUDELFT/tudelft_outback.xml b/conf/airframes/TUDELFT/tudelft_outback.xml index c39542ebaf..3940faac3e 100644 --- a/conf/airframes/TUDELFT/tudelft_outback.xml +++ b/conf/airframes/TUDELFT/tudelft_outback.xml @@ -28,6 +28,7 @@ + @@ -55,6 +56,7 @@ + diff --git a/conf/modules/telemetry_intermcu.xml b/conf/modules/telemetry_intermcu.xml new file mode 100644 index 0000000000..46004c6544 --- /dev/null +++ b/conf/modules/telemetry_intermcu.xml @@ -0,0 +1,31 @@ + + + + + Telemetry over InterMCU + This module transmits Telemetry of the process "InterMCU" from AP to FBW. The FBW then transmits this telemetry further through PPRZ over an UART. + + + + +
+ +
+ + + + + + + + + + + + + + + + + + diff --git a/conf/telemetry/TUDELFT/tudelft_outback.xml b/conf/telemetry/TUDELFT/tudelft_outback.xml index aff9583c7a..476de8c88f 100644 --- a/conf/telemetry/TUDELFT/tudelft_outback.xml +++ b/conf/telemetry/TUDELFT/tudelft_outback.xml @@ -148,6 +148,7 @@ + @@ -158,5 +159,11 @@ + + + + + + diff --git a/sw/airborne/modules/telemetry/telemetry_intermcu.h b/sw/airborne/modules/telemetry/telemetry_intermcu.h new file mode 100644 index 0000000000..342f4d4f1a --- /dev/null +++ b/sw/airborne/modules/telemetry/telemetry_intermcu.h @@ -0,0 +1,37 @@ +/* + * Copyright (C) 2016 Freek van Tienen + * + * 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 modules/telemetry/telemetry_intermcu.h + * @brief Telemetry through InterMCU + */ + +#ifndef TELEMETRY_INTERMCU_H +#define TELEMETRY_INTERMCU_H + +#include "std.h" + +/* External functions */ +void telemetry_intermcu_init(void); +void telemetry_intermcu_periodic(void); +void telemetry_intermcu_on_msg(uint8_t msg_id, uint8_t* msg, uint8_t size); + +#endif /* TELEMETRY_INTERMCU_H */ diff --git a/sw/airborne/modules/telemetry/telemetry_intermcu_ap.c b/sw/airborne/modules/telemetry/telemetry_intermcu_ap.c new file mode 100644 index 0000000000..0e3a9106d8 --- /dev/null +++ b/sw/airborne/modules/telemetry/telemetry_intermcu_ap.c @@ -0,0 +1,108 @@ +/* + * Copyright (C) 2016 Freek van Tienen + * + * 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 modules/telemetry/telemetry_intermcu_ap.c + * @brief Telemetry through InterMCU + */ + +#define PERIODIC_C_INTERMCU +#include "telemetry_intermcu.h" +#include "subsystems/intermcu.h" +#include "pprzlink/intermcu_msg.h" +#include "pprzlink/short_transport.h" +#include "generated/periodic_telemetry.h" +#include "subsystems/datalink/telemetry.h" + +/* Default maximum telemetry message size */ +#ifndef TELEMERTY_INTERMCU_MSG_SIZE +#define TELEMERTY_INTERMCU_MSG_SIZE 128 +#endif + +/* Structure for handling telemetry over InterMCU */ +struct telemetry_intermcu_t { + struct link_device dev; ///< Device structure for communication + struct short_transport trans; ///< Transport without any extra encoding + uint8_t buf[TELEMERTY_INTERMCU_MSG_SIZE]; ///< Buffer for the messages + uint8_t buf_idx; ///< Index of the buffer +}; + +/* Telemetry InterMCU throughput */ +static struct telemetry_intermcu_t telemetry_intermcu; + +/* Static functions */ +static bool telemetry_intermcu_check_free_space(struct telemetry_intermcu_t *p, long *fd __attribute__((unused)), uint16_t len); +static void telemetry_intermcu_put_byte(struct telemetry_intermcu_t *p, long fd __attribute__((unused)), uint8_t data); +static void telemetry_intermcu_put_buffer(struct telemetry_intermcu_t *p, long fd, uint8_t *data, uint16_t len); +static void telemetry_intermcu_send_message(struct telemetry_intermcu_t *p, long fd __attribute__((unused))); + +/* InterMCU initialization */ +void telemetry_intermcu_init(void) +{ + // Initialize transport structure + short_transport_init(&telemetry_intermcu.trans); + + // Configure the device + telemetry_intermcu.dev.check_free_space = (check_free_space_t)telemetry_intermcu_check_free_space; + telemetry_intermcu.dev.put_byte = (put_byte_t)telemetry_intermcu_put_byte; + telemetry_intermcu.dev.put_buffer = (put_buffer_t)telemetry_intermcu_put_buffer; + telemetry_intermcu.dev.send_message = (send_message_t)telemetry_intermcu_send_message; + telemetry_intermcu.dev.periph = (void *)&telemetry_intermcu; +} + +/* InterMCU periodic handling of telemetry */ +void telemetry_intermcu_periodic(void) +{ + periodic_telemetry_send_InterMCU(DefaultPeriodic, &telemetry_intermcu.trans.trans_tx, &telemetry_intermcu.dev); +} + +void telemetry_intermcu_on_msg(uint8_t msg_id __attribute__((unused)), uint8_t* msg __attribute__((unused)), uint8_t size __attribute__((unused))) +{ + +} + +static bool telemetry_intermcu_check_free_space(struct telemetry_intermcu_t *p, long *fd __attribute__((unused)), uint16_t len) +{ + return ((p->buf_idx + len) < (TELEMERTY_INTERMCU_MSG_SIZE - 1)); +} + +static void telemetry_intermcu_put_byte(struct telemetry_intermcu_t *p, long fd __attribute__((unused)), uint8_t data) +{ + if(p->buf_idx >= (TELEMERTY_INTERMCU_MSG_SIZE - 1)) + return; + + p->buf[p->buf_idx++] = data; +} + +static void telemetry_intermcu_put_buffer(struct telemetry_intermcu_t *p, long fd __attribute__((unused)), uint8_t *data, uint16_t len) +{ + int i; + for (i = 0; i < len; i++) { + p->buf[p->buf_idx++] = data[i]; + } +} + +static void telemetry_intermcu_send_message(struct telemetry_intermcu_t *p, long fd __attribute__((unused))) +{ + pprz_msg_send_IMCU_TELEMETRY(&(intermcu.transport.trans_tx), intermcu.device, + INTERMCU_AP, &p->buf[1], (p->buf_idx - 2), &p->buf[2]); + p->buf_idx = 0; +} diff --git a/sw/airborne/modules/telemetry/telemetry_intermcu_fbw.c b/sw/airborne/modules/telemetry/telemetry_intermcu_fbw.c new file mode 100644 index 0000000000..181d644cad --- /dev/null +++ b/sw/airborne/modules/telemetry/telemetry_intermcu_fbw.c @@ -0,0 +1,74 @@ +/* + * Copyright (C) 2016 Freek van Tienen + * + * 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 modules/telemetry/telemetry_intermcu_fbw.c + * @brief Telemetry through InterMCU + */ + +#include "telemetry_intermcu.h" +#include "subsystems/intermcu.h" +#include "pprzlink/pprz_transport.h" +#include "pprzlink/intermcu_msg.h" +#include "subsystems/datalink/telemetry.h" + +/* Structure for handling telemetry over InterMCU */ +struct telemetry_intermcu_t { + struct link_device *dev; ///< Device structure for communication + struct pprz_transport trans; ///< Transport without any extra encoding +}; + +/* Telemetry InterMCU throughput */ +static struct telemetry_intermcu_t telemetry_intermcu; + +/* Statically defined functions */ +static void telemetry_intermcu_repack(struct transport_tx *trans, struct link_device *dev, uint8_t ac_id, uint8_t msg_id, uint8_t *msg, uint8_t size); + +/* InterMCU initialization */ +void telemetry_intermcu_init(void) +{ + // Initialize transport structure + pprz_transport_init(&telemetry_intermcu.trans); + + // Set the link device + telemetry_intermcu.dev = &(TELEMETRY_INTERMCU_DEV.device); +} + +/* InterMCU periodic handling of telemetry */ +void telemetry_intermcu_periodic(void) +{ + +} + +void telemetry_intermcu_on_msg(uint8_t msg_id, uint8_t* msg, uint8_t size) +{ + telemetry_intermcu_repack(&(telemetry_intermcu.trans.trans_tx), telemetry_intermcu.dev, AC_ID, msg_id, msg, size); +} + +static void telemetry_intermcu_repack(struct transport_tx *trans, struct link_device *dev, uint8_t ac_id, uint8_t msg_id, uint8_t *msg, uint8_t size) +{ + trans->count_bytes(trans->impl, dev, trans->size_of(trans->impl, size + 2 /* msg header overhead */)); + trans->start_message(trans->impl, dev, 0, size + 2 /* msg header overhead */); + trans->put_bytes(trans->impl, dev, 0, DL_TYPE_UINT8, DL_FORMAT_SCALAR, &ac_id, 1); + trans->put_bytes(trans->impl, dev, 0, DL_TYPE_UINT8, DL_FORMAT_SCALAR, &msg_id, 1); + trans->put_bytes(trans->impl, dev, 0, DL_TYPE_UINT8, DL_FORMAT_ARRAY, (void *) msg, size); + trans->end_message(trans->impl, dev, 0); +} diff --git a/sw/airborne/subsystems/intermcu/intermcu_fbw.c b/sw/airborne/subsystems/intermcu/intermcu_fbw.c index 5b9a4a38e0..2f97d3aa5c 100644 --- a/sw/airborne/subsystems/intermcu/intermcu_fbw.c +++ b/sw/airborne/subsystems/intermcu/intermcu_fbw.c @@ -29,6 +29,7 @@ #include "subsystems/radio_control.h" #include "subsystems/electrical.h" #include "mcu_periph/uart.h" +#include "modules/telemetry/telemetry_intermcu.h" #include "modules/spektrum_soft_bind/spektrum_soft_bind_fbw.h" @@ -150,6 +151,13 @@ static void intermcu_parse_msg(void (*commands_frame_handler)(void)) commands_frame_handler(); break; } + case DL_IMCU_TELEMETRY: { + uint8_t id = DL_IMCU_TELEMETRY_msg_id(imcu_msg_buf); + uint8_t size = DL_IMCU_TELEMETRY_msg_length(imcu_msg_buf); + uint8_t *msg = DL_IMCU_TELEMETRY_msg(imcu_msg_buf); + telemetry_intermcu_on_msg(id, msg, size); + break; + } #if defined(SPEKTRUM_HAS_SOFT_BIND_PIN) //TODO: make subscribable module parser case DL_IMCU_SPEKTRUM_SOFT_BIND: