Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-25 01:05:27 +03:00
Code Issues Wiki Activity

Add BST support for TBS CorePro to betaflight

Browse source 
Conflicts:
	Makefile
	src/main/drivers/bus_bst.h
	src/main/drivers/bus_bst_stm32f30x.c
	src/main/io/i2c_bst.c
	src/main/io/i2c_bst.h
	src/main/main.c
	src/main/mw.c
	src/main/target/COLIBRI_RACE/target.h

corrections
This commit is contained in:
Larry 2015-11-13 13:10:00 +08:00 • committed by borisbstyle
parent 6f8e9014f5
commit 8e2b485eac
6 changed files with 103 additions and 45 deletions
Download patch file Download diff file Expand all files Collapse all files

4
Makefile
View file

@ -577,8 +577,8 @@ CHEBUZZF3_SRC = \
COLIBRI_RACE_SRC = \ COLIBRI_RACE_SRC = \
$(STM32F30x_COMMON_SRC) \ $(STM32F30x_COMMON_SRC) \
io/i2c_bst.c \ io/i2c_bst.c \
drivers/bus_bst_stm32f30x.c \ drivers/bus_bst_stm32f30x.c \
drivers/display_ug2864hsweg01.c \ drivers/display_ug2864hsweg01.c \
drivers/accgyro_mpu.c \ drivers/accgyro_mpu.c \
drivers/accgyro_mpu6500.c \ drivers/accgyro_mpu6500.c \

10
src/main/drivers/bus_bst.h
View file

@ -17,19 +17,20 @@
#pragma once #pragma once
#define TBS_CORE_PNP_PRO 0x80 #define TBS_CORE_PNP_PRO 0x80
#define RESERVED 0x8A #define RESERVED 0x8A
#define PNP_PRO_DIDITAL_CURRENT_SENSOR 0xC0 #define PNP_PRO_DIDITAL_CURRENT_SENSOR 0xC0
#define PNP_PRO_GPS 0xC2 #define PNP_PRO_GPS 0xC2
#define TSB_BLACKBOX 0xC4 #define TSB_BLACKBOX 0xC4
#define CLEANFLIGHT_FC 0xC8 #define CLEANFLIGHT_FC 0xC8
#define CROSSFIRE_UHF_RECEIVER 0xEC #define CROSSFIRE_UHF_RECEIVER 0xEC
#define GPS_POSITION_FRAME_ID 0x02 #define GPS_POSITION_FRAME_ID 0x02
#define GPS_TIME_FRAME_ID 0x03 #define GPS_TIME_FRAME_ID 0x03
#define FC_ATTITUDE_FRAME_ID 0x1E #define FC_ATTITUDE_FRAME_ID 0x1E
#define RC_CHANNEL_FRAME_ID 0x15 #define RC_CHANNEL_FRAME_ID 0x15
#define CROSSFIRE_RSSI_FRAME_ID 0x14 #define CROSSFIRE_RSSI_FRAME_ID 0x14
#define CLEANFLIGHT_MODE_FRAME_ID 0x20
typedef enum BSTDevice { typedef enum BSTDevice {
BSTDEV_1, BSTDEV_1,
@ -52,3 +53,4 @@ void crc8Cal(uint8_t data_in);

11
src/main/drivers/bus_bst_stm32f30x.c
View file

@ -302,6 +302,13 @@ bool bstSlaveWrite(uint8_t* data) {
/* Decrement the read bytes counter */ /* Decrement the read bytes counter */
len--; len--;
} }
/* Wait until TXIS flag is set */
bstTimeout = BST_LONG_TIMEOUT;
while (I2C_GetFlagStatus(BSTx, I2C_ISR_TXIS) == RESET) {
if ((bstTimeout--) == 0) {
return bstTimeoutUserCallback(BSTx);
}
}
/* If all operations OK */ /* If all operations OK */
return true; return true;
} }
@ -379,7 +386,7 @@ void crc8Cal(uint8_t data_in)
if (CRC8 & 0x80) { if (CRC8 & 0x80) {
MSB_Flag = true; MSB_Flag = true;
} }
CRC8 <<= 1; CRC8 <<= 1;
/* MSB_Set = 80; */ /* MSB_Set = 80; */
@ -387,7 +394,7 @@ void crc8Cal(uint8_t data_in)
CRC8++; CRC8++;
} }
data_in <<= 1; data_in <<= 1;
if (MSB_Flag == true) { if (MSB_Flag == true) {
CRC8 ^= Polynom; CRC8 ^= Polynom;
} }

103
src/main/io/i2c_bst.c
View file

@ -275,9 +275,6 @@ static uint8_t activeBoxIdCount = 0;
// from mixer.c // from mixer.c
extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
extern float anglerad[2];
extern int16_t heading;
// cause reboot after BST processing complete // cause reboot after BST processing complete
static bool isRebootScheduled = false; static bool isRebootScheduled = false;
@ -651,7 +648,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
case BST_ATTITUDE: case BST_ATTITUDE:
for (i = 0; i < 2; i++) for (i = 0; i < 2; i++)
bstWrite16(attitude.raw[i]); bstWrite16(attitude.raw[i]);
//bstWrite16(heading); //FIXME //bstWrite16(heading);
break; break;
case BST_ALTITUDE: case BST_ALTITUDE:
#if defined(BARO) || defined(SONAR) #if defined(BARO) || defined(SONAR)
@ -682,6 +679,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
bstWrite8(masterConfig.disarm_kill_switch); bstWrite8(masterConfig.disarm_kill_switch);
break; break;
case BST_LOOP_TIME: case BST_LOOP_TIME:
//bstWrite16(masterConfig.looptime);
bstWrite16(cycleTime); bstWrite16(cycleTime);
break; break;
case BST_RC_TUNING: case BST_RC_TUNING:
@ -995,7 +993,10 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
bstWrite8(masterConfig.profile[0].rcControlsConfig.yaw_deadband); bstWrite8(masterConfig.profile[0].rcControlsConfig.yaw_deadband);
break; break;
case BST_FC_FILTERS: case BST_FC_FILTERS:
bstWrite16(masterConfig.gyro_lpf);
bstWrite8(0);//masterConfig.profile[0].pidProfile.gyro_cut_hz);
bstWrite8(masterConfig.profile[0].pidProfile.dterm_cut_hz); bstWrite8(masterConfig.profile[0].pidProfile.dterm_cut_hz);
bstWrite8(masterConfig.profile[0].pidProfile.yaw_pterm_cut_hz);
break; break;
default: default:
// 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
@ -1045,7 +1046,7 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand)
rxMspFrameReceive(frame, channelCount); rxMspFrameReceive(frame, channelCount);
} }
} }
case BST_SET_ACC_TRIM: case BST_SET_ACC_TRIM:
currentProfile->accelerometerTrims.values.pitch = bstRead16(); currentProfile->accelerometerTrims.values.pitch = bstRead16();
currentProfile->accelerometerTrims.values.roll = bstRead16(); currentProfile->accelerometerTrims.values.roll = bstRead16();
@ -1056,6 +1057,7 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand)
break; break;
case BST_SET_LOOP_TIME: case BST_SET_LOOP_TIME:
//masterConfig.looptime = bstRead16(); //masterConfig.looptime = bstRead16();
cycleTime = bstRead16();
break; break;
case BST_SET_PID_CONTROLLER: case BST_SET_PID_CONTROLLER:
currentProfile->pidProfile.pidController = bstRead8(); currentProfile->pidProfile.pidController = bstRead8();
@ -1456,6 +1458,8 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand)
masterConfig.profile[0].rcControlsConfig.yaw_deadband = bstRead8(); masterConfig.profile[0].rcControlsConfig.yaw_deadband = bstRead8();
break; break;
case BST_SET_FC_FILTERS: case BST_SET_FC_FILTERS:
masterConfig.gyro_lpf = bstRead16();
bstRead8();//masterConfig.profile[0].pidProfile.gyro_cut_hz = bstRead8();
masterConfig.profile[0].pidProfile.dterm_cut_hz = bstRead8(); masterConfig.profile[0].pidProfile.dterm_cut_hz = bstRead8();
masterConfig.profile[0].pidProfile.yaw_pterm_cut_hz = bstRead8(); masterConfig.profile[0].pidProfile.yaw_pterm_cut_hz = bstRead8();
break; break;
@ -1521,30 +1525,35 @@ static void bstSlaveProcessInCommand(void)
} }
/*************************************************************************************************/ /*************************************************************************************************/
#define UPDATE_AT_20HZ ((1000 * 1000) / 20) #define UPDATE_AT_02HZ ((1000 * 1000) / 2)
static uint32_t next20hzUpdateAt = 0; static uint32_t next02hzUpdateAt_1 = 0;
#define UPDATE_AT_10HZ ((1000 * 1000) / 10) #define UPDATE_AT_10HZ ((1000 * 1000) / 10)
static uint32_t next10hzUpdateAt_1 = 0; static uint32_t next10hzUpdateAt_1 = 0;
static uint32_t next10hzUpdateAt_2 = 0;
void bstProcess(void) void bstProcess(void)
{ {
if(coreProReady) { if(coreProReady) {
uint32_t now = micros(); uint32_t now = micros();
if(now >= next20hzUpdateAt && !bstWriteBusy()) { if(now >= next02hzUpdateAt_1 && !bstWriteBusy()) {
writeRCChannelToBST(); writeFCModeToBST();
next20hzUpdateAt = now + UPDATE_AT_20HZ; next02hzUpdateAt_1 = now + UPDATE_AT_02HZ;
} }
if(now >= next10hzUpdateAt_1 && !bstWriteBusy()) { if(now >= next10hzUpdateAt_1 && !bstWriteBusy()) {
//writeRollPitchYawToBST(); FIXME writeRCChannelToBST();
next10hzUpdateAt_1 = now + UPDATE_AT_10HZ; next10hzUpdateAt_1 = now + UPDATE_AT_10HZ;
} }
if(now >= next10hzUpdateAt_2 && !bstWriteBusy()) {
writeRollPitchYawToBST();
next10hzUpdateAt_2 = now + UPDATE_AT_10HZ;
}
if(sensors(SENSOR_GPS) && !bstWriteBusy()) if(sensors(SENSOR_GPS) && !bstWriteBusy())
writeGpsPositionPrameToBST(); writeGpsPositionPrameToBST();
} }
//Check if the BST input command available to out address //Check if the BST input command available to out address
bstSlaveProcessInCommand(); bstSlaveProcessInCommand();
if (isRebootScheduled) { if (isRebootScheduled) {
stopMotors(); stopMotors();
handleOneshotFeatureChangeOnRestart(); handleOneshotFeatureChangeOnRestart();
@ -1614,11 +1623,10 @@ bool writeGpsPositionPrameToBST(void)
return false; return false;
} }
/* FIXME
bool writeRollPitchYawToBST(void) bool writeRollPitchYawToBST(void)
{ {
int16_t X = -anglerad[1]*10000; int16_t X = -attitude.values.pitch * (M_PIf / 1800.0f) * 10000;
int16_t Y = anglerad[0]*10000; int16_t Y = attitude.values.roll * (M_PIf / 1800.0f) * 10000;
int16_t Z = 0;//radiusHeading * 10000; int16_t Z = 0;//radiusHeading * 10000;
bstMasterStartBuffer(PUBLIC_ADDRESS); bstMasterStartBuffer(PUBLIC_ADDRESS);
@ -1629,29 +1637,68 @@ bool writeRollPitchYawToBST(void)
return bstMasterWrite(masterWriteData); return bstMasterWrite(masterWriteData);
} }
*/
bool writeRCChannelToBST(void) bool writeRCChannelToBST(void)
{ {
uint8_t i = 0, tmp = 0; uint8_t i = 0;
tmp = IS_ENABLED(ARMING_FLAG(ARMED)) |
IS_ENABLED(FLIGHT_MODE(ANGLE_MODE)) << 1 |
IS_ENABLED(FLIGHT_MODE(HORIZON_MODE)) << 2 |
IS_ENABLED(FLIGHT_MODE(BARO_MODE)) << 3 |
IS_ENABLED(FLIGHT_MODE(MAG_MODE)) << 4 |
IS_ENABLED(FLIGHT_MODE(HEADFREE_MODE)) << 5 |
IS_ENABLED(FLIGHT_MODE(SONAR_MODE)) << 6 |
IS_ENABLED(FLIGHT_MODE(FAILSAFE_MODE)) << 7;
bstMasterStartBuffer(PUBLIC_ADDRESS); bstMasterStartBuffer(PUBLIC_ADDRESS);
bstMasterWrite8(RC_CHANNEL_FRAME_ID); bstMasterWrite8(RC_CHANNEL_FRAME_ID);
for(i = 0; i < (USABLE_TIMER_CHANNEL_COUNT-1); i++) { for(i = 0; i < (USABLE_TIMER_CHANNEL_COUNT-1); i++) {
bstMasterWrite16(rcData[i]); bstMasterWrite16(rcData[i]);
} }
bstMasterWrite8(tmp);
return bstMasterWrite(masterWriteData); return bstMasterWrite(masterWriteData);
} }
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;
tmp = IS_ENABLED(ARMING_FLAG(ARMED)) |
IS_ENABLED(FLIGHT_MODE(ANGLE_MODE)) << 1 |
IS_ENABLED(FLIGHT_MODE(HORIZON_MODE)) << 2 |
IS_ENABLED(FLIGHT_MODE(BARO_MODE)) << 3 |
IS_ENABLED(FLIGHT_MODE(MAG_MODE)) << 4 |
IS_ENABLED(FLIGHT_MODE(HEADFREE_MODE)) << 5 |
IS_ENABLED(FLIGHT_MODE(SONAR_MODE)) << 6 |
IS_ENABLED(FLIGHT_MODE(FAILSAFE_MODE)) << 7;
bstMasterStartBuffer(PUBLIC_ADDRESS);
bstMasterWrite8(CLEANFLIGHT_MODE_FRAME_ID);
bstMasterWrite8(tmp);
bstMasterWrite8(sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4);
#endif
return bstMasterWrite(masterWriteData);
}
/*************************************************************************************************/ /*************************************************************************************************/

1
src/main/io/i2c_bst.h
View file

@ -27,4 +27,5 @@ void bstProcess(void);
bool writeGpsPositionPrameToBST(void); bool writeGpsPositionPrameToBST(void);
bool writeRollPitchYawToBST(void); bool writeRollPitchYawToBST(void);
bool writeRCChannelToBST(void); bool writeRCChannelToBST(void);
bool writeFCModeToBST(void);

19
src/main/target/COLIBRI_RACE/target.h
View file

@ -57,12 +57,9 @@
#define USABLE_TIMER_CHANNEL_COUNT 11 #define USABLE_TIMER_CHANNEL_COUNT 11
#define EXTI_CALLBACK_HANDLER_COUNT 1 // MPU data ready
#define GYRO #define GYRO
#define USE_GYRO_MPU6500 #define USE_GYRO_MPU6500
#define USE_GYRO_SPI_MPU6500 #define USE_GYRO_SPI_MPU6500
#define GYRO_MPU6500_ALIGN CW270_DEG #define GYRO_MPU6500_ALIGN CW270_DEG
#define ACC #define ACC
@ -70,6 +67,12 @@
#define USE_ACC_SPI_MPU6500 #define USE_ACC_SPI_MPU6500
#define ACC_MPU6500_ALIGN CW270_DEG #define ACC_MPU6500_ALIGN CW270_DEG
// MPU6500 interrupt
//#define DEBUG_MPU_DATA_READY_INTERRUPT
#define USE_MPU_DATA_READY_SIGNAL
#define ENSURE_MPU_DATA_READY_IS_LOW
// External I2C BARO
#define BARO #define BARO
#define USE_BARO_MS5611 #define USE_BARO_MS5611
@ -77,6 +80,7 @@
#define MAG #define MAG
#define USE_MAG_HMC5883 #define USE_MAG_HMC5883
#define USE_MAG_AK8975 #define USE_MAG_AK8975
//#define MAG_AK8975_ALIGN CW0_DEG_FLIP
#define BEEPER #define BEEPER
#define LED0 #define LED0
@ -110,6 +114,9 @@
#define UART3_TX_PINSOURCE GPIO_PinSource10 #define UART3_TX_PINSOURCE GPIO_PinSource10
#define UART3_RX_PINSOURCE GPIO_PinSource11 #define UART3_RX_PINSOURCE GPIO_PinSource11
#define USE_ESCSERIAL
#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1
#define USE_I2C #define USE_I2C
#define I2C_DEVICE (I2CDEV_2) #define I2C_DEVICE (I2CDEV_2)
@ -129,7 +136,6 @@
/* Configure the CRC peripheral to use the polynomial x8 + x7 + x6 + x4 + x2 + 1 */ /* Configure the CRC peripheral to use the polynomial x8 + x7 + x6 + x4 + x2 + 1 */
#define BST_CRC_POLYNOM 0xD5 #define BST_CRC_POLYNOM 0xD5
#define USE_ADC #define USE_ADC
#define ADC_INSTANCE ADC1 #define ADC_INSTANCE ADC1
@ -152,11 +158,6 @@
#define EXTERNAL1_ADC_GPIO_PIN GPIO_Pin_3 #define EXTERNAL1_ADC_GPIO_PIN GPIO_Pin_3
#define EXTERNAL1_ADC_CHANNEL ADC_Channel_9 #define EXTERNAL1_ADC_CHANNEL ADC_Channel_9
// MPU6500 interrupt
//#define DEBUG_MPU_DATA_READY_INTERRUPT
#define USE_MPU_DATA_READY_SIGNAL
#define ENSURE_MPU_DATA_READY_IS_LOW
#define BLACKBOX #define BLACKBOX
#define GPS #define GPS
#define GTUNE #define GTUNE