rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #1 from borisbstyle/betaflight 

pull latest
This commit is contained in:
4712betaflight 2016-04-01 00:40:13 +02:00
parent 0aac025494 24b97aed01
commit 9d737474b8
48 changed files with 1686 additions and 770 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
.gitignore vendored
View File

@ -5,6 +5,8 @@
*.dep
*.bak
*.uvgui.*
*.swp
*.save
.project
.settings
.cproject

1
Makefile
View File

@ -299,6 +299,7 @@ COMMON_SRC = build_config.c \
drivers/gyro_sync.c \
drivers/dma.c \
drivers/buf_writer.c \
drivers/serial_escserial.c \
io/beeper.c \
io/rc_controls.c \
io/rc_curves.c \

1
README.md
View File

@ -42,6 +42,7 @@ Cleanflight also has additional features not found in baseflight.
* Graupner HoTT telemetry.
* Multiple simultaneous telemetry providers.
* Configurable serial ports for Serial RX, Telemetry, MSP, GPS - Use most devices on any port, softserial too.
* Optional lost buzzer on port 6 for CC3D (set enable_buzzer_p6 = ON)
* And many more minor bug fixes.
For a list of features, changes and some discussion please review the thread on MultiWii forums and consult the documentation.

0
fake_travis_build.sh Normal file → Executable file
View File

4
src/main/blackbox/blackbox.c
View File

@ -401,11 +401,7 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition)
case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0:
case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_1:
case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_2:
if (IS_PID_CONTROLLER_FP_BASED(currentProfile->pidProfile.pidController)) {
return currentProfile->pidProfile.D_f[condition - FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0] != 0;
} else {
return currentProfile->pidProfile.D8[condition - FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0] != 0;
}
case FLIGHT_LOG_FIELD_CONDITION_MAG:
#ifdef MAG

23
src/main/common/filter.c
View File

@ -96,3 +96,26 @@ float applyBiQuadFilter(float sample, biquad_t *state)
return result;
}
int32_t filterApplyAverage(int32_t input, uint8_t averageCount, int32_t averageState[DELTA_MAX_SAMPLES]) {
int count;
int32_t averageSum;
for (count = averageCount-1; count > 0; count--) averageState[count] = averageState[count-1];
averageState[0] = input;
for (count = 0; count < averageCount; count++) averageSum += averageState[count];
return averageSum / averageCount;
}
float filterApplyAveragef(float input, uint8_t averageCount, float averageState[DELTA_MAX_SAMPLES]) {
int count;
float averageSum;
for (count = averageCount-1; count > 0; count--) averageState[count] = averageState[count-1];
averageState[0] = input;
for (count = 0; count < averageCount; count++) averageSum += averageState[count];
return averageSum / averageCount;
}

4
src/main/common/filter.h
View File

