diff --git a/Makefile b/Makefile index 99faf86ef1..ff5045377d 100644 --- a/Makefile +++ b/Makefile @@ -99,10 +99,11 @@ XSENS_PROTOCOL_H=$(STATICINCLUDE)/xsens_protocol.h DL_PROTOCOL_H=$(STATICINCLUDE)/dl_protocol.h DL_PROTOCOL2_H=$(STATICINCLUDE)/dl_protocol2.h ABI_MESSAGES_H=$(STATICINCLUDE)/abi_messages.h +INTERMCU_MSG_H=$(STATICINCLUDE)/intermcu_msg.h MAVLINK_DIR=$(STATICINCLUDE)/mavlink/ MAVLINK_PROTOCOL_H=$(MAVLINK_DIR)protocol.h -GEN_HEADERS = $(MESSAGES_H) $(UBX_PROTOCOL_H) $(MTK_PROTOCOL_H) $(XSENS_PROTOCOL_H) $(DL_PROTOCOL_H) $(ABI_MESSAGES_H) $(MAVLINK_PROTOCOL_H) +GEN_HEADERS = $(MESSAGES_H) $(UBX_PROTOCOL_H) $(MTK_PROTOCOL_H) $(XSENS_PROTOCOL_H) $(DL_PROTOCOL_H) $(ABI_MESSAGES_H) $(INTERMCU_MSG_H) $(MAVLINK_PROTOCOL_H) all: ground_segment ext lpctools @@ -244,6 +245,14 @@ $(ABI_MESSAGES_H) : $(ABI_XML) generators $(Q)mv $($@_TMP) $@ $(Q)chmod a+r $@ +$(INTERMCU_MSG_H) : $(MESSAGES_XML) generators + $(Q)test -d $(STATICINCLUDE) || mkdir -p $(STATICINCLUDE) + @echo GENERATE $@ + $(eval $@_TMP := $(shell $(MKTEMP))) + $(Q)PAPARAZZI_SRC=$(PAPARAZZI_SRC) PAPARAZZI_HOME=$(PAPARAZZI_HOME) $(GENERATORS)/gen_messages.out $< intermcu > $($@_TMP) + $(Q)mv $($@_TMP) $@ + $(Q)chmod a+r $@ + $(MAVLINK_PROTOCOL_H) : @echo GENERATE $(MAVLINK_DIR) $(Q)make -C $(PAPARAZZI_HOME)/sw/ext mavlink diff --git a/conf/airframes/TUDelft/airframes/logo600.xml b/conf/airframes/TUDelft/airframes/logo600.xml index 03cd7aa3f8..2ea2c4b5a3 100644 --- a/conf/airframes/TUDelft/airframes/logo600.xml +++ b/conf/airframes/TUDelft/airframes/logo600.xml @@ -16,13 +16,6 @@ - - - - - - - @@ -30,6 +23,7 @@ + diff --git a/conf/airframes/TUDelft/airframes/test.xml b/conf/airframes/TUDelft/airframes/test.xml index 3811973103..75b3329df7 100644 --- a/conf/airframes/TUDelft/airframes/test.xml +++ b/conf/airframes/TUDelft/airframes/test.xml @@ -18,20 +18,14 @@ - - - - - - - - + + @@ -48,7 +42,6 @@ - diff --git a/conf/airframes/TUDelft/conf.xml b/conf/airframes/TUDelft/conf.xml index 49367bc29a..f33153388f 100644 --- a/conf/airframes/TUDelft/conf.xml +++ b/conf/airframes/TUDelft/conf.xml @@ -90,12 +90,12 @@ - + @@ -2875,6 +2875,17 @@ + + + + + + + + + + + diff --git a/sw/airborne/firmwares/rotorcraft/main.c b/sw/airborne/firmwares/rotorcraft/main.c index 9de71c234c..02c7a9b1d4 100644 --- a/sw/airborne/firmwares/rotorcraft/main.c +++ b/sw/airborne/firmwares/rotorcraft/main.c @@ -164,12 +164,19 @@ STATIC_INLINE void main_init(void) stateInit(); +#ifndef INTER_MCU_AP actuators_init(); +#else + intermcu_init(); +#endif + #if USE_MOTOR_MIXING motor_mixing_init(); #endif +#ifndef INTER_MCU_AP radio_control_init(); +#endif #if USE_BARO_BOARD baro_init(); @@ -203,6 +210,10 @@ STATIC_INLINE void main_init(void) downlink_init(); #endif +#ifdef INTER_MCU_AP + intermcu_init(); +#endif + // register the timers for the periodic functions main_periodic_tid = sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL); modules_tid = sys_time_register_timer(1. / MODULES_FREQUENCY, NULL); @@ -266,7 +277,11 @@ STATIC_INLINE void main_periodic(void) autopilot_periodic(); /* set actuators */ //actuators_set(autopilot_motors_on); +#ifndef INTER_MCU_AP SetActuatorsFromCommands(commands, autopilot_mode); +#else + intermcu_set_actuators(commands, autopilot_mode); +#endif if (autopilot_in_flight) { RunOnceEvery(PERIODIC_FREQUENCY, autopilot_flight_time++); diff --git a/sw/airborne/firmwares/rotorcraft/main_fbw.c b/sw/airborne/firmwares/rotorcraft/main_fbw.c index 6c897a4520..78642db201 100644 --- a/sw/airborne/firmwares/rotorcraft/main_fbw.c +++ b/sw/airborne/firmwares/rotorcraft/main_fbw.c @@ -42,7 +42,7 @@ #include "subsystems/radio_control.h" -#include "subsystems/intermcu.h" +#include "subsystems/intermcu/intermcu_fbw.h" #include "firmwares/rotorcraft/main_fbw.h" #include "firmwares/rotorcraft/autopilot_rc_helpers.h" @@ -99,6 +99,8 @@ STATIC_INLINE void main_init(void) mcu_int_enable(); + intermcu_init(); + // register the timers for the periodic functions main_periodic_tid = sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL); // modules_tid = sys_time_register_timer(1. / MODULES_FREQUENCY, NULL); @@ -214,14 +216,14 @@ static void autopilot_on_rc_frame(void) #endif } - /* Formward radiocontrol to AP */ + /* Forward radiocontrol to AP */ intermcu_on_rc_frame(); } static void autopilot_on_ap_command(void) { if (fbw_mode != FBW_MODE_MANUAL) { - SetCommands(from_ap.commands); + SetCommands(intermcu_commands); } } diff --git a/sw/airborne/mcu_periph/link_device.h b/sw/airborne/mcu_periph/link_device.h index e282686d79..68bd498daa 100644 --- a/sw/airborne/mcu_periph/link_device.h +++ b/sw/airborne/mcu_periph/link_device.h @@ -48,6 +48,10 @@ struct link_device { char_available_t char_available; ///< check if a new character is available get_byte_t get_byte; ///< get a new char void *periph; ///< pointer to parent implementation + + uint16_t nb_msgs; ///< The number of messages send + uint8_t nb_ovrn; ///< The number of overruns + uint32_t nb_bytes; ///< The number of bytes send }; #endif // LINK_DEVICE_H diff --git a/sw/airborne/subsystems/datalink/downlink.c b/sw/airborne/subsystems/datalink/downlink.c index 72370c212d..4fd8467571 100644 --- a/sw/airborne/subsystems/datalink/downlink.c +++ b/sw/airborne/subsystems/datalink/downlink.c @@ -28,8 +28,6 @@ #include "subsystems/datalink/downlink.h" -struct downlink downlink; - #if PERIODIC_TELEMETRY #include "subsystems/datalink/telemetry.h" #include "subsystems/datalink/datalink.h" @@ -45,29 +43,30 @@ static void send_downlink(struct transport_tx *trans, struct link_device *dev) uint32_t now_ts = get_sys_time_msec(); // compute downlink byte rate if (now_ts > last_ts) { - uint16_t down_rate = (1000 * ((uint32_t)downlink.nb_bytes - last_down_nb_bytes)) / (now_ts - last_ts); + uint16_t down_rate = (1000 * ((uint32_t)dev->nb_bytes - last_down_nb_bytes)) / (now_ts - last_ts); uint16_t up_rate = (1000 * ((uint32_t)datalink_nb_msgs - last_up_nb_msgs)) / (now_ts - last_ts); last_ts = now_ts; #if defined DATALINK || defined SITL - last_down_nb_bytes = downlink.nb_bytes; + last_down_nb_bytes = dev->nb_bytes; last_up_nb_msgs = datalink_nb_msgs; #else last_down_nb_bytes = 0; last_up_nb_msgs = 0; #endif - pprz_msg_send_DATALINK_REPORT(trans, dev, AC_ID, &datalink_time, &datalink_nb_msgs, &downlink.nb_msgs, &down_rate, - &up_rate, &downlink.nb_ovrn); + pprz_msg_send_DATALINK_REPORT(trans, dev, AC_ID, &datalink_time, &datalink_nb_msgs, &dev->nb_msgs, &down_rate, + &up_rate, &dev->nb_ovrn); } } #endif void downlink_init(void) { - downlink.nb_ovrn = 0; - downlink.nb_bytes = 0; - downlink.nb_msgs = 0; + // Set initial counters + (DefaultDevice).device.nb_ovrn = 0; + (DefaultDevice).device.nb_bytes = 0; + (DefaultDevice).device.nb_msgs = 0; #if defined DATALINK diff --git a/sw/airborne/subsystems/datalink/downlink.h b/sw/airborne/subsystems/datalink/downlink.h index 1dae2bb634..aeb2044464 100644 --- a/sw/airborne/subsystems/datalink/downlink.h +++ b/sw/airborne/subsystems/datalink/downlink.h @@ -83,15 +83,6 @@ #define DefaultDevice DOWNLINK_DEVICE #endif -/** Downlink structure */ -struct downlink { - uint8_t nb_ovrn; ///< Counter of messages not sent because of unavailability of the output buffer - uint16_t nb_bytes; ///< Number of bytes send over telemetry - uint16_t nb_msgs; ///< Number of messages send over telemetry -}; - -extern struct downlink downlink; - // Init function extern void downlink_init(void); diff --git a/sw/airborne/subsystems/datalink/ivy_transport.c b/sw/airborne/subsystems/datalink/ivy_transport.c index b3eab14ccc..30931c45d4 100644 --- a/sw/airborne/subsystems/datalink/ivy_transport.c +++ b/sw/airborne/subsystems/datalink/ivy_transport.c @@ -149,25 +149,25 @@ static void start_message(struct ivy_transport *trans, struct link_device *dev _ trans->ivy_p = trans->ivy_buf; } -static void end_message(struct ivy_transport *trans, struct link_device *dev __attribute__((unused))) +static void end_message(struct ivy_transport *trans, struct link_device *dev) { *(--trans->ivy_p) = '\0'; if (trans->ivy_dl_enabled) { IvySendMsg("%s", trans->ivy_buf); - downlink.nb_msgs++; + dev->nb_msgs++; } } static void overrun(struct ivy_transport *trans __attribute__((unused)), - struct link_device *dev __attribute__((unused))) + struct link_device *dev) { - downlink.nb_ovrn++; + dev->nb_ovrn++; } static void count_bytes(struct ivy_transport *trans __attribute__((unused)), - struct link_device *dev __attribute__((unused)), uint8_t bytes) + struct link_device *dev, uint8_t bytes) { - downlink.nb_bytes += bytes; + dev->nb_bytes += bytes; } static int check_available_space(struct ivy_transport *trans __attribute__((unused)), diff --git a/sw/airborne/subsystems/datalink/pprz_transport.c b/sw/airborne/subsystems/datalink/pprz_transport.c index 375cb63cbc..dc6b7f91bc 100644 --- a/sw/airborne/subsystems/datalink/pprz_transport.c +++ b/sw/airborne/subsystems/datalink/pprz_transport.c @@ -81,12 +81,12 @@ static uint8_t size_of(struct pprz_transport *trans __attribute__((unused)), uin static void start_message(struct pprz_transport *trans, struct link_device *dev, uint8_t payload_len) { - downlink.nb_msgs++; dev->put_byte(dev->periph, STX); const uint8_t msg_len = size_of(trans, payload_len); dev->put_byte(dev->periph, msg_len); trans->ck_a_tx = msg_len; trans->ck_b_tx = msg_len; + dev->nb_msgs++; } static void end_message(struct pprz_transport *trans, struct link_device *dev) @@ -97,15 +97,15 @@ static void end_message(struct pprz_transport *trans, struct link_device *dev) } static void overrun(struct pprz_transport *trans __attribute__((unused)), - struct link_device *dev __attribute__((unused))) + struct link_device *dev) { - downlink.nb_ovrn++; + dev->nb_ovrn++; } static void count_bytes(struct pprz_transport *trans __attribute__((unused)), - struct link_device *dev __attribute__((unused)), uint8_t bytes) + struct link_device *dev, uint8_t bytes) { - downlink.nb_bytes += bytes; + dev->nb_bytes += bytes; } static int check_available_space(struct pprz_transport *trans __attribute__((unused)), struct link_device *dev, diff --git a/sw/airborne/subsystems/datalink/xbee.c b/sw/airborne/subsystems/datalink/xbee.c index 9346e5d621..0a8864d7db 100644 --- a/sw/airborne/subsystems/datalink/xbee.c +++ b/sw/airborne/subsystems/datalink/xbee.c @@ -81,7 +81,6 @@ static uint8_t size_of(struct xbee_transport *trans __attribute__((unused)), uin static void start_message(struct xbee_transport *trans, struct link_device *dev, uint8_t payload_len) { - downlink.nb_msgs++; dev->put_byte(dev->periph, XBEE_START); const uint16_t len = payload_len + XBEE_API_OVERHEAD; dev->put_byte(dev->periph, (len >> 8)); @@ -89,6 +88,7 @@ static void start_message(struct xbee_transport *trans, struct link_device *dev, trans->cs_tx = 0; const uint8_t header[] = XBEE_TX_HEADER; put_bytes(trans, dev, DL_TYPE_UINT8, DL_FORMAT_SCALAR, XBEE_TX_OVERHEAD + 1, header); + dev->nb_msgs++; } static void end_message(struct xbee_transport *trans, struct link_device *dev) @@ -99,15 +99,15 @@ static void end_message(struct xbee_transport *trans, struct link_device *dev) } static void overrun(struct xbee_transport *trans __attribute__((unused)), - struct link_device *dev __attribute__((unused))) + struct link_device *dev) { - downlink.nb_ovrn++; + dev->nb_ovrn++; } static void count_bytes(struct xbee_transport *trans __attribute__((unused)), - struct link_device *dev __attribute__((unused)), uint8_t bytes) + struct link_device *dev, uint8_t bytes) { - downlink.nb_bytes += bytes; + dev->nb_bytes += bytes; } static int check_available_space(struct xbee_transport *trans __attribute__((unused)), struct link_device *dev, diff --git a/sw/airborne/subsystems/intermcu.h b/sw/airborne/subsystems/intermcu.h index eaea351db9..ecdbffc8e8 100644 --- a/sw/airborne/subsystems/intermcu.h +++ b/sw/airborne/subsystems/intermcu.h @@ -23,57 +23,26 @@ #ifndef INTERMCU_ROTORCRAFT_H #define INTERMCU_ROTORCRAFT_H - -#include - +#include "std.h" #include "subsystems/commands.h" +#define INTERMCU_AP 0 +#define INTERMCU_FBW 1 -void intermcu_init(void); -void intermcu_on_rc_frame(void); -void intermcu_send_status(uint8_t mode); -void intermcu_periodic(void); -void InterMcuEvent(void (*frame_handler)(void)); +#define INTERMCU_LOST_CNT 25 /* 50ms with a 512Hz timer TODO fixed value */ -#include "subsystems/commands.h" -#include RADIO_CONTROL_TYPE_H - - -/** Data structure shared by fbw and ap processes */ -struct fbw_state { - pprz_t channels[RADIO_CONTROL_NB_CHANNEL]; - uint8_t ppm_cpt; - uint8_t status; ///< - uint8_t nb_err; - uint16_t vsupply; ///< 1e-1 V - int32_t current; ///< milliAmps - float energy; ///< mAh +enum intermcu_status { + INTERMCU_OK, + INTERMCU_LOST }; -struct ap_state { - pprz_t commands[COMMANDS_NB]; - pprz_t command_roll_trim; - pprz_t command_pitch_trim; - pprz_t command_yaw_trim; -}; - -extern struct ap_state from_ap; - - -/////////////////////////////////////////////////////////////////////////////// -// InterMCU Watchdog - -#define INTERMCU_LOST_CNT 25 /* 50ms with a 512Hz timer */ - -#define INTERMCU_OK 0 -#define INTERMCU_LOST 1 - -struct InterMCU { - uint8_t status; +struct intermcu_t { + enum intermcu_status status; uint8_t time_since_last_frame; }; +extern struct intermcu_t inter_mcu; -extern struct InterMCU inter_mcu; - +void intermcu_init(void); +void intermcu_periodic(void); #endif diff --git a/sw/airborne/subsystems/intermcu/intermcu.c b/sw/airborne/subsystems/intermcu/intermcu.c deleted file mode 100644 index 22d2deb34e..0000000000 --- a/sw/airborne/subsystems/intermcu/intermcu.c +++ /dev/null @@ -1,416 +0,0 @@ -/* - * Copyright (C) 2015 The Paparazzi Team - * - * 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. - * - */ - -#include - -#include "subsystems/intermcu.h" -#include "subsystems/electrical.h" - -#include "mcu_periph/uart.h" -#include "led.h" - - -#if COMMANDS_NB > 8 -#error "INTERMCU UART CAN ONLY SEND 8 COMMANDS OR THE UART WILL BE OVERFILLED" -#endif - -#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" -#endif - - -////////////////////////////////////////////////////////////////////////////////////////////// -// LINK - -// Use uart interface directly -#define InterMcuLinkDev (&((INTERMCU_LINK).device)) -#define InterMcuLinkTransmit(c) InterMcuLinkDev->put_byte(InterMcuLinkDev->periph, c) -#define InterMcuLinkChAvailable() InterMcuLinkDev->char_available(InterMcuLinkDev->periph) -#define InterMcuLinkGetch() InterMcuLinkDev->get_byte(InterMcuLinkDev->periph) -#define InterMcuLinkSendMessage() {} - -//#define InterMcuUartSetBaudrate(_a) uart_periph_set_baudrate(&(INTERMCU_LINK), _a) - -////////////////////////////////////////////////////////////////////////////////////////////// -// PROTOCOL - -#define INTERMCU_SYNC1 0xB5 -#define INTERMCU_SYNC2 0x62 - -#define InterMcuInitCheksum() { intermcu_data.send_ck_a = intermcu_data.send_ck_b = 0; } -#define InterMcuUpdateChecksum(c) { intermcu_data.send_ck_a += c; intermcu_data.send_ck_b += intermcu_data.send_ck_a; } -#define InterMcuTrailer() { InterMcuLinkTransmit(intermcu_data.send_ck_a); InterMcuLinkTransmit(intermcu_data.send_ck_b); InterMcuLinkSendMessage(); } - -#define InterMcuSend1(c) { uint8_t i8=c; InterMcuLinkTransmit(i8); InterMcuUpdateChecksum(i8); } -#define InterMcuSend2(c) { uint16_t i16=c; InterMcuSend1(i16&0xff); InterMcuSend1(i16 >> 8); } -#define InterMcuSend1ByAddr(x) { InterMcuSend1(*x); } -#define InterMcuSend2ByAddr(x) { InterMcuSend1(*x); InterMcuSend1(*(x+1)); } -#define InterMcuSend4ByAddr(x) { InterMcuSend1(*x); InterMcuSend1(*(x+1)); InterMcuSend1(*(x+2)); InterMcuSend1(*(x+3)); } - -#define InterMcuHeader(nav_id, msg_id, len) { \ - InterMcuLinkTransmit(INTERMCU_SYNC1); \ - InterMcuLinkTransmit(INTERMCU_SYNC2); \ - InterMcuInitCheksum(); \ - InterMcuSend1(nav_id); \ - InterMcuSend1(msg_id); \ - InterMcuSend2(len); \ - } - -////////////////////////////////////////////////////////////////////////////////////////////// -// MESSAGES - -// class -#define MSG_INTERMCU_ID 100 - - -#define MSG_INTERMCU_COMMAND_ID 0x05 -#define MSG_INTERMCU_COMMAND_LENGTH (2*(COMMANDS_NB)) -#define MSG_INTERMCU_COMMAND(_intermcu_payload, nr) (uint16_t)(*((uint8_t*)_intermcu_payload+0+(2*(nr)))|*((uint8_t*)_intermcu_payload+1+(2*(nr)))<<8) - -#define InterMcuSend_INTERMCU_COMMAND(cmd) { \ - InterMcuHeader(MSG_INTERMCU_ID, MSG_INTERMCU_COMMAND_ID, MSG_INTERMCU_COMMAND_LENGTH);\ - for (int i=0;i INTERMCU_MAX_PAYLOAD) { - goto error; - } - intermcu_data.msg_idx = 0; - intermcu_data.status++; - break; - case LINK_MCU_GOT_LEN2: - intermcu_data.msg_buf[intermcu_data.msg_idx] = c; - intermcu_data.msg_idx++; - if (intermcu_data.msg_idx >= intermcu_data.len) { - intermcu_data.status++; - } - break; - case LINK_MCU_GOT_PAYLOAD: - if (c != intermcu_data.ck_a) { - goto error; - } - intermcu_data.status++; - break; - case LINK_MCU_GOT_CHECKSUM1: - if (c != intermcu_data.ck_b) { - goto error; - } - intermcu_data.msg_available = TRUE; - goto restart; - break; - default: - goto error; - } - return; -error: - intermcu_data.error_cnt++; -restart: - intermcu_data.status = LINK_MCU_UNINIT; - return; -} - - - -////////////////////////////////////////////////////////////////////////////////////////////// -// USER - - - -struct fbw_state from_fbw; -struct ap_state from_ap; - -struct fbw_state* fbw_state = &from_fbw; -struct ap_state* ap_state = &from_ap; - -bool_t inter_mcu_received_fbw = FALSE; -bool_t inter_mcu_received_ap = FALSE; -bool_t link_mcu_received; - - -inline void parse_mavpilot_msg(void); - - -#ifdef INTER_MCU_AP -#include "subsystems/datalink/telemetry.h" - -#define RC_OK 0 -#define RC_LOST 1 -#define RC_REALLY_LOST 2 - - -static void send_commands(struct transport_tx *trans, struct link_device *dev) -{ - pprz_msg_send_COMMANDS(trans, dev, AC_ID, COMMANDS_NB, ap_state->commands); -} - - -static void send_fbw_status(struct transport_tx *trans, struct link_device *dev) -{ - uint8_t rc_status = 0; - uint8_t fbw_status = 0; - if (bit_is_set(fbw_state->status, STATUS_MODE_AUTO)) { - fbw_status = FBW_MODE_AUTO; - } - if (bit_is_set(fbw_state->status, STATUS_MODE_FAILSAFE)) { - fbw_status = FBW_MODE_FAILSAFE; - } - if (bit_is_set(fbw_state->status, STATUS_RADIO_REALLY_LOST)) { - rc_status = RC_REALLY_LOST; - } else if (bit_is_set(fbw_state->status, RC_OK)) { - rc_status = RC_OK; - } else { - rc_status = RC_LOST; - } - pprz_msg_send_FBW_STATUS(trans, dev, AC_ID, - &(rc_status), &(fbw_state->ppm_cpt), &(fbw_status), &(fbw_state->vsupply), &(fbw_state->current)); -} -#endif - -void intermcu_init(void) -{ - intermcu_data.status = LINK_MCU_UNINIT; - intermcu_data.msg_available = FALSE; - intermcu_data.error_cnt = 0; -#ifdef INTER_MCU_AP -#if PERIODIC_TELEMETRY - // If FBW has not telemetry, then AP can send some of the info - register_periodic_telemetry(DefaultPeriodic, "COMMANDS", send_commands); - register_periodic_telemetry(DefaultPeriodic, "FBW_STATUS", send_fbw_status); -#endif -#endif -} - -void parse_mavpilot_msg(void) -{ - if (intermcu_data.msg_class == MSG_INTERMCU_ID) { - if (intermcu_data.msg_id == MSG_INTERMCU_COMMAND_ID) { - for (int i = 0; i < COMMANDS_NB; i++) { - ap_state->commands[i] = ((pprz_t)MSG_INTERMCU_COMMAND(intermcu_data.msg_buf, i)); - } - -#ifdef LINK_MCU_LED - LED_TOGGLE(LINK_MCU_LED); -#endif - inter_mcu_received_ap = TRUE; - } else if (intermcu_data.msg_id == MSG_INTERMCU_RADIO_ID) { - - for (int i = 0; i < RADIO_CONTROL_NB_CHANNEL; i++) { - fbw_state->channels[i] = ((pprz_t)MSG_INTERMCU_RADIO(intermcu_data.msg_buf, i)); - } - } else if (intermcu_data.msg_id == MSG_INTERMCU_TRIM_ID) { - ap_state->command_roll_trim = ((pprz_t) MSG_INTERMCU_TRIM_ROLL(intermcu_data.msg_buf)); - ap_state->command_pitch_trim = ((pprz_t) MSG_INTERMCU_TRIM_PITCH(intermcu_data.msg_buf)); - } else if (intermcu_data.msg_id == MSG_INTERMCU_FBW_ID) { - fbw_state->ppm_cpt = MSG_INTERMCU_FBW_MOD(intermcu_data.msg_buf); - fbw_state->status = MSG_INTERMCU_FBW_STAT(intermcu_data.msg_buf); - fbw_state->nb_err = MSG_INTERMCU_FBW_ERR(intermcu_data.msg_buf); - fbw_state->vsupply = MSG_INTERMCU_FBW_VOLT(intermcu_data.msg_buf); - fbw_state->current = MSG_INTERMCU_FBW_CURRENT(intermcu_data.msg_buf); - -#ifdef LINK_MCU_LED - LED_TOGGLE(LINK_MCU_LED); -#endif - inter_mcu_received_fbw = TRUE; - } - } -} - - -#ifdef INTER_MCU_AP -void link_mcu_send(void) -{ - InterMcuSend_INTERMCU_COMMAND(ap_state->commands); - InterMcuSend_INTERMCU_TRIM(ap_state->command_roll_trim, ap_state->command_pitch_trim); -} -#endif - -#ifdef INTER_MCU_FBW - -void intermcu_on_rc_frame(void) -{ - //TODO - //inter_mcu_fill_fbw_state(); /** Prepares the next message for AP */ - - InterMcuSend_INTERMCU_RADIO(fbw_state->channels); - -} -void intermcu_send_status(uint8_t mode) -{ - // Send Status - InterMcuSend_INTERMCU_FBW( - fbw_state->ppm_cpt, - fbw_state->status, - fbw_state->nb_err, - electrical.vsupply, - fbw_state->current); -} - -#endif - - -struct InterMCU inter_mcu; - - -void intermcu_periodic(void) -{ - if (inter_mcu.time_since_last_frame >= INTERMCU_LOST_CNT) { - inter_mcu.status = INTERMCU_LOST; - } else { - inter_mcu.time_since_last_frame++; - } -} - -void InterMcuEvent(void (*frame_handler)(void)) -{ - /* A message has been received */ - if (InterMcuLinkChAvailable()) { - while (InterMcuLinkChAvailable()) { - intermcu_parse(InterMcuLinkGetch()); - if (intermcu_data.msg_available) { - parse_mavpilot_msg(); - intermcu_data.msg_available = FALSE; - inter_mcu.time_since_last_frame = 0; - (*frame_handler)(); - } - } - } -} diff --git a/sw/airborne/subsystems/intermcu/intermcu_ap.c b/sw/airborne/subsystems/intermcu/intermcu_ap.c new file mode 100644 index 0000000000..2dd12a1cbb --- /dev/null +++ b/sw/airborne/subsystems/intermcu/intermcu_ap.c @@ -0,0 +1,100 @@ +/* + * Copyright (C) 2015 The Paparazzi Team + * + * 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. + * + */ + +#include "intermcu_ap.h" +#include "intermcu_msg.h" +#include "subsystems/radio_control.h" +#include "subsystems/datalink/pprz_transport.h" +#include "mcu_periph/uart.h" + +#if COMMANDS_NB > 8 +#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)); + +void intermcu_init(void) +{ + pprz_transport_init(&intermcu_transport); +} + +void intermcu_periodic(void) +{ + /* Check for interMCU loss */ + if (inter_mcu.time_since_last_frame >= INTERMCU_LOST_CNT) { + inter_mcu.status = INTERMCU_LOST; + } else { + inter_mcu.time_since_last_frame++; + } +} + +void intermcu_set_actuators(pprz_t *command_values, uint8_t ap_mode __attribute__((unused))) +{ + pprz_msg_send_IMCU_COMMANDS(&(intermcu_transport.trans_tx), intermcu_device, + INTERMCU_AP, 0, COMMANDS_NB, command_values); //TODO: Fix status +} + +static inline void intermcu_parse_msg(struct transport_rx * trans, void (*rc_frame_handler)(void)) +{ + /* Parse the Inter MCU message */ + uint8_t msg_id = trans->payload[1]; + switch (msg_id) { + case DL_IMCU_RADIO_COMMANDS: { + uint8_t i; + uint8_t size = DL_IMCU_RADIO_COMMANDS_values_length(trans->payload); + int16_t *rc_values = DL_IMCU_RADIO_COMMANDS_values(trans->payload); + for(i = 0; i < size; i++) + radio_control.values[i] = rc_values[i]; + + radio_control.frame_cpt++; + radio_control.time_since_last_frame = 0; + radio_control.status = RC_OK; + rc_frame_handler(); + break; + } + + default: + break; + } + + // Set to receive another message + trans->msg_received = FALSE; +} + +void RadioControlEvent(void (*frame_handler)(void)) +{ + /* 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)); + } + + if (intermcu_transport.trans_rx.msg_received) { + intermcu_parse_msg(&(intermcu_transport.trans_rx), frame_handler); + } + } +} diff --git a/sw/airborne/subsystems/intermcu/intermcu_ap.h b/sw/airborne/subsystems/intermcu/intermcu_ap.h new file mode 100644 index 0000000000..8644e083f6 --- /dev/null +++ b/sw/airborne/subsystems/intermcu/intermcu_ap.h @@ -0,0 +1,50 @@ +/* + * Copyright (C) 2015 The Paparazzi Team + * + * 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. + * + */ + +#ifndef INTERMCU_AP_ROTORCRAFT_H +#define INTERMCU_AP_ROTORCRAFT_H + +#include "subsystems/intermcu.h" + +void intermcu_set_actuators(pprz_t *command_values, uint8_t ap_mode); +void RadioControlEvent(void (*frame_handler)(void)); + +/* We need radio defines for the Autopilot */ +#define RADIO_THROTTLE 0 +#define RADIO_ROLL 1 +#define RADIO_PITCH 2 +#define RADIO_YAW 3 +#define RADIO_GEAR 4 +#define RADIO_FLAP 5 +#define RADIO_AUX1 5 +#define RADIO_AUX2 6 +#define RADIO_AUX3 7 +#define RADIO_CONTROL_NB_CHANNEL 8 + +#ifndef RADIO_MODE +#define RADIO_MODE RADIO_GEAR +#endif +#ifndef RADIO_KILL_SWITCH +#define RADIO_KILL_SWITCH RADIO_FLAP +#endif + +#endif /* INTERMCU_AP_ROTORCRAFT_H */ diff --git a/sw/airborne/subsystems/intermcu/intermcu_fbw.c b/sw/airborne/subsystems/intermcu/intermcu_fbw.c new file mode 100644 index 0000000000..bc60eb478f --- /dev/null +++ b/sw/airborne/subsystems/intermcu/intermcu_fbw.c @@ -0,0 +1,108 @@ +/* + * Copyright (C) 2015 The Paparazzi Team + * + * 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. + * + */ + + +#include "intermcu_fbw.h" +#include "intermcu_msg.h" +#include "subsystems/radio_control.h" +#include "mcu_periph/uart.h" +#include "subsystems/datalink/pprz_transport.h" + +#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" +#endif + +// Used for communication +static struct link_device *intermcu_device = (&((INTERMCU_LINK).device)); +static struct pprz_transport intermcu_transport; + +struct intermcu_t inter_mcu; +pprz_t intermcu_commands[COMMANDS_NB]; +static inline void intermcu_parse_msg(struct transport_rx * trans, void (*commands_frame_handler)(void)); + +void intermcu_init(void) +{ + pprz_transport_init(&intermcu_transport); +} + +void intermcu_periodic(void) +{ + /* Check for interMCU loss */ + if (inter_mcu.time_since_last_frame >= INTERMCU_LOST_CNT) { + inter_mcu.status = INTERMCU_LOST; + } else { + inter_mcu.time_since_last_frame++; + } +} + +void intermcu_on_rc_frame(void) +{ + pprz_msg_send_IMCU_RADIO_COMMANDS(&(intermcu_transport.trans_tx), intermcu_device, + INTERMCU_FBW, 0, RADIO_CONTROL_NB_CHANNEL, radio_control.values); //TODO: Fix status +} + +void intermcu_send_status(uint8_t mode) +{ + // Send Status + (void)mode; + //FIXME +} + +static inline void intermcu_parse_msg(struct transport_rx * trans, void (*commands_frame_handler)(void)) +{ + /* Parse the Inter MCU message */ + uint8_t msg_id = trans->payload[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); + for(i = 0; i < size; i++) + intermcu_commands[i] = new_commands[i]; + + commands_frame_handler(); + break; + } + + default: + break; + } + + // Set to receive another message + trans->msg_received = FALSE; +} + +void InterMcuEvent(void (*frame_handler)(void)) +{ + /* 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)); + } + + if (intermcu_transport.trans_rx.msg_received) { + intermcu_parse_msg(&(intermcu_transport.trans_rx), frame_handler); + } + } +} diff --git a/sw/airborne/subsystems/intermcu/intermcu_fbw.h b/sw/airborne/subsystems/intermcu/intermcu_fbw.h new file mode 100644 index 0000000000..899144d7c3 --- /dev/null +++ b/sw/airborne/subsystems/intermcu/intermcu_fbw.h @@ -0,0 +1,33 @@ +/* + * Copyright (C) 2015 The Paparazzi Team + * + * 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. + * + */ + +#ifndef INTERMCU_FBW_ROTORCRAFT_H +#define INTERMCU_FBW_ROTORCRAFT_H + +#include "subsystems/intermcu.h" + +extern pprz_t intermcu_commands[COMMANDS_NB]; +void intermcu_on_rc_frame(void); +void intermcu_send_status(uint8_t mode); +void InterMcuEvent(void (*frame_handler)(void)); + +#endif /* INTERMCU_FBW_ROTORCRAFT_H */ diff --git a/sw/tools/generators/gen_messages.ml b/sw/tools/generators/gen_messages.ml index bae8ffd9d5..b84421469f 100644 --- a/sw/tools/generators/gen_messages.ml +++ b/sw/tools/generators/gen_messages.ml @@ -383,7 +383,7 @@ let () = end; (** Macros for airborne datalink (receiving) *) - if class_name = "datalink" then + if class_name = "datalink" || class_name = "intermcu" then List.iter (Gen_onboard.print_get_macros h true) messages; Printf.fprintf h "#endif // _VAR_MESSAGES_%s_H_\n" class_name