CF/BF - Further isolate BST to the Colibri_Race target. Remove even
more unused code. It was partly isolated before, but now it's even clearer that only this target uses it.
This commit is contained in:
parent
a60a79eb82
commit
5ab2117a69
|
@ -65,10 +65,6 @@
|
||||||
#include "drivers/exti.h"
|
#include "drivers/exti.h"
|
||||||
#include "drivers/vtx_soft_spi_rtc6705.h"
|
#include "drivers/vtx_soft_spi_rtc6705.h"
|
||||||
|
|
||||||
#ifdef USE_BST
|
|
||||||
#include "bus_bst.h"
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#include "fc/config.h"
|
#include "fc/config.h"
|
||||||
#include "fc/fc_init.h"
|
#include "fc/fc_init.h"
|
||||||
#include "fc/fc_msp.h"
|
#include "fc/fc_msp.h"
|
||||||
|
@ -316,10 +312,6 @@ void init(void)
|
||||||
initInverters();
|
initInverters();
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifdef USE_BST
|
|
||||||
bstInit(BST_DEVICE);
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#ifdef TARGET_BUS_INIT
|
#ifdef TARGET_BUS_INIT
|
||||||
targetBusInit();
|
targetBusInit();
|
||||||
#else
|
#else
|
||||||
|
|
|
@ -17,22 +17,15 @@
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#define TBS_CORE_PNP_PRO 0x80
|
#define BST_BUFFER_SIZE 128
|
||||||
#define RESERVED 0x8A
|
|
||||||
#define PNP_PRO_DIDITAL_CURRENT_SENSOR 0xC0
|
|
||||||
#define PNP_PRO_GPS 0xC2
|
|
||||||
#define TSB_BLACKBOX 0xC4
|
|
||||||
#define CLEANFLIGHT_FC 0xC8
|
|
||||||
#define CROSSFIRE_UHF_RECEIVER 0xEC
|
|
||||||
|
|
||||||
#define GPS_POSITION_FRAME_ID 0x02
|
#define I2C_ADDR_TBS_CORE_PNP_PRO 0x80
|
||||||
#define GPS_TIME_FRAME_ID 0x03
|
#define I2C_ADDR_RESERVED 0x8A
|
||||||
#define FC_ATTITUDE_FRAME_ID 0x1E
|
#define I2C_ADDR_PNP_PRO_DIDITAL_CURRENT_SENSOR 0xC0
|
||||||
#define RC_CHANNEL_FRAME_ID 0x15
|
#define I2C_ADDR_PNP_PRO_GPS 0xC2
|
||||||
#define CROSSFIRE_RSSI_FRAME_ID 0x14
|
#define I2C_ADDR_TSB_BLACKBOX 0xC4
|
||||||
#define CLEANFLIGHT_MODE_FRAME_ID 0x20
|
#define I2C_ADDR_CROSSFIRE_UHF_RECEIVER 0xEC
|
||||||
|
#define I2C_ADDR_CLEANFLIGHT_FC 0xC8
|
||||||
#define DATA_BUFFER_SIZE 128
|
|
||||||
|
|
||||||
typedef enum BSTDevice {
|
typedef enum BSTDevice {
|
||||||
BSTDEV_1,
|
BSTDEV_1,
|
||||||
|
|
|
@ -60,17 +60,17 @@ volatile bool coreProReady = false;
|
||||||
// BST TimeoutUserCallback
|
// BST TimeoutUserCallback
|
||||||
///////////////////////////////////////////////////////////////////////////////
|
///////////////////////////////////////////////////////////////////////////////
|
||||||
|
|
||||||
uint8_t dataBuffer[DATA_BUFFER_SIZE] = {0};
|
uint8_t dataBuffer[BST_BUFFER_SIZE] = {0};
|
||||||
uint8_t dataBufferPointer = 0;
|
uint8_t dataBufferPointer = 0;
|
||||||
uint8_t bstWriteDataLen = 0;
|
uint8_t bstWriteDataLen = 0;
|
||||||
|
|
||||||
uint32_t micros(void);
|
uint32_t micros(void);
|
||||||
|
|
||||||
uint8_t writeData[DATA_BUFFER_SIZE] = {0};
|
uint8_t writeData[BST_BUFFER_SIZE] = {0};
|
||||||
uint8_t currentWriteBufferPointer = 0;
|
uint8_t currentWriteBufferPointer = 0;
|
||||||
bool receiverAddress = false;
|
bool receiverAddress = false;
|
||||||
|
|
||||||
uint8_t readData[DATA_BUFFER_SIZE] = {0};
|
uint8_t readData[BST_BUFFER_SIZE] = {0};
|
||||||
uint8_t bufferPointer = 0;
|
uint8_t bufferPointer = 0;
|
||||||
|
|
||||||
bool cleanflight_data_ready = false;
|
bool cleanflight_data_ready = false;
|
||||||
|
@ -221,7 +221,7 @@ void bstInitPort(I2C_TypeDef *BSTx/*, uint8_t Address*/)
|
||||||
BST_InitStructure.I2C_Mode = I2C_Mode_I2C;
|
BST_InitStructure.I2C_Mode = I2C_Mode_I2C;
|
||||||
BST_InitStructure.I2C_AnalogFilter = I2C_AnalogFilter_Enable;
|
BST_InitStructure.I2C_AnalogFilter = I2C_AnalogFilter_Enable;
|
||||||
BST_InitStructure.I2C_DigitalFilter = 0x00;
|
BST_InitStructure.I2C_DigitalFilter = 0x00;
|
||||||
BST_InitStructure.I2C_OwnAddress1 = CLEANFLIGHT_FC;
|
BST_InitStructure.I2C_OwnAddress1 = I2C_ADDR_CLEANFLIGHT_FC;
|
||||||
BST_InitStructure.I2C_Ack = I2C_Ack_Enable;
|
BST_InitStructure.I2C_Ack = I2C_Ack_Enable;
|
||||||
BST_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit;
|
BST_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit;
|
||||||
BST_InitStructure.I2C_Timing = 0x30E0257A; // 100 Khz, 72Mhz Clock, Analog Filter Delay ON, Rise 100, Fall 10.
|
BST_InitStructure.I2C_Timing = 0x30E0257A; // 100 Khz, 72Mhz Clock, Analog Filter Delay ON, Rise 100, Fall 10.
|
||||||
|
@ -269,7 +269,7 @@ void bstInitPort(I2C_TypeDef *BSTx/*, uint8_t Address*/)
|
||||||
BST_InitStructure.I2C_Mode = I2C_Mode_I2C;
|
BST_InitStructure.I2C_Mode = I2C_Mode_I2C;
|
||||||
BST_InitStructure.I2C_AnalogFilter = I2C_AnalogFilter_Enable;
|
BST_InitStructure.I2C_AnalogFilter = I2C_AnalogFilter_Enable;
|
||||||
BST_InitStructure.I2C_DigitalFilter = 0x00;
|
BST_InitStructure.I2C_DigitalFilter = 0x00;
|
||||||
BST_InitStructure.I2C_OwnAddress1 = CLEANFLIGHT_FC;
|
BST_InitStructure.I2C_OwnAddress1 = I2C_ADDR_CLEANFLIGHT_FC;
|
||||||
BST_InitStructure.I2C_Ack = I2C_Ack_Enable;
|
BST_InitStructure.I2C_Ack = I2C_Ack_Enable;
|
||||||
BST_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit;
|
BST_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit;
|
||||||
BST_InitStructure.I2C_Timing = 0x30E0257A; // 100 Khz, 72Mhz Clock, Analog Filter Delay ON, Rise 100, Fall 10.
|
BST_InitStructure.I2C_Timing = 0x30E0257A; // 100 Khz, 72Mhz Clock, Analog Filter Delay ON, Rise 100, Fall 10.
|
||||||
|
|
|
@ -75,47 +75,12 @@
|
||||||
#include "bus_bst.h"
|
#include "bus_bst.h"
|
||||||
#include "i2c_bst.h"
|
#include "i2c_bst.h"
|
||||||
|
|
||||||
#define BST_PROTOCOL_VERSION 0
|
#define GPS_POSITION_FRAME_ID 0x02
|
||||||
|
#define GPS_TIME_FRAME_ID 0x03
|
||||||
#define API_VERSION_MAJOR 1 // increment when major changes are made
|
#define FC_ATTITUDE_FRAME_ID 0x1E
|
||||||
#define API_VERSION_MINOR 13 // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR
|
#define RC_CHANNEL_FRAME_ID 0x15
|
||||||
|
#define CROSSFIRE_RSSI_FRAME_ID 0x14
|
||||||
#define API_VERSION_LENGTH 2
|
#define CLEANFLIGHT_MODE_FRAME_ID 0x20
|
||||||
|
|
||||||
#define MULTIWII_IDENTIFIER "MWII";
|
|
||||||
#define CLEANFLIGHT_IDENTIFIER "CLFL"
|
|
||||||
#define BASEFLIGHT_IDENTIFIER "BAFL";
|
|
||||||
|
|
||||||
#define FLIGHT_CONTROLLER_IDENTIFIER_LENGTH 4
|
|
||||||
static const char * const flightControllerIdentifier = CLEANFLIGHT_IDENTIFIER; // 4 UPPER CASE alpha numeric characters that identify the flight controller.
|
|
||||||
|
|
||||||
#define FLIGHT_CONTROLLER_VERSION_LENGTH 3
|
|
||||||
#define FLIGHT_CONTROLLER_VERSION_MASK 0xFFF
|
|
||||||
|
|
||||||
static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER;
|
|
||||||
#define BOARD_IDENTIFIER_LENGTH 4 // 4 UPPER CASE alpha numeric characters that identify the board being used.
|
|
||||||
#define BOARD_HARDWARE_REVISION_LENGTH 2
|
|
||||||
|
|
||||||
// These are baseflight specific flags but they are useless now since MW 2.3 uses the upper 4 bits for the navigation version.
|
|
||||||
#define CAP_PLATFORM_32BIT ((uint32_t)1 << 31)
|
|
||||||
#define CAP_BASEFLIGHT_CONFIG ((uint32_t)1 << 30)
|
|
||||||
|
|
||||||
// MW 2.3 stores NAVI_VERSION in the top 4 bits of the capability mask.
|
|
||||||
#define CAP_NAVI_VERSION_BIT_4_MSB ((uint32_t)1 << 31)
|
|
||||||
#define CAP_NAVI_VERSION_BIT_3 ((uint32_t)1 << 30)
|
|
||||||
#define CAP_NAVI_VERSION_BIT_2 ((uint32_t)1 << 29)
|
|
||||||
#define CAP_NAVI_VERSION_BIT_1_LSB ((uint32_t)1 << 28)
|
|
||||||
|
|
||||||
#define CAP_DYNBALANCE ((uint32_t)1 << 2)
|
|
||||||
#define CAP_FLAPS ((uint32_t)1 << 3)
|
|
||||||
#define CAP_NAVCAP ((uint32_t)1 << 4)
|
|
||||||
#define CAP_EXTAUX ((uint32_t)1 << 5)
|
|
||||||
|
|
||||||
#define BST_API_VERSION 1 //out message
|
|
||||||
#define BST_FC_VARIANT 2 //out message
|
|
||||||
#define BST_FC_VERSION 3 //out message
|
|
||||||
#define BST_BOARD_INFO 4 //out message
|
|
||||||
#define BST_BUILD_INFO 5 //out message
|
|
||||||
|
|
||||||
//
|
//
|
||||||
// MSP commands for Cleanflight original features
|
// MSP commands for Cleanflight original features
|
||||||
|
@ -123,41 +88,25 @@ static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER;
|
||||||
|
|
||||||
#define BST_MODE_RANGES 34 //out message Returns all mode ranges
|
#define BST_MODE_RANGES 34 //out message Returns all mode ranges
|
||||||
#define BST_SET_MODE_RANGE 35 //in message Sets a single mode range
|
#define BST_SET_MODE_RANGE 35 //in message Sets a single mode range
|
||||||
|
|
||||||
#define BST_FEATURE 36
|
#define BST_FEATURE 36
|
||||||
#define BST_SET_FEATURE 37
|
#define BST_SET_FEATURE 37
|
||||||
|
|
||||||
#define BST_RX_CONFIG 44
|
#define BST_RX_CONFIG 44
|
||||||
#define BST_SET_RX_CONFIG 45
|
#define BST_SET_RX_CONFIG 45
|
||||||
|
|
||||||
#define BST_LED_COLORS 46
|
#define BST_LED_COLORS 46
|
||||||
#define BST_SET_LED_COLORS 47
|
#define BST_SET_LED_COLORS 47
|
||||||
|
|
||||||
#define BST_LED_STRIP_CONFIG 48
|
#define BST_LED_STRIP_CONFIG 48
|
||||||
#define BST_SET_LED_STRIP_CONFIG 49
|
#define BST_SET_LED_STRIP_CONFIG 49
|
||||||
|
|
||||||
#define BST_LOOP_TIME 83 //out message Returns FC cycle time i.e looptime parameter
|
#define BST_LOOP_TIME 83 //out message Returns FC cycle time i.e looptime parameter
|
||||||
#define BST_SET_LOOP_TIME 84 //in message Sets FC cycle time i.e looptime parameter
|
#define BST_SET_LOOP_TIME 84 //in message Sets FC cycle time i.e looptime parameter
|
||||||
|
#define BST_RX_MAP 64 //out message Get channel map (also returns number of channels total)
|
||||||
//
|
#define BST_SET_RX_MAP 65 //in message Set rx map, numchannels to set comes from BST_RX_MAP
|
||||||
// Baseflight MSP commands (if enabled they exist in Cleanflight)
|
#define BST_REBOOT 68 //in message Reboot
|
||||||
//
|
#define BST_DISARM 70 //in message Disarm
|
||||||
#define BST_RX_MAP 64 //out message get channel map (also returns number of channels total)
|
#define BST_ENABLE_ARM 71 //in message Enable arm
|
||||||
#define BST_SET_RX_MAP 65 //in message set rx map, numchannels to set comes from BST_RX_MAP
|
|
||||||
|
|
||||||
#define BST_REBOOT 68 //in message reboot settings
|
|
||||||
|
|
||||||
|
|
||||||
#define BST_DISARM 70 //in message to disarm
|
|
||||||
#define BST_ENABLE_ARM 71 //in message to enable arm
|
|
||||||
|
|
||||||
#define BST_DEADBAND 72 //out message
|
#define BST_DEADBAND 72 //out message
|
||||||
#define BST_SET_DEADBAND 73 //in message
|
#define BST_SET_DEADBAND 73 //in message
|
||||||
|
|
||||||
#define BST_FC_FILTERS 74 //out message
|
#define BST_FC_FILTERS 74 //out message
|
||||||
#define BST_SET_FC_FILTERS 75 //in message
|
#define BST_SET_FC_FILTERS 75 //in message
|
||||||
|
|
||||||
|
|
||||||
#define BST_STATUS 101 //out message cycletime & errors_count & sensor present & box activation & current setting number
|
#define BST_STATUS 101 //out message cycletime & errors_count & sensor present & box activation & current setting number
|
||||||
#define BST_RC_TUNING 111 //out message rc rate, rc expo, rollpitch rate, yaw rate, dyn throttle PID
|
#define BST_RC_TUNING 111 //out message rc rate, rc expo, rollpitch rate, yaw rate, dyn throttle PID
|
||||||
#define BST_PID 112 //out message P I D coeff (9 are used currently)
|
#define BST_PID 112 //out message P I D coeff (9 are used currently)
|
||||||
|
@ -182,8 +131,6 @@ extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
|
||||||
// cause reboot after BST processing complete
|
// cause reboot after BST processing complete
|
||||||
static bool isRebootScheduled = false;
|
static bool isRebootScheduled = false;
|
||||||
|
|
||||||
#define BOARD_IDENTIFIER_LENGTH 4
|
|
||||||
|
|
||||||
typedef struct box_e {
|
typedef struct box_e {
|
||||||
const uint8_t boxId; // see boxId_e
|
const uint8_t boxId; // see boxId_e
|
||||||
const char *boxName; // GUI-readable box name
|
const char *boxName; // GUI-readable box name
|
||||||
|
@ -223,8 +170,8 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
|
||||||
{ CHECKBOX_ITEM_COUNT, NULL, 0xFF }
|
{ CHECKBOX_ITEM_COUNT, NULL, 0xFF }
|
||||||
};
|
};
|
||||||
|
|
||||||
extern uint8_t readData[DATA_BUFFER_SIZE];
|
extern uint8_t readData[BST_BUFFER_SIZE];
|
||||||
extern uint8_t writeData[DATA_BUFFER_SIZE];
|
extern uint8_t writeData[BST_BUFFER_SIZE];
|
||||||
|
|
||||||
/*************************************************************************************************/
|
/*************************************************************************************************/
|
||||||
uint8_t writeBufferPointer = 1;
|
uint8_t writeBufferPointer = 1;
|
||||||
|
@ -310,39 +257,6 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
|
||||||
uint32_t i, tmp, junk;
|
uint32_t i, tmp, junk;
|
||||||
|
|
||||||
switch(bstRequest) {
|
switch(bstRequest) {
|
||||||
case BST_API_VERSION:
|
|
||||||
bstWrite8(BST_PROTOCOL_VERSION);
|
|
||||||
|
|
||||||
bstWrite8(API_VERSION_MAJOR);
|
|
||||||
bstWrite8(API_VERSION_MINOR);
|
|
||||||
break;
|
|
||||||
case BST_FC_VARIANT:
|
|
||||||
for (i = 0; i < FLIGHT_CONTROLLER_IDENTIFIER_LENGTH; i++) {
|
|
||||||
bstWrite8(flightControllerIdentifier[i]);
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
case BST_FC_VERSION:
|
|
||||||
bstWrite8(FC_VERSION_MAJOR);
|
|
||||||
bstWrite8(FC_VERSION_MINOR);
|
|
||||||
bstWrite8(FC_VERSION_PATCH_LEVEL);
|
|
||||||
break;
|
|
||||||
case BST_BOARD_INFO:
|
|
||||||
for (i = 0; i < BOARD_IDENTIFIER_LENGTH; i++) {
|
|
||||||
bstWrite8(boardIdentifier[i]);
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
case BST_BUILD_INFO:
|
|
||||||
for (i = 0; i < BUILD_DATE_LENGTH; i++) {
|
|
||||||
bstWrite8(buildDate[i]);
|
|
||||||
}
|
|
||||||
for (i = 0; i < BUILD_TIME_LENGTH; i++) {
|
|
||||||
bstWrite8(buildTime[i]);
|
|
||||||
}
|
|
||||||
|
|
||||||
for (i = 0; i < GIT_SHORT_REVISION_LENGTH; i++) {
|
|
||||||
bstWrite8(shortGitRevision[i]);
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
case BST_STATUS:
|
case BST_STATUS:
|
||||||
bstWrite16(getTaskDeltaTime(TASK_GYROPID));
|
bstWrite16(getTaskDeltaTime(TASK_GYROPID));
|
||||||
#ifdef USE_I2C
|
#ifdef USE_I2C
|
||||||
|
@ -388,7 +302,6 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
|
||||||
bstWrite8(getCurrentPidProfileIndex());
|
bstWrite8(getCurrentPidProfileIndex());
|
||||||
break;
|
break;
|
||||||
case BST_LOOP_TIME:
|
case BST_LOOP_TIME:
|
||||||
//bstWrite16(masterConfig.looptime);
|
|
||||||
bstWrite16(getTaskDeltaTime(TASK_GYROPID));
|
bstWrite16(getTaskDeltaTime(TASK_GYROPID));
|
||||||
break;
|
break;
|
||||||
case BST_RC_TUNING:
|
case BST_RC_TUNING:
|
||||||
|
@ -502,7 +415,6 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
|
||||||
// we do not know how to handle the (valid) message, indicate error BST
|
// we do not know how to handle the (valid) message, indicate error BST
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
//bstSlaveWrite(writeData);
|
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -519,7 +431,6 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand)
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
case BST_SET_LOOP_TIME:
|
case BST_SET_LOOP_TIME:
|
||||||
//masterConfig.looptime = bstRead16();
|
|
||||||
bstRead16();
|
bstRead16();
|
||||||
break;
|
break;
|
||||||
case BST_SET_PID:
|
case BST_SET_PID:
|
||||||
|
@ -593,7 +504,6 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand)
|
||||||
if (ARMING_FLAG(ARMED)) {
|
if (ARMING_FLAG(ARMED)) {
|
||||||
ret = BST_FAILED;
|
ret = BST_FAILED;
|
||||||
bstWrite8(ret);
|
bstWrite8(ret);
|
||||||
//bstSlaveWrite(writeData);
|
|
||||||
return ret;
|
return ret;
|
||||||
}
|
}
|
||||||
writeEEPROM();
|
writeEEPROM();
|
||||||
|
@ -677,9 +587,10 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand)
|
||||||
ret = BST_FAILED;
|
ret = BST_FAILED;
|
||||||
}
|
}
|
||||||
bstWrite8(ret);
|
bstWrite8(ret);
|
||||||
//bstSlaveWrite(writeData);
|
|
||||||
if(ret == BST_FAILED)
|
if(ret == BST_FAILED)
|
||||||
return false;
|
return false;
|
||||||
|
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -694,25 +605,26 @@ static bool bstSlaveUSBCommandFeedback(/*uint8_t bstFeedback*/)
|
||||||
bstWrite8(FC_VERSION_MINOR); //Firmware ID
|
bstWrite8(FC_VERSION_MINOR); //Firmware ID
|
||||||
bstWrite8(0x00);
|
bstWrite8(0x00);
|
||||||
bstWrite8(0x00);
|
bstWrite8(0x00);
|
||||||
//bstSlaveWrite(writeData);
|
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
/*************************************************************************************************/
|
/*************************************************************************************************/
|
||||||
#define BST_RESET_TIME 1.2*1000*1000 //micro-seconds
|
#define BST_RESET_TIME 1.2*1000*1000 //micro-seconds
|
||||||
|
|
||||||
uint32_t resetBstTimer = 0;
|
uint32_t resetBstTimer = 0;
|
||||||
bool needResetCheck = true;
|
bool needResetCheck = true;
|
||||||
|
|
||||||
extern bool cleanflight_data_ready;
|
extern bool cleanflight_data_ready;
|
||||||
|
|
||||||
void bstProcessInCommand(void)
|
void bstProcessInCommand(void)
|
||||||
{
|
{
|
||||||
readBufferPointer = 2;
|
readBufferPointer = 2;
|
||||||
if(bstCurrentAddress() == CLEANFLIGHT_FC) {
|
if(bstCurrentAddress() == I2C_ADDR_CLEANFLIGHT_FC) {
|
||||||
if(bstReadCRC() == CRC8 && bstRead8()==BST_USB_COMMANDS) {
|
if(bstReadCRC() == CRC8 && bstRead8()==BST_USB_COMMANDS) {
|
||||||
uint8_t i;
|
uint8_t i;
|
||||||
writeBufferPointer = 1;
|
writeBufferPointer = 1;
|
||||||
cleanflight_data_ready = false;
|
cleanflight_data_ready = false;
|
||||||
for(i = 0; i < DATA_BUFFER_SIZE; i++) {
|
for(i = 0; i < BST_BUFFER_SIZE; i++) {
|
||||||
writeData[i] = 0;
|
writeData[i] = 0;
|
||||||
}
|
}
|
||||||
switch (bstRead8()) {
|
switch (bstRead8()) {
|
||||||
|
@ -780,9 +692,10 @@ void taskBstMasterProcess(timeUs_t currentTimeUs)
|
||||||
sendCounter = 0;
|
sendCounter = 0;
|
||||||
next20hzUpdateAt_1 = currentTimeUs + UPDATE_AT_20HZ;
|
next20hzUpdateAt_1 = currentTimeUs + UPDATE_AT_20HZ;
|
||||||
}
|
}
|
||||||
|
#ifdef GPS
|
||||||
if(sensors(SENSOR_GPS) && !bstWriteBusy())
|
if(sensors(SENSOR_GPS) && !bstWriteBusy())
|
||||||
writeGpsPositionPrameToBST();
|
writeGpsPositionPrameToBST();
|
||||||
|
#endif
|
||||||
|
|
||||||
}
|
}
|
||||||
bstMasterWriteLoop();
|
bstMasterWriteLoop();
|
||||||
|
@ -795,7 +708,7 @@ void taskBstMasterProcess(timeUs_t currentTimeUs)
|
||||||
|
|
||||||
/*************************************************************************************************/
|
/*************************************************************************************************/
|
||||||
static uint8_t masterWriteBufferPointer;
|
static uint8_t masterWriteBufferPointer;
|
||||||
static uint8_t masterWriteData[DATA_BUFFER_SIZE];
|
static uint8_t masterWriteData[BST_BUFFER_SIZE];
|
||||||
|
|
||||||
static void bstMasterStartBuffer(uint8_t address)
|
static void bstMasterStartBuffer(uint8_t address)
|
||||||
{
|
{
|
||||||
|
@ -831,9 +744,9 @@ static uint16_t alt = 0;
|
||||||
static uint8_t numOfSat = 0;
|
static uint8_t numOfSat = 0;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
#ifdef GPS
|
||||||
bool writeGpsPositionPrameToBST(void)
|
bool writeGpsPositionPrameToBST(void)
|
||||||
{
|
{
|
||||||
#ifdef GPS
|
|
||||||
if((lat != GPS_coord[LAT]) || (lon != GPS_coord[LON]) || (alt != GPS_altitude) || (numOfSat != GPS_numSat)) {
|
if((lat != GPS_coord[LAT]) || (lon != GPS_coord[LON]) || (alt != GPS_altitude) || (numOfSat != GPS_numSat)) {
|
||||||
lat = GPS_coord[LAT];
|
lat = GPS_coord[LAT];
|
||||||
lon = GPS_coord[LON];
|
lon = GPS_coord[LON];
|
||||||
|
@ -858,10 +771,8 @@ bool writeGpsPositionPrameToBST(void)
|
||||||
return bstMasterWrite(masterWriteData);
|
return bstMasterWrite(masterWriteData);
|
||||||
} else
|
} else
|
||||||
return false;
|
return false;
|
||||||
#else
|
|
||||||
return true;
|
|
||||||
#endif
|
|
||||||
}
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
bool writeRollPitchYawToBST(void)
|
bool writeRollPitchYawToBST(void)
|
||||||
{
|
{
|
||||||
|
@ -892,37 +803,6 @@ bool writeRCChannelToBST(void)
|
||||||
|
|
||||||
bool writeFCModeToBST(void)
|
bool writeFCModeToBST(void)
|
||||||
{
|
{
|
||||||
#ifdef CLEANFLIGHT_FULL_STATUS_SET
|
|
||||||
uint32_t tmp = 0;
|
|
||||||
tmp = IS_ENABLED(FLIGHT_MODE(ANGLE_MODE)) << BOXANGLE |
|
|
||||||
IS_ENABLED(FLIGHT_MODE(HORIZON_MODE)) << BOXHORIZON |
|
|
||||||
IS_ENABLED(FLIGHT_MODE(BARO_MODE)) << BOXBARO |
|
|
||||||
IS_ENABLED(FLIGHT_MODE(MAG_MODE)) << BOXMAG |
|
|
||||||
IS_ENABLED(FLIGHT_MODE(HEADFREE_MODE)) << BOXHEADFREE |
|
|
||||||
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXHEADADJ)) << BOXHEADADJ |
|
|
||||||
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCAMSTAB)) << BOXCAMSTAB |
|
|
||||||
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCAMTRIG)) << BOXCAMTRIG |
|
|
||||||
IS_ENABLED(FLIGHT_MODE(GPS_HOME_MODE)) << BOXGPSHOME |
|
|
||||||
IS_ENABLED(FLIGHT_MODE(GPS_HOLD_MODE)) << BOXGPSHOLD |
|
|
||||||
IS_ENABLED(FLIGHT_MODE(PASSTHRU_MODE)) << BOXPASSTHRU |
|
|
||||||
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBEEPERON)) << BOXBEEPERON |
|
|
||||||
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLEDMAX)) << BOXLEDMAX |
|
|
||||||
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLEDLOW)) << BOXLEDLOW |
|
|
||||||
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLLIGHTS)) << BOXLLIGHTS |
|
|
||||||
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCALIB)) << BOXCALIB |
|
|
||||||
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGOV)) << BOXGOV |
|
|
||||||
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXOSD)) << BOXOSD |
|
|
||||||
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXTELEMETRY)) << BOXTELEMETRY |
|
|
||||||
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGTUNE)) << BOXGTUNE |
|
|
||||||
IS_ENABLED(FLIGHT_MODE(SONAR_MODE)) << BOXSONAR |
|
|
||||||
IS_ENABLED(ARMING_FLAG(ARMED)) << BOXARM |
|
|
||||||
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOX)) << BOXBLACKBOX |
|
|
||||||
IS_ENABLED(FLIGHT_MODE(FAILSAFE_MODE)) << BOXFAILSAFE;
|
|
||||||
bstMasterStartBuffer(PUBLIC_ADDRESS);
|
|
||||||
bstMasterWrite8(CLEANFLIGHT_MODE_FRAME_ID);
|
|
||||||
bstMasterWrite32(tmp);
|
|
||||||
bstMasterWrite16(sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4);
|
|
||||||
#else
|
|
||||||
uint8_t tmp = 0;
|
uint8_t tmp = 0;
|
||||||
tmp = IS_ENABLED(ARMING_FLAG(ARMED)) |
|
tmp = IS_ENABLED(ARMING_FLAG(ARMED)) |
|
||||||
IS_ENABLED(FLIGHT_MODE(ANGLE_MODE)) << 1 |
|
IS_ENABLED(FLIGHT_MODE(ANGLE_MODE)) << 1 |
|
||||||
|
@ -937,7 +817,6 @@ bool writeFCModeToBST(void)
|
||||||
bstMasterWrite8(CLEANFLIGHT_MODE_FRAME_ID);
|
bstMasterWrite8(CLEANFLIGHT_MODE_FRAME_ID);
|
||||||
bstMasterWrite8(tmp);
|
bstMasterWrite8(tmp);
|
||||||
bstMasterWrite8(sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4);
|
bstMasterWrite8(sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4);
|
||||||
#endif
|
|
||||||
|
|
||||||
return bstMasterWrite(masterWriteData);
|
return bstMasterWrite(masterWriteData);
|
||||||
}
|
}
|
||||||
|
|
|
@ -23,6 +23,11 @@
|
||||||
#include "drivers/dma.h"
|
#include "drivers/dma.h"
|
||||||
#include "drivers/timer.h"
|
#include "drivers/timer.h"
|
||||||
|
|
||||||
|
#ifdef USE_BST
|
||||||
|
#include "bus_bst.h"
|
||||||
|
#endif
|
||||||
|
|
||||||
|
|
||||||
const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
|
const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
|
||||||
{ TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_PPM, 0, GPIO_AF_6, NULL, 0 }, // PWM1 - PA8
|
{ TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_PPM, 0, GPIO_AF_6, NULL, 0 }, // PWM1 - PA8
|
||||||
{ TIM3, IO_TAG(PC6), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_2, DMA1_Channel6, DMA1_CH6_HANDLER }, // PWM2 - PC6
|
{ TIM3, IO_TAG(PC6), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_2, DMA1_Channel6, DMA1_CH6_HANDLER }, // PWM2 - PC6
|
||||||
|
@ -37,3 +42,12 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
|
||||||
{ TIM15, IO_TAG(PB15), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM11 - PB15
|
{ TIM15, IO_TAG(PB15), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM11 - PB15
|
||||||
{ TIM16, IO_TAG(PA6), TIM_Channel_1, TIM_USE_LED, 1, GPIO_AF_1, DMA1_Channel3, DMA1_CH3_HANDLER }, // PWM11 - PB15
|
{ TIM16, IO_TAG(PA6), TIM_Channel_1, TIM_USE_LED, 1, GPIO_AF_1, DMA1_Channel3, DMA1_CH3_HANDLER }, // PWM11 - PB15
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
|
#ifdef USE_BST
|
||||||
|
void targetBusInit(void)
|
||||||
|
{
|
||||||
|
bstInit(BST_DEVICE);
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
|
|
@ -22,6 +22,7 @@
|
||||||
#define BST_DEVICE_NAME_LENGTH 12
|
#define BST_DEVICE_NAME_LENGTH 12
|
||||||
#define TARGET_CONFIG
|
#define TARGET_CONFIG
|
||||||
#define TARGET_VALIDATECONFIG
|
#define TARGET_VALIDATECONFIG
|
||||||
|
#define TARGET_BUS_INIT
|
||||||
|
|
||||||
#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT
|
#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue