co-pliot init commit
This commit is contained in:
shanggl
Hugo Chiang
parent
26c0979c92
commit
6bea228e20
|
|
@ -68,6 +68,11 @@ typedef struct dmaTimerMapping_s {
|
|||
#define DMA_REQUEST_UART2_TX DMAMUX_DMAREQ_ID_USART2_TX
|
||||
#define DMA_REQUEST_UART3_RX DMAMUX_DMAREQ_ID_USART3_RX
|
||||
#define DMA_REQUEST_UART3_TX DMAMUX_DMAREQ_ID_USART3_TX
|
||||
#define DMA_REQUEST_UART4_RX DMAMUX_DMAREQ_ID_UART4_RX
|
||||
#define DMA_REQUEST_UART4_TX DMAMUX_DMAREQ_ID_UART4_TX
|
||||
#define DMA_REQUEST_UART5_RX DMAMUX_DMAREQ_ID_UART5_RX
|
||||
#define DMA_REQUEST_UART5_TX DMAMUX_DMAREQ_ID_UART5_TX
|
||||
|
||||
#define DMA_REQUEST_UART6_RX DMAMUX_DMAREQ_ID_USART6_RX
|
||||
#define DMA_REQUEST_UART6_TX DMAMUX_DMAREQ_ID_USART6_TX
|
||||
//#define DMA_REQUEST_UART9_RX DMA_REQUEST_LPUART1_RX
|
||||
|
|
@ -117,10 +122,10 @@ static const dmaPeripheralMapping_t dmaPeripheralMapping[] = {
|
|||
REQMAP_DIR(UART, 2, RX),
|
||||
REQMAP_DIR(UART, 3, TX),
|
||||
REQMAP_DIR(UART, 3, RX),
|
||||
// REQMAP_DIR(UART, 4, TX),
|
||||
// REQMAP_DIR(UART, 4, RX),
|
||||
// REQMAP_DIR(UART, 5, TX),
|
||||
// REQMAP_DIR(UART, 5, RX),
|
||||
REQMAP_DIR(UART, 4, TX),
|
||||
REQMAP_DIR(UART, 4, RX),
|
||||
REQMAP_DIR(UART, 5, TX),
|
||||
REQMAP_DIR(UART, 5, RX),
|
||||
REQMAP_DIR(UART, 6, TX),
|
||||
REQMAP_DIR(UART, 6, RX),
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -0,0 +1,148 @@
|
|||
/*
|
||||
* 模糊PID 协处理器
|
||||
* 通过 uart5 调用协处理器进行数据处理
|
||||
*
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
#ifdef USE_FUZZI_CO_PROCESSOR
|
||||
#include <io/fuzzi_co_processor.h>
|
||||
#include <io/serial.h>
|
||||
|
||||
|
||||
|
||||
static serialPort_t *coProcessorPort;
|
||||
static int8_t sendCnt;//发送次数，有可能会溢出
|
||||
static int8_t recvCnt;//接收次数
|
||||
|
||||
/*
|
||||
协处理器初始化，默认先使用 uart5
|
||||
*/
|
||||
|
||||
bool FuzziCoProcessorInit( void ){
|
||||
|
||||
const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_CO_PROCESSOR);
|
||||
|
||||
if (portConfig) {
|
||||
portOptions_e portOptions = 0;
|
||||
portOptions = SERIAL_BIDIR;
|
||||
coProcessorPort = openSerialPort(portConfig->identifier, FUNCTION_CO_PROCESSOR, NULL, NULL, 500000, MODE_RXTX, portOptions);
|
||||
//fixme : for debug use port5
|
||||
// coProcessorPort = openSerialPort(CO_PROCESSOR_UART, FUNCTION_CO_PROCESSOR, NULL, NULL, CO_PROCESSOR_UART_BAUD, MODE_RXTX, portOptions);
|
||||
|
||||
}
|
||||
|
||||
if (!coProcessorPort) {
|
||||
return false;
|
||||
}
|
||||
//FINISH SERIAL PASS THROUGH
|
||||
|
||||
return true;
|
||||
}
|
||||
/*
|
||||
在mainpid loop 中 ，发送error信息到协处理器
|
||||
|
||||
*/
|
||||
static void FuzziCoProcessorSendError(int16_t errRoll,int16_t errPitch ,int16_t errYaw,int16_t errHigh){
|
||||
|
||||
int8_t txBuffer[8];
|
||||
/* 发送数据帧类型
|
||||
0X55
|
||||
0XE0
|
||||
SEND_CNT
|
||||
ROLL_E_HIGH , ROLL_E_LOW
|
||||
PITCH_E_HIGH, PITCH_E_LOW
|
||||
YAW_E_HIGH ,YAW_E_LOW
|
||||
CRC
|
||||
*/
|
||||
txBuffer[0]=0x55;
|
||||
txBuffer[1]=0xE0;
|
||||
txBuffer[2]=sendCnt;
|
||||
txBuffer[3]= (int8_t)(errRoll >> 8); txBuffer[4] = (int8_t)(errRoll<<8);
|
||||
txBuffer[5]= (int8_t)(errPitch >> 8); txBuffer[6] = (int8_t)(errPitch<<8);
|
||||
txBuffer[7]= (int8_t)(errYaw >> 8) ; txBuffer[8] = (int8_t)(errYaw<<8);
|
||||
UNUSED(errHigh);
|
||||
|
||||
for(int8_t i=0;i<10;i++)
|
||||
{
|
||||
serialWrite(coProcessorPort, txBuffer[8]);
|
||||
}
|
||||
|
||||
sendCnt++;
|
||||
}
|
||||
|
||||
//process 0x55 0xE1 timecount r.p r.i r.d p.p p.i p.d y.p y.i 0xCC
|
||||
|
||||
static void FuzziReceiveFrame(uint8_t c)
|
||||
{
|
||||
static enum coRecvState_e {
|
||||
CO_HEADER_1, // Waiting for preamble 1 (0x55)
|
||||
CO_HEADER_2, // Waiting for preamble 2 (0xE1)
|
||||
CO_DATA, // Receiving data
|
||||
CO_WAITCRC, // Waiting for CRC 0XCC
|
||||
} state = CO_HEADER_1;
|
||||
|
||||
static int r_index;
|
||||
|
||||
switch(state){
|
||||
case CO_HEADER_1:
|
||||
if(0x55==c){
|
||||
state=CO_HEADER_2;
|
||||
}
|
||||
else{
|
||||
state=CO_HEADER_1;
|
||||
}
|
||||
break;
|
||||
case CO_HEADER_2:
|
||||
if(0xE1==c){
|
||||
state=CO_DATA;
|
||||
r_index=0;
|
||||
}
|
||||
else{
|
||||
state=CO_HEADER_1;
|
||||
}
|
||||
break;
|
||||
case CO_DATA:
|
||||
coRecvBuffer[r_index] = c;
|
||||
if (++r_index == 9) {
|
||||
state = CO_WAITCRC;
|
||||
}
|
||||
break;
|
||||
case CO_WAITCRC:
|
||||
int8_t crc=c;
|
||||
//check crc
|
||||
//PROCESS RECV
|
||||
pid_buffer[0].P=coRecvBuffer[1];
|
||||
pid_buffer[0].I=coRecvBuffer[2];
|
||||
pid_buffer[0].D=coRecvBuffer[3];
|
||||
pid_buffer[1].P=coRecvBuffer[4];
|
||||
pid_buffer[1].I=coRecvBuffer[5];
|
||||
pid_buffer[1].D=coRecvBuffer[6];
|
||||
pid_buffer[2].P=coRecvBuffer[7];
|
||||
pid_buffer[2].I=coRecvBuffer[8];
|
||||
recvCnt++;
|
||||
state=CO_HEADER_1;
|
||||
break;
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
//在 mainpid Loop 中调用，读取串口缓存到 pid buffer 之后直接从pid buffer 获取 pid信息
|
||||
static void FuzziCoProcessorRecv(){
|
||||
|
||||
if (coProcessorPort == NULL) {
|
||||
return;
|
||||
}
|
||||
|
||||
while (serialRxBytesWaiting(coProcessorPort) > 0) {
|
||||
uint8_t c = serialRead(coProcessorPort);
|
||||
FuzziReceiveFrame(c);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,25 @@
|
|||
/*
|
||||
* co_fuzzi_processor.h
|
||||
*
|
||||
* Created on: 2022年10月22日
|
||||
* Author: shanggl
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <flight/pid.h>
|
||||
|
||||
//for Roll=0 Pitch Yaw High
|
||||
static pidf_t pid_buffer[4];
|
||||
|
||||
static int8_t coRecvBuffer[12];// 9 used for now
|
||||
|
||||
bool FuzziCoProcessorInit(void);
|
||||
|
||||
static void FuzziCoProcessorSendError(float errPitch,float errRoll,float errYaw,float errHigh);
|
||||
|
||||
static void FuzziCoProcessorRecv();
|
||||
|
||||
|
||||
|
||||
|
||||
|
|
@ -91,7 +91,7 @@ const serialPortIdentifier_e serialPortIdentifiers[SERIAL_PORT_COUNT] = {
|
|||
SERIAL_PORT_USART7,
|
||||
#endif
|
||||
#ifdef USE_UART8
|
||||
SERIAL_PORT_USART8,
|
||||
SERIAL_PORT_USART8,
|
||||
#endif
|
||||
#ifdef USE_UART9
|
||||
SERIAL_PORT_UART9,
|
||||
|
|
|
|||
|
|
@ -50,6 +50,7 @@ typedef enum {
|
|||
FUNCTION_RCDEVICE = (1 << 14), // 16384
|
||||
FUNCTION_LIDAR_TF = (1 << 15), // 32768
|
||||
FUNCTION_FRSKY_OSD = (1 << 16), // 65536
|
||||
FUNCTION_CO_PROCESSOR = (1 << 17),
|
||||
} serialPortFunction_e;
|
||||
|
||||
#define TELEMETRY_SHAREABLE_PORT_FUNCTIONS_MASK (FUNCTION_TELEMETRY_FRSKY_HUB | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_MAVLINK)
|
||||
|
|
|
|||
|
|
@ -190,6 +190,11 @@
|
|||
#define UART4_RX_PIN PA1
|
||||
#define UART4_TX_PIN PA0
|
||||
|
||||
|
||||
#define USE_UART5
|
||||
#define UART5_RX_PIN PD2
|
||||
#define UART5_TX_PIN PC12
|
||||
|
||||
#define USE_UART8
|
||||
#define UART8_RX_PIN PC3
|
||||
#define UART8_TX_PIN PC2
|
||||
|
|
@ -197,12 +202,18 @@
|
|||
//#define USE_SOFTSERIAL1
|
||||
//#define USE_SOFTSERIAL2
|
||||
|
||||
#define SERIAL_PORT_COUNT 6 // VCP UART1 UART2 UART3 UART4 UART5
|
||||
#define SERIAL_PORT_COUNT 7 // VCP UART1 UART2 UART3 UART4 UART5 uart8
|
||||
|
||||
#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL
|
||||
#define SERIALRX_PROVIDER SERIALRX_CRSF
|
||||
#define SERIALRX_UART SERIAL_PORT_USART2
|
||||
|
||||
//*******************CO-Proceseor*******************
|
||||
#define USE_FUZZI_CO_PROCESSOR
|
||||
#define CO_PROCESSOR_UART SERIAL_PORT_UART5
|
||||
#define CO_PROCESSOR_UART_BAUD 500000
|
||||
#define UART5_TX_DMA_CHANNEL DMA2_CHANNEL3_BASE
|
||||
#define UART5_RX_DMA_CHANNEL DMA2_CHANNEL4_BASE
|
||||
|
||||
|
||||
// *************** ADC *****************************
|
||||
|
|
|
|||
|
|
@ -21,5 +21,6 @@ TARGET_SRC = \
|
|||
drivers/rx/rx_sx1280.c \
|
||||
rx/expresslrs_telemetry.c \
|
||||
rx/expresslrs_common.c \
|
||||
rx/expresslrs.c
|
||||
rx/expresslrs.c\
|
||||
io/fuzzi_co_processor.c
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue