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

[intermcu] Rewrite communication

Browse Source
This commit is contained in:
Freek van Tienen
2015-11-09 01:49:37 +01:00
committed by Christophe De Wagter
parent 2914cce269
commit 4be0987a77
21 changed files with 391 additions and 524 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Makefile
+10 -1
View File
@@ -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
conf/airframes/TUDelft/airframes/logo600.xml
+1 -7
View File
@@ -16,13 +16,6 @@
<subsystem name="fdm" type="jsbsim"/>
</target>
<subsystem name="radio_control" type="spektrum">
<configure name="USE_SECONDARY_SPEKTRUM_RECEIVER" value="1"/>
<define name="RADIO_CONTROL_SPEKTRUM_NO_SIGN" value="1"/>
<define name="RADIO_KILL_SWITCH" value="RADIO_AUX2"/>
</subsystem>
<subsystem name="actuators" type="spektrum"/>
<subsystem name="telemetry" type="xbee_api"/>
<subsystem name="imu" type="lisa_mx_v2.1"/>
<subsystem name="gps" type="ublox"/>
@@ -30,6 +23,7 @@
<subsystem name="ahrs" type="int_cmpl_quat"/>
<subsystem name="ins" type="hff"/>
<subsystem name="guidance" type="indi"/>
<subsystem name="intermcu" type="uart"/>
</firmware>
<firmware name="rotorcraft">
conf/airframes/TUDelft/airframes/test.xml
+2 -9
View File
@@ -18,20 +18,14 @@
<subsystem name="fdm" type="jsbsim"/>
</target>
<subsystem name="radio_control" type="spektrum">
<configure name="USE_SECONDARY_SPEKTRUM_RECEIVER" value="1"/>
<define name="RADIO_CONTROL_SPEKTRUM_NO_SIGN" value="1"/>
<define name="RADIO_KILL_SWITCH" value="RADIO_AUX2"/>
</subsystem>
<subsystem name="actuators" type="pwm"/>
<subsystem name="telemetry" type="xbee_api"/>
<!--subsystem name="telemetry" type="xbee_api"/-->
<subsystem name="imu" type="lisa_m_v2.1"/>
<subsystem name="gps" type="ublox"/>
<subsystem name="stabilization" type="int_quat"/>
<subsystem name="ahrs" type="int_cmpl_quat"/>
<subsystem name="ins" type="hff"/>
<subsystem name="guidance" type="indi"/>
<subsystem name="intermcu" type="uart"/>
</firmware>
<firmware name="rotorcraft">
@@ -48,7 +42,6 @@
</subsystem>
<subsystem name="actuators" type="pwm"/>
<subsystem name="intermcu" type="uart"/>
</firmware>
conf/airframes/TUDelft/conf.xml
+2 -2
View File
@@ -90,12 +90,12 @@
<aircraft
name="Logo600"
ac_id="5"
airframe="airframes/TUDelft/airframes/logo600.xml"
airframe="airframes/TUDelft/airframes/test.xml"
radio="radios/dummy.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/dummy.xml"
settings="settings/rotorcraft_basic.xml settings/control/stabilization_att_int.xml settings/control/rotorcraft_guidance.xml"
settings_modules="modules/geo_mag.xml modules/air_data.xml modules/gps_ubx_ucenter.xml"
settings_modules="modules/geo_mag.xml modules/air_data.xml"
gui_color="blue"
/>
<aircraft
conf/firmwares/subsystems/rotorcraft/intermcu_uart.makefile
+12 -7
View File
@@ -5,15 +5,20 @@
ifeq ($(TARGET),fbw)
INTERMCU_PORT ?= UART2
INTERMCU_PORT_LOWER = $(shell echo $(INTERMCU_PORT) | tr A-Z a-z)
fbw.CFLAGS += -DINTERMCU_LINK=$(INTERMCU_PORT_LOWER) -DUSE_$(INTERMCU_PORT) -D$(INTERMCU_PORT)_BAUD=B57600
fbw.CFLAGS += -DINTERMCU_LINK=$(INTERMCU_PORT_LOWER) -DUSE_$(INTERMCU_PORT) -D$(INTERMCU_PORT)_BAUD=B230400
fbw.CFLAGS += -DINTER_MCU_FBW
fbw.srcs += subsystems/datalink/pprz_transport.c
fbw.srcs += subsystems/intermcu/intermcu_fbw.c
else
INTERMCU_PORT ?= UART5
INTERMCU_PORT ?= UART2
INTERMCU_PORT_LOWER = $(shell echo $(INTERMCU_PORT) | tr A-Z a-z)
ap.CFLAGS += -DINTERMCU_LINK=$(INTERMCU_PORT_LOWER) -DUSE_$(INTERMCU_PORT) -D$(INTERMCU_PORT)_BAUD=B57600
ap.CFLAGS += -DINTER_MCU_AP
endif
ap.CFLAGS += -DINTERMCU_LINK=$(INTERMCU_PORT_LOWER) -DUSE_$(INTERMCU_PORT) -D$(INTERMCU_PORT)_BAUD=B230400
ap.CFLAGS += -DRADIO_CONTROL_TYPE_H=\"subsystems/intermcu/intermcu_ap.h\" -DRADIO_CONTROL
ap.CFLAGS += -DRADIO_CONTROL_LED=$(RADIO_CONTROL_LED) -DINTER_MCU_AP
ifneq ($(TARGET),sim)
$(TARGET).srcs += ./subsystems/intermcu/intermcu.c
ifneq ($(TARGET),sim)
$(TARGET).srcs += subsystems/intermcu/intermcu_ap.c
$(TARGET).srcs += subsystems/radio_control.c
$(TARGET).srcs += subsystems/datalink/pprz_transport.c
endif
endif
conf/messages.xml
+12 -1
View File
@@ -858,7 +858,7 @@
<field name="data_rate" type="uint32" unit="bytes/s"/>
<field name="last_msg" type="uint8[]"/>
</message>
<!--107 is free -->
<!--108 is free -->
<!--109 is free -->
@@ -2875,6 +2875,17 @@
</message>
</msg_class>
<msg_class name="intermcu">
<message name="IMCU_COMMANDS" id="1">
<field name="status" type="uint8"/>
<field name="values" type="int16[]"/>
</message>
<message name="IMCU_RADIO_COMMANDS" id="2">
<field name="status" type="uint8"/>
<field name="values" type="int16[]"/>
</message>
</msg_class>
</protocol>
sw/airborne/firmwares/rotorcraft/main.c
+15
View File
@@ -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++);
sw/airborne/firmwares/rotorcraft/main_fbw.c
+5 -3
View File
@@ -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);
}
}
sw/airborne/mcu_periph/link_device.h
+4
View File
@@ -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
sw/airborne/subsystems/datalink/downlink.c
+8 -9
View File
@@ -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
sw/airborne/subsystems/datalink/downlink.h
-9
View File
@@ -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);
sw/airborne/subsystems/datalink/ivy_transport.c
+6 -6
View File
@@ -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)),
sw/airborne/subsystems/datalink/pprz_transport.c
+5 -5
View File
@@ -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,
sw/airborne/subsystems/datalink/xbee.c
+5 -5
View File
@@ -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,
sw/airborne/subsystems/intermcu.h
+12 -43
View File
@@ -23,57 +23,26 @@
#ifndef INTERMCU_ROTORCRAFT_H
#define INTERMCU_ROTORCRAFT_H
#include <stdint.h>
#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
sw/airborne/subsystems/intermcu/intermcu.c
-416
View File
@@ -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 <inttypes.h>
#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<COMMANDS_NB;i++) { \
uint16_t _cmd = cmd[i]; \
InterMcuSend2ByAddr((uint8_t*)&_cmd);\
} \
InterMcuTrailer();\
}
#define MSG_INTERMCU_RADIO_ID 0x08
#define MSG_INTERMCU_RADIO_LENGTH (2*(RADIO_CONTROL_NB_CHANNEL))
#define MSG_INTERMCU_RADIO(_intermcu_payload, nr) (uint16_t)(*((uint8_t*)_intermcu_payload+0+(2*(nr)))|*((uint8_t*)_intermcu_payload+1+(2*(nr)))<<8)
#define InterMcuSend_INTERMCU_RADIO(cmd) { \
InterMcuHeader(MSG_INTERMCU_ID, MSG_INTERMCU_RADIO_ID, MSG_INTERMCU_RADIO_LENGTH);\
for (int i=0;i<RADIO_CONTROL_NB_CHANNEL;i++) { \
uint16_t _cmd = cmd[i]; \
InterMcuSend2ByAddr((uint8_t*)&_cmd);\
} \
InterMcuTrailer();\
}
#define MSG_INTERMCU_FBW_ID 0x06
#define MSG_INTERMCU_FBW_MOD(_intermcu_payload) (uint8_t)(*((uint8_t*)_intermcu_payload+0))
#define MSG_INTERMCU_FBW_STAT(_intermcu_payload) (uint8_t)(*((uint8_t*)_intermcu_payload+1))
#define MSG_INTERMCU_FBW_ERR(_intermcu_payload) (uint8_t)(*((uint8_t*)_intermcu_payload+2))
#define MSG_INTERMCU_FBW_VOLT(_intermcu_payload) (uint16_t)(*((uint8_t*)_intermcu_payload+3)|*((uint8_t*)_intermcu_payload+1+3)<<8)
//FIXME: Current is now 4BYTES
#define MSG_INTERMCU_FBW_CURRENT(_intermcu_payload) (uint16_t)(*((uint8_t*)_intermcu_payload+5)|*((uint8_t*)_intermcu_payload+1+5)<<8)
#define InterMcuSend_INTERMCU_FBW(mod,stat,err,volt,current) { \
InterMcuHeader(MSG_INTERMCU_ID, MSG_INTERMCU_FBW_ID, 7);\
uint8_t _mod = mod; InterMcuSend1ByAddr((uint8_t*)&_mod);\
uint8_t _stat = stat; InterMcuSend1ByAddr((uint8_t*)&_stat);\
uint8_t _err = err; InterMcuSend1ByAddr((uint8_t*)&_err);\
uint16_t _volt = volt; InterMcuSend2ByAddr((uint8_t*)&_volt);\
uint16_t _current = current; InterMcuSend2ByAddr((uint8_t*)&_current);\
InterMcuTrailer();\
}
#define MSG_INTERMCU_TRIM_ID 0x07
#define MSG_INTERMCU_TRIM_ROLL(_intermcu_payload) (uint16_t)(*((uint8_t*)_intermcu_payload+0)|*((uint8_t*)_intermcu_payload+1)<<8)
#define MSG_INTERMCU_TRIM_PITCH(_intermcu_payload) (uint16_t)(*((uint8_t*)_intermcu_payload+2)|*((uint8_t*)_intermcu_payload+3)<<8)
#define InterMcuSend_INTERMCU_TRIM(roll,pitch) { \
InterMcuHeader(MSG_INTERMCU_ID, MSG_INTERMCU_TRIM_ID, 4);\
uint16_t _roll = roll; InterMcuSend2ByAddr((uint8_t*)&_roll);\
uint16_t _pitch = pitch; InterMcuSend2ByAddr((uint8_t*)&_pitch);\
InterMcuTrailer();\
}
//////////////////////////////////////////////////////////////////////////////////////////////
// PARSER
/* parser status */
#define LINK_MCU_UNINIT 0
#define LINK_MCU_GOT_SYNC1 1
#define LINK_MCU_GOT_SYNC2 2
#define LINK_MCU_GOT_CLASS 3
#define LINK_MCU_GOT_ID 4
#define LINK_MCU_GOT_LEN1 5
#define LINK_MCU_GOT_LEN2 6
#define LINK_MCU_GOT_PAYLOAD 7
#define LINK_MCU_GOT_CHECKSUM1 8
#define INTERMCU_MAX_PAYLOAD 255
struct InterMcuData {
bool_t msg_available;
uint8_t msg_buf[INTERMCU_MAX_PAYLOAD] __attribute__((aligned));
uint8_t msg_id;
uint8_t msg_class;
uint8_t status;
uint16_t len;
uint8_t msg_idx;
uint8_t ck_a, ck_b;
uint8_t send_ck_a, send_ck_b;
uint8_t error_cnt;
};
struct InterMcuData intermcu_data;
/* INTERMCU parsing */
void intermcu_parse(uint8_t c);
void intermcu_parse(uint8_t c)
{
if (intermcu_data.status < LINK_MCU_GOT_PAYLOAD) {
intermcu_data.ck_a += c;
intermcu_data.ck_b += intermcu_data.ck_a;
}
switch (intermcu_data.status) {
case LINK_MCU_UNINIT:
if (c == INTERMCU_SYNC1) {
intermcu_data.status++;
}
break;
case LINK_MCU_GOT_SYNC1:
if (c != INTERMCU_SYNC2) {
goto error;
}
intermcu_data.ck_a = 0;
intermcu_data.ck_b = 0;
intermcu_data.status++;
break;
case LINK_MCU_GOT_SYNC2:
if (intermcu_data.msg_available) {
/* Previous message has not yet been parsed: discard this one */
goto error;
}
intermcu_data.msg_class = c;
intermcu_data.status++;
break;
case LINK_MCU_GOT_CLASS:
intermcu_data.msg_id = c;
intermcu_data.status++;
break;
case LINK_MCU_GOT_ID:
intermcu_data.len = c;
intermcu_data.status++;
break;
case LINK_MCU_GOT_LEN1:
intermcu_data.len |= (c << 8);
if (intermcu_data.len > 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)();
}
}
}
}
sw/airborne/subsystems/intermcu/intermcu_ap.c
+100
View File
@@ -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);
}
}
}
sw/airborne/subsystems/intermcu/intermcu_ap.h
+50
View File
@@ -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 */
sw/airborne/subsystems/intermcu/intermcu_fbw.c
+108
View File
@@ -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);
}
}
}
sw/airborne/subsystems/intermcu/intermcu_fbw.h
+33
View File
@@ -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 */
sw/tools/generators/gen_messages.ml
+1 -1
View File
@@ -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