@ -15,6 +15,8 @@
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#define DELTA_MAX_SAMPLES 12
typedef struct filterStatePt1_s {
float state;
float RC;
@ -29,4 +31,6 @@ typedef struct biquad_s {
float applyBiQuadFilter(float sample, biquad_t *state);
float filterApplyPt1(float input, filterStatePt1_t *filter, float f_cut, float dt);
int32_t filterApplyAverage(int32_t input, uint8_t averageCount, int32_t averageState[DELTA_MAX_SAMPLES]);
float filterApplyAveragef(float input, uint8_t averageCount, float averageState[DELTA_MAX_SAMPLES]);
void BiQuadNewLpf(float filterCutFreq, biquad_t *newState, uint32_t refreshRate);

52
src/main/config/config.c
View File

@ -134,7 +134,7 @@ static uint32_t activeFeaturesLatch = 0;
static uint8_t currentControlRateProfileIndex = 0;
controlRateConfig_t *currentControlRateProfile;
static const uint8_t EEPROM_CONF_VERSION = 129;
static const uint8_t EEPROM_CONF_VERSION = 131;
static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims)
{
@ -147,14 +147,14 @@ static void resetPidProfile(pidProfile_t *pidProfile)
{
pidProfile->pidController = 1;
pidProfile->P8[ROLL] = 42;
pidProfile->I8[ROLL] = 40;
pidProfile->P8[ROLL] = 45;
pidProfile->I8[ROLL] = 30;
pidProfile->D8[ROLL] = 18;
pidProfile->P8[PITCH] = 54;
pidProfile->I8[PITCH] = 40;
pidProfile->D8[PITCH] = 22;
pidProfile->P8[PITCH] = 45;
pidProfile->I8[PITCH] = 30;
pidProfile->D8[PITCH] = 18;
pidProfile->P8[YAW] = 90;
pidProfile->I8[YAW] = 50;
pidProfile->I8[YAW] = 40;
pidProfile->D8[YAW] = 0;
pidProfile->P8[PIDALT] = 50;
pidProfile->I8[PIDALT] = 0;
@ -168,30 +168,22 @@ static void resetPidProfile(pidProfile_t *pidProfile)
pidProfile->P8[PIDNAVR] = 25; // NAV_P * 10;
pidProfile->I8[PIDNAVR] = 33; // NAV_I * 100;
pidProfile->D8[PIDNAVR] = 83; // NAV_D * 1000;
pidProfile->P8[PIDLEVEL] = 30;
pidProfile->I8[PIDLEVEL] = 30;
pidProfile->P8[PIDLEVEL] = 50;
pidProfile->I8[PIDLEVEL] = 50;
pidProfile->D8[PIDLEVEL] = 100;
pidProfile->P8[PIDMAG] = 40;
pidProfile->P8[PIDVEL] = 120;
pidProfile->I8[PIDVEL] = 45;
pidProfile->D8[PIDVEL] = 1;
pidProfile->P8[PIDVEL] = 55;
pidProfile->I8[PIDVEL] = 55;
pidProfile->D8[PIDVEL] = 0;
pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX;
pidProfile->yaw_lpf_hz = 70.0f;
pidProfile->dterm_average_count = 4;
pidProfile->dterm_lpf_hz = 0; // filtering ON by default
pidProfile->rollPitchItermResetRate = 200;
pidProfile->yawItermResetRate = 50;
pidProfile->dterm_lpf_hz = 70.0f; // filtering ON by default
pidProfile->deltaMethod = DELTA_FROM_MEASUREMENT;
pidProfile->P_f[ROLL] = 1.1f;
pidProfile->I_f[ROLL] = 0.4f;
pidProfile->D_f[ROLL] = 0.01f;
pidProfile->P_f[PITCH] = 1.4f;
pidProfile->I_f[PITCH] = 0.4f;
pidProfile->D_f[PITCH] = 0.01f;
pidProfile->P_f[YAW] = 4.0f;
pidProfile->I_f[YAW] = 0.4f;
pidProfile->D_f[YAW] = 0.00f;
pidProfile->A_level = 4.0f;
pidProfile->H_level = 4.0f;
pidProfile->H_sensitivity = 75;
#ifdef GTUNE
@ -272,6 +264,7 @@ void resetBatteryConfig(batteryConfig_t *batteryConfig)
batteryConfig->vbatmaxcellvoltage = 43;
batteryConfig->vbatmincellvoltage = 33;
batteryConfig->vbatwarningcellvoltage = 35;
batteryConfig->vbathysteresis = 1;
batteryConfig->vbatPidCompensation = 0;
batteryConfig->currentMeterOffset = 0;
batteryConfig->currentMeterScale = 400; // for Allegro ACS758LCB-100U (40mV/A)
@ -312,7 +305,7 @@ void resetSerialConfig(serialConfig_t *serialConfig)
static void resetControlRateConfig(controlRateConfig_t *controlRateConfig) {
controlRateConfig->rcRate8 = 100;
controlRateConfig->rcExpo8 = 70;
controlRateConfig->rcExpo8 = 60;
controlRateConfig->thrMid8 = 50;
controlRateConfig->thrExpo8 = 0;
controlRateConfig->dynThrPID = 0;
@ -320,7 +313,7 @@ static void resetControlRateConfig(controlRateConfig_t *controlRateConfig) {
controlRateConfig->tpa_breakpoint = 1500;
for (uint8_t axis = 0; axis < FLIGHT_DYNAMICS_INDEX_COUNT; axis++) {
controlRateConfig->rates[axis] = 0;
controlRateConfig->rates[axis] = 50;
}
}
@ -410,7 +403,7 @@ static void resetConf(void)
masterConfig.dcm_ki = 0; // 0.003 * 10000
masterConfig.gyro_lpf = 0; // 256HZ default
masterConfig.gyro_sync_denom = 8;
masterConfig.gyro_soft_lpf_hz = 60;
masterConfig.gyro_soft_lpf_hz = 80.0f;
masterConfig.pid_process_denom = 1;
@ -459,14 +452,13 @@ static void resetConf(void)
masterConfig.rxConfig.rcSmoothing = 0;
masterConfig.rxConfig.fpvCamAngleDegrees = 0;
masterConfig.rxConfig.max_aux_channel = 6;
masterConfig.rxConfig.acroPlusFactor = 30;
masterConfig.rxConfig.acroPlusOffset = 40;
masterConfig.rxConfig.superExpoFactor = 30;
resetAllRxChannelRangeConfigurations(masterConfig.rxConfig.channelRanges);
masterConfig.inputFilteringMode = INPUT_FILTERING_DISABLED;
masterConfig.retarded_arm = 0; // TODO - Cleanup retarded arm support
masterConfig.gyro_cal_on_first_arm = 0; // TODO - Cleanup retarded arm support
masterConfig.disarm_kill_switch = 1;
masterConfig.auto_disarm_delay = 5;
masterConfig.small_angle = 25;

2
src/main/config/config_master.h
View File

@ -99,7 +99,7 @@ typedef struct master_t {
inputFilteringMode_e inputFilteringMode; // Use hardware input filtering, e.g. for OrangeRX PPM/PWM receivers.
uint8_t retarded_arm; // allow disarm/arm on throttle down + roll left/right
uint8_t gyro_cal_on_first_arm; // allow disarm/arm on throttle down + roll left/right
uint8_t disarm_kill_switch; // allow disarm via AUX switch regardless of throttle value
uint8_t auto_disarm_delay; // allow automatically disarming multicopters after auto_disarm_delay seconds of zero throttle. Disabled when 0
uint8_t small_angle;

3
src/main/config/runtime_config.h
View File

@ -21,7 +21,8 @@
typedef enum {
OK_TO_ARM = (1 << 0),
PREVENT_ARMING = (1 << 1),
ARMED = (1 << 2)
ARMED = (1 << 2),
WAS_EVER_ARMED = (1 << 3)
} armingFlag_e;
extern uint8_t armingFlags;

4
src/main/drivers/bus_bst.h
View File

@ -32,6 +32,8 @@
#define CROSSFIRE_RSSI_FRAME_ID 0x14
#define CLEANFLIGHT_MODE_FRAME_ID 0x20
#define DATA_BUFFER_SIZE 64
typedef enum BSTDevice {
BSTDEV_1,
BSTDEV_2,
@ -39,6 +41,7 @@ typedef enum BSTDevice {
} BSTDevice;
void bstInit(BSTDevice index);
uint32_t bstTimeoutUserCallback(void);
uint16_t bstGetErrorCounter(void);
bool bstWriteBusy(void);
@ -47,7 +50,6 @@ bool bstSlaveRead(uint8_t* buf);
bool bstSlaveWrite(uint8_t* data);
void bstMasterWriteLoop(void);
void bstMasterReadLoop(void);
void crc8Cal(uint8_t data_in);

292
src/main/drivers/bus_bst_stm32f30x.c
View File

@ -12,6 +12,7 @@
#include <build_config.h>
#include "nvic.h"
#include "bus_bst.h"
@ -45,8 +46,6 @@
#define BST2_SDA_CLK_SOURCE RCC_AHBPeriph_GPIOA
#endif
static uint32_t bstTimeout;
static volatile uint16_t bst1ErrorCount = 0;
static volatile uint16_t bst2ErrorCount = 0;
@ -59,19 +58,121 @@ volatile bool coreProReady = false;
// BST TimeoutUserCallback
///////////////////////////////////////////////////////////////////////////////
uint32_t bstTimeoutUserCallback(I2C_TypeDef *BSTx)
uint8_t dataBuffer[DATA_BUFFER_SIZE] = {0};
uint8_t dataBufferPointer = 0;
uint8_t bstWriteDataLen = 0;
uint32_t micros(void);
uint8_t writeData[DATA_BUFFER_SIZE] = {0};
uint8_t currentWriteBufferPointer = 0;
bool receiverAddress = false;
uint8_t readData[DATA_BUFFER_SIZE] = {0};
uint8_t bufferPointer = 0;
void bstProcessInCommand(void);
void I2C_EV_IRQHandler()
{
if(I2C_GetITStatus(BSTx, I2C_IT_ADDR)) {
CRC8 = 0;
if(I2C_GetTransferDirection(BSTx) == I2C_Direction_Receiver) {
currentWriteBufferPointer = 0;
receiverAddress = true;
I2C_SendData(BSTx, (uint8_t) writeData[currentWriteBufferPointer++]);
I2C_ITConfig(BSTx, I2C_IT_TXI, ENABLE);
} else {
readData[0] = I2C_GetAddressMatched(BSTx);
bufferPointer = 1;
}
I2C_ClearITPendingBit(BSTx, I2C_IT_ADDR);
} else if(I2C_GetITStatus(BSTx, I2C_IT_RXNE)) {
uint8_t data = I2C_ReceiveData(BSTx);
readData[bufferPointer] = data;
if(bufferPointer > 1) {
if(readData[1]+1 == bufferPointer) {
crc8Cal(0);
bstProcessInCommand();
} else {
crc8Cal(data);
}
}
bufferPointer++;
I2C_ClearITPendingBit(BSTx, I2C_IT_RXNE);
} else if(I2C_GetITStatus(BSTx, I2C_IT_TXIS)) {
if(receiverAddress) {
if(currentWriteBufferPointer > 0) {
if(writeData[0] == currentWriteBufferPointer) {
receiverAddress = false;
crc8Cal(0);
I2C_SendData(BSTx, (uint8_t) CRC8);
I2C_ITConfig(BSTx, I2C_IT_TXI, DISABLE);
} else {
crc8Cal((uint8_t) writeData[currentWriteBufferPointer]);
I2C_SendData(BSTx, (uint8_t) writeData[currentWriteBufferPointer++]);
}
}
} else if(bstWriteDataLen) {
I2C_SendData(BSTx, (uint8_t) dataBuffer[dataBufferPointer]);
if(bstWriteDataLen > 1)
dataBufferPointer++;
if(dataBufferPointer == bstWriteDataLen) {
I2C_ITConfig(BSTx, I2C_IT_TXI, DISABLE);
dataBufferPointer = 0;
bstWriteDataLen = 0;
}
} else {
}
I2C_ClearITPendingBit(BSTx, I2C_IT_TXIS);
} else if(I2C_GetITStatus(BSTx, I2C_IT_NACKF)) {
if(receiverAddress) {
receiverAddress = false;
I2C_ITConfig(BSTx, I2C_IT_TXI, DISABLE);
}
I2C_ClearITPendingBit(BSTx, I2C_IT_NACKF);
} else if(I2C_GetITStatus(BSTx, I2C_IT_STOPF)) {
if(bstWriteDataLen && dataBufferPointer == bstWriteDataLen) {
dataBufferPointer = 0;
bstWriteDataLen = 0;
}
I2C_ClearITPendingBit(BSTx, I2C_IT_STOPF);
} else if(I2C_GetITStatus(BSTx, I2C_IT_BERR)
|| I2C_GetITStatus(BSTx, I2C_IT_ARLO)
|| I2C_GetITStatus(BSTx, I2C_IT_OVR)) {
bstTimeoutUserCallback();
I2C_ClearITPendingBit(BSTx, I2C_IT_BERR | I2C_IT_ARLO | I2C_IT_OVR);
}
}
void I2C1_EV_IRQHandler()
{
I2C_EV_IRQHandler();
}
void I2C2_EV_IRQHandler()
{
I2C_EV_IRQHandler();
}
uint32_t bstTimeoutUserCallback()
{
if (BSTx == I2C1) {
bst1ErrorCount++;
} else {
bst2ErrorCount++;
}
I2C_GenerateSTOP(BSTx, ENABLE);
receiverAddress = false;
dataBufferPointer = 0;
bstWriteDataLen = 0;
I2C_ITConfig(BSTx, I2C_IT_TXI, DISABLE);
I2C_SoftwareResetCmd(BSTx);
return false;
}
void bstInitPort(I2C_TypeDef *BSTx/*, uint8_t Address*/)
{
NVIC_InitTypeDef nvic;
GPIO_InitTypeDef GPIO_InitStructure;
I2C_InitTypeDef BST_InitStructure;
@ -104,14 +205,22 @@ void bstInitPort(I2C_TypeDef *BSTx/*, uint8_t Address*/)
BST_InitStructure.I2C_AnalogFilter = I2C_AnalogFilter_Enable;
BST_InitStructure.I2C_DigitalFilter = 0x00;
BST_InitStructure.I2C_OwnAddress1 = CLEANFLIGHT_FC;
//BST_InitStructure.I2C_OwnAddress1 = PNP_PRO_GPS;
//BST_InitStructure.I2C_OwnAddress1 = Address;
BST_InitStructure.I2C_Ack = I2C_Ack_Enable;
BST_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit;
BST_InitStructure.I2C_Timing = 0x30E0257A; // 100 Khz, 72Mhz Clock, Analog Filter Delay ON, Rise 100, Fall 10.
I2C_Init(I2C1, &BST_InitStructure);
I2C_GeneralCallCmd(I2C1, ENABLE);
nvic.NVIC_IRQChannel = I2C1_EV_IRQn;
nvic.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_BST_READ_DATA);
nvic.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_BST_READ_DATA);
nvic.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&nvic);
I2C_ITConfig(I2C1, I2C_IT_ADDRI | I2C_IT_RXI | I2C_IT_STOPI | I2C_IT_NACKI | I2C_IT_ERRI, ENABLE);
I2C_Cmd(I2C1, ENABLE);
}
@ -144,14 +253,22 @@ void bstInitPort(I2C_TypeDef *BSTx/*, uint8_t Address*/)
BST_InitStructure.I2C_AnalogFilter = I2C_AnalogFilter_Enable;
BST_InitStructure.I2C_DigitalFilter = 0x00;
BST_InitStructure.I2C_OwnAddress1 = CLEANFLIGHT_FC;
//BST_InitStructure.I2C_OwnAddress1 = PNP_PRO_GPS;
//BST_InitStructure.I2C_OwnAddress1 = Address;
BST_InitStructure.I2C_Ack = I2C_Ack_Enable;
BST_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit;
BST_InitStructure.I2C_Timing = 0x30E0257A; // 100 Khz, 72Mhz Clock, Analog Filter Delay ON, Rise 100, Fall 10.
I2C_Init(I2C2, &BST_InitStructure);
I2C_GeneralCallCmd(I2C2, ENABLE);
nvic.NVIC_IRQChannel = I2C2_EV_IRQn;
nvic.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_BST_READ_DATA);
nvic.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_BST_READ_DATA);
nvic.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&nvic);
I2C_ITConfig(I2C2, I2C_IT_ADDRI | I2C_IT_RXI | I2C_IT_STOPI | I2C_IT_NACKI | I2C_IT_ERRI, ENABLE);
I2C_Cmd(I2C2, ENABLE);
}
}
@ -163,8 +280,6 @@ void bstInit(BSTDevice index)
} else {
BSTx = I2C2;
}
//bstInitPort(BSTx, PNP_PRO_GPS);
//bstInitPort(BSTx, CLEANFLIGHT_FC);
bstInitPort(BSTx);
}
@ -179,9 +294,6 @@ uint16_t bstGetErrorCounter(void)
}
/*************************************************************************************************/
uint8_t dataBuffer[64] = {0};
uint8_t bufferPointer = 0;
uint8_t bstWriteDataLen = 0;
bool bstWriteBusy(void)
{
@ -195,7 +307,7 @@ bool bstMasterWrite(uint8_t* data)
{
if(bstWriteDataLen==0) {
CRC8 = 0;
bufferPointer = 0;
dataBufferPointer = 0;
dataBuffer[0] = *data;
dataBuffer[1] = *(data+1);
bstWriteDataLen = dataBuffer[1] + 2;
@ -213,163 +325,27 @@ bool bstMasterWrite(uint8_t* data)
return false;
}
bool bstSlaveRead(uint8_t* buf) {
if(I2C_GetAddressMatched(BSTx)==CLEANFLIGHT_FC && I2C_GetTransferDirection(BSTx) == I2C_Direction_Transmitter) {
//if(I2C_GetTransferDirection(BSTx) == I2C_Direction_Transmitter) {
uint8_t len = 0;
CRC8 = 0;
I2C_ClearFlag(BSTx, I2C_FLAG_ADDR);
/* Wait until RXNE flag is set */
bstTimeout = BST_LONG_TIMEOUT;
while (I2C_GetFlagStatus(BSTx, I2C_ISR_RXNE) == RESET) {
if ((bstTimeout--) == 0) {
return bstTimeoutUserCallback(BSTx);
}
}
len = I2C_ReceiveData(BSTx);
*buf = len;
buf++;
while (len) {
/* Wait until RXNE flag is set */
bstTimeout = BST_LONG_TIMEOUT;
while (I2C_GetFlagStatus(BSTx, I2C_ISR_RXNE) == RESET) {
if ((bstTimeout--) == 0) {
return bstTimeoutUserCallback(BSTx);
}
}
/* Read data from RXDR */
*buf = I2C_ReceiveData(BSTx);
if(len == 1)
crc8Cal(0);
else
crc8Cal((uint8_t)*buf);
/* Point to the next location where the byte read will be saved */
buf++;
/* Decrement the read bytes counter */
len--;
}
/* If all operations OK */
return true;
}
return false;
}
bool bstSlaveWrite(uint8_t* data) {
bstTimeout = BST_LONG_TIMEOUT;
while (I2C_GetFlagStatus(BSTx, I2C_ISR_ADDR) == RESET) {
if ((bstTimeout--) == 0) {
return bstTimeoutUserCallback(BSTx);
}
}
if(I2C_GetAddressMatched(BSTx)==CLEANFLIGHT_FC && I2C_GetTransferDirection(BSTx) == I2C_Direction_Receiver) {
//if(I2C_GetTransferDirection(BSTx) == I2C_Direction_Receiver) {
uint8_t len = 0;
CRC8 = 0;
I2C_ClearFlag(BSTx, I2C_FLAG_ADDR);
/* Wait until TXIS flag is set */
bstTimeout = BST_LONG_TIMEOUT;
while (I2C_GetFlagStatus(BSTx, I2C_ISR_TXIS) == RESET) {
if ((bstTimeout--) == 0) {
return bstTimeoutUserCallback(BSTx);
}
}
len = *data;
data++;
I2C_SendData(BSTx, (uint8_t) len);
while(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(len == 1) {
crc8Cal(0);
I2C_SendData(BSTx, (uint8_t) CRC8);
} else {
crc8Cal((uint8_t)*data);
I2C_SendData(BSTx, (uint8_t)*data);
}
/* Point to the next location where the byte read will be saved */
data++;
/* Decrement the read bytes counter */
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 */
return true;
}
return false;
}
/*************************************************************************************************/
uint32_t bstMasterWriteTimeout = 0;
void bstMasterWriteLoop(void)
{
while(bstWriteDataLen) {
if(bufferPointer == 0) {
static uint32_t bstMasterWriteTimeout = 0;
uint32_t currentTime = micros();
if(bstWriteDataLen && dataBufferPointer==0) {
bool scl_set = false;
if(BSTx == I2C1)
scl_set = BST1_SCL_GPIO->IDR&BST1_SCL_PIN;
else
scl_set = BST2_SCL_GPIO->IDR&BST2_SCL_PIN;
if(I2C_GetFlagStatus(BSTx, I2C_ISR_BUSY)==RESET && scl_set) {
/* Configure slave address, nbytes, reload, end mode and start or stop generation */
I2C_TransferHandling(BSTx, dataBuffer[bufferPointer], 1, I2C_Reload_Mode, I2C_Generate_Start_Write);
if(I2C_GetFlagStatus(BSTx, I2C_FLAG_BUSY)==RESET && scl_set) {
I2C_TransferHandling(BSTx, dataBuffer[dataBufferPointer], dataBuffer[dataBufferPointer+1]+1, I2C_AutoEnd_Mode, I2C_Generate_Start_Write);
I2C_ITConfig(BSTx, I2C_IT_TXI, ENABLE);
dataBufferPointer = 1;
bstMasterWriteTimeout = micros();
bufferPointer++;
}
} else if(bufferPointer == 1) {
if(I2C_GetFlagStatus(BSTx, I2C_ISR_TXIS)==SET) {
/* Send Register len */
I2C_SendData(BSTx, (uint8_t) dataBuffer[bufferPointer]);
bstMasterWriteTimeout = micros();
} else if(I2C_GetFlagStatus(BSTx, I2C_ISR_TCR)==SET) {
/* Configure slave address, nbytes, reload, end mode and start or stop generation */
I2C_TransferHandling(BSTx, dataBuffer[bufferPointer-1], dataBuffer[bufferPointer], I2C_AutoEnd_Mode, I2C_No_StartStop);
bstMasterWriteTimeout = micros();
bufferPointer++;
}
} else if(bufferPointer == bstWriteDataLen) {
if(I2C_GetFlagStatus(BSTx, I2C_ISR_STOPF)==SET) {
I2C_ClearFlag(BSTx, I2C_ICR_STOPCF);
bstWriteDataLen = 0;
bufferPointer = 0;
}
} else {
if(I2C_GetFlagStatus(BSTx, I2C_ISR_TXIS)==SET) {
I2C_SendData(BSTx, (uint8_t) dataBuffer[bufferPointer]);
bstMasterWriteTimeout = micros();
bufferPointer++;
}
}
uint32_t currentTime = micros();
if(currentTime>bstMasterWriteTimeout+5000) {
I2C_SoftwareResetCmd(BSTx);
bstWriteDataLen = 0;
bufferPointer = 0;
}
} else if(currentTime>bstMasterWriteTimeout+BST_SHORT_TIMEOUT) {
bstTimeoutUserCallback();
}
}
void bstMasterReadLoop(void)
{
}
/*************************************************************************************************/
void crc8Cal(uint8_t data_in)
{

1
src/main/drivers/nvic.h
View File

@ -8,6 +8,7 @@
#define NVIC_PRIO_TIMER NVIC_BUILD_PRIORITY(1, 1)
#define NVIC_PRIO_BARO_EXT NVIC_BUILD_PRIORITY(0x0f, 0x0f)
#define NVIC_PRIO_WS2811_DMA NVIC_BUILD_PRIORITY(1, 2) // TODO - is there some reason to use high priority? (or to use DMA IRQ at all?)
#define NVIC_PRIO_BST_READ_DATA NVIC_BUILD_PRIORITY(0x0f, 0x0f)
#define NVIC_PRIO_TRANSPONDER_DMA NVIC_BUILD_PRIORITY(3, 0)
#define NVIC_PRIO_SERIALUART1_TXDMA NVIC_BUILD_PRIORITY(1, 1)
#define NVIC_PRIO_SERIALUART1_RXDMA NVIC_BUILD_PRIORITY(1, 1)

888
src/main/drivers/serial_escserial.c Executable file
View File

@ -0,0 +1,888 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight 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 3 of the License, or
* (at your option) any later version.
*
* Cleanflight 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 Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <stdint.h>
#include <stdlib.h>
#include "platform.h"
#if defined(USE_ESCSERIAL)
#include "build_config.h"
#include "common/utils.h"
#include "common/atomic.h"
#include "common/printf.h"
#include "nvic.h"
#include "system.h"
#include "gpio.h"
#include "timer.h"
#include "serial.h"
#include "serial_escserial.h"
#include "drivers/light_led.h"
#include "io/serial.h"
#include "flight/mixer.h"
#define RX_TOTAL_BITS 10
#define TX_TOTAL_BITS 10
#define MAX_ESCSERIAL_PORTS 1
static serialPort_t *escPort = NULL;
static serialPort_t *passPort = NULL;
typedef struct escSerial_s {
serialPort_t port;
const timerHardware_t *rxTimerHardware;
volatile uint8_t rxBuffer[ESCSERIAL_BUFFER_SIZE];
const timerHardware_t *txTimerHardware;
volatile uint8_t txBuffer[ESCSERIAL_BUFFER_SIZE];
uint8_t isSearchingForStartBit;
uint8_t rxBitIndex;
uint8_t rxLastLeadingEdgeAtBitIndex;
uint8_t rxEdge;
uint8_t isTransmittingData;
uint8_t isReceivingData;
int8_t bitsLeftToTransmit;
uint16_t internalTxBuffer; // includes start and stop bits
uint16_t internalRxBuffer; // includes start and stop bits
uint16_t receiveTimeout;
uint16_t transmissionErrors;
uint16_t receiveErrors;
uint8_t escSerialPortIndex;
timerCCHandlerRec_t timerCb;
timerCCHandlerRec_t edgeCb;
} escSerial_t;
extern timerHardware_t* serialTimerHardware;
extern escSerial_t escSerialPorts[];
extern const struct serialPortVTable escSerialVTable[];
escSerial_t escSerialPorts[MAX_ESCSERIAL_PORTS];
void onEscSerialTimer(timerCCHandlerRec_t *cbRec, captureCompare_t capture);
void onEscSerialRxPinChange(timerCCHandlerRec_t *cbRec, captureCompare_t capture);
void onEscSerialTimerBL(timerCCHandlerRec_t *cbRec, captureCompare_t capture);
void onEscSerialRxPinChangeBL(timerCCHandlerRec_t *cbRec, captureCompare_t capture);
static void escSerialICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity);
void setEscTxSignal(escSerial_t *escSerial, uint8_t state)
{
if (state) {
digitalHi(escSerial->rxTimerHardware->gpio, escSerial->rxTimerHardware->pin);
} else {
digitalLo(escSerial->rxTimerHardware->gpio, escSerial->rxTimerHardware->pin);
}
}
static void escSerialGPIOConfig(GPIO_TypeDef *gpio, uint16_t pin, GPIO_Mode mode)
{
gpio_config_t cfg;
cfg.pin = pin;
cfg.mode = mode;
cfg.speed = Speed_2MHz;
gpioInit(gpio, &cfg);
}
void escSerialInputPortConfig(const timerHardware_t *timerHardwarePtr)
{
#ifdef STM32F10X
escSerialGPIOConfig(timerHardwarePtr->gpio, timerHardwarePtr->pin, Mode_IPU);
#else
escSerialGPIOConfig(timerHardwarePtr->gpio, timerHardwarePtr->pin, Mode_AF_PP_PU);
#endif
//escSerialGPIOConfig(timerHardwarePtr->gpio, timerHardwarePtr->pin, timerHardwarePtr->gpioInputMode);
timerChClearCCFlag(timerHardwarePtr);
timerChITConfig(timerHardwarePtr,ENABLE);
}
static bool isTimerPeriodTooLarge(uint32_t timerPeriod)
{
return timerPeriod > 0xFFFF;
}
static void escSerialTimerTxConfigBL(const timerHardware_t *timerHardwarePtr, uint8_t reference, uint32_t baud)
{
uint32_t clock = SystemCoreClock/2;
uint32_t timerPeriod;
TIM_DeInit(timerHardwarePtr->tim);
do {
timerPeriod = clock / baud;
if (isTimerPeriodTooLarge(timerPeriod)) {
if (clock > 1) {
clock = clock / 2; // this is wrong - mhz stays the same ... This will double baudrate until ok (but minimum baudrate is < 1200)
} else {
// TODO unable to continue, unable to determine clock and timerPeriods for the given baud
}
}
} while (isTimerPeriodTooLarge(timerPeriod));
uint8_t mhz = clock / 1000000;
timerConfigure(timerHardwarePtr, timerPeriod, mhz);
timerChCCHandlerInit(&escSerialPorts[reference].timerCb, onEscSerialTimerBL);
timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].timerCb, NULL);
}
static void escSerialTimerRxConfigBL(const timerHardware_t *timerHardwarePtr, uint8_t reference, portOptions_t options)
{
// start bit is usually a FALLING signal
uint8_t mhz = SystemCoreClock / 2000000;
TIM_DeInit(timerHardwarePtr->tim);
timerConfigure(timerHardwarePtr, 0xFFFF, mhz);
escSerialICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, (options & SERIAL_INVERTED) ? TIM_ICPolarity_Rising : TIM_ICPolarity_Falling);
timerChCCHandlerInit(&escSerialPorts[reference].edgeCb, onEscSerialRxPinChangeBL);
timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].edgeCb, NULL);
}
static void escSerialTimerTxConfig(const timerHardware_t *timerHardwarePtr, uint8_t reference)
{
uint32_t timerPeriod=34;
TIM_DeInit(timerHardwarePtr->tim);
timerConfigure(timerHardwarePtr, timerPeriod, 1);
timerChCCHandlerInit(&escSerialPorts[reference].timerCb, onEscSerialTimer);
timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].timerCb, NULL);
}
static void escSerialICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity)
{
TIM_ICInitTypeDef TIM_ICInitStructure;
TIM_ICStructInit(&TIM_ICInitStructure);
TIM_ICInitStructure.TIM_Channel = channel;
TIM_ICInitStructure.TIM_ICPolarity = polarity;
TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI;
TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;
TIM_ICInitStructure.TIM_ICFilter = 0x0;
TIM_ICInit(tim, &TIM_ICInitStructure);
}
static void escSerialTimerRxConfig(const timerHardware_t *timerHardwarePtr, uint8_t reference)
{
// start bit is usually a FALLING signal
TIM_DeInit(timerHardwarePtr->tim);
timerConfigure(timerHardwarePtr, 0xFFFF, 1);
escSerialICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, TIM_ICPolarity_Falling);
timerChCCHandlerInit(&escSerialPorts[reference].edgeCb, onEscSerialRxPinChange);
timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].edgeCb, NULL);
}
static void escSerialOutputPortConfig(const timerHardware_t *timerHardwarePtr)
{
escSerialGPIOConfig(timerHardwarePtr->gpio, timerHardwarePtr->pin, Mode_Out_PP);
timerChITConfig(timerHardwarePtr,DISABLE);
}
static void resetBuffers(escSerial_t *escSerial)
{
escSerial->port.rxBufferSize = ESCSERIAL_BUFFER_SIZE;
escSerial->port.rxBuffer = escSerial->rxBuffer;
escSerial->port.rxBufferTail = 0;
escSerial->port.rxBufferHead = 0;
escSerial->port.txBuffer = escSerial->txBuffer;
escSerial->port.txBufferSize = ESCSERIAL_BUFFER_SIZE;
escSerial->port.txBufferTail = 0;
escSerial->port.txBufferHead = 0;
}
serialPort_t *openEscSerial(escSerialPortIndex_e portIndex, serialReceiveCallbackPtr callback, uint16_t output, uint32_t baud, portOptions_t options, uint8_t mode)
{
escSerial_t *escSerial = &(escSerialPorts[portIndex]);
escSerial->rxTimerHardware = &(timerHardware[output]);
escSerial->txTimerHardware = &(timerHardware[ESCSERIAL_TIMER_TX_HARDWARE]);
escSerial->port.vTable = escSerialVTable;
escSerial->port.baudRate = baud;
escSerial->port.mode = MODE_RXTX;
escSerial->port.options = options;
escSerial->port.callback = callback;
resetBuffers(escSerial);
escSerial->isTransmittingData = false;
escSerial->isSearchingForStartBit = true;
escSerial->rxBitIndex = 0;
escSerial->transmissionErrors = 0;
escSerial->receiveErrors = 0;
escSerial->receiveTimeout = 0;
escSerial->escSerialPortIndex = portIndex;
escSerialInputPortConfig(escSerial->rxTimerHardware);
setEscTxSignal(escSerial, ENABLE);
delay(50);
if(mode==0){
escSerialTimerTxConfig(escSerial->txTimerHardware, portIndex);
escSerialTimerRxConfig(escSerial->rxTimerHardware, portIndex);
}
if(mode==1){
escSerialTimerTxConfigBL(escSerial->txTimerHardware, portIndex, baud);
escSerialTimerRxConfigBL(escSerial->rxTimerHardware, portIndex, options);
}
return &escSerial->port;
}
void escSerialInputPortDeConfig(const timerHardware_t *timerHardwarePtr)
{
timerChClearCCFlag(timerHardwarePtr);
timerChITConfig(timerHardwarePtr,DISABLE);
escSerialGPIOConfig(timerHardwarePtr->gpio, timerHardwarePtr->pin, Mode_IPU);
}
void closeEscSerial(escSerialPortIndex_e portIndex, uint16_t output)
{
escSerial_t *escSerial = &(escSerialPorts[portIndex]);
escSerial->rxTimerHardware = &(timerHardware[output]);
escSerial->txTimerHardware = &(timerHardware[ESCSERIAL_TIMER_TX_HARDWARE]);
escSerialInputPortDeConfig(escSerial->rxTimerHardware);
timerChConfigCallbacks(escSerial->txTimerHardware,NULL,NULL);
timerChConfigCallbacks(escSerial->rxTimerHardware,NULL,NULL);
TIM_DeInit(escSerial->txTimerHardware->tim);
TIM_DeInit(escSerial->rxTimerHardware->tim);
}
/*********************************************/
void processEscTxState(escSerial_t *escSerial)
{
uint8_t mask;
static uint8_t bitq=0, transmitStart=0;
if (escSerial->isReceivingData) {
return;
}
if(transmitStart==0)
{
setEscTxSignal(escSerial, 1);
}
if (!escSerial->isTransmittingData) {
char byteToSend;
reload:
if (isEscSerialTransmitBufferEmpty((serialPort_t *)escSerial)) {
// canreceive
transmitStart=0;
return;
}
if(transmitStart<3)
{
if(transmitStart==0)
byteToSend = 0xff;
if(transmitStart==1)
byteToSend = 0xff;
if(transmitStart==2)
byteToSend = 0x7f;
transmitStart++;
}
else{
// data to send
byteToSend = escSerial->port.txBuffer[escSerial->port.txBufferTail++];
if (escSerial->port.txBufferTail >= escSerial->port.txBufferSize) {
escSerial->port.txBufferTail = 0;
}
}
// build internal buffer, data bits (MSB to LSB)
escSerial->internalTxBuffer = byteToSend;
escSerial->bitsLeftToTransmit = 8;
escSerial->isTransmittingData = true;
//set output
escSerialOutputPortConfig(escSerial->rxTimerHardware);
return;
}
if (escSerial->bitsLeftToTransmit) {
mask = escSerial->internalTxBuffer & 1;
if(mask)
{
if(bitq==0 || bitq==1)
{
setEscTxSignal(escSerial, 1);
}
if(bitq==2 || bitq==3)
{
setEscTxSignal(escSerial, 0);
}
}
else
{
if(bitq==0 || bitq==2)
{
setEscTxSignal(escSerial, 1);
}
if(bitq==1 ||bitq==3)
{
setEscTxSignal(escSerial, 0);
}
}
bitq++;
if(bitq>3)
{
escSerial->internalTxBuffer >>= 1;
escSerial->bitsLeftToTransmit--;
bitq=0;
if(escSerial->bitsLeftToTransmit==0)
{
goto reload;
}
}
return;
}
if (isEscSerialTransmitBufferEmpty((serialPort_t *)escSerial)) {
escSerial->isTransmittingData = false;
escSerialInputPortConfig(escSerial->rxTimerHardware);
}
}
/*-----------------------BL*/
/*********************************************/
void processEscTxStateBL(escSerial_t *escSerial)
{
uint8_t mask;
if (escSerial->isReceivingData) {
return;
}
if (!escSerial->isTransmittingData) {
char byteToSend;
if (isEscSerialTransmitBufferEmpty((serialPort_t *)escSerial)) {
// canreceive
return;
}
// data to send
byteToSend = escSerial->port.txBuffer[escSerial->port.txBufferTail++];
if (escSerial->port.txBufferTail >= escSerial->port.txBufferSize) {
escSerial->port.txBufferTail = 0;
}
// build internal buffer, MSB = Stop Bit (1) + data bits (MSB to LSB) + start bit(0) LSB
escSerial->internalTxBuffer = (1 << (TX_TOTAL_BITS - 1)) | (byteToSend << 1);
escSerial->bitsLeftToTransmit = TX_TOTAL_BITS;
escSerial->isTransmittingData = true;
//set output
escSerialOutputPortConfig(escSerial->rxTimerHardware);
return;
}
if (escSerial->bitsLeftToTransmit) {
mask = escSerial->internalTxBuffer & 1;
escSerial->internalTxBuffer >>= 1;
setEscTxSignal(escSerial, mask);
escSerial->bitsLeftToTransmit--;
return;
}
escSerial->isTransmittingData = false;
if (isEscSerialTransmitBufferEmpty((serialPort_t *)escSerial)) {
escSerialInputPortConfig(escSerial->rxTimerHardware);
}
}
enum {
TRAILING,
LEADING
};
void applyChangedBitsEscBL(escSerial_t *escSerial)
{
if (escSerial->rxEdge == TRAILING) {
uint8_t bitToSet;
for (bitToSet = escSerial->rxLastLeadingEdgeAtBitIndex; bitToSet < escSerial->rxBitIndex; bitToSet++) {
escSerial->internalRxBuffer |= 1 << bitToSet;
}
}
}
void prepareForNextEscRxByteBL(escSerial_t *escSerial)
{
// prepare for next byte
escSerial->rxBitIndex = 0;
escSerial->isSearchingForStartBit = true;
if (escSerial->rxEdge == LEADING) {
escSerial->rxEdge = TRAILING;
escSerialICConfig(
escSerial->rxTimerHardware->tim,
escSerial->rxTimerHardware->channel,
(escSerial->port.options & SERIAL_INVERTED) ? TIM_ICPolarity_Rising : TIM_ICPolarity_Falling
);
}
}
#define STOP_BIT_MASK (1 << 0)
#define START_BIT_MASK (1 << (RX_TOTAL_BITS - 1))
void extractAndStoreEscRxByteBL(escSerial_t *escSerial)
{
if ((escSerial->port.mode & MODE_RX) == 0) {
return;
}
uint8_t haveStartBit = (escSerial->internalRxBuffer & START_BIT_MASK) == 0;
uint8_t haveStopBit = (escSerial->internalRxBuffer & STOP_BIT_MASK) == 1;
if (!haveStartBit || !haveStopBit) {
escSerial->receiveErrors++;
return;
}
uint8_t rxByte = (escSerial->internalRxBuffer >> 1) & 0xFF;
if (escSerial->port.callback) {
escSerial->port.callback(rxByte);
} else {
escSerial->port.rxBuffer[escSerial->port.rxBufferHead] = rxByte;
escSerial->port.rxBufferHead = (escSerial->port.rxBufferHead + 1) % escSerial->port.rxBufferSize;
}
}
void processEscRxStateBL(escSerial_t *escSerial)
{
if (escSerial->isSearchingForStartBit) {
return;
}
escSerial->rxBitIndex++;
if (escSerial->rxBitIndex == RX_TOTAL_BITS - 1) {
applyChangedBitsEscBL(escSerial);
return;
}
if (escSerial->rxBitIndex == RX_TOTAL_BITS) {
if (escSerial->rxEdge == TRAILING) {
escSerial->internalRxBuffer |= STOP_BIT_MASK;
}
extractAndStoreEscRxByteBL(escSerial);
prepareForNextEscRxByteBL(escSerial);
}
}
void onEscSerialTimerBL(timerCCHandlerRec_t *cbRec, captureCompare_t capture)
{
UNUSED(capture);
escSerial_t *escSerial = container_of(cbRec, escSerial_t, timerCb);
processEscTxStateBL(escSerial);
processEscRxStateBL(escSerial);
}
void onEscSerialRxPinChangeBL(timerCCHandlerRec_t *cbRec, captureCompare_t capture)
{
UNUSED(capture);
escSerial_t *escSerial = container_of(cbRec, escSerial_t, edgeCb);
bool inverted = escSerial->port.options & SERIAL_INVERTED;
if ((escSerial->port.mode & MODE_RX) == 0) {
return;
}
if (escSerial->isSearchingForStartBit) {
// synchronise bit counter
// FIXME this reduces functionality somewhat as receiving breaks concurrent transmission on all ports because
// the next callback to the onSerialTimer will happen too early causing transmission errors.
TIM_SetCounter(escSerial->txTimerHardware->tim, escSerial->txTimerHardware->tim->ARR / 2);
if (escSerial->isTransmittingData) {
escSerial->transmissionErrors++;
}
escSerialICConfig(escSerial->rxTimerHardware->tim, escSerial->rxTimerHardware->channel, inverted ? TIM_ICPolarity_Falling : TIM_ICPolarity_Rising);
escSerial->rxEdge = LEADING;
escSerial->rxBitIndex = 0;
escSerial->rxLastLeadingEdgeAtBitIndex = 0;
escSerial->internalRxBuffer = 0;
escSerial->isSearchingForStartBit = false;
return;
}
if (escSerial->rxEdge == LEADING) {
escSerial->rxLastLeadingEdgeAtBitIndex = escSerial->rxBitIndex;
}
applyChangedBitsEscBL(escSerial);
if (escSerial->rxEdge == TRAILING) {
escSerial->rxEdge = LEADING;
escSerialICConfig(escSerial->rxTimerHardware->tim, escSerial->rxTimerHardware->channel, inverted ? TIM_ICPolarity_Falling : TIM_ICPolarity_Rising);
} else {
escSerial->rxEdge = TRAILING;
escSerialICConfig(escSerial->rxTimerHardware->tim, escSerial->rxTimerHardware->channel, inverted ? TIM_ICPolarity_Rising : TIM_ICPolarity_Falling);
}
}
/*-------------------------BL*/
void extractAndStoreEscRxByte(escSerial_t *escSerial)
{
if ((escSerial->port.mode & MODE_RX) == 0) {
return;
}
uint8_t rxByte = (escSerial->internalRxBuffer) & 0xFF;
if (escSerial->port.callback) {
escSerial->port.callback(rxByte);
} else {
escSerial->port.rxBuffer[escSerial->port.rxBufferHead] = rxByte;
escSerial->port.rxBufferHead = (escSerial->port.rxBufferHead + 1) % escSerial->port.rxBufferSize;
}
}
void onEscSerialTimer(timerCCHandlerRec_t *cbRec, captureCompare_t capture)
{
UNUSED(capture);
escSerial_t *escSerial = container_of(cbRec, escSerial_t, timerCb);
if(escSerial->isReceivingData)
{
escSerial->receiveTimeout++;
if(escSerial->receiveTimeout>8)
{
escSerial->isReceivingData=0;
escSerial->receiveTimeout=0;
escSerialICConfig(escSerial->rxTimerHardware->tim, escSerial->rxTimerHardware->channel, TIM_ICPolarity_Falling);
}
}
processEscTxState(escSerial);
}
void onEscSerialRxPinChange(timerCCHandlerRec_t *cbRec, captureCompare_t capture)
{
UNUSED(capture);
static uint8_t zerofirst=0;
static uint8_t bits=0;
static uint16_t bytes=0;
escSerial_t *escSerial = container_of(cbRec, escSerial_t, edgeCb);
//clear timer
TIM_SetCounter(escSerial->rxTimerHardware->tim,0);
if(capture > 40 && capture < 90)
{
zerofirst++;
if(zerofirst>1)
{
zerofirst=0;
escSerial->internalRxBuffer = escSerial->internalRxBuffer>>1;
bits++;
}
}
else if(capture>90 && capture < 200)
{
zerofirst=0;
escSerial->internalRxBuffer = escSerial->internalRxBuffer>>1;
escSerial->internalRxBuffer |= 0x80;
bits++;
}
else
{
if(!escSerial->isReceivingData)
{
//start
//lets reset
escSerial->isReceivingData = 1;
zerofirst=0;
bytes=0;
bits=1;
escSerial->internalRxBuffer = 0x80;
escSerialICConfig(escSerial->rxTimerHardware->tim, escSerial->rxTimerHardware->channel, TIM_ICPolarity_Rising);
}
}
escSerial->receiveTimeout = 0;
if(bits==8)
{
bits=0;
bytes++;
if(bytes>3)
{
extractAndStoreEscRxByte(escSerial);
}
escSerial->internalRxBuffer=0;
}
}
uint8_t escSerialTotalBytesWaiting(serialPort_t *instance)
{
if ((instance->mode & MODE_RX) == 0) {
return 0;
}
escSerial_t *s = (escSerial_t *)instance;
return (s->port.rxBufferHead - s->port.rxBufferTail) & (s->port.rxBufferSize - 1);
}
uint8_t escSerialReadByte(serialPort_t *instance)
{
uint8_t ch;
if ((instance->mode & MODE_RX) == 0) {
return 0;
}
if (escSerialTotalBytesWaiting(instance) == 0) {
return 0;
}
ch = instance->rxBuffer[instance->rxBufferTail];
instance->rxBufferTail = (instance->rxBufferTail + 1) % instance->rxBufferSize;
return ch;
}
void escSerialWriteByte(serialPort_t *s, uint8_t ch)
{
if ((s->mode & MODE_TX) == 0) {
return;
}
s->txBuffer[s->txBufferHead] = ch;
s->txBufferHead = (s->txBufferHead + 1) % s->txBufferSize;
}
void escSerialSetBaudRate(serialPort_t *s, uint32_t baudRate)
{
UNUSED(s);
UNUSED(baudRate);
}
void escSerialSetMode(serialPort_t *instance, portMode_t mode)
{
instance->mode = mode;
}
bool isEscSerialTransmitBufferEmpty(serialPort_t *instance)
{
// start listening
return instance->txBufferHead == instance->txBufferTail;
}
uint8_t escSerialTxBytesFree(serialPort_t *instance)
{
if ((instance->mode & MODE_TX) == 0) {
return 0;
}
escSerial_t *s = (escSerial_t *)instance;
uint8_t bytesUsed = (s->port.txBufferHead - s->port.txBufferTail) & (s->port.txBufferSize - 1);
return (s->port.txBufferSize - 1) - bytesUsed;
}
const struct serialPortVTable escSerialVTable[] = {
{
escSerialWriteByte,
escSerialTotalBytesWaiting,
escSerialTxBytesFree,
escSerialReadByte,
escSerialSetBaudRate,
isEscSerialTransmitBufferEmpty,
escSerialSetMode,
.writeBuf = NULL,
}
};
void escSerialInitialize()
{
StopPwmAllMotors();
for (volatile uint8_t i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) {
// set outputs to pullup
if(timerHardware[i].outputEnable==1)
{
escSerialGPIOConfig(timerHardware[i].gpio,timerHardware[i].pin, Mode_IPU); //GPIO_Mode_IPU
}
}
}
typedef enum {
IDLE,
HEADER_START,
HEADER_M,
HEADER_ARROW,
HEADER_SIZE,
HEADER_CMD,
COMMAND_RECEIVED
} mspState_e;
typedef struct mspPort_s {
uint8_t offset;
uint8_t dataSize;
uint8_t checksum;
uint8_t indRX;
uint8_t inBuf[10];
mspState_e c_state;
uint8_t cmdMSP;
} mspPort_t;
static mspPort_t currentPort;
static bool ProcessExitCommand(uint8_t c)
{
if (currentPort.c_state == IDLE) {
if (c == '$') {
currentPort.c_state = HEADER_START;
} else {
return false;
}
} else if (currentPort.c_state == HEADER_START) {
currentPort.c_state = (c == 'M') ? HEADER_M : IDLE;
} else if (currentPort.c_state == HEADER_M) {
currentPort.c_state = (c == '<') ? HEADER_ARROW : IDLE;
} else if (currentPort.c_state == HEADER_ARROW) {
if (c > 10) {
currentPort.c_state = IDLE;
} else {
currentPort.dataSize = c;
currentPort.offset = 0;
currentPort.checksum = 0;
currentPort.indRX = 0;
currentPort.checksum ^= c;
currentPort.c_state = HEADER_SIZE;
}
} else if (currentPort.c_state == HEADER_SIZE) {
currentPort.cmdMSP = c;
currentPort.checksum ^= c;
currentPort.c_state = HEADER_CMD;
} else if (currentPort.c_state == HEADER_CMD && currentPort.offset < currentPort.dataSize) {
currentPort.checksum ^= c;
currentPort.inBuf[currentPort.offset++] = c;
} else if (currentPort.c_state == HEADER_CMD && currentPort.offset >= currentPort.dataSize) {
if (currentPort.checksum == c) {
currentPort.c_state = COMMAND_RECEIVED;
if((currentPort.cmdMSP == 0xF4) && (currentPort.dataSize==0))
{
currentPort.c_state = IDLE;
return true;
}
} else {
currentPort.c_state = IDLE;
}
}
return false;
}
// mode 0=sk, mode 1=bl output=timerHardware PWM channel.
void escEnablePassthrough(serialPort_t *escPassthroughPort, uint16_t output, uint8_t mode)
{
bool exitEsc = false;
LED0_OFF;
LED1_OFF;
StopPwmAllMotors();
passPort = escPassthroughPort;
uint8_t first_output = 0;
for (volatile uint8_t i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) {
if(timerHardware[i].outputEnable==1)
{
first_output=i;
break;
}
}
//doesn't work with messy timertable
uint8_t motor_output=first_output+output-1;
if(motor_output >=USABLE_TIMER_CHANNEL_COUNT)
return;
escPort = openEscSerial(ESCSERIAL1, NULL, motor_output, 19200, 0, mode);
uint8_t ch;
while(1) {
if (serialRxBytesWaiting(escPort)) {
LED0_ON;
while(serialRxBytesWaiting(escPort))
{
ch = serialRead(escPort);
serialWrite(escPassthroughPort, ch);
}
LED0_OFF;
}
if (serialRxBytesWaiting(escPassthroughPort)) {
LED1_ON;
while(serialRxBytesWaiting(escPassthroughPort))
{
ch = serialRead(escPassthroughPort);
exitEsc = ProcessExitCommand(ch);
if(exitEsc)
{
serialWrite(escPassthroughPort, 0x24);
serialWrite(escPassthroughPort, 0x4D);
serialWrite(escPassthroughPort, 0x3E);
serialWrite(escPassthroughPort, 0x00);
serialWrite(escPassthroughPort, 0xF4);
serialWrite(escPassthroughPort, 0xF4);
closeEscSerial(ESCSERIAL1, output);
return;
}
if(mode){
serialWrite(escPassthroughPort, ch); // blheli loopback
}
serialWrite(escPort, ch);
}
LED1_OFF;
}
delay(5);
}
}
#endif

