From b7b9edecf9dfd06e9ed7a67ea35c7fc8d5569459 Mon Sep 17 00:00:00 2001 From: Freek van Tienen Date: Wed, 20 Apr 2016 17:32:47 +0200 Subject: [PATCH] [intermcu] Cleanup and FBW status (#1608) --- conf/telemetry/rotorcraft_with_logger.xml | 4 +- sw/airborne/firmwares/rotorcraft/main_fbw.c | 9 +- sw/airborne/modules/px4_flash/px4_flash.c | 2 +- sw/airborne/subsystems/intermcu.h | 12 +- sw/airborne/subsystems/intermcu/intermcu_ap.c | 101 +++++++++-------- sw/airborne/subsystems/intermcu/intermcu_ap.h | 12 +- .../subsystems/intermcu/intermcu_fbw.c | 103 +++++++++++------- 7 files changed, 149 insertions(+), 94 deletions(-) diff --git a/conf/telemetry/rotorcraft_with_logger.xml b/conf/telemetry/rotorcraft_with_logger.xml index 033606fdbb..ea59d32d58 100644 --- a/conf/telemetry/rotorcraft_with_logger.xml +++ b/conf/telemetry/rotorcraft_with_logger.xml @@ -26,7 +26,9 @@ - + + + diff --git a/sw/airborne/firmwares/rotorcraft/main_fbw.c b/sw/airborne/firmwares/rotorcraft/main_fbw.c index 566aef1186..5a8243e604 100644 --- a/sw/airborne/firmwares/rotorcraft/main_fbw.c +++ b/sw/airborne/firmwares/rotorcraft/main_fbw.c @@ -105,7 +105,7 @@ STATIC_INLINE void main_init(void) modules_tid = sys_time_register_timer(1. / MODULES_FREQUENCY, NULL); radio_control_tid = sys_time_register_timer((1. / 60.), NULL); electrical_tid = sys_time_register_timer(0.1, NULL); - telemetry_tid = sys_time_register_timer((1. / 20.), NULL); + telemetry_tid = sys_time_register_timer((1. / 10.), NULL); } @@ -150,7 +150,7 @@ STATIC_INLINE void main_periodic(void) intermcu_periodic(); /* Safety logic */ - bool ap_lost = (inter_mcu.status == INTERMCU_LOST); + bool ap_lost = (intermcu.status == INTERMCU_LOST); bool rc_lost = (radio_control.status == RC_REALLY_LOST); if (rc_lost) { if (ap_lost) { @@ -189,13 +189,14 @@ STATIC_INLINE void main_periodic(void) } #endif - static uint16_t dv = 0; // TODO make module out of led blink? /* set failsafe commands */ if (fbw_mode == FBW_MODE_FAILSAFE) { autopilot_motors_on = false; SetCommands(commands_failsafe); + #ifdef FBW_MODE_LED + static uint16_t dv = 0; if (!(dv++ % (PERIODIC_FREQUENCY / 20))) { LED_TOGGLE(FBW_MODE_LED);} } else if (fbw_mode == FBW_MODE_MANUAL) { if (!(dv++ % (PERIODIC_FREQUENCY))) { LED_TOGGLE(FBW_MODE_LED);} @@ -228,7 +229,7 @@ static void autopilot_on_rc_frame(void) } /* Trying to switch to auto when AP is lost */ - if ((inter_mcu.status == INTERMCU_LOST) && + if ((intermcu.status == INTERMCU_LOST) && (fbw_mode == FBW_MODE_AUTO)) { fbw_mode = AP_LOST_FBW_MODE; } diff --git a/sw/airborne/modules/px4_flash/px4_flash.c b/sw/airborne/modules/px4_flash/px4_flash.c index 54b8ab9aba..dad00a6927 100644 --- a/sw/airborne/modules/px4_flash/px4_flash.c +++ b/sw/airborne/modules/px4_flash/px4_flash.c @@ -158,7 +158,7 @@ void px4flash_event(void) //stop all intermcu communication: - disable_inter_comm(true); + intermcu_set_enabled(FALSE); px4iobl_tid = sys_time_register_timer(5.0, NULL); //10 (fbw pprz bl timeout)-5 (px4 fmu bl timeout) /* diff --git a/sw/airborne/subsystems/intermcu.h b/sw/airborne/subsystems/intermcu.h index 031ad03a5a..4ac35ffb5f 100644 --- a/sw/airborne/subsystems/intermcu.h +++ b/sw/airborne/subsystems/intermcu.h @@ -29,6 +29,7 @@ #include "std.h" #include "subsystems/commands.h" +#include "pprzlink/pprz_transport.h" #define INTERMCU_AP 0 #define INTERMCU_FBW 1 @@ -51,13 +52,18 @@ enum intermcu_PX4_baud_status { }; struct intermcu_t { - enum intermcu_status status; - uint8_t time_since_last_frame; + struct link_device *device; ///< Device used for communication + struct pprz_transport transport; ///< Transport over communication line (PPRZ) + enum intermcu_status status; ///< Status of the INTERMCU + uint8_t time_since_last_frame; ///< Time since last frame + bool enabled; ///< If the InterMCU communication is enabled + bool msg_available; ///< If we have an InterMCU message + #ifdef BOARD_PX4IO enum intermcu_PX4_baud_status stable_px4_baud; #endif }; -extern struct intermcu_t inter_mcu; +extern struct intermcu_t intermcu; diff --git a/sw/airborne/subsystems/intermcu/intermcu_ap.c b/sw/airborne/subsystems/intermcu/intermcu_ap.c index f46eb7de0d..ebd8232bf9 100644 --- a/sw/airborne/subsystems/intermcu/intermcu_ap.c +++ b/sw/airborne/subsystems/intermcu/intermcu_ap.c @@ -1,5 +1,5 @@ /* - * Copyright (C) 2015 The Paparazzi Team + * Copyright (C) 2015 Freek van Tienen * * This file is part of paparazzi. * @@ -27,10 +27,8 @@ #include "intermcu_ap.h" #include "pprzlink/intermcu_msg.h" #include "subsystems/radio_control.h" -#include "pprzlink/pprz_transport.h" #include "mcu_periph/uart.h" -#include "subsystems/datalink/telemetry.h" #include "subsystems/electrical.h" #include "autopilot.h" @@ -38,75 +36,87 @@ #error "INTERMCU UART CAN ONLY SEND 8 COMMANDS OR THE UART WILL BE OVERFILLED" #endif -// Used for communication -static struct link_device *intermcu_device = (&((INTERMCU_LINK).device)); -static struct pprz_transport intermcu_transport; -struct intermcu_t inter_mcu; -static inline void intermcu_parse_msg(struct transport_rx *trans, void (*rc_frame_handler)(void)); +/* Main InterMCU defines */ +struct intermcu_t intermcu = { + .device = (&((INTERMCU_LINK).device)), + .enabled = true, + .msg_available = false +}; +uint8_t imcu_msg_buf[128] __attribute__((aligned)); ///< The InterMCU message buffer +static struct fbw_status_t fbw_status; +static inline void intermcu_parse_msg(void (*rc_frame_handler)(void)); +#if PERIODIC_TELEMETRY +#include "subsystems/datalink/telemetry.h" +/* Send FBW status */ static void send_status(struct transport_tx *trans, struct link_device *dev) { pprz_msg_send_FBW_STATUS(trans, dev, AC_ID, - &(radio_control.status), &(radio_control.frame_rate), &(inter_mcu.status), &electrical.vsupply, - &electrical.current); // due to limitation of GCS, send the electrical from ap as if it comes from fbw... + &fbw_status.rc_status, &fbw_status.frame_rate, &fbw_status.mode, &fbw_status.vsupply, + &fbw_status.current); } +#endif +/* InterMCU initialization */ void intermcu_init(void) { - pprz_transport_init(&intermcu_transport); + pprz_transport_init(&intermcu.transport); +#if PERIODIC_TELEMETRY register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_FBW_STATUS, send_status); - +#endif } +/* Check for InterMCU loss */ void intermcu_periodic(void) { /* Check for interMCU loss */ - if (inter_mcu.time_since_last_frame >= INTERMCU_LOST_CNT) { - inter_mcu.status = INTERMCU_LOST; + if (intermcu.time_since_last_frame >= INTERMCU_LOST_CNT) { + intermcu.status = INTERMCU_LOST; } else { - inter_mcu.time_since_last_frame++; + intermcu.time_since_last_frame++; } } -static bool disable_comm; -void disable_inter_comm(bool value) +/* Enable or disable the communication of the InterMCU */ +void intermcu_set_enabled(bool value) { - disable_comm = value; + intermcu.enabled = value; } +/* Send the actuators to the FBW */ void intermcu_set_actuators(pprz_t *command_values, uint8_t ap_mode __attribute__((unused))) { - if (!disable_comm) { - uint8_t autopilot_motors_on_tmp = autopilot_motors_on; - pprz_msg_send_IMCU_COMMANDS(&(intermcu_transport.trans_tx), intermcu_device, - INTERMCU_AP, &autopilot_motors_on_tmp, COMMANDS_NB, command_values); //TODO: Append more status + if (intermcu.enabled) { + pprz_msg_send_IMCU_COMMANDS(&(intermcu.transport.trans_tx), intermcu.device, + INTERMCU_AP, &autopilot_motors_on, COMMANDS_NB, command_values); //TODO: Append more status } } +/* Send the spektrum Bind message */ void intermcu_send_spektrum_bind(void) { - if (!disable_comm) { - pprz_msg_send_IMCU_SPEKTRUM_SOFT_BIND(&(intermcu_transport.trans_tx), intermcu_device, INTERMCU_AP); + if (intermcu.enabled) { + pprz_msg_send_IMCU_SPEKTRUM_SOFT_BIND(&(intermcu.transport.trans_tx), intermcu.device, INTERMCU_AP); } } - -static inline void intermcu_parse_msg(struct transport_rx *trans, void (*rc_frame_handler)(void)) +/* Parse incomming InterMCU messages */ +#pragma GCC diagnostic ignored "-Wcast-align" +static inline void intermcu_parse_msg(void (*rc_frame_handler)(void)) { /* Parse the Inter MCU message */ - uint8_t msg_id = trans->payload[1]; + uint8_t msg_id = imcu_msg_buf[1]; switch (msg_id) { case DL_IMCU_RADIO_COMMANDS: { uint8_t i; - uint8_t size = DL_IMCU_RADIO_COMMANDS_values_length(trans->payload); - inter_mcu.status = DL_IMCU_RADIO_COMMANDS_status(trans->payload); - int16_t *rc_values = DL_IMCU_RADIO_COMMANDS_values(trans->payload); + uint8_t size = DL_IMCU_RADIO_COMMANDS_values_length(imcu_msg_buf); + intermcu.status = DL_IMCU_RADIO_COMMANDS_status(imcu_msg_buf); for (i = 0; i < size; i++) { - radio_control.values[i] = rc_values[i]; + radio_control.values[i] = DL_IMCU_RADIO_COMMANDS_values(imcu_msg_buf)[i]; } radio_control.frame_cpt++; @@ -116,26 +126,31 @@ static inline void intermcu_parse_msg(struct transport_rx *trans, void (*rc_fram break; } + case DL_IMCU_FBW_STATUS: { + fbw_status.rc_status = DL_IMCU_FBW_STATUS_rc_status(imcu_msg_buf); + fbw_status.frame_rate = DL_IMCU_FBW_STATUS_frame_rate(imcu_msg_buf); + fbw_status.mode = DL_IMCU_FBW_STATUS_mode(imcu_msg_buf); + fbw_status.vsupply = DL_IMCU_FBW_STATUS_vsupply(imcu_msg_buf); + fbw_status.current = DL_IMCU_FBW_STATUS_current(imcu_msg_buf); + break; + } + default: break; } - - // Set to receive another message - trans->msg_received = false; } +#pragma GCC diagnostic pop +/* Radio control event misused as InterMCU event for frame_handler */ void RadioControlEvent(void (*frame_handler)(void)) { - if (!disable_comm) { - /* Parse incoming bytes */ - if (intermcu_device->char_available(intermcu_device->periph)) { - while (intermcu_device->char_available(intermcu_device->periph) && !intermcu_transport.trans_rx.msg_received) { - parse_pprz(&intermcu_transport, intermcu_device->get_byte(intermcu_device->periph)); - } + /* Parse incoming bytes */ + if (intermcu.enabled) { + pprz_check_and_parse(intermcu.device, &intermcu.transport, imcu_msg_buf, &intermcu.msg_available); - if (intermcu_transport.trans_rx.msg_received) { - intermcu_parse_msg(&(intermcu_transport.trans_rx), frame_handler); - } + if (intermcu.msg_available) { + intermcu_parse_msg(frame_handler); + intermcu.msg_available = false; } } } diff --git a/sw/airborne/subsystems/intermcu/intermcu_ap.h b/sw/airborne/subsystems/intermcu/intermcu_ap.h index aba906327e..d3eb036ccf 100644 --- a/sw/airborne/subsystems/intermcu/intermcu_ap.h +++ b/sw/airborne/subsystems/intermcu/intermcu_ap.h @@ -1,5 +1,5 @@ /* - * Copyright (C) 2015 The Paparazzi Team + * Copyright (C) 2015 Freek van Tienen * * This file is part of paparazzi. * @@ -33,7 +33,7 @@ void intermcu_set_actuators(pprz_t *command_values, uint8_t ap_mode); void RadioControlEvent(void (*frame_handler)(void)); void intermcu_send_spektrum_bind(void); -void disable_inter_comm(bool value); +void intermcu_set_enabled(bool value); /* We need radio defines for the Autopilot */ #define RADIO_THROTTLE 0 @@ -47,5 +47,13 @@ void disable_inter_comm(bool value); #define RADIO_AUX3 7 #define RADIO_CONTROL_NB_CHANNEL 8 +/* Structure for FBW status */ +struct fbw_status_t { + uint8_t rc_status; + uint8_t frame_rate; + uint8_t mode; + uint16_t vsupply; + int32_t current; +}; #endif /* INTERMCU_AP_ROTORCRAFT_H */ diff --git a/sw/airborne/subsystems/intermcu/intermcu_fbw.c b/sw/airborne/subsystems/intermcu/intermcu_fbw.c index 3274774d41..0279f8ef80 100644 --- a/sw/airborne/subsystems/intermcu/intermcu_fbw.c +++ b/sw/airborne/subsystems/intermcu/intermcu_fbw.c @@ -1,5 +1,5 @@ /* - * Copyright (C) 2015 The Paparazzi Team + * Copyright (C) 2015 Freek van Tienen * * This file is part of paparazzi. * @@ -27,8 +27,9 @@ #include "intermcu_fbw.h" #include "pprzlink/intermcu_msg.h" #include "subsystems/radio_control.h" +#include "subsystems/electrical.h" #include "mcu_periph/uart.h" -#include "pprzlink/pprz_transport.h" + #include "modules/spektrum_soft_bind/spektrum_soft_bind_fbw.h" #include BOARD_CONFIG @@ -40,48 +41,51 @@ static uint8_t px4RebootSequence[] = {0x41, 0xd7, 0x32, 0x0a, 0x46, 0x39}; static uint8_t px4RebootSequenceCount = 0; tid_t px4bl_tid; ///< id for time out of the px4 bootloader reset #endif -bool autopilot_motors_on = false; #if RADIO_CONTROL_NB_CHANNEL > 8 #undef RADIO_CONTROL_NB_CHANNEL #define RADIO_CONTROL_NB_CHANNEL 8 -#warning "INTERMCU UART WILL ONLY SEND 8 RADIO CHANNELS" +INFO("InterMCU UART will only send 8 radio channels!") #endif -// Used for communication -static struct link_device *intermcu_device = (&((INTERMCU_LINK).device)); -static struct pprz_transport intermcu_transport; +/* Main InterMCU structure */ +struct intermcu_t intermcu = { + .device = (&((INTERMCU_LINK).device)), + .enabled = true, + .msg_available = false +}; +uint8_t imcu_msg_buf[128] __attribute__((aligned)); ///< The InterMCU message buffer -struct intermcu_t inter_mcu; pprz_t intermcu_commands[COMMANDS_NB]; -static void intermcu_parse_msg(struct transport_rx *trans, void (*commands_frame_handler)(void)); +bool autopilot_motors_on = false; +static void intermcu_parse_msg(void (*commands_frame_handler)(void)); + #ifdef BOARD_PX4IO static void checkPx4RebootCommand(unsigned char b); #endif void intermcu_init(void) { - pprz_transport_init(&intermcu_transport); + pprz_transport_init(&intermcu.transport); + #ifdef BOARD_PX4IO px4bl_tid = sys_time_register_timer(10.0, NULL); #endif - } void intermcu_periodic(void) { /* Check for interMCU loss */ - if (inter_mcu.time_since_last_frame >= INTERMCU_LOST_CNT) { - inter_mcu.status = INTERMCU_LOST; + if (intermcu.time_since_last_frame >= INTERMCU_LOST_CNT) { + intermcu.status = INTERMCU_LOST; } else { - inter_mcu.time_since_last_frame++; + intermcu.time_since_last_frame++; } } void intermcu_on_rc_frame(uint8_t fbw_mode) { - - pprz_t values[8]; + pprz_t values[9]; values[INTERMCU_RADIO_THROTTLE] = radio_control.values[RADIO_THROTTLE]; values[INTERMCU_RADIO_ROLL] = radio_control.values[RADIO_ROLL]; @@ -105,21 +109,21 @@ void intermcu_on_rc_frame(uint8_t fbw_mode) values[INTERMCU_RADIO_AUX2] = radio_control.values[RADIO_AUX2]; #endif #ifdef RADIO_AUX3 - values[INTERMCU_RADIO_AUX2] = radio_control.values[RADIO_AUX2]; + values[INTERMCU_RADIO_AUX3] = radio_control.values[RADIO_AUX3]; #endif - pprz_msg_send_IMCU_RADIO_COMMANDS(&(intermcu_transport.trans_tx), intermcu_device, + pprz_msg_send_IMCU_RADIO_COMMANDS(&(intermcu.transport.trans_tx), intermcu.device, INTERMCU_FBW, &fbw_mode, RADIO_CONTROL_NB_CHANNEL, values); } void intermcu_send_status(uint8_t mode) { // Send Status - (void)mode; - //FIXME + pprz_msg_send_IMCU_FBW_STATUS(&(intermcu.transport.trans_tx), intermcu.device, INTERMCU_FBW, + &mode, &(radio_control.status), &(radio_control.frame_rate), &electrical.vsupply, + &electrical.current); } - void intermcu_blink_fbw_led(uint16_t dv) { #ifdef FBW_MODE_LED @@ -131,26 +135,29 @@ void intermcu_blink_fbw_led(uint16_t dv) } else { LED_TOGGLE(FBW_MODE_LED); // toggle makes random blinks if intermcu comm problem! } +#else + (void)dv; #endif } -static void intermcu_parse_msg(struct transport_rx *trans, void (*commands_frame_handler)(void)) +#pragma GCC diagnostic ignored "-Wcast-align" +static void intermcu_parse_msg(void (*commands_frame_handler)(void)) { /* Parse the Inter MCU message */ - uint8_t msg_id = trans->payload[1]; + uint8_t msg_id = imcu_msg_buf[1]; switch (msg_id) { case DL_IMCU_COMMANDS: { uint8_t i; - uint8_t size = DL_IMCU_COMMANDS_values_length(trans->payload); - int16_t *new_commands = DL_IMCU_COMMANDS_values(trans->payload); - uint8_t status = DL_IMCU_COMMANDS_status(trans->payload); + uint8_t size = DL_IMCU_COMMANDS_values_length(imcu_msg_buf); + int16_t *new_commands = DL_IMCU_COMMANDS_values(imcu_msg_buf); + uint8_t status = DL_IMCU_COMMANDS_status(imcu_msg_buf); autopilot_motors_on = status & 0x1; for (i = 0; i < size; i++) { intermcu_commands[i] = new_commands[i]; } - inter_mcu.status = INTERMCU_OK; - inter_mcu.time_since_last_frame = 0; + intermcu.status = INTERMCU_OK; + intermcu.time_since_last_frame = 0; commands_frame_handler(); break; } @@ -160,34 +167,50 @@ static void intermcu_parse_msg(struct transport_rx *trans, void (*commands_frame received_spektrum_soft_bind(); break; #endif + default: break; } - - // Set to receive another message - trans->msg_received = false; } +#pragma GCC diagnostic pop void InterMcuEvent(void (*frame_handler)(void)) { + uint8_t i, c; + + // Check if there are messages in the device + if (intermcu.device->char_available(intermcu.device->periph)) { + while (intermcu.device->char_available(intermcu.device->periph) && !intermcu.transport.trans_rx.msg_received) { + c = intermcu.device->get_byte(intermcu.device->periph); + parse_pprz(&intermcu.transport, c); - /* Parse incoming bytes */ - if (intermcu_device->char_available(intermcu_device->periph)) { - while (intermcu_device->char_available(intermcu_device->periph) && !intermcu_transport.trans_rx.msg_received) { - unsigned char b = intermcu_device->get_byte(intermcu_device->periph); #ifdef BOARD_PX4IO - checkPx4RebootCommand(b); + // TODO: create a hook + checkPx4RebootCommand(c); #endif - parse_pprz(&intermcu_transport, b); } - if (intermcu_transport.trans_rx.msg_received) { - intermcu_parse_msg(&(intermcu_transport.trans_rx), frame_handler); + // If we have a message copy it + if (intermcu.transport.trans_rx.msg_received) { + for (i = 0; i < intermcu.transport.trans_rx.payload_len; i++) { + imcu_msg_buf[i] = intermcu.transport.trans_rx.payload[i]; + } + + intermcu.msg_available = true; + intermcu.transport.trans_rx.msg_received = false; } } + + if (intermcu.msg_available) { + intermcu_parse_msg(frame_handler); + intermcu.msg_available = false; + } } + + +/* SOME STUFF FOR PX4IO BOOTLOADER (TODO: move this code) */ #ifdef BOARD_PX4IO -static void checkPx4RebootCommand(unsigned char b) +static void checkPx4RebootCommand(uint8_t b) { if (inter_mcu.stable_px4_baud == CHANGING_BAUD && sys_time_check_and_ack_timer(px4bl_tid)) { //to prevent a short intermcu comm loss, give some time to changing the baud