From b7129a5fe796d7b2a0c4a41b3dff2728c75fa56b Mon Sep 17 00:00:00 2001 From: dewagter Date: Fri, 26 Sep 2014 06:29:28 +0200 Subject: [PATCH] [fixedwing] Option to have telemetry through the FBW - for outback safety (so it can decode flight termination modes even when AP is down) - Keep datalink while in flight termination mode closes #844 --- .../fixedwing/fbw_datalink.makefile | 5 + .../firmwares/fixedwing/fbw_datalink.c | 91 +++++++++++++++++++ .../firmwares/fixedwing/fbw_datalink.h | 32 +++++++ sw/airborne/firmwares/fixedwing/main_fbw.c | 17 +++- 4 files changed, 144 insertions(+), 1 deletion(-) create mode 100644 conf/firmwares/subsystems/fixedwing/fbw_datalink.makefile create mode 100644 sw/airborne/firmwares/fixedwing/fbw_datalink.c create mode 100644 sw/airborne/firmwares/fixedwing/fbw_datalink.h diff --git a/conf/firmwares/subsystems/fixedwing/fbw_datalink.makefile b/conf/firmwares/subsystems/fixedwing/fbw_datalink.makefile new file mode 100644 index 0000000000..4983c1fab2 --- /dev/null +++ b/conf/firmwares/subsystems/fixedwing/fbw_datalink.makefile @@ -0,0 +1,5 @@ + +fbw.srcs += firmwares/fixedwing/fbw_datalink.c +fbw.CFLAGS += -DFBW_DATALINK +fbw.CFLAGS += -DMODEM_LINK=$(MODEM_PORT) -DUSE_$(MODEM_PORT) -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD) +fbw.CFLAGS += -DAUTOPILOT_LINK=$(AUTOPILOT_PORT) -DUSE_$(AUTOPILOT_PORT) -D$(AUTOPILOT_PORT)_BAUD=$(MODEM_BAUD) diff --git a/sw/airborne/firmwares/fixedwing/fbw_datalink.c b/sw/airborne/firmwares/fixedwing/fbw_datalink.c new file mode 100644 index 0000000000..d09a3fa017 --- /dev/null +++ b/sw/airborne/firmwares/fixedwing/fbw_datalink.c @@ -0,0 +1,91 @@ +/* + * Copyright (C) 2014 Christophe De Wagter + * + * 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 fbw_datalink.c + * + * Datalink through FBW (FlyByWire) process/mcu. + * So it can decode flight termination modes even when AP is down. + * + */ + +#include "firmwares/fixedwing/fbw_datalink.h" +#include "mcu_periph/uart.h" +#include "led.h" + + +#define __ModemLink(dev, _x) dev##_x +#define _ModemLink(dev, _x) __ModemLink(dev, _x) +#define ModemLink(_x) _ModemLink(MODEM_LINK, _x) +#define ModemBuffer() ModemLink(ChAvailable()) + + +#define __AutopilotLink(dev, _x) dev##_x +#define _AutopilotLink(dev, _x) __AutopilotLink(dev, _x) +#define AutopilotLink(_x) _AutopilotLink(AUTOPILOT_LINK, _x) + +#define AutopilotBuffer() AutopilotLink(ChAvailable()) + +static inline void autopilot_parse(char c) +{ + ModemLink(Transmit(c)); +} + +static inline void modem_parse(char c) +{ + AutopilotLink(Transmit(c)); +} + +#define ReadAutopilotBuffer() { \ + while (AutopilotLink(ChAvailable())) \ + autopilot_parse(AutopilotLink(Getch())); \ + } + +#define ReadModemBuffer() { \ + while (ModemLink(ChAvailable())) \ + modem_parse(ModemLink(Getch())); \ + } + +void fbw_datalink_periodic(void) +{ +#ifdef MODEM_LINK_LED + LED_OFF(MODEM_LINK_LED); +#endif +#ifdef AUTOPILOT_LINK_LED + LED_OFF(AUTOPILOT_LINK_LED); +#endif +} + +void fbw_datalink_event(void) +{ +#ifdef MODEM_LINK_LED + if (ModemLink(ChAvailable())) { + LED_ON(MODEM_LINK_LED); + } +#endif +#ifdef AUTOPILOT_LINK_LED + if (AutopilotLink(ChAvailable())) { + LED_ON(AUTOPILOT_LINK_LED); + } +#endif + + ReadModemBuffer(); + ReadAutopilotBuffer(); +} diff --git a/sw/airborne/firmwares/fixedwing/fbw_datalink.h b/sw/airborne/firmwares/fixedwing/fbw_datalink.h new file mode 100644 index 0000000000..d60a1e7938 --- /dev/null +++ b/sw/airborne/firmwares/fixedwing/fbw_datalink.h @@ -0,0 +1,32 @@ +/* + * Copyright (C) 2014 Christophe De Wagter + * + * 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 fbw_datalink.h + * Handling of messages coming from ground in FTD + */ + +#ifndef FBW_DATALINK_H +#define FBW_DATALINK_H + +extern void fbw_datalink_periodic(void); +extern void fbw_datalink_event(void); + +#endif /* FBW_DATALINK_H */ diff --git a/sw/airborne/firmwares/fixedwing/main_fbw.c b/sw/airborne/firmwares/fixedwing/main_fbw.c index 819b5f4ad0..26f8ee1a20 100644 --- a/sw/airborne/firmwares/fixedwing/main_fbw.c +++ b/sw/airborne/firmwares/fixedwing/main_fbw.c @@ -48,6 +48,10 @@ #include "subsystems/datalink/telemetry.h" #endif +#ifdef FBW_DATALINK +#include "firmwares/fixedwing/fbw_datalink.h" +#endif + uint8_t fbw_mode; #include "inter_mcu.h" @@ -265,7 +269,11 @@ void event_task_fbw( void) { #if OUTBACK_CHALLENGE_VERY_DANGEROUS_RULE_AP_CAN_FORCE_FAILSAFE if (crash == 1) { - for (;;) {} + for (;;) { +#if FBW_DATALINK + fbw_datalink_event(); +#endif + } } #endif @@ -282,12 +290,19 @@ void event_task_fbw( void) { #endif /* MCU_SPI_LINK */ #endif /* INTER_MCU */ +#ifdef FBW_DATALINK + fbw_datalink_event(); +#endif } /************* PERIODIC ******************************************************/ void periodic_task_fbw( void ) { +#ifdef FBW_DATALINK + fbw_datalink_periodic(); +#endif + #ifdef RADIO_CONTROL radio_control_periodic_task(); if (fbw_mode == FBW_MODE_MANUAL && radio_control.status == RC_REALLY_LOST) {