36
src/main/drivers/serial_escserial.h Executable file
View File

@ -0,0 +1,36 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight 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 3 of the License, or
* (at your option) any later version.
*
* Cleanflight 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 Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#define ESCSERIAL_BUFFER_SIZE 1024
typedef enum {
ESCSERIAL1 = 0,
ESCSERIAL2
} escSerialPortIndex_e;
serialPort_t *openEscSerial(escSerialPortIndex_e portIndex, serialReceiveCallbackPtr callback, uint16_t output, uint32_t baud, portOptions_t options, uint8_t mode);
// serialPort API
void escSerialWriteByte(serialPort_t *instance, uint8_t ch);
uint8_t escSerialTotalBytesWaiting(serialPort_t *instance);
uint8_t escSerialReadByte(serialPort_t *instance);
void escSerialSetBaudRate(serialPort_t *s, uint32_t baudRate);
bool isEscSerialTransmitBufferEmpty(serialPort_t *s);
void escSerialInitialize();
void escEnablePassthrough(serialPort_t *escPassthroughPort, uint16_t output, uint8_t mode);

3
src/main/flight/failsafe.c
View File

@ -172,7 +172,8 @@ void failsafeUpdateState(void)
bool failsafeSwitchIsOn = IS_RC_MODE_ACTIVE(BOXFAILSAFE);
beeperMode_e beeperMode = BEEPER_SILENCE;
if (!receivingRxData) {
// Beep RX lost only if we are not seeing data and we have been armed earlier
if (!receivingRxData && ARMING_FLAG(WAS_EVER_ARMED)) {
beeperMode = BEEPER_RX_LOST;
}

43
src/main/flight/mixer.c
View File

@ -748,35 +748,6 @@ STATIC_UNIT_TESTED void servoMixer(void)
#endif
void acroPlusApply(void) {
int axis;
for (axis = 0; axis < 2; axis++) {
int16_t factor;
fix12_t wowFactor;
int16_t rcCommandDeflection = constrain(rcCommand[axis], -500, 500); // Limit stick input to 500 (rcCommand 100)
int16_t acroPlusStickOffset = rxConfig->acroPlusOffset * 5;
int16_t motorRange = escAndServoConfig->maxthrottle - escAndServoConfig->minthrottle;
if (feature(FEATURE_3D)) motorRange = (motorRange - (flight3DConfig->deadband3d_high - flight3DConfig->deadband3d_low)) / 2;
/* acro plus factor handling */
if (rxConfig->acroPlusFactor && ABS(rcCommandDeflection) > acroPlusStickOffset + 10) {
if (rcCommandDeflection > 0) {
rcCommandDeflection -= acroPlusStickOffset;
} else {
rcCommandDeflection += acroPlusStickOffset;
}
wowFactor = qConstruct(ABS(rcCommandDeflection) * rxConfig->acroPlusFactor / 100, 500);
factor = qMultiply(wowFactor, (rcCommandDeflection * motorRange) / 500);
wowFactor = Q12 - wowFactor;
} else {
wowFactor = Q12;
factor = 0;
}
axisPID[axis] = factor + qMultiply(wowFactor, axisPID[axis]);
}
}
void mixTable(void)
{
uint32_t i;
@ -785,10 +756,6 @@ void mixTable(void)
bool isFailsafeActive = failsafeIsActive(); // TODO - Find out if failsafe checks are really needed here in mixer code
if (IS_RC_MODE_ACTIVE(BOXACROPLUS)) {
acroPlusApply();
}
if (motorCount >= 4 && mixerConfig->yaw_jump_prevention_limit < YAW_JUMP_PREVENTION_LIMIT_HIGH) {
// prevent "yaw jump" during yaw correction
axisPID[YAW] = constrain(axisPID[YAW], -mixerConfig->yaw_jump_prevention_limit - ABS(rcCommand[YAW]), mixerConfig->yaw_jump_prevention_limit + ABS(rcCommand[YAW]));
@ -822,18 +789,18 @@ void mixTable(void)
int16_t throttleMin, throttleMax;
static int16_t throttlePrevious = 0; // Store the last throttle direction for deadband transitions
// Find min and max throttle based on condition. Use rcData for 3D to prevent loss of power due to min_check
// Find min and max throttle based on condition.
if (feature(FEATURE_3D)) {
if (!ARMING_FLAG(ARMED)) throttlePrevious = rxConfig->midrc; // When disarmed set to mid_rc. It always results in positive direction after arming.
if ((rcData[THROTTLE] <= (rxConfig->midrc - flight3DConfig->deadband3d_throttle))) { // Out of band handling
if ((rcCommand[THROTTLE] <= (rxConfig->midrc - flight3DConfig->deadband3d_throttle))) { // Out of band handling
throttleMax = flight3DConfig->deadband3d_low;
throttleMin = escAndServoConfig->minthrottle;
throttlePrevious = throttle = rcData[THROTTLE];
} else if (rcData[THROTTLE] >= (rxConfig->midrc + flight3DConfig->deadband3d_throttle)) { // Positive handling
throttlePrevious = throttle = rcCommand[THROTTLE];
} else if (rcCommand[THROTTLE] >= (rxConfig->midrc + flight3DConfig->deadband3d_throttle)) { // Positive handling
throttleMax = escAndServoConfig->maxthrottle;
throttleMin = flight3DConfig->deadband3d_high;
throttlePrevious = throttle = rcData[THROTTLE];
throttlePrevious = throttle = rcCommand[THROTTLE];
} else if ((throttlePrevious <= (rxConfig->midrc - flight3DConfig->deadband3d_throttle))) { // Deadband handling from negative to positive
throttle = throttleMax = flight3DConfig->deadband3d_low;
throttleMin = escAndServoConfig->minthrottle;

195
src/main/flight/pid.c
View File

@ -58,8 +58,6 @@ int16_t axisPID[3];
int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
#endif
#define DELTA_MAX_SAMPLES 12
// PIDweight is a scale factor for PIDs which is derived from the throttle and TPA setting, and 100 = 100% scale means no PID reduction
uint8_t dynP8[3], dynI8[3], dynD8[3], PIDweight[3];
@ -81,6 +79,14 @@ void setTargetPidLooptime(uint8_t pidProcessDenom) {
targetPidLooptime = targetLooptime * pidProcessDenom;
}
float calculateExpoPlus(int axis, rxConfig_t *rxConfig) {
float propFactor;
propFactor = 1.0f - ((rxConfig->superExpoFactor / 100.0f) * (ABS(rcCommand[axis]) / 500.0f));
return propFactor;
}
void pidResetErrorAngle(void)
{
errorAngleI[ROLL] = 0;
@ -107,35 +113,17 @@ void pidResetErrorGyroState(uint8_t resetOption)
}
}
void scaleItermToRcInput(int axis, pidProfile_t *pidProfile) {
float rcCommandReflection = (float)rcCommand[axis] / 500.0f;
static float iTermScaler[3] = {1.0f, 1.0f, 1.0f};
static float antiWindUpIncrement = 0;
float getdT (void) {
static float dT;
if (!dT) dT = (float)targetPidLooptime * 0.000001f;
if (!antiWindUpIncrement) antiWindUpIncrement = (0.001 / 500) * targetPidLooptime; // Calculate increment for 500ms period
if (ABS(rcCommandReflection) > 0.7f && (!flightModeFlags)) { /* scaling should not happen in level modes */
/* Reset Iterm on high stick inputs. No scaling necessary here */
iTermScaler[axis] = 0.0f;
errorGyroI[axis] = 0;
errorGyroIf[axis] = 0.0f;
} else {
/* Prevent rapid windup during acro recoveries. Slowly enable Iterm for period of 500ms */
if (iTermScaler[axis] < 1) {
iTermScaler[axis] = constrainf(iTermScaler[axis] + antiWindUpIncrement, 0.0f, 1.0f);
if (pidProfile->pidController != PID_CONTROLLER_LUX_FLOAT) {
errorGyroI[axis] *= iTermScaler[axis];
} else {
errorGyroIf[axis] *= iTermScaler[axis];
}
}
}
return dT;
}
const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH };
static biquad_t deltaBiQuadState[3];
static bool deltaStateIsSet;
static filterStatePt1_t deltaFilterState[3];
static filterStatePt1_t yawFilterState;
static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
@ -143,18 +131,12 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
float RateError, AngleRate, gyroRate;
float ITerm,PTerm,DTerm;
static float lastErrorForDelta[3];
static float previousDelta[3][DELTA_MAX_SAMPLES];
static float previousAverageDelta[3];
float delta, deltaSum;
int axis, deltaCount;
static float deltaState[3][DELTA_MAX_SAMPLES];
float delta;
int axis;
float horizonLevelStrength = 1;
float dT = (float)targetPidLooptime * 0.000001f;
if (!deltaStateIsSet && pidProfile->dterm_lpf_hz) {
for (axis = 0; axis < 3; axis++) BiQuadNewLpf(pidProfile->dterm_lpf_hz, &deltaBiQuadState[axis], targetPidLooptime);
deltaStateIsSet = true;
}
float tpaFactor = PIDweight[0] / 100.0f; // tpa is now float
if (FLIGHT_MODE(HORIZON_MODE)) {
// Figure out the raw stick positions
@ -191,11 +173,11 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
#endif
if (FLIGHT_MODE(ANGLE_MODE)) {
// ANGLE mode - control is angle based, so control loop is needed
AngleRate = errorAngle * pidProfile->A_level;
AngleRate = errorAngle * pidProfile->P8[PIDLEVEL] / 10.0f;
} else {
// HORIZON mode - direct sticks control is applied to rate PID
// mix up angle error to desired AngleRate to add a little auto-level feel
AngleRate += errorAngle * pidProfile->H_level * horizonLevelStrength;
AngleRate += errorAngle * pidProfile->I8[PIDLEVEL] / 10.0f * horizonLevelStrength;
}
}
}
@ -208,10 +190,12 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
// multiplication of rcCommand corresponds to changing the sticks scaling here
RateError = AngleRate - gyroRate;
if (lowThrottlePidReduction) RateError /= 4;
// -----calculate P component
PTerm = RateError * pidProfile->P_f[axis] * PIDweight[axis] / 100;
if (IS_RC_MODE_ACTIVE(BOXSUPEREXPO) && axis != YAW) {
PTerm = (pidProfile->P8[axis] / 40.0f * tpaFactor) * (AngleRate - gyroRate * calculateExpoPlus(axis, rxConfig));
} else {
PTerm = RateError * (pidProfile->P8[axis] / 40.0f) * tpaFactor;
}
// Constrain YAW by yaw_p_limit value if not servo driven in that case servolimits apply
if((motorCount >= 4 && pidProfile->yaw_p_limit) && axis == YAW) {
@ -219,22 +203,31 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
}
// -----calculate I component.
errorGyroIf[axis] = constrainf(errorGyroIf[axis] + RateError * dT * pidProfile->I_f[axis] * 10, -250.0f, 250.0f);
errorGyroIf[axis] = constrainf(errorGyroIf[axis] + RateError * getdT() * pidProfile->I8[axis] / 10.0f, -250.0f, 250.0f);
if (IS_RC_MODE_ACTIVE(BOXSUPEREXPO) && axis != YAW) {
if (ABS(gyroRate) >= pidProfile->rollPitchItermResetRate) errorGyroIf[axis] = 0;
}
if (axis == YAW) {
if (ABS(gyroRate) >= pidProfile->yawItermResetRate) errorGyroIf[axis] = 0;
}
if (IS_RC_MODE_ACTIVE(BOXAIRMODE) || IS_RC_MODE_ACTIVE(BOXACROPLUS)) {
if (IS_RC_MODE_ACTIVE(BOXACROPLUS)) scaleItermToRcInput(axis, pidProfile);
if (antiWindupProtection || motorLimitReached) {
errorGyroIf[axis] = constrainf(errorGyroIf[axis], -errorGyroIfLimit[axis], errorGyroIfLimit[axis]);
} else {
errorGyroIfLimit[axis] = ABS(errorGyroIf[axis]);
}
}
// limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated.
// I coefficient (I8) moved before integration to make limiting independent from PID settings
ITerm = errorGyroIf[axis];
//-----calculate D-term
if (axis == YAW) {
if (pidProfile->yaw_lpf_hz) PTerm = filterApplyPt1(PTerm, &yawFilterState, pidProfile->yaw_lpf_hz, getdT());
DTerm = 0;
} else {
if (pidProfile->deltaMethod == DELTA_FROM_ERROR) {
delta = RateError - lastErrorForDelta[axis];
lastErrorForDelta[axis] = RateError;
@ -245,26 +238,21 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
// Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference
// would be scaled by different dt each time. Division by dT fixes that.
delta *= (1.0f / dT);
delta *= (1.0f / getdT());
// Filter delta
if (pidProfile->dterm_lpf_hz) delta = filterApplyPt1(delta, &deltaFilterState[axis], pidProfile->dterm_lpf_hz, getdT());
if (deltaStateIsSet) {
delta = applyBiQuadFilter(delta, &deltaBiQuadState[axis]);
} else {
// Apply moving average
deltaSum = 0;
for (deltaCount = pidProfile->dterm_average_count-1; deltaCount > 0; deltaCount--) previousDelta[axis][deltaCount] = previousDelta[axis][deltaCount-1];
previousDelta[axis][0] = delta;
for (deltaCount = 0; deltaCount < pidProfile->dterm_average_count; deltaCount++) deltaSum += previousDelta[axis][deltaCount];
delta = ((deltaSum / pidProfile->dterm_average_count) + previousAverageDelta[axis]) / 2; // Keep same original scaling + double pass averaging
previousAverageDelta[axis] = delta;
}
if (pidProfile->dterm_average_count) delta = filterApplyAveragef(delta, pidProfile->dterm_average_count, deltaState[axis]);
DTerm = constrainf(delta * pidProfile->D_f[axis] * PIDweight[axis] / 100, -300.0f, 300.0f);
DTerm = constrainf(delta * (float)pidProfile->D8[axis] * 0.001f * tpaFactor, -300.0f, 300.0f);
}
// -----calculate total PID output
axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -1000, 1000);
if (lowThrottlePidReduction) axisPID[axis] /= 4;
if (lowThrottlePidReduction) axisPID[axis] /= 3;
#ifdef GTUNE
if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
@ -285,17 +273,11 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control
{
UNUSED(rxConfig);
int axis, deltaCount, prop = 0;
int axis, prop = 0;
int32_t rc, error, errorAngle, delta, gyroError;
int32_t PTerm, ITerm, PTermACC, ITermACC, DTerm;
static int16_t lastErrorForDelta[2];
static int32_t previousDelta[2][DELTA_MAX_SAMPLES];
static int32_t previousAverageDelta[2];
if (!deltaStateIsSet && pidProfile->dterm_lpf_hz) {
for (axis = 0; axis < 2; axis++) BiQuadNewLpf(pidProfile->dterm_lpf_hz, &deltaBiQuadState[axis], targetPidLooptime);
deltaStateIsSet = true;
}
static int32_t deltaState[3][DELTA_MAX_SAMPLES];
if (FLIGHT_MODE(HORIZON_MODE)) {
prop = MIN(MAX(ABS(rcCommand[PITCH]), ABS(rcCommand[ROLL])), 512);
@ -306,26 +288,21 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control
rc = rcCommand[axis] << 1;
if (lowThrottlePidReduction) rc /= 4;
gyroError = gyroADC[axis] / 4;
error = rc - gyroError;
errorGyroI[axis] = constrain(errorGyroI[axis] + ((error * (uint16_t)targetPidLooptime) >> 12) , -16000, +16000); // WindUp 16 bits is ok here
errorGyroI[axis] = constrain(errorGyroI[axis] + ((error * (uint16_t)targetPidLooptime) >> 11) , -16000, +16000); // WindUp 16 bits is ok here
if (ABS(gyroADC[axis]) > (640 * 4)) {
errorGyroI[axis] = 0;
}
// Anti windup protection
if (IS_RC_MODE_ACTIVE(BOXAIRMODE) || IS_RC_MODE_ACTIVE(BOXACROPLUS)) {
if (IS_RC_MODE_ACTIVE(BOXACROPLUS)) scaleItermToRcInput(axis, pidProfile);
if (antiWindupProtection || motorLimitReached) {
errorGyroI[axis] = constrain(errorGyroI[axis], -errorGyroILimit[axis], errorGyroILimit[axis]);
} else {
errorGyroILimit[axis] = ABS(errorGyroI[axis]);
}
}
ITerm = (errorGyroI[axis] >> 7) * pidProfile->I8[axis] >> 6; // 16 bits is ok here 16000/125 = 128 ; 128*250 = 32000
@ -368,23 +345,17 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control
// Scale delta to looptime
delta = (delta * ((uint16_t) 0xFFFF)) / ((uint16_t)targetPidLooptime << 5);
if (deltaStateIsSet) {
DTerm = lrintf(applyBiQuadFilter((float) delta, &deltaBiQuadState[axis])) * 3; // Keep same scaling as unfiltered delta
} else {
// Apply moving average
DTerm = 0;
for (deltaCount = pidProfile->dterm_average_count-1; deltaCount > 0; deltaCount--) previousDelta[axis][deltaCount] = previousDelta[axis][deltaCount-1];
previousDelta[axis][0] = delta;
for (deltaCount = 0; deltaCount < pidProfile->dterm_average_count; deltaCount++) DTerm += previousDelta[axis][deltaCount];
DTerm = (((DTerm / pidProfile->dterm_average_count) << 1) + previousAverageDelta[axis]) >> 1; // Keep same original scaling + double pass averaging
previousAverageDelta[axis] = DTerm;
}
// Filer delta
if (pidProfile->dterm_lpf_hz) delta = filterApplyPt1((float)delta, &deltaFilterState[axis], pidProfile->dterm_lpf_hz, getdT());
DTerm = ((int32_t)DTerm * dynD8[axis]) >> 5; // 32 bits is needed for calculation
// Apply moving average and multiply to get original scaling
if (pidProfile->dterm_average_count) delta = filterApplyAverage(delta, pidProfile->dterm_average_count, deltaState[axis]) * 2;
DTerm = (delta * dynD8[axis]) >> 5; // 32 bits is needed for calculation
axisPID[axis] = PTerm + ITerm + DTerm;
if (lowThrottlePidReduction) axisPID[axis] /= 4;
if (lowThrottlePidReduction) axisPID[axis] /= 3;
#ifdef GTUNE
if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
@ -421,7 +392,7 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control
axisPID[FD_YAW] = PTerm + ITerm;
if (lowThrottlePidReduction) axisPID[FD_YAW] /= 4;
if (pidProfile->yaw_lpf_hz) axisPID[FD_YAW] = filterApplyPt1(axisPID[FD_YAW], &yawFilterState, pidProfile->yaw_lpf_hz, getdT());
#ifdef GTUNE
if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
@ -441,20 +412,14 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co
{
UNUSED(rxConfig);
int axis, deltaCount;
int32_t PTerm, ITerm, DTerm, delta, deltaSum;
int axis;
int32_t PTerm, ITerm, DTerm, delta;
static int32_t lastErrorForDelta[3] = { 0, 0, 0 };
static int32_t previousDelta[3][DELTA_MAX_SAMPLES];
static int32_t previousAverageDelta[3];
static int32_t deltaState[3][DELTA_MAX_SAMPLES];
int32_t AngleRateTmp, RateError, gyroRate;
int8_t horizonLevelStrength = 100;
if (!deltaStateIsSet && pidProfile->dterm_lpf_hz) {
for (axis = 0; axis < 3; axis++) BiQuadNewLpf(pidProfile->dterm_lpf_hz, &deltaBiQuadState[axis], targetPidLooptime);
deltaStateIsSet = true;
}
if (FLIGHT_MODE(HORIZON_MODE)) {
// Figure out the raw stick positions
const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, rxConfig->midrc));
@ -474,7 +439,7 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co
// -----Get the desired angle rate depending on flight mode
if (axis == FD_YAW) {
// YAW is always gyro-controlled (MAG correction is applied to rcCommand)
AngleRateTmp = ((int32_t)(rate + 27) * rcCommand[YAW]) >> 5;
AngleRateTmp = ((int32_t)(rate + 47) * rcCommand[YAW]) >> 5;
} else {
AngleRateTmp = ((int32_t)(rate + 27) * rcCommand[axis]) >> 4;
if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {
@ -504,10 +469,12 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co
gyroRate = gyroADC[axis] / 4;
RateError = AngleRateTmp - gyroRate;
if (lowThrottlePidReduction) RateError /= 4;
// -----calculate P component
if (IS_RC_MODE_ACTIVE(BOXSUPEREXPO) && axis != YAW) {
PTerm = (pidProfile->P8[axis] * PIDweight[axis] / 100) * (AngleRateTmp - (int32_t)(gyroRate * calculateExpoPlus(axis, rxConfig))) >> 7;
} else {
PTerm = (RateError * pidProfile->P8[axis] * PIDweight[axis] / 100) >> 7;
}
// Constrain YAW by yaw_p_limit value if not servo driven in that case servolimits apply
if((motorCount >= 4 && pidProfile->yaw_p_limit) && axis == YAW) {
@ -525,18 +492,27 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co
// I coefficient (I8) moved before integration to make limiting independent from PID settings
errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t) - GYRO_I_MAX << 13, (int32_t) + GYRO_I_MAX << 13);
if (IS_RC_MODE_ACTIVE(BOXAIRMODE) || IS_RC_MODE_ACTIVE(BOXACROPLUS)) {
if (IS_RC_MODE_ACTIVE(BOXACROPLUS)) scaleItermToRcInput(axis, pidProfile);
if (IS_RC_MODE_ACTIVE(BOXSUPEREXPO) && axis != YAW) {
if (ABS(gyroRate / 4) >= pidProfile->rollPitchItermResetRate) errorGyroI[axis] = 0;
}
if (axis == YAW) {
if (ABS(gyroRate / 4) >= pidProfile->yawItermResetRate) errorGyroI[axis] = 0;
}
if (antiWindupProtection || motorLimitReached) {
errorGyroI[axis] = constrain(errorGyroI[axis], -errorGyroILimit[axis], errorGyroILimit[axis]);
} else {
errorGyroILimit[axis] = ABS(errorGyroI[axis]);
}
}
ITerm = errorGyroI[axis] >> 13;
//-----calculate D-term
if (axis == YAW) {
if (pidProfile->yaw_lpf_hz) PTerm = filterApplyPt1(PTerm, &yawFilterState, pidProfile->yaw_lpf_hz, getdT());
DTerm = 0;
} else {
if (pidProfile->deltaMethod == DELTA_FROM_ERROR) {
delta = RateError - lastErrorForDelta[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
lastErrorForDelta[axis] = RateError;
@ -549,24 +525,19 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co
// would be scaled by different dt each time. Division by dT fixes that.
delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)targetPidLooptime >> 4))) >> 6;
if (deltaStateIsSet) {
delta = lrintf(applyBiQuadFilter((float) delta, &deltaBiQuadState[axis])) * 3; // Keep same scaling as unfiltered delta
} else {
// Filter delta
if (pidProfile->dterm_lpf_hz) delta = filterApplyPt1((float)delta, &deltaFilterState[axis], pidProfile->dterm_lpf_hz, getdT());
// Apply moving average
deltaSum = 0;
for (deltaCount = pidProfile->dterm_average_count -1; deltaCount > 0; deltaCount--) previousDelta[axis][deltaCount] = previousDelta[axis][deltaCount-1];
previousDelta[axis][0] = delta;
for (deltaCount = 0; deltaCount < pidProfile->dterm_average_count; deltaCount++) deltaSum += previousDelta[axis][deltaCount];
delta = (((deltaSum / pidProfile->dterm_average_count) << 1) + previousAverageDelta[axis]) >> 1; // Keep same original scaling + double pass averaging
previousAverageDelta[axis] = delta;
}
if (pidProfile->dterm_average_count) delta = filterApplyAverage(delta, pidProfile->dterm_average_count, deltaState[axis]) * 2;
DTerm = (delta * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8;
}
// -----calculate total PID output
axisPID[axis] = PTerm + ITerm + DTerm;
if (lowThrottlePidReduction) axisPID[axis] /= 4;
if (lowThrottlePidReduction) axisPID[axis] /= 3;
#ifdef GTUNE
if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {

10
src/main/flight/pid.h
View File

@ -20,7 +20,7 @@
#define GYRO_I_MAX 256 // Gyro I limiter
#define YAW_P_LIMIT_MIN 100 // Maximum value for yaw P limiter
#define YAW_P_LIMIT_MAX 300 // Maximum value for yaw P limiter
#define YAW_P_LIMIT_MAX 400 // Maximum value for yaw P limiter
typedef enum {
PIDROLL,
@ -63,14 +63,12 @@ typedef struct pidProfile_s {
uint8_t I8[PID_ITEM_COUNT];
uint8_t D8[PID_ITEM_COUNT];
float P_f[3]; // float p i and d factors for lux float pid controller
float I_f[3];
float D_f[3];
float A_level;
float H_level;
uint8_t H_sensitivity;
float dterm_lpf_hz; // Delta Filter in hz
float yaw_lpf_hz; // Additional yaw filter when yaw axis too noisy
uint16_t rollPitchItermResetRate; // Experimental threshold for resetting iterm for pitch and roll on certain rates
uint16_t yawItermResetRate; // Experimental threshold for resetting iterm for yaw on certain rates
uint8_t deltaMethod; // Alternative delta Calculation
uint16_t yaw_p_limit;
uint8_t dterm_average_count; // Configurable delta count for dterm

114
src/main/io/i2c_bst.c
View File

@ -290,7 +290,6 @@ static const char pidnames[] =
"MAG;"
"VEL;";
#define DATA_BUFFER_SIZE 64
#define BOARD_IDENTIFIER_LENGTH 4
typedef struct box_e {
@ -332,8 +331,8 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
{ CHECKBOX_ITEM_COUNT, NULL, 0xFF }
};
uint8_t readData[DATA_BUFFER_SIZE];
uint8_t writeData[DATA_BUFFER_SIZE];
extern uint8_t readData[DATA_BUFFER_SIZE];
extern uint8_t writeData[DATA_BUFFER_SIZE];
/*************************************************************************************************/
uint8_t writeBufferPointer = 1;
@ -354,7 +353,12 @@ static void bstWrite32(uint32_t data)
bstWrite16((uint16_t)(data >> 16));
}
uint8_t readBufferPointer = 3;
uint8_t readBufferPointer = 4;
static uint8_t bstCurrentAddress(void)
{
return readData[0];
}
static uint8_t bstRead8(void)
{
return readData[readBufferPointer++] & 0xff;
@ -376,12 +380,12 @@ static uint32_t bstRead32(void)
static uint8_t bstReadDataSize(void)
{
return readData[0]-4;
return readData[1]-5;
}
static uint8_t bstReadCRC(void)
{
return readData[readData[0]];
return readData[readData[1]+1];
}
static void s_struct(uint8_t *cb, uint8_t siz)
@ -495,6 +499,7 @@ static void bstWriteDataflashReadReply(uint32_t address, uint8_t size)
/*************************************************************************************************/
#define BST_USB_COMMANDS 0x0A
#define BST_GENERAL_HEARTBEAT 0x0B
#define BST_USB_DEVICE_INFO_REQUEST 0x04 //Handshake
#define BST_USB_DEVICE_INFO_FRAME 0x05 //Handshake
#define BST_READ_COMMANDS 0x26
@ -695,30 +700,11 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
bstWrite8(currentControlRateProfile->rcYawExpo8);
break;
case BST_PID:
if (IS_PID_CONTROLLER_FP_BASED(currentProfile->pidProfile.pidController)) { // convert float stuff into uint8_t to keep backwards compatability with all 8-bit shit with new pid
for (i = 0; i < 3; i++) {
bstWrite8(constrain(lrintf(currentProfile->pidProfile.P_f[i] * 10.0f), 0, 250));
bstWrite8(constrain(lrintf(currentProfile->pidProfile.I_f[i] * 100.0f), 0, 250));
bstWrite8(constrain(lrintf(currentProfile->pidProfile.D_f[i] * 1000.0f), 0, 100));
}
for (i = 3; i < PID_ITEM_COUNT; i++) {
if (i == PIDLEVEL) {
bstWrite8(constrain(lrintf(currentProfile->pidProfile.A_level * 10.0f), 0, 250));
bstWrite8(constrain(lrintf(currentProfile->pidProfile.H_level * 10.0f), 0, 250));
bstWrite8(constrain(lrintf(currentProfile->pidProfile.H_sensitivity), 0, 250));
} else {
bstWrite8(currentProfile->pidProfile.P8[i]);
bstWrite8(currentProfile->pidProfile.I8[i]);
bstWrite8(currentProfile->pidProfile.D8[i]);
}
}
} else {
for (i = 0; i < PID_ITEM_COUNT; i++) {
bstWrite8(currentProfile->pidProfile.P8[i]);
bstWrite8(currentProfile->pidProfile.I8[i]);
bstWrite8(currentProfile->pidProfile.D8[i]);
}
}
break;
case BST_PIDNAMES:
bstWriteNames(pidnames);
@ -999,7 +985,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
// we do not know how to handle the (valid) message, indicate error BST
return false;
}
bstSlaveWrite(writeData);
//bstSlaveWrite(writeData);
return true;
}
@ -1061,30 +1047,11 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand)
pidSetController(currentProfile->pidProfile.pidController);
break;
case BST_SET_PID:
if (IS_PID_CONTROLLER_FP_BASED(currentProfile->pidProfile.pidController)) {
for (i = 0; i < 3; i++) {
currentProfile->pidProfile.P_f[i] = (float)bstRead8() / 10.0f;
currentProfile->pidProfile.I_f[i] = (float)bstRead8() / 100.0f;
currentProfile->pidProfile.D_f[i] = (float)bstRead8() / 1000.0f;
}
for (i = 3; i < PID_ITEM_COUNT; i++) {
if (i == PIDLEVEL) {
currentProfile->pidProfile.A_level = (float)bstRead8() / 10.0f;
currentProfile->pidProfile.H_level = (float)bstRead8() / 10.0f;
currentProfile->pidProfile.H_sensitivity = bstRead8();
} else {
currentProfile->pidProfile.P8[i] = bstRead8();
currentProfile->pidProfile.I8[i] = bstRead8();
currentProfile->pidProfile.D8[i] = bstRead8();
}
}
} else {
for (i = 0; i < PID_ITEM_COUNT; i++) {
currentProfile->pidProfile.P8[i] = bstRead8();
currentProfile->pidProfile.I8[i] = bstRead8();
currentProfile->pidProfile.D8[i] = bstRead8();
}
}
break;
case BST_SET_MODE_RANGE:
i = bstRead8();
@ -1236,7 +1203,7 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand)
if (ARMING_FLAG(ARMED)) {
ret = BST_FAILED;
bstWrite8(ret);
bstSlaveWrite(writeData);
//bstSlaveWrite(writeData);
return ret;
}
writeEEPROM();
@ -1465,7 +1432,7 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand)
ret = BST_FAILED;
}
bstWrite8(ret);
bstSlaveWrite(writeData);
//bstSlaveWrite(writeData);
if(ret == BST_FAILED)
return false;
return true;
@ -1482,18 +1449,19 @@ static bool bstSlaveUSBCommandFeedback(/*uint8_t bstFeedback*/)
bstWrite8(FC_VERSION_MINOR); //Firmware ID
bstWrite8(0x00);
bstWrite8(0x00);
bstSlaveWrite(writeData);
//bstSlaveWrite(writeData);
return true;
}
/*************************************************************************************************/
bool slaveModeOn = false;
static void bstSlaveProcessInCommand(void)
#define BST_RESET_TIME 1.2*1000*1000 //micro-seconds
uint32_t resetBstTimer = 0;
bool needResetCheck = true;
void bstProcessInCommand(void)
{
if(bstSlaveRead(readData)) {
slaveModeOn = true;
readBufferPointer = 1;
//Check if the CRC match
readBufferPointer = 2;
if(bstCurrentAddress() == CLEANFLIGHT_FC) {
if(bstReadCRC() == CRC8 && bstRead8()==BST_USB_COMMANDS) {
uint8_t i;
writeBufferPointer = 1;
@ -1519,8 +1487,23 @@ static void bstSlaveProcessInCommand(void)
break;
}
}
} else {
slaveModeOn = false;
} else if(bstCurrentAddress() == 0x00) {
if(bstReadCRC() == CRC8 && bstRead8()==BST_GENERAL_HEARTBEAT) {
resetBstTimer = micros();
needResetCheck = true;
}
}
}
void resetBstChecker(void)
{
if(needResetCheck) {
uint32_t currentTimer = micros();
if(currentTimer >= (resetBstTimer + BST_RESET_TIME))
{
bstTimeoutUserCallback();
needResetCheck = false;
}
}
}
@ -1555,26 +1538,13 @@ void taskBstMasterProcess(void)
if(sensors(SENSOR_GPS) && !bstWriteBusy())
writeGpsPositionPrameToBST();
}
}
void taskBstCheckCommand(void)
{
//Check if the BST input command available to out address
bstSlaveProcessInCommand();
bstMasterWriteLoop();
if (isRebootScheduled) {
stopMotors();
handleOneshotFeatureChangeOnRestart();
systemReset();
}
}
void bstMasterWriteLoop(void);
void taskBstReadWrite(void)
{
taskBstCheckCommand();
if(!slaveModeOn)
bstMasterWriteLoop();
resetBstChecker();
}
/*************************************************************************************************/
@ -1705,7 +1675,7 @@ bool writeFCModeToBST(void)
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(IS_RC_MODE_ACTIVE(BOXAIRMODE)) << 5 |
IS_ENABLED(FLIGHT_MODE(SONAR_MODE)) << 6 |
IS_ENABLED(FLIGHT_MODE(FAILSAFE_MODE)) << 7;

