commit
9d737474b8
|
@ -5,6 +5,8 @@
|
|||
*.dep
|
||||
*.bak
|
||||
*.uvgui.*
|
||||
*.swp
|
||||
*.save
|
||||
.project
|
||||
.settings
|
||||
.cproject
|
||||
|
|
1
Makefile
1
Makefile
|
@ -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 \
|
||||
|
|
|
@ -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.
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
return currentProfile->pidProfile.D8[condition - FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0] != 0;
|
||||
|
||||
case FLIGHT_LOG_FIELD_CONDITION_MAG:
|
||||
#ifdef MAG
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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);
|
||||
|
||||
|
|
|
@ -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,101 +58,219 @@ 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 (BSTx == I2C1) {
|
||||
bst1ErrorCount++;
|
||||
} else {
|
||||
bst2ErrorCount++;
|
||||
}
|
||||
I2C_SoftwareResetCmd(BSTx);
|
||||
return false;
|
||||
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*/)
|
||||
{
|
||||
GPIO_InitTypeDef GPIO_InitStructure;
|
||||
I2C_InitTypeDef BST_InitStructure;
|
||||
NVIC_InitTypeDef nvic;
|
||||
GPIO_InitTypeDef GPIO_InitStructure;
|
||||
I2C_InitTypeDef BST_InitStructure;
|
||||
|
||||
if (BSTx == I2C1) {
|
||||
RCC_AHBPeriphClockCmd(BST1_SCL_CLK_SOURCE | BST1_SDA_CLK_SOURCE, ENABLE);
|
||||
RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C1, ENABLE);
|
||||
RCC_I2CCLKConfig(RCC_I2C1CLK_SYSCLK);
|
||||
if(BSTx == I2C1) {
|
||||
RCC_AHBPeriphClockCmd(BST1_SCL_CLK_SOURCE | BST1_SDA_CLK_SOURCE, ENABLE);
|
||||
RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C1, ENABLE);
|
||||
RCC_I2CCLKConfig(RCC_I2C1CLK_SYSCLK);
|
||||
|
||||
GPIO_PinAFConfig(BST1_SCL_GPIO, BST1_SCL_PIN_SOURCE, BST1_SCL_GPIO_AF);
|
||||
GPIO_PinAFConfig(BST1_SDA_GPIO, BST1_SDA_PIN_SOURCE, BST1_SDA_GPIO_AF);
|
||||
GPIO_PinAFConfig(BST1_SCL_GPIO, BST1_SCL_PIN_SOURCE, BST1_SCL_GPIO_AF);
|
||||
GPIO_PinAFConfig(BST1_SDA_GPIO, BST1_SDA_PIN_SOURCE, BST1_SDA_GPIO_AF);
|
||||
|
||||
GPIO_StructInit(&GPIO_InitStructure);
|
||||
I2C_StructInit(&BST_InitStructure);
|
||||
GPIO_StructInit(&GPIO_InitStructure);
|
||||
I2C_StructInit(&BST_InitStructure);
|
||||
|
||||
// Init pins
|
||||
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
|
||||
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
|
||||
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
|
||||
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
|
||||
// Init pins
|
||||
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
|
||||
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
|
||||
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
|
||||
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
|
||||
|
||||
GPIO_InitStructure.GPIO_Pin = BST1_SCL_PIN;
|
||||
GPIO_Init(BST1_SCL_GPIO, &GPIO_InitStructure);
|
||||
GPIO_InitStructure.GPIO_Pin = BST1_SCL_PIN;
|
||||
GPIO_Init(BST1_SCL_GPIO, &GPIO_InitStructure);
|
||||
|
||||
GPIO_InitStructure.GPIO_Pin = BST1_SDA_PIN;
|
||||
GPIO_Init(BST1_SDA_GPIO, &GPIO_InitStructure);
|
||||
GPIO_InitStructure.GPIO_Pin = BST1_SDA_PIN;
|
||||
GPIO_Init(BST1_SDA_GPIO, &GPIO_InitStructure);
|
||||
|
||||
I2C_StructInit(&BST_InitStructure);
|
||||
I2C_StructInit(&BST_InitStructure);
|
||||
|
||||
BST_InitStructure.I2C_Mode = I2C_Mode_I2C;
|
||||
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.
|
||||
BST_InitStructure.I2C_Mode = I2C_Mode_I2C;
|
||||
BST_InitStructure.I2C_AnalogFilter = I2C_AnalogFilter_Enable;
|
||||
BST_InitStructure.I2C_DigitalFilter = 0x00;
|
||||
BST_InitStructure.I2C_OwnAddress1 = CLEANFLIGHT_FC;
|
||||
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_Init(I2C1, &BST_InitStructure);
|
||||
|
||||
I2C_Cmd(I2C1, ENABLE);
|
||||
}
|
||||
I2C_GeneralCallCmd(I2C1, ENABLE);
|
||||
|
||||
if (BSTx == I2C2) {
|
||||
RCC_AHBPeriphClockCmd(BST2_SCL_CLK_SOURCE | BST2_SDA_CLK_SOURCE, ENABLE);
|
||||
RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C2, ENABLE);
|
||||
RCC_I2CCLKConfig(RCC_I2C2CLK_SYSCLK);
|
||||
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);
|
||||
|
||||
GPIO_PinAFConfig(BST2_SCL_GPIO, BST2_SCL_PIN_SOURCE, BST2_SCL_GPIO_AF);
|
||||
GPIO_PinAFConfig(BST2_SDA_GPIO, BST2_SDA_PIN_SOURCE, BST2_SDA_GPIO_AF);
|
||||
I2C_ITConfig(I2C1, I2C_IT_ADDRI | I2C_IT_RXI | I2C_IT_STOPI | I2C_IT_NACKI | I2C_IT_ERRI, ENABLE);
|
||||
|
||||
I2C_Cmd(I2C1, ENABLE);
|
||||
}
|
||||
|
||||
GPIO_StructInit(&GPIO_InitStructure);
|
||||
I2C_StructInit(&BST_InitStructure);
|
||||
if(BSTx == I2C2) {
|
||||
RCC_AHBPeriphClockCmd(BST2_SCL_CLK_SOURCE | BST2_SDA_CLK_SOURCE, ENABLE);
|
||||
RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C2, ENABLE);
|
||||
RCC_I2CCLKConfig(RCC_I2C2CLK_SYSCLK);
|
||||
|
||||
// Init pins
|
||||
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
|
||||
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
|
||||
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
|
||||
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
|
||||
GPIO_PinAFConfig(BST2_SCL_GPIO, BST2_SCL_PIN_SOURCE, BST2_SCL_GPIO_AF);
|
||||
GPIO_PinAFConfig(BST2_SDA_GPIO, BST2_SDA_PIN_SOURCE, BST2_SDA_GPIO_AF);
|
||||
|
||||
GPIO_InitStructure.GPIO_Pin = BST2_SCL_PIN;
|
||||
GPIO_Init(BST2_SCL_GPIO, &GPIO_InitStructure);
|
||||
GPIO_StructInit(&GPIO_InitStructure);
|
||||
I2C_StructInit(&BST_InitStructure);
|
||||
|
||||
GPIO_InitStructure.GPIO_Pin = BST2_SDA_PIN;
|
||||
GPIO_Init(BST2_SDA_GPIO, &GPIO_InitStructure);
|
||||
// Init pins
|
||||
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
|
||||
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
|
||||
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
|
||||
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
|
||||
|
||||
I2C_StructInit(&BST_InitStructure);
|
||||
GPIO_InitStructure.GPIO_Pin = BST2_SCL_PIN;
|
||||
GPIO_Init(BST2_SCL_GPIO, &GPIO_InitStructure);
|
||||
|
||||
BST_InitStructure.I2C_Mode = I2C_Mode_I2C;
|
||||
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.
|
||||
GPIO_InitStructure.GPIO_Pin = BST2_SDA_PIN;
|
||||
GPIO_Init(BST2_SDA_GPIO, &GPIO_InitStructure);
|
||||
|
||||
I2C_Init(I2C2, &BST_InitStructure);
|
||||
I2C_StructInit(&BST_InitStructure);
|
||||
|
||||
I2C_Cmd(I2C2, ENABLE);
|
||||
}
|
||||
BST_InitStructure.I2C_Mode = I2C_Mode_I2C;
|
||||
BST_InitStructure.I2C_AnalogFilter = I2C_AnalogFilter_Enable;
|
||||
BST_InitStructure.I2C_DigitalFilter = 0x00;
|
||||
BST_InitStructure.I2C_OwnAddress1 = CLEANFLIGHT_FC;
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
void bstInit(BSTDevice index)
|
||||
|
@ -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) {
|
||||
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);
|
||||
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;
|
||||
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_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();
|
||||
}
|
||||
} else if(currentTime>bstMasterWriteTimeout+BST_SHORT_TIMEOUT) {
|
||||
bstTimeoutUserCallback();
|
||||
}
|
||||
}
|
||||
|
||||
void bstMasterReadLoop(void)
|
||||
{
|
||||
}
|
||||
/*************************************************************************************************/
|
||||
void crc8Cal(uint8_t data_in)
|
||||
{
|
||||
|
|
|
@ -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)
|
||||
|
|
|
@ -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
|
|
@ -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);
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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,15 +203,20 @@ 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(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]);
|
||||
}
|
||||
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 (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.
|
||||
|
@ -235,36 +224,35 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
|
|||
ITerm = errorGyroIf[axis];
|
||||
|
||||
//-----calculate D-term
|
||||
if (pidProfile->deltaMethod == DELTA_FROM_ERROR) {
|
||||
delta = RateError - lastErrorForDelta[axis];
|
||||
lastErrorForDelta[axis] = RateError;
|
||||
if (axis == YAW) {
|
||||
if (pidProfile->yaw_lpf_hz) PTerm = filterApplyPt1(PTerm, &yawFilterState, pidProfile->yaw_lpf_hz, getdT());
|
||||
DTerm = 0;
|
||||
} else {
|
||||
delta = -(gyroRate - lastErrorForDelta[axis]); // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
|
||||
lastErrorForDelta[axis] = gyroRate;
|
||||
}
|
||||
if (pidProfile->deltaMethod == DELTA_FROM_ERROR) {
|
||||
delta = RateError - lastErrorForDelta[axis];
|
||||
lastErrorForDelta[axis] = RateError;
|
||||
} else {
|
||||
delta = -(gyroRate - lastErrorForDelta[axis]); // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
|
||||
lastErrorForDelta[axis] = gyroRate;
|
||||
}
|
||||
|
||||
// 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);
|
||||
// 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 / 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,25 +288,20 @@ 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]);
|
||||
}
|
||||
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
|
||||
PTerm = (RateError * pidProfile->P8[axis] * PIDweight[axis] / 100) >> 7;
|
||||
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,48 +492,52 @@ 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 (antiWindupProtection || motorLimitReached) {
|
||||
errorGyroI[axis] = constrain(errorGyroI[axis], -errorGyroILimit[axis], errorGyroILimit[axis]);
|
||||
} else {
|
||||
errorGyroILimit[axis] = ABS(errorGyroI[axis]);
|
||||
}
|
||||
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 (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;
|
||||
if (axis == YAW) {
|
||||
if (pidProfile->yaw_lpf_hz) PTerm = filterApplyPt1(PTerm, &yawFilterState, pidProfile->yaw_lpf_hz, getdT());
|
||||
DTerm = 0;
|
||||
} else {
|
||||
delta = -(gyroRate - lastErrorForDelta[axis]); // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
|
||||
lastErrorForDelta[axis] = gyroRate;
|
||||
}
|
||||
|
||||
// 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 = (delta * ((uint16_t) 0xFFFF / ((uint16_t)targetPidLooptime >> 4))) >> 6;
|
||||
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;
|
||||
} else {
|
||||
delta = -(gyroRate - lastErrorForDelta[axis]); // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
|
||||
lastErrorForDelta[axis] = gyroRate;
|
||||
}
|
||||
|
||||
// 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 = (delta * ((uint16_t) 0xFFFF / ((uint16_t)targetPidLooptime >> 4))) >> 6;
|
||||
|
||||
// Filter delta
|
||||
if (pidProfile->dterm_lpf_hz) delta = filterApplyPt1((float)delta, &deltaFilterState[axis], pidProfile->dterm_lpf_hz, getdT());
|
||||
|
||||
if (deltaStateIsSet) {
|
||||
delta = lrintf(applyBiQuadFilter((float) delta, &deltaBiQuadState[axis])) * 3; // Keep same scaling as unfiltered delta
|
||||
} 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) << 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;
|
||||
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)) {
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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,29 +700,10 @@ 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]);
|
||||
}
|
||||
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:
|
||||
|
@ -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();
|
||||
}
|
||||
}
|
||||
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;
|
||||
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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);
|
||||
}
|
||||
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);
|
||||
}
|
||||
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);
|
||||
}
|
||||
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);
|
||||
}
|
||||
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);
|
||||
}
|
||||
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;
|
||||
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);
|
||||
}
|
||||
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);
|
||||
}
|
||||
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);
|
||||
}
|
||||
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;
|
||||
|
|
|
@ -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);
|
||||
|
||||
|
|
|
@ -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]
|
||||
}
|
||||
}
|
||||
|
|
|
@ -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);
|
||||
|
@ -1901,7 +1905,7 @@ static void cliDump(char *cmdline)
|
|||
if (mask & (1 << i))
|
||||
cliPrintf("beeper -%s\r\n", beeperNameForTableIndex(i));
|
||||
else
|
||||
cliPrintf("beeper %s\r\n", beeperNameForTableIndex(i));
|
||||
cliPrintf("beeper %s\r\n", beeperNameForTableIndex(i));
|
||||
}
|
||||
#endif
|
||||
|
||||
|
@ -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");
|
||||
|
||||
cliPrint("\r\n# profile\r\n");
|
||||
cliProfile("");
|
||||
|
||||
printSectionBreak();
|
||||
|
||||
dumpValues(PROFILE_VALUE);
|
||||
|
||||
cliPrint("\r\n# rateprofile\r\n");
|
||||
cliRateProfile("");
|
||||
|
||||
printSectionBreak();
|
||||
|
||||
dumpValues(PROFILE_RATE_VALUE);
|
||||
cliDumpProfile(masterConfig.current_profile_index);
|
||||
}
|
||||
if (dumpMask & DUMP_RATES) {
|
||||
cliPrint("\r\n# dump rates\r\n");
|
||||
|
||||
cliPrint("\r\n# rateprofile\r\n");
|
||||
cliRateProfile("");
|
||||
|
||||
printSectionBreak();
|
||||
|
||||
dumpValues(PROFILE_RATE_VALUE);
|
||||
}
|
||||
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);
|
||||
|
||||
}
|
||||
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;
|
||||
|
|
|
@ -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,29 +880,10 @@ 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]);
|
||||
}
|
||||
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:
|
||||
|
@ -1331,29 +1317,10 @@ 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();
|
||||
}
|
||||
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:
|
||||
|
@ -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;
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
||||
|
|
|
@ -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];
|
||||
}
|
||||
|
||||
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);
|
||||
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;
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -81,7 +81,6 @@ typedef enum {
|
|||
#endif
|
||||
|
||||
#ifdef USE_BST
|
||||
TASK_BST_READ_WRITE,
|
||||
TASK_BST_MASTER_PROCESS,
|
||||
#endif
|
||||
|
||||
|
|
|
@ -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,
|
||||
|
|
|
@ -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 {
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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();
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -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);
|
||||
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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)
|
||||
|
|
|
@ -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)
|
||||
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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)
|
||||
|
|
Loading…
Reference in New Issue