7
src/main/io/i2c_bst.h
View File

@ -19,13 +19,10 @@
#include "drivers/bus_bst.h"
void taskBstReadWrite(void);
void bstProcessInCommand(void);
void bstSlaveProcessInCommand(void);
void taskBstMasterProcess(void);
void taskBstCheckCommand(void);
//void writeGpsPositionPrameToBST(void);
//void writeGPSTimeFrameToBST(void);
//void writeDataToBST(void);
bool writeGpsPositionPrameToBST(void);
bool writeRollPitchYawToBST(void);
bool writeRCChannelToBST(void);

78
src/main/io/rc_controls.c
View File

@ -34,6 +34,7 @@
#include "drivers/system.h"
#include "drivers/sensor.h"
#include "drivers/accgyro.h"
#include "drivers/gyro_sync.h"
#include "sensors/barometer.h"
#include "sensors/battery.h"
@ -129,7 +130,7 @@ rollPitchStatus_e calculateRollPitchCenterStatus(rxConfig_t *rxConfig)
return NOT_CENTERED;
}
void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStatus, bool retarded_arm, bool disarm_kill_switch)
void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStatus, bool disarm_kill_switch)
{
static uint8_t rcDelayCommand; // this indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motors
static uint8_t rcSticks; // this hold sticks position for command combos
@ -194,15 +195,6 @@ void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStat
rcDelayCommand = 0; // reset so disarm tone will repeat
}
}
// Disarm on roll (only when retarded_arm is enabled)
if (retarded_arm && (rcSticks == THR_LO + YAW_CE + PIT_CE + ROL_LO)) {
if (ARMING_FLAG(ARMED))
mwDisarm();
else {
beeper(BEEPER_DISARM_REPEAT); // sound tone while stick held
rcDelayCommand = 0; // reset so disarm tone will repeat
}
}
}
if (ARMING_FLAG(ARMED)) {
@ -215,7 +207,7 @@ void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStat
if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) {
// GYRO calibration
gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);
gyroSetCalibrationCycles(calculateCalibratingCycles());
#ifdef GPS
if (feature(FEATURE_GPS)) {
@ -260,12 +252,6 @@ void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStat
mwArm();
return;
}
if (retarded_arm && (rcSticks == THR_LO + YAW_CE + PIT_CE + ROL_HI)) {
// Arm via ROLL
mwArm();
return;
}
}
if (rcSticks == THR_HI + YAW_LO + PIT_LO + ROL_CE) {
@ -490,7 +476,6 @@ void configureAdjustment(uint8_t index, uint8_t auxSwitchChannelIndex, const adj
void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustmentFunction, int delta) {
int newValue;
float newFloatValue;
if (delta > 0) {
beeperConfirmationBeeps(2);
@ -537,114 +522,63 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm
break;
case ADJUSTMENT_PITCH_ROLL_P:
case ADJUSTMENT_PITCH_P:
if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
newFloatValue = constrainf(pidProfile->P_f[PIDPITCH] + (float)(delta / 100.0f), 0, 100); // FIXME magic numbers repeated in serial_cli.c
pidProfile->P_f[PIDPITCH] = newFloatValue;
blackboxLogInflightAdjustmentEventFloat(ADJUSTMENT_PITCH_P, newFloatValue);
} else {
newValue = constrain((int)pidProfile->P8[PIDPITCH] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
pidProfile->P8[PIDPITCH] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_P, newValue);
}
if (adjustmentFunction == ADJUSTMENT_PITCH_P) {
break;
}
// follow though for combined ADJUSTMENT_PITCH_ROLL_P
case ADJUSTMENT_ROLL_P:
if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
newFloatValue = constrainf(pidProfile->P_f[PIDROLL] + (float)(delta / 100.0f), 0, 100); // FIXME magic numbers repeated in serial_cli.c
pidProfile->P_f[PIDROLL] = newFloatValue;
blackboxLogInflightAdjustmentEventFloat(ADJUSTMENT_ROLL_P, newFloatValue);
} else {
newValue = constrain((int)pidProfile->P8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
pidProfile->P8[PIDROLL] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_P, newValue);
}
break;
case ADJUSTMENT_PITCH_ROLL_I:
case ADJUSTMENT_PITCH_I:
if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
newFloatValue = constrainf(pidProfile->I_f[PIDPITCH] + (float)(delta / 100.0f), 0, 100); // FIXME magic numbers repeated in serial_cli.c
pidProfile->I_f[PIDPITCH] = newFloatValue;
blackboxLogInflightAdjustmentEventFloat(ADJUSTMENT_PITCH_I, newFloatValue);
} else {
newValue = constrain((int)pidProfile->I8[PIDPITCH] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
pidProfile->I8[PIDPITCH] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_I, newValue);
}
if (adjustmentFunction == ADJUSTMENT_PITCH_I) {
break;
}
// follow though for combined ADJUSTMENT_PITCH_ROLL_I
case ADJUSTMENT_ROLL_I:
if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
newFloatValue = constrainf(pidProfile->I_f[PIDROLL] + (float)(delta / 100.0f), 0, 100); // FIXME magic numbers repeated in serial_cli.c
pidProfile->I_f[PIDROLL] = newFloatValue;
blackboxLogInflightAdjustmentEventFloat(ADJUSTMENT_ROLL_I, newFloatValue);
} else {
newValue = constrain((int)pidProfile->I8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
pidProfile->I8[PIDROLL] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_I, newValue);
}
break;
case ADJUSTMENT_PITCH_ROLL_D:
case ADJUSTMENT_PITCH_D:
if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
newFloatValue = constrainf(pidProfile->D_f[PIDPITCH] + (float)(delta / 1000.0f), 0, 100); // FIXME magic numbers repeated in serial_cli.c
pidProfile->D_f[PIDPITCH] = newFloatValue;
blackboxLogInflightAdjustmentEventFloat(ADJUSTMENT_PITCH_D, newFloatValue);
} else {
newValue = constrain((int)pidProfile->D8[PIDPITCH] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
pidProfile->D8[PIDPITCH] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_D, newValue);
}
if (adjustmentFunction == ADJUSTMENT_PITCH_D) {
break;
}
// follow though for combined ADJUSTMENT_PITCH_ROLL_D
case ADJUSTMENT_ROLL_D:
if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
newFloatValue = constrainf(pidProfile->D_f[PIDROLL] + (float)(delta / 1000.0f), 0, 100); // FIXME magic numbers repeated in serial_cli.c
pidProfile->D_f[PIDROLL] = newFloatValue;
blackboxLogInflightAdjustmentEventFloat(ADJUSTMENT_ROLL_D, newFloatValue);
} else {
newValue = constrain((int)pidProfile->D8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
pidProfile->D8[PIDROLL] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_D, newValue);
}
break;
case ADJUSTMENT_YAW_P:
if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
newFloatValue = constrainf(pidProfile->P_f[PIDYAW] + (float)(delta / 100.0f), 0, 100); // FIXME magic numbers repeated in serial_cli.c
pidProfile->P_f[PIDYAW] = newFloatValue;
blackboxLogInflightAdjustmentEventFloat(ADJUSTMENT_YAW_P, newFloatValue);
} else {
newValue = constrain((int)pidProfile->P8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
pidProfile->P8[PIDYAW] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_P, newValue);
}
break;
case ADJUSTMENT_YAW_I:
if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
newFloatValue = constrainf(pidProfile->I_f[PIDYAW] + (float)(delta / 100.0f), 0, 100); // FIXME magic numbers repeated in serial_cli.c
pidProfile->I_f[PIDYAW] = newFloatValue;
blackboxLogInflightAdjustmentEventFloat(ADJUSTMENT_YAW_I, newFloatValue);
} else {
newValue = constrain((int)pidProfile->I8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
pidProfile->I8[PIDYAW] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_I, newValue);
}
break;
case ADJUSTMENT_YAW_D:
if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
newFloatValue = constrainf(pidProfile->D_f[PIDYAW] + (float)(delta / 1000.0f), 0, 100); // FIXME magic numbers repeated in serial_cli.c
pidProfile->D_f[PIDYAW] = newFloatValue;
blackboxLogInflightAdjustmentEventFloat(ADJUSTMENT_YAW_D, newFloatValue);
} else {
newValue = constrain((int)pidProfile->D8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
pidProfile->D8[PIDYAW] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_D, newValue);
}
break;
default:
break;

5
src/main/io/rc_controls.h
View File

@ -49,7 +49,8 @@ typedef enum {
BOXBLACKBOX,
BOXFAILSAFE,
BOXAIRMODE,
BOXACROPLUS,
BOXSUPEREXPO,
BOX3DDISABLESWITCH,
CHECKBOX_ITEM_COUNT
} boxId_e;
@ -158,7 +159,7 @@ bool areUsingSticksToArm(void);
bool areSticksInApModePosition(uint16_t ap_mode);
throttleStatus_e calculateThrottleStatus(rxConfig_t *rxConfig, uint16_t deadband3d_throttle);
void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStatus, bool retarded_arm, bool disarm_kill_switch);
void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStatus, bool disarm_kill_switch);
void updateActivatedModes(modeActivationCondition_t *modeActivationConditions);

5
src/main/io/rc_curves.c
View File

@ -24,6 +24,8 @@
#include "io/rc_curves.h"
#include "config/config.h"
int16_t lookupPitchRollRC[PITCH_LOOKUP_LENGTH]; // lookup table for expo & RC rate PITCH+ROLL
int16_t lookupYawRC[YAW_LOOKUP_LENGTH]; // lookup table for expo & RC rate YAW
int16_t lookupThrottleRC[THROTTLE_LOOKUP_LENGTH]; // lookup table for expo & mid THROTTLE
@ -48,6 +50,7 @@ void generateYawCurve(controlRateConfig_t *controlRateConfig)
void generateThrottleCurve(controlRateConfig_t *controlRateConfig, escAndServoConfig_t *escAndServoConfig)
{
uint8_t i;
uint16_t minThrottle = (feature(FEATURE_3D && IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH)) ? PWM_RANGE_MIN : escAndServoConfig->minthrottle);
for (i = 0; i < THROTTLE_LOOKUP_LENGTH; i++) {
int16_t tmp = 10 * i - controlRateConfig->thrMid8;
@ -57,6 +60,6 @@ void generateThrottleCurve(controlRateConfig_t *controlRateConfig, escAndServoCo
if (tmp < 0)
y = controlRateConfig->thrMid8;
lookupThrottleRC[i] = 10 * controlRateConfig->thrMid8 + tmp * (100 - controlRateConfig->thrExpo8 + (int32_t) controlRateConfig->thrExpo8 * (tmp * tmp) / (y * y)) / 10;
lookupThrottleRC[i] = escAndServoConfig->minthrottle + (int32_t) (escAndServoConfig->maxthrottle - escAndServoConfig->minthrottle) * lookupThrottleRC[i] / 1000; // [MINTHROTTLE;MAXTHROTTLE]
lookupThrottleRC[i] = minThrottle + (int32_t) (escAndServoConfig->maxthrottle - minThrottle) * lookupThrottleRC[i] / 1000; // [MINTHROTTLE;MAXTHROTTLE]
}
}

152
src/main/io/serial_cli.c
View File

@ -47,6 +47,7 @@
#include "drivers/pwm_rx.h"
#include "drivers/sdcard.h"
#include "drivers/gyro_sync.h"
#include "drivers/serial_escserial.h"
#include "drivers/buf_writer.h"
@ -109,6 +110,8 @@ static void cliAdjustmentRange(char *cmdline);
static void cliMotorMix(char *cmdline);
static void cliDefaults(char *cmdline);
static void cliDump(char *cmdLine);
void cliDumpProfile(uint8_t profileIndex);
void cliDumpRateProfile(uint8_t rateProfileIndex) ;
static void cliExit(char *cmdline);
static void cliFeature(char *cmdline);
static void cliMotor(char *cmdline);
@ -139,6 +142,9 @@ static void cliRxRange(char *cmdline);
#ifdef GPS
static void cliGpsPassthrough(char *cmdline);
#endif
#ifdef USE_ESCSERIAL
static void cliEscPassthrough(char *cmdline);
#endif
static void cliHelp(char *cmdline);
static void cliMap(char *cmdline);
@ -269,6 +275,9 @@ const clicmd_t cmdTable[] = {
"[name]", cliGet),
#ifdef GPS
CLI_COMMAND_DEF("gpspassthrough", "passthrough gps to serial", NULL, cliGpsPassthrough),
#endif
#ifdef USE_ESCSERIAL
CLI_COMMAND_DEF("escprog", "passthrough esc to serial", "<mode [sk/bl]> <index>", cliEscPassthrough),
#endif
CLI_COMMAND_DEF("help", NULL, NULL, cliHelp),
#ifdef LED_STRIP
@ -571,6 +580,7 @@ const clivalue_t valueTable[] = {
{ "servo_pwm_rate", VAR_UINT16 | MASTER_VALUE, &masterConfig.servo_pwm_rate, .config.minmax = { 50, 498 } },
{ "disarm_kill_switch", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.disarm_kill_switch, .config.lookup = { TABLE_OFF_ON } },
{ "gyro_cal_on_first_arm", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyro_cal_on_first_arm, .config.lookup = { TABLE_OFF_ON } },
{ "auto_disarm_delay", VAR_UINT8 | MASTER_VALUE, &masterConfig.auto_disarm_delay, .config.minmax = { 0, 60 } },
{ "small_angle", VAR_UINT8 | MASTER_VALUE, &masterConfig.small_angle, .config.minmax = { 0, 180 } },
@ -631,6 +641,7 @@ const clivalue_t valueTable[] = {
{ "vbat_max_cell_voltage", VAR_UINT8 | MASTER_VALUE, &masterConfig.batteryConfig.vbatmaxcellvoltage, .config.minmax = { 10, 50 } },
{ "vbat_min_cell_voltage", VAR_UINT8 | MASTER_VALUE, &masterConfig.batteryConfig.vbatmincellvoltage, .config.minmax = { 10, 50 } },
{ "vbat_warning_cell_voltage", VAR_UINT8 | MASTER_VALUE, &masterConfig.batteryConfig.vbatwarningcellvoltage, .config.minmax = { 10, 50 } },
{ "vbat_hysteresis", VAR_UINT8 | MASTER_VALUE, &masterConfig.batteryConfig.vbathysteresis, .config.minmax = { 0, 250 } },
{ "vbat_pid_compensation", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.batteryConfig.vbatPidCompensation, .config.lookup = { TABLE_OFF_ON } },
{ "current_meter_scale", VAR_INT16 | MASTER_VALUE, &masterConfig.batteryConfig.currentMeterScale, .config.minmax = { -10000, 10000 } },
{ "current_meter_offset", VAR_UINT16 | MASTER_VALUE, &masterConfig.batteryConfig.currentMeterOffset, .config.minmax = { 0, 3300 } },
@ -649,7 +660,7 @@ const clivalue_t valueTable[] = {
{ "gyro_lpf", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyro_lpf, .config.lookup = { TABLE_GYRO_LPF } },
{ "gyro_sync_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyro_sync_denom, .config.minmax = { 1, 8 } },
{ "gyro_soft_lpf", VAR_FLOAT | MASTER_VALUE, &masterConfig.gyro_soft_lpf_hz, .config.minmax = { 0, 500 } },
{ "gyro_lowpass_hz", VAR_FLOAT | MASTER_VALUE, &masterConfig.gyro_soft_lpf_hz, .config.minmax = { 0, 500 } },
{ "moron_threshold", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyroConfig.gyroMovementCalibrationThreshold, .config.minmax = { 0, 128 } },
{ "imu_dcm_kp", VAR_UINT16 | MASTER_VALUE, &masterConfig.dcm_kp, .config.minmax = { 0, 50000 } },
{ "imu_dcm_ki", VAR_UINT16 | MASTER_VALUE, &masterConfig.dcm_ki, .config.minmax = { 0, 50000 } },
@ -683,8 +694,7 @@ const clivalue_t valueTable[] = {
{ "yaw_rate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rates[FD_YAW], .config.minmax = { 0, CONTROL_RATE_CONFIG_YAW_RATE_MAX } },
{ "tpa_rate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].dynThrPID, .config.minmax = { 0, CONTROL_RATE_CONFIG_TPA_MAX} },
{ "tpa_breakpoint", VAR_UINT16 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].tpa_breakpoint, .config.minmax = { PWM_RANGE_MIN, PWM_RANGE_MAX} },
{ "acro_plus_factor", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.acroPlusFactor, .config.minmax = {1, 100 } },
{ "acro_plus_offset", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.acroPlusOffset, .config.minmax = {1, 90 } },
{ "super_expo_factor", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.superExpoFactor, .config.minmax = {1, 100 } },
{ "failsafe_delay", VAR_UINT8 | MASTER_VALUE, &masterConfig.failsafeConfig.failsafe_delay, .config.minmax = { 0, 200 } },
{ "failsafe_off_delay", VAR_UINT8 | MASTER_VALUE, &masterConfig.failsafeConfig.failsafe_off_delay, .config.minmax = { 0, 200 } },
@ -717,8 +727,11 @@ const clivalue_t valueTable[] = {
{ "mag_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mag_hardware, .config.lookup = { TABLE_MAG_HARDWARE } },
{ "mag_declination", VAR_INT16 | MASTER_VALUE, &masterConfig.mag_declination, .config.minmax = { -18000, 18000 } },
{ "pid_delta_method", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.deltaMethod, .config.lookup = { TABLE_DELTA_METHOD } },
{ "dterm_lpf_hz", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } },
{ "dterm_average_count", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_average_count, .config.minmax = {2, 12 } },
{ "dterm_lowpass_hz", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } },
{ "dterm_average_count", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_average_count, .config.minmax = {0, 12 } },
{ "iterm_reset_degrees", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermResetRate, .config.minmax = {50, 1000 } },
{ "yaw_iterm_reset_degrees", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawItermResetRate, .config.minmax = {25, 1000 } },
{ "yaw_lowpass_hz", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } },
{ "pid_process_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.pid_process_denom, .config.minmax = { 1, 8 } },
{ "pid_controller", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.pidController, .config.lookup = { TABLE_PID_CONTROLLER } },
@ -733,18 +746,6 @@ const clivalue_t valueTable[] = {
{ "i_yaw", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[YAW], .config.minmax = { 0, 200 } },
{ "d_yaw", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[YAW], .config.minmax = { 0, 200 } },
{ "p_pitchf", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P_f[PITCH], .config.minmax = { 0, 100 } },
{ "i_pitchf", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I_f[PITCH], .config.minmax = { 0, 100 } },
{ "d_pitchf", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D_f[PITCH], .config.minmax = { 0, 100 } },
{ "p_rollf", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P_f[ROLL], .config.minmax = { 0, 100 } },
{ "i_rollf", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I_f[ROLL], .config.minmax = { 0, 100 } },
{ "d_rollf", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D_f[ROLL], .config.minmax = { 0, 100 } },
{ "p_yawf", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P_f[YAW], .config.minmax = { 0, 100 } },
{ "i_yawf", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I_f[YAW], .config.minmax = { 0, 100 } },
{ "d_yawf", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D_f[YAW], .config.minmax = { 0, 100 } },
{ "level_horizon", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.H_level, .config.minmax = { 0, 10 } },
{ "level_angle", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.A_level, .config.minmax = { 0, 10 } },
{ "sensitivity_horizon", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.H_sensitivity, .config.minmax = { 0, 250 } },
{ "p_alt", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P8[PIDALT], .config.minmax = { 0, 200 } },
@ -1787,10 +1788,9 @@ typedef enum {
DUMP_MASTER = (1 << 0),
DUMP_PROFILE = (1 << 1),
DUMP_RATES = (1 << 2),
DUMP_ALL = (1 << 3),
} dumpFlags_e;
#define DUMP_ALL (DUMP_MASTER | DUMP_PROFILE | DUMP_RATES)
static const char* const sectionBreak = "\r\n";
@ -1806,7 +1806,7 @@ static void cliDump(char *cmdline)
float thr, roll, pitch, yaw;
#endif
uint8_t dumpMask = DUMP_ALL;
uint8_t dumpMask = DUMP_MASTER;
if (strcasecmp(cmdline, "master") == 0) {
dumpMask = DUMP_MASTER; // only
}
@ -1817,7 +1817,11 @@ static void cliDump(char *cmdline)
dumpMask = DUMP_RATES;
}
if (dumpMask & DUMP_MASTER) {
if (strcasecmp(cmdline, "all") == 0) {
dumpMask = DUMP_ALL; // All profiles and rates
}
if ((dumpMask & DUMP_MASTER) || (dumpMask & DUMP_ALL)) {
cliPrint("\r\n# version\r\n");
cliVersion(NULL);
@ -1958,38 +1962,62 @@ static void cliDump(char *cmdline)
cliPrint("\r\n# rxfail\r\n");
cliRxFail("");
if (dumpMask & DUMP_ALL) {
uint8_t activeProfile = masterConfig.current_profile_index;
uint8_t currentRateIndex = currentProfile->activeRateProfile;
uint8_t profileCount;
uint8_t rateCount;
for (profileCount=0; profileCount<MAX_PROFILE_COUNT;profileCount++) {
cliDumpProfile(profileCount);
for (rateCount=0; rateCount<MAX_RATEPROFILES; rateCount++)
cliDumpRateProfile(rateCount);
}
changeProfile(activeProfile);
changeControlRateProfile(currentRateIndex);
} else {
cliDumpProfile(masterConfig.current_profile_index);
cliDumpRateProfile(currentProfile->activeRateProfile);
}
}
if (dumpMask & DUMP_PROFILE) {
cliPrint("\r\n# dump profile\r\n");
cliDumpProfile(masterConfig.current_profile_index);
}
if (dumpMask & DUMP_RATES) {
cliDumpRateProfile(currentProfile->activeRateProfile);
}
}
void cliDumpProfile(uint8_t profileIndex) {
if (profileIndex >= MAX_PROFILE_COUNT) // Faulty values
return;
changeProfile(profileIndex);
cliPrint("\r\n# profile\r\n");
cliProfile("");
cliPrintf("############################# PROFILE VALUES ####################################\r\n");
cliProfile("");
printSectionBreak();
dumpValues(PROFILE_VALUE);
cliRateProfile("");
}
void cliDumpRateProfile(uint8_t rateProfileIndex) {
if (rateProfileIndex >= MAX_RATEPROFILES) // Faulty values
return;
changeControlRateProfile(rateProfileIndex);
cliPrint("\r\n# rateprofile\r\n");
cliRateProfile("");
printSectionBreak();
dumpValues(PROFILE_RATE_VALUE);
}
if (dumpMask & DUMP_RATES) {
cliPrint("\r\n# dump rates\r\n");
cliPrint("\r\n# rateprofile\r\n");
cliRateProfile("");
printSectionBreak();
dumpValues(PROFILE_RATE_VALUE);
}
}
void cliEnter(serialPort_t *serialPort)
{
cliMode = 1;
@ -2173,6 +2201,56 @@ static void cliGpsPassthrough(char *cmdline)
}
#endif
#ifdef USE_ESCSERIAL
static void cliEscPassthrough(char *cmdline)
{
uint8_t mode = 0;
int index = 0;
int i = 0;
char *pch = NULL;
char *saveptr;
if (isEmpty(cmdline)) {
cliShowParseError();
return;
}
pch = strtok_r(cmdline, " ", &saveptr);
while (pch != NULL) {
switch (i) {
case 0:
if(strncasecmp(pch, "sk", strlen(pch)) == 0)
{
mode = 0;
}
else if(strncasecmp(pch, "bl", strlen(pch)) == 0)
{
mode = 1;
}
else
{
cliShowParseError();
return;
}
break;
case 1:
index = atoi(pch);
if ((index >= 0) && (index < USABLE_TIMER_CHANNEL_COUNT)) {
printf("passthru at pwm output %d enabled\r\n", index);
}
else {
printf("invalid pwm output, valid range: 0 to %d\r\n", USABLE_TIMER_CHANNEL_COUNT);
return;
}
break;
}
i++;
pch = strtok_r(NULL, " ", &saveptr);
}
escEnablePassthrough(cliPort,index,mode);
}
#endif
static void cliHelp(char *cmdline)
{
uint32_t i = 0;

95
src/main/io/serial_msp.c
View File

@ -44,6 +44,7 @@
#include "drivers/gyro_sync.h"
#include "drivers/sdcard.h"
#include "drivers/buf_writer.h"
#include "drivers/serial_escserial.h"
#include "rx/rx.h"
#include "rx/msp.h"
@ -95,6 +96,9 @@
#ifdef USE_SERIAL_1WIRE
#include "io/serial_1wire.h"
#endif
#ifdef USE_ESCSERIAL
#include "drivers/serial_escserial.h"
#endif
static serialPort_t *mspSerialPort;
extern uint16_t cycleTime; // FIXME dependency on mw.c
@ -216,7 +220,8 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
{ BOXBLACKBOX, "BLACKBOX;", 26 },
{ BOXFAILSAFE, "FAILSAFE;", 27 },
{ BOXAIRMODE, "AIR MODE;", 28 },
{ BOXACROPLUS, "ACRO PLUS;", 29 },
{ BOXSUPEREXPO, "SUPER EXPO;", 29 },
{ BOX3DDISABLESWITCH, "DISABLE 3D SWITCH;", 30},
{ CHECKBOX_ITEM_COUNT, NULL, 0xFF }
};
@ -543,8 +548,8 @@ void mspInit(serialConfig_t *serialConfig)
}
activeBoxIds[activeBoxIdCount++] = BOXAIRMODE;
activeBoxIds[activeBoxIdCount++] = BOXACROPLUS;
activeBoxIds[activeBoxIdCount++] = BOXSUPEREXPO;
activeBoxIds[activeBoxIdCount++] = BOX3DDISABLESWITCH;
if (sensors(SENSOR_BARO)) {
activeBoxIds[activeBoxIdCount++] = BOXBARO;
@ -650,7 +655,7 @@ static uint32_t packFlightModeFlags(void)
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOX)) << BOXBLACKBOX |
IS_ENABLED(FLIGHT_MODE(FAILSAFE_MODE)) << BOXFAILSAFE |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAIRMODE)) << BOXAIRMODE |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXACROPLUS)) << BOXACROPLUS;
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXSUPEREXPO)) << BOXSUPEREXPO;
for (i = 0; i < activeBoxIdCount; i++) {
int flag = (tmp & (1 << activeBoxIds[i]));
@ -875,30 +880,11 @@ static bool processOutCommand(uint8_t cmdMSP)
break;
case MSP_PID:
headSerialReply(3 * PID_ITEM_COUNT);
if (IS_PID_CONTROLLER_FP_BASED(currentProfile->pidProfile.pidController)) { // convert float stuff into uint8_t to keep backwards compatability with all 8-bit shit with new pid
for (i = 0; i < 3; i++) {
serialize8(constrain(lrintf(currentProfile->pidProfile.P_f[i] * 10.0f), 0, 255));
serialize8(constrain(lrintf(currentProfile->pidProfile.I_f[i] * 100.0f), 0, 255));
serialize8(constrain(lrintf(currentProfile->pidProfile.D_f[i] * 1000.0f), 0, 255));
}
for (i = 3; i < PID_ITEM_COUNT; i++) {
if (i == PIDLEVEL) {
serialize8(constrain(lrintf(currentProfile->pidProfile.A_level * 10.0f), 0, 255));
serialize8(constrain(lrintf(currentProfile->pidProfile.H_level * 10.0f), 0, 255));
serialize8(constrain(lrintf(currentProfile->pidProfile.H_sensitivity), 0, 255));
} else {
serialize8(currentProfile->pidProfile.P8[i]);
serialize8(currentProfile->pidProfile.I8[i]);
serialize8(currentProfile->pidProfile.D8[i]);
}
}
} else {
for (i = 0; i < PID_ITEM_COUNT; i++) {
serialize8(currentProfile->pidProfile.P8[i]);
serialize8(currentProfile->pidProfile.I8[i]);
serialize8(currentProfile->pidProfile.D8[i]);
}
}
break;
case MSP_PIDNAMES:
headSerialReply(sizeof(pidnames) - 1);
@ -1331,30 +1317,11 @@ static bool processInCommand(void)
if (oldPid != currentProfile->pidProfile.pidController) setGyroSamplingSpeed(0); // recalculate looptimes for new PID
break;
case MSP_SET_PID:
if (IS_PID_CONTROLLER_FP_BASED(currentProfile->pidProfile.pidController)) {
for (i = 0; i < 3; i++) {
currentProfile->pidProfile.P_f[i] = (float)read8() / 10.0f;
currentProfile->pidProfile.I_f[i] = (float)read8() / 100.0f;
currentProfile->pidProfile.D_f[i] = (float)read8() / 1000.0f;
}
for (i = 3; i < PID_ITEM_COUNT; i++) {
if (i == PIDLEVEL) {
currentProfile->pidProfile.A_level = (float)read8() / 10.0f;
currentProfile->pidProfile.H_level = (float)read8() / 10.0f;
currentProfile->pidProfile.H_sensitivity = read8();
} else {
currentProfile->pidProfile.P8[i] = read8();
currentProfile->pidProfile.I8[i] = read8();
currentProfile->pidProfile.D8[i] = read8();
}
}
} else {
for (i = 0; i < PID_ITEM_COUNT; i++) {
currentProfile->pidProfile.P8[i] = read8();
currentProfile->pidProfile.I8[i] = read8();
currentProfile->pidProfile.D8[i] = read8();
}
}
break;
case MSP_SET_MODE_RANGE:
i = read8();
@ -1840,6 +1807,50 @@ static bool processInCommand(void)
}
break;
#endif
#ifdef USE_ESCSERIAL
case MSP_SET_ESCSERIAL:
// get channel number
i = read8();
// we do not give any data back, assume channel number is transmitted OK
if (i == 0xFF) {
// 0xFF -> preinitialize the Passthrough
// switch all motor lines HI
escSerialInitialize();
// and come back right afterwards
// rem: App: Wait at least appx. 500 ms for BLHeli to jump into
// bootloader mode before try to connect any ESC
}
else {
// Check for channel number 1..USABLE_TIMER_CHANNEL_COUNT-1
if ((i > 0) && (i < USABLE_TIMER_CHANNEL_COUNT)) {
// because we do not come back after calling escEnablePassthrough
// proceed with a success reply first
headSerialReply(0);
tailSerialReply();
// flush the transmit buffer
bufWriterFlush(writer);
// wait for all data to send
while (!isSerialTransmitBufferEmpty(mspSerialPort)) {
delay(50);
}
// Start to activate here
// motor 1 => index 0
escEnablePassthrough(mspSerialPort,i,0); //sk blmode
// MPS uart is active again
} else {
// ESC channel higher than max. allowed
// rem: BLHeliSuite will not support more than 8
headSerialError(0);
}
// proceed as usual with MSP commands
// and wait to switch to next channel
// rem: App needs to call MSP_BOOT to deinitialize Passthrough
}
break;
#endif
default:
// we do not know how to handle the (valid) message, indicate error MSP $M!
return false;

1
src/main/io/serial_msp.h
View File

@ -255,6 +255,7 @@ static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER;
#define MSP_SERVO_MIX_RULES 241 //out message Returns servo mixer configuration
#define MSP_SET_SERVO_MIX_RULE 242 //in message Sets servo mixer configuration
#define MSP_SET_1WIRE 243 //in message Sets 1Wire paththrough
#define MSP_SET_ESCSERIAL 244 //in message Sets escserial passthrough
// Each MSP port requires state and a receive buffer, revisit this default if someone needs more than 2 MSP ports.
#define MAX_MSP_PORT_COUNT 2

3
src/main/main.c
View File

@ -598,7 +598,7 @@ void init(void)
if (masterConfig.mixerMode == MIXER_GIMBAL) {
accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
}
gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);
gyroSetCalibrationCycles(calculateCalibratingCycles());
#ifdef BARO
baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES);
#endif
@ -725,7 +725,6 @@ int main(void) {
setTaskEnabled(TASK_TRANSPONDER, feature(FEATURE_TRANSPONDER));
#endif
#ifdef USE_BST
setTaskEnabled(TASK_BST_READ_WRITE, true);
setTaskEnabled(TASK_BST_MASTER_PROCESS, true);
#endif

49
src/main/mw.c
View File

@ -103,8 +103,6 @@ enum {
uint16_t cycleTime = 0; // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loop
float dT;
int16_t magHold;
int16_t headFreeModeHold;
@ -204,10 +202,21 @@ void filterRc(void){
}
void scaleRcCommandToFpvCamAngle(void) {
//recalculate sin/cos only when masterConfig.rxConfig.fpvCamAngleDegrees changed
static uint8_t lastFpvCamAngleDegrees = 0;
static float cosFactor = 1.0;
static float sinFactor = 0.0;
if (lastFpvCamAngleDegrees != masterConfig.rxConfig.fpvCamAngleDegrees){
lastFpvCamAngleDegrees = masterConfig.rxConfig.fpvCamAngleDegrees;
cosFactor = cos_approx(masterConfig.rxConfig.fpvCamAngleDegrees * RAD);
sinFactor = sin_approx(masterConfig.rxConfig.fpvCamAngleDegrees * RAD);
}
int16_t roll = rcCommand[ROLL];
int16_t yaw = rcCommand[YAW];
rcCommand[ROLL] = constrain(cos(masterConfig.rxConfig.fpvCamAngleDegrees*RAD) * roll - sin(masterConfig.rxConfig.fpvCamAngleDegrees*RAD) * yaw, -500, 500);
rcCommand[YAW] = constrain(cos(masterConfig.rxConfig.fpvCamAngleDegrees*RAD) * yaw + sin(masterConfig.rxConfig.fpvCamAngleDegrees*RAD) * roll, -500, 500);
rcCommand[ROLL] = constrain(roll * cosFactor - yaw * sinFactor, -500, 500);
rcCommand[YAW] = constrain(yaw * cosFactor + roll * sinFactor, -500, 500);
}
void annexCode(void)
@ -270,11 +279,21 @@ void annexCode(void)
rcCommand[axis] = -rcCommand[axis];
}
if (feature(FEATURE_3D)) {
tmp = constrain(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX);
tmp = (uint32_t)(tmp - PWM_RANGE_MIN);
} else {
tmp = constrain(rcData[THROTTLE], masterConfig.rxConfig.mincheck, PWM_RANGE_MAX);
tmp = (uint32_t)(tmp - masterConfig.rxConfig.mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - masterConfig.rxConfig.mincheck);
}
tmp2 = tmp / 100;
rcCommand[THROTTLE] = lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100; // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE]
if (feature(FEATURE_3D) && IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH) && !failsafeIsActive()) {
fix12_t throttleScaler = qConstruct(rcCommand[THROTTLE] - 1000, 1000);
rcCommand[THROTTLE] = masterConfig.rxConfig.midrc + qMultiply(throttleScaler, PWM_RANGE_MAX - masterConfig.rxConfig.midrc);
}
if (FLIGHT_MODE(HEADFREE_MODE)) {
float radDiff = degreesToRadians(DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headFreeModeHold);
float cosDiff = cos_approx(radDiff);
@ -347,6 +366,15 @@ void releaseSharedTelemetryPorts(void) {
void mwArm(void)
{
static bool armingCalibrationWasInitialisedOnce;
if (masterConfig.gyro_cal_on_first_arm && !armingCalibrationWasInitialisedOnce) {
gyroSetCalibrationCycles(calculateCalibratingCycles());
armingCalibrationWasInitialisedOnce = true;
}
if (!isGyroCalibrationComplete()) return; // prevent arming before gyro is calibrated
if (ARMING_FLAG(OK_TO_ARM)) {
if (ARMING_FLAG(ARMED)) {
return;
@ -356,6 +384,7 @@ void mwArm(void)
}
if (!ARMING_FLAG(PREVENT_ARMING)) {
ENABLE_ARMING_FLAG(ARMED);
ENABLE_ARMING_FLAG(WAS_EVER_ARMED);
headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
#ifdef BLACKBOX
@ -534,7 +563,7 @@ void processRx(void)
}
}
processRcStickPositions(&masterConfig.rxConfig, throttleStatus, masterConfig.retarded_arm, masterConfig.disarm_kill_switch);
processRcStickPositions(&masterConfig.rxConfig, throttleStatus, masterConfig.disarm_kill_switch);
if (feature(FEATURE_INFLIGHT_ACC_CAL)) {
updateInflightCalibrationState();
@ -741,6 +770,14 @@ void taskMotorUpdate(void) {
}
}
uint8_t setPidUpdateCountDown(void) {
if (masterConfig.gyro_soft_lpf_hz) {
return masterConfig.pid_process_denom - 1;
} else {
return 1;
}
}
// Check for oneshot125 protection. With fast looptimes oneshot125 pulse duration gets more near the pid looptime
bool shouldUpdateMotorsAfterPIDLoop(void) {
if (targetPidLooptime > 375 ) {
@ -782,7 +819,7 @@ void taskMainPidLoopCheck(void) {
if (pidUpdateCountdown) {
pidUpdateCountdown--;
} else {
pidUpdateCountdown = masterConfig.pid_process_denom - 1;
pidUpdateCountdown = setPidUpdateCountDown();
taskMainPidLoop();
if (shouldUpdateMotorsAfterPIDLoop()) taskMotorUpdate();
runTaskMainSubprocesses = true;

3
src/main/rx/rx.h
View File

@ -124,8 +124,7 @@ typedef struct rxConfig_s {
uint8_t rcSmoothing;
uint8_t fpvCamAngleDegrees; // Camera angle to be scaled into rc commands
uint8_t max_aux_channel;
uint8_t acroPlusFactor; // Air mode acrobility factor
uint8_t acroPlusOffset; // Air mode stick offset
uint8_t superExpoFactor; // Super Expo Factor
uint16_t rx_min_usec;
uint16_t rx_max_usec;

1
src/main/scheduler.h
View File

@ -81,7 +81,6 @@ typedef enum {
#endif
#ifdef USE_BST
TASK_BST_READ_WRITE,
TASK_BST_MASTER_PROCESS,
#endif

7
src/main/scheduler_tasks.c
View File

@ -188,13 +188,6 @@ cfTask_t cfTasks[TASK_COUNT] = {
#endif
#ifdef USE_BST
[TASK_BST_READ_WRITE] = {
.taskName = "BST_MASTER_WRITE",
.taskFunc = taskBstReadWrite,
.desiredPeriod = 1000000 / 100, // 100 Hz
.staticPriority = TASK_PRIORITY_IDLE,
},
[TASK_BST_MASTER_PROCESS] = {
.taskName = "BST_MASTER_PROCESS",
.taskFunc = taskBstMasterProcess,

10
src/main/sensors/battery.c
View File

@ -83,8 +83,6 @@ static void updateBatteryVoltage(void)
}
#define VBATTERY_STABLE_DELAY 40
/* Batt Hysteresis of +/-100mV */
#define VBATT_HYSTERESIS 1
void updateBattery(void)
{
@ -123,23 +121,23 @@ void updateBattery(void)
switch(batteryState)
{
case BATTERY_OK:
if (vbat <= (batteryWarningVoltage - VBATT_HYSTERESIS)) {
if (vbat <= (batteryWarningVoltage - batteryConfig->vbathysteresis)) {
batteryState = BATTERY_WARNING;
beeper(BEEPER_BAT_LOW);
}
break;
case BATTERY_WARNING:
if (vbat <= (batteryCriticalVoltage - VBATT_HYSTERESIS)) {
if (vbat <= (batteryCriticalVoltage - batteryConfig->vbathysteresis)) {
batteryState = BATTERY_CRITICAL;
beeper(BEEPER_BAT_CRIT_LOW);
} else if (vbat > (batteryWarningVoltage + VBATT_HYSTERESIS)){
} else if (vbat > (batteryWarningVoltage + batteryConfig->vbathysteresis)){
batteryState = BATTERY_OK;
} else {
beeper(BEEPER_BAT_LOW);
}
break;
case BATTERY_CRITICAL:
if (vbat > (batteryCriticalVoltage + VBATT_HYSTERESIS)){
if (vbat > (batteryCriticalVoltage + batteryConfig->vbathysteresis)){
batteryState = BATTERY_WARNING;
beeper(BEEPER_BAT_LOW);
} else {

1
src/main/sensors/battery.h
View File

@ -40,6 +40,7 @@ typedef struct batteryConfig_s {
uint8_t vbatmaxcellvoltage; // maximum voltage per cell, used for auto-detecting battery voltage in 0.1V units, default is 43 (4.3V)
uint8_t vbatmincellvoltage; // minimum voltage per cell, this triggers battery critical alarm, in 0.1V units, default is 33 (3.3V)
uint8_t vbatwarningcellvoltage; // warning voltage per cell, this triggers battery warning alarm, in 0.1V units, default is 35 (3.5V)
uint8_t vbathysteresis; // hysteresis for alarm, default 1 = 0.1V
uint8_t vbatPidCompensation; // Scale PIDsum to battery voltage
int16_t currentMeterScale; // scale the current sensor output voltage to milliamps. Value in 1/10th mV/A

10
src/main/sensors/gyro.c
View File

@ -77,9 +77,13 @@ bool isOnFinalGyroCalibrationCycle(void)
return calibratingG == 1;
}
uint16_t calculateCalibratingCycles(void) {
return (CALIBRATING_GYRO_CYCLES / targetLooptime) * CALIBRATING_GYRO_CYCLES;
}
bool isOnFirstGyroCalibrationCycle(void)
{
return calibratingG == CALIBRATING_GYRO_CYCLES;
return calibratingG == calculateCalibratingCycles();
}
static void performAcclerationCalibration(uint8_t gyroMovementCalibrationThreshold)
@ -108,10 +112,10 @@ static void performAcclerationCalibration(uint8_t gyroMovementCalibrationThresho
float dev = devStandardDeviation(&var[axis]);
// check deviation and startover in case the model was moved
if (gyroMovementCalibrationThreshold && dev > gyroMovementCalibrationThreshold) {
gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);
gyroSetCalibrationCycles(calculateCalibratingCycles());
return;
}
gyroZero[axis] = (g[axis] + (CALIBRATING_GYRO_CYCLES / 2)) / CALIBRATING_GYRO_CYCLES;
gyroZero[axis] = (g[axis] + (calculateCalibratingCycles() / 2)) / calculateCalibratingCycles();
}
}

1
src/main/sensors/gyro.h
View File

@ -43,4 +43,5 @@ void useGyroConfig(gyroConfig_t *gyroConfigToUse, float gyro_lpf_hz);
void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired);
void gyroUpdate(void);
bool isGyroCalibrationComplete(void);
uint16_t calculateCalibratingCycles(void);

3
src/main/target/ALIENFLIGHTF3/target.h
View File

@ -51,6 +51,9 @@
//#define DEBUG_MPU_DATA_READY_INTERRUPT
#define USE_MPU_DATA_READY_SIGNAL
#define USE_ESCSERIAL
#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1
// Using MPU6050 for the moment.
#define GYRO
#define USE_GYRO_MPU6050

2
src/main/target/CC3D/target.h
View File

@ -27,6 +27,8 @@
#define INVERTER_PERIPHERAL RCC_APB2Periph_GPIOB
#define INVERTER_USART USART1
#define USE_ESCSERIAL
#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1
#define BEEP_GPIO GPIOA
#define BEEP_PIN Pin_15 // PA15 (Beeper)

3
src/main/target/COLIBRI_RACE/target.h
View File

@ -110,6 +110,9 @@
#define UART3_TX_PINSOURCE GPIO_PinSource10
#define UART3_RX_PINSOURCE GPIO_PinSource11
#define USE_ESCSERIAL
#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1
#define USE_I2C
#define I2C_DEVICE (I2CDEV_2)

3
src/main/target/IRCFUSIONF3/target.h
View File

@ -38,6 +38,9 @@
#define USE_MAG_DATA_READY_SIGNAL
#define ENSURE_MAG_DATA_READY_IS_HIGH
#define USE_ESCSERIAL
#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1
#define GYRO
#define USE_GYRO_MPU6050

3
src/main/target/LUX_RACE/target.h
View File

@ -58,6 +58,9 @@
#define EXTI_CALLBACK_HANDLER_COUNT 1 // MPU data ready
#define USE_ESCSERIAL
#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1
#define GYRO
#define USE_GYRO_MPU6500
#define USE_GYRO_SPI_MPU6500

3
src/main/target/MOTOLAB/target.h
View File

@ -35,6 +35,9 @@
#define USABLE_TIMER_CHANNEL_COUNT 9
#define USE_ESCSERIAL
#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1
// MPU6050 interrupts
#define EXTI15_10_CALLBACK_HANDLER_COUNT 1 // MPU data ready
#define USE_MPU_DATA_READY_SIGNAL

3
src/main/target/NAZE/target.h
View File

@ -29,6 +29,9 @@
#define LED1_PIN Pin_4 // PB4 (LED)
#define LED1_PERIPHERAL RCC_APB2Periph_GPIOB
#define USE_ESCSERIAL
#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1
#define BEEP_GPIO GPIOA
#define BEEP_PIN Pin_12 // PA12 (Beeper)
#define BEEP_PERIPHERAL RCC_APB2Periph_GPIOA

3
src/main/target/RMDO/target.h
View File

@ -28,6 +28,9 @@
#define BEEP_PERIPHERAL RCC_AHBPeriph_GPIOC
#define BEEPER_INVERTED
#define USE_ESCSERIAL
#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1
#define USABLE_TIMER_CHANNEL_COUNT 17
#define EXTI_CALLBACK_HANDLER_COUNT 2 // MPU data ready and MAG data ready

3
src/main/target/SPARKY/target.h
View File

@ -34,6 +34,9 @@
#define USABLE_TIMER_CHANNEL_COUNT 11
#define USE_ESCSERIAL
#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1
// MPU6050 interrupts
#define EXTI15_10_CALLBACK_HANDLER_COUNT 1 // MPU data ready
#define USE_MPU_DATA_READY_SIGNAL

2
src/main/target/SPRACINGF3/target.h
View File

@ -38,6 +38,8 @@
#define USE_MAG_DATA_READY_SIGNAL
#define ENSURE_MAG_DATA_READY_IS_HIGH
#define USE_ESCSERIAL
#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1
#define GYRO
#define USE_GYRO_MPU6050

2
src/main/target/SPRACINGF3MINI/target.h
View File

@ -41,6 +41,8 @@
#define USE_MAG_DATA_READY_SIGNAL
#define ENSURE_MAG_DATA_READY_IS_HIGH
#define USE_ESCSERIAL
#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1
#define GYRO
//#define USE_FAKE_GYRO

4
src/main/version.h
View File

@ -16,8 +16,8 @@
*/
#define FC_VERSION_MAJOR 2 // increment when a major release is made (big new feature, etc)
#define FC_VERSION_MINOR 5 // increment when a minor release is made (small new feature, change etc)
#define FC_VERSION_PATCH_LEVEL 3 // increment when a bug is fixed
#define FC_VERSION_MINOR 6 // increment when a minor release is made (small new feature, change etc)
#define FC_VERSION_PATCH_LEVEL 0 // increment when a bug is fixed
#define STR_HELPER(x) #x
#define STR(x) STR_HELPER(x)