diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml new file mode 100644 index 00000000..15333b29 --- /dev/null +++ b/.github/workflows/build.yml @@ -0,0 +1,50 @@ +on: + push: + tags: + - '*' + pull_request: + branches: + - '*' + # repository_dispatch is a newer github-actions feature that will allow building from triggers other than code merge/PR + repository_dispatch: + types: [build] + +name: Build IMU-F +jobs: + build: + timeout-minutes: 15 + runs-on: ubuntu-latest + steps: + + # curl, by default, may timeout easily + - name: curl fix + run: function curl () { command curl --connect-timeout 30 --retry 10 "$@" ; } + + - name: Checkout + uses: actions/checkout@v2 + with: + fetch-depth: 15 + + - name: ARM SDK + uses: fiam/arm-none-eabi-gcc@v1.0.2 + with: + release: '6-2017-q1' + + - name: Setup Python + uses: actions/setup-python@v1 + + - name: Make + run: ./make.py -T=F3 + + - name: Rename Binary + id: file_version + run: | + VER=$(grep FIRMWARE_VERSION ./src/version.h | awk '{print $3}') + mv output/F3.bin output/IMUF_${VER}.bin + echo "::set-output name=VER::${VER}" + + - name: Upload artifcats + uses: actions/upload-artifact@v2-preview + with: + name: IMUF_${{steps.file_version.outputs.VER}} + path: output/*.bin diff --git a/.gitignore b/.gitignore index eecf0499..2eb62fcb 100644 --- a/.gitignore +++ b/.gitignore @@ -5,3 +5,4 @@ redirection redirection.junk kalman/test .exe +gcc-arm* diff --git a/README.md b/README.md index 69cab931..ef7175b9 100644 --- a/README.md +++ b/README.md @@ -1,15 +1,44 @@ # IMU-F -IMU-F flight controller project - -# setup -clone the project - -```bash -brew install openocd python -curl -o ~/Downloads/gcc_arm-6-2017-q1.tar.bz2 https://developer.arm.com/-/media/Files/downloads/gnu-rm/6_1-2017q1/gcc-arm-none-eabi-6-2017-q1-update-mac.tar.bz2 -tar -zxvf ~/Downloads/gcc_arm-6-2017-q1.tar.bz2 -sudo mkdir /usr/local/gcc_arm-6-2017-q1 -sudo mv ~/Downloads/gcc_arm-6-2017-q1 /usr/local/gcc_arm-6-2017-q1 -sudo ln -s /usr/local/gcc_arm-6-2017-q1 /usr/local/gcc_arm -echo 'export PATH="$PATH:/usr/local/gcc_arm/bin"' >> ~/.bash_profile +IMU-F firmware for Helio/Strix/Mode2Flux + +## Prerequisites + +Linux: +```shell +sudo apt -y install git python3 openocd #(or python2 should work) +``` + +MacOSX: + +```shell +brew install git openocd python +``` + + +## Clone repository +```shell +git clone https://github.com/emuflight/imu-f.git +``` + +## Download ARMv6: +``` +cd imu-f + +case $(uname) in + "Linux" ) + OS="linux" + ;; + "Darwin" ) + OS="mac" + ;; +esac +echo ${OS} + +curl -L -s "https://developer.arm.com/-/media/Files/downloads/gnu-rm/6_1-2017q1/gcc-arm-none-eabi-6-2017-q1-update-${OS}.tar.bz2" | tar xjv +``` + +## Build +```shell +export PATH="$PATH:$(pwd)/gcc-arm-none-eabi-6-2017-q1-update/bin" +python make.py -C -T F3 ``` diff --git a/make.py b/make.py index 8382c02d..f4a91860 100755 --- a/make.py +++ b/make.py @@ -31,7 +31,10 @@ def __getattr__(self, attr): try: from colorama import init, Fore, Back, Style - init(convert=True) + if platform.system() == 'Windows': + init(convert=True) + else: + init(convert=False) except ImportError: Fore = Style = ColorFallback() @@ -176,7 +179,7 @@ def configure_target(TARGET): #extra source files to include not in the below dirs SOURCE_FILES = [ this_dir + "/assembly/startup/startup_stm32f303xc.s", - LIBRARY_PATH + "/CMSIS_std/DSP_Lib/Source/TransformFunctions/arm_bitreversal2.s" + LIBRARY_PATH + "/CMSIS_std/DSP_Lib/Source/TransformFunctions/arm_bitreversal2.S" ] #All include dirs @@ -524,7 +527,7 @@ def ProcessList(fileNames, target_config): linkerObjs.append(os.path.join("output", makeObject(fileName, target_config.target))) if FileModified(fileName, target_config): - if fileName[-2:] == ".s": + if (fileName[-2:] == ".s") or ((fileName[-2:] == ".S")): commands.append(asm_command.format( INPUT_FILE=fileName.path, OUTPUT_FILE=makeObject(fileName.path, target_config.target), diff --git a/src/board_comm/board_comm.c b/src/board_comm/board_comm.c index 636bc7ad..47356cb6 100644 --- a/src/board_comm/board_comm.c +++ b/src/board_comm/board_comm.c @@ -42,7 +42,7 @@ void board_comm_init(void) #ifndef BOARD_COMM_CS_TYPE //exti is used for NSS gpio_exti_init(BOARD_COMM_EXTI_PORT, BOARD_COMM_EXTI_PORT_SRC, BOARD_COMM_EXTI_PIN, BOARD_COMM_EXTI_PIN_SRC, BOARD_COMM_EXTI_LINE, EXTI_Trigger_Rising, BOARD_COMM_EXTI_IRQn, BOARD_COMM_EXTI_ISR_PRE_PRI, BOARD_COMM_EXTI_ISR_SUB_PRI); - spi_init(&boardCommSpiInitStruct, &boardCommDmaInitStruct, BOARD_COMM_SPI, SPI_Mode_Slave, SPI_NSS_Soft, SPI_CPOL_Low, SPI_CPHA_1Edge, SPI_BaudRatePrescaler_2); + spi_init(&boardCommSpiInitStruct, &boardCommDmaInitStruct, BOARD_COMM_SPI, SPI_Mode_Slave, SPI_NSS_Soft, SPI_CPOL_Low, SPI_CPHA_1Edge, SPI_BaudRatePrescaler_2); #else single_gpio_init(BOARD_COMM_CS_PORT, BOARD_COMM_CS_PIN_SRC, BOARD_COMM_CS_PIN, BOARD_COMM_CS_ALTERNATE, BOARD_COMM_CS_TYPE, GPIO_OType_PP, GPIO_PuPd_NOPULL); spi_init(&boardCommSpiInitStruct, &boardCommDmaInitStruct, BOARD_COMM_SPI, SPI_Mode_Slave, BOARD_COMM_CS_TYPE, SPI_CPOL_Low, SPI_CPHA_1Edge, SPI_BaudRatePrescaler_2); @@ -66,7 +66,7 @@ int parse_imuf_command(volatile imufCommand_t* command) void start_listening(void) { - spiDoneFlag = 0; //flag for use during runtime to limit ISR overhead, might be able to remove this completely + spiDoneFlag = 0; //flag for use during runtime to limit ISR overhead, might be able to remove this completely append_crc_to_data_v((volatile uint32_t *)bcTxPtr, 11); //11 will put the crc at the location it needs to be which is imufCommand.crc //this takes 0.78us to run cleanup_spi(BOARD_COMM_SPI, BOARD_COMM_TX_DMA, BOARD_COMM_RX_DMA, BOARD_COMM_SPI_RST_MSK); @@ -90,8 +90,8 @@ void board_comm_spi_callback_function(void) { //command checks out //run the command and generate the reply - run_command(&bcRx,&bcTx); - + run_command(&bcRx,&bcTx); + } else if ( (bcTx.command == BC_IMUF_SETUP) ) //we just replied that we got proper setup commands, let's activate them now { @@ -103,7 +103,7 @@ void board_comm_spi_callback_function(void) reset_matrix(); //reset oreintation matrix in case it's been changes reset_loop(); //set loop speed } - else + else { //bad command, listen for another bcTx.command = BC_IMUF_LISTENING; @@ -148,9 +148,22 @@ static void run_command(volatile imufCommand_t* command, volatile imufCommand_t* filterConfig.i_pitch_q = (int16_t)(command->param3 & 0xFFFF); filterConfig.i_yaw_q = (int16_t)(command->param4 >> 16); gyroSettingsConfig.orientation = (uint32_t)((uint16_t)(command->param8 & 0xFFFF)); + filterConfig.i_roll_lpf_hz = (int16_t)(command->param4 & 0xFFFF); + filterConfig.i_pitch_lpf_hz = (int16_t)(command->param5 >> 16); + filterConfig.i_yaw_lpf_hz = (int16_t)(command->param5 & 0xFFFF); gyroSettingsConfig.smallX = (int32_t) ((int16_t)(command->param8 >> 16)); gyroSettingsConfig.smallY = (int32_t) ((int16_t)(command->param9 & 0xFFFF)); gyroSettingsConfig.smallZ = (int32_t) ((int16_t)(command->param9 >> 16)); + filterConfig.sharpness = (int16_t) ((int16_t)(command->param10 >> 16)); + if (!filterConfig.sharpness) + { + filterConfig.sharpness = 2500; + } + filterConfig.acc_lpf_hz = (int16_t)(command->param10 & 0xFFFF); + if (!filterConfig.acc_lpf_hz) + { + filterConfig.acc_lpf_hz = 256; + } memset((uint8_t *)reply, 0, sizeof(imufCommand_t)); reply->command = BC_IMUF_SETUP; @@ -166,4 +179,4 @@ static void run_command(volatile imufCommand_t* command, volatile imufCommand_t* default: break; } -} \ No newline at end of file +} diff --git a/src/filter/biquad.c b/src/filter/biquad.c index 49aca4a3..393cbfc8 100644 --- a/src/filter/biquad.c +++ b/src/filter/biquad.c @@ -11,41 +11,42 @@ void biquad_init(float filterCutFreq, biquad_axis_state_t *state, float refreshR float dbGain = 4.0; - samplingRate = (1 / refreshRateSeconds); + samplingRate = (1.0f / refreshRateSeconds); - omega = 2 * (float)M_PI_FLOAT * (float) filterCutFreq / samplingRate; + omega = 2.0f * (float)M_PI_FLOAT * (float) filterCutFreq / samplingRate; sn = (float)sinf((float)omega); cs = (float)cosf((float)omega); - alpha = sn * (float)sinf( (float)((float)M_LN2_FLOAT / 2 * (float)bandwidth * (omega / sn)) ); + alpha = sn * (float)sinf( (float)((float)M_LN2_FLOAT / 2.0f * (float)bandwidth * (omega / sn)) ); (void)(beta); switch (filterType) { case FILTER_TYPE_LOWPASS: - b0 = (1 - cs) /2; - b1 = 1 - cs; - b2 = (1 - cs) /2; - a0 = 1 + alpha; - a1 = -2 * cs; - a2 = 1 - alpha; + b0 = (1.0f - cs) *0.5f; + b1 = 1.0f - cs; + b2 = (1.0f - cs) *0.5f; + a0 = 1.0f + alpha; + a1 = -2.0f * cs; + a2 = 1.0f - alpha; break; case FILTER_TYPE_NOTCH: - b0 = 1; - b1 = -2 * cs; - b2 = 1; - a0 = 1 + alpha; - a1 = -2 * cs; - a2 = 1 - alpha; + b0 = 1.0f; + b1 = -2.0f * cs; + b2 = 1.0f; + a0 = 1.0f + alpha; + a1 = -2.0f * cs; + a2 = 1.0f - alpha; break; } //don't let these states be used until they're updated __disable_irq(); - state->a0 = b0 / a0; - state->a1 = b1 / a0; - state->a2 = b2 / a0; - state->a3 = a1 / a0; - state->a4 = a2 / a0; + const float a0r = 1.0f / a0; + state->a0 = b0 * a0r; + state->a1 = b1 * a0r; + state->a2 = b2 * a0r; + state->a3 = a1 * a0r; + state->a4 = a2 * a0r; __enable_irq(); } diff --git a/src/filter/filter.c b/src/filter/filter.c index 981d7ac7..ce62747f 100644 --- a/src/filter/filter.c +++ b/src/filter/filter.c @@ -4,19 +4,44 @@ #include "kalman.h" #include "biquad.h" -volatile filter_config_t filterConfig = { - DEFAULT_ROLL_Q, - DEFAULT_PITCH_Q, - DEFAULT_YAW_Q, - MIN_WINDOW_SIZE, - (float)DEFAULT_ROLL_Q, - (float)DEFAULT_PITCH_Q, - (float)DEFAULT_YAW_Q, - (float)BASE_LPF_HZ, - (float)BASE_LPF_HZ, - (float)BASE_LPF_HZ, +volatile filter_config_t filterConfig = +{ + DEFAULT_ROLL_Q, + DEFAULT_PITCH_Q, + DEFAULT_YAW_Q, + MIN_WINDOW_SIZE, + + (float)DEFAULT_ROLL_Q, + (float)DEFAULT_PITCH_Q, + (float)DEFAULT_YAW_Q, + + (float)BASE_LPF_HZ, + (float)BASE_LPF_HZ, + (float)BASE_LPF_HZ, + + 40.0f, + + BASE_LPF_HZ, + BASE_LPF_HZ, + BASE_LPF_HZ, + 100, }; +// PT1 Low Pass filter +bool acc_filter_initialized = false; +typedef struct pt1Filter_s { + float state; + float k; +} pt1Filter_t; + +pt1Filter_t ax_filter; +pt1Filter_t ay_filter; +pt1Filter_t az_filter; + +float pt1FilterGain(uint16_t f_cut, float dT); +void pt1FilterInit(pt1Filter_t *filter, float k, float val); +float pt1FilterApply(pt1Filter_t *filter, float input); + biquad_state_t lpfFilterStateRate; volatile uint32_t setPointNew; volatile axisDataInt_t setPointInt; @@ -24,6 +49,8 @@ volatile axisData_t oldSetPoint; volatile axisData_t setPoint; volatile int allowFilterInit = 1; +float sharpness; + void allow_filter_init(void) { allowFilterInit = 1; @@ -36,13 +63,24 @@ void filter_biquad_init(float freq, biquad_axis_state_t *filterState) void filter_init(void) { +//#define ACC_CUTOFF (60.0f) +#define ACC_READ_RATE (1.0f / 1000.0f) + memset((uint32_t *)&setPoint, 0, sizeof(axisData_t)); memset((uint32_t *)&oldSetPoint, 0, sizeof(axisData_t)); memset((uint32_t *)&setPointInt, 0, sizeof(axisDataInt_t)); kalman_init(); - filter_biquad_init(BASE_LPF_HZ, &(lpfFilterStateRate.x)); - filter_biquad_init(BASE_LPF_HZ, &(lpfFilterStateRate.y)); - filter_biquad_init(BASE_LPF_HZ, &(lpfFilterStateRate.z)); + filter_biquad_init(filterConfig.i_roll_lpf_hz, &(lpfFilterStateRate.x)); + filter_biquad_init(filterConfig.i_pitch_lpf_hz, &(lpfFilterStateRate.y)); + filter_biquad_init(filterConfig.i_yaw_lpf_hz, &(lpfFilterStateRate.z)); + + // set imuf acc cutoff frequency + const float k = pt1FilterGain((float)filterConfig.acc_lpf_hz, ACC_READ_RATE); + pt1FilterInit(&ax_filter, k, 0.0f); + pt1FilterInit(&ay_filter, k, 0.0f); + pt1FilterInit(&az_filter, k, 0.0f); + + sharpness = (float)filterConfig.sharpness / 250.0f; } void filter_data(volatile axisData_t *gyroRateData, volatile axisData_t *gyroAccData, float gyroTempData, filteredData_t *filteredData) @@ -71,22 +109,6 @@ void filter_data(volatile axisData_t *gyroRateData, volatile axisData_t *gyroAcc if (setPointNew) { setPointNew = 0; - if (setPoint.x != 0.0f && oldSetPoint.x != setPoint.x) - { - filterConfig.roll_lpf_hz = CONSTRAIN(BASE_LPF_HZ * ABS(1.0f - (setPoint.x / filteredData->rateData.x)), 10.0f, 500.0f); - filter_biquad_init(filterConfig.roll_lpf_hz, &(lpfFilterStateRate.x)); - } - if (setPoint.y != 0.0f && oldSetPoint.y != setPoint.y) - { - filterConfig.pitch_lpf_hz = CONSTRAIN(BASE_LPF_HZ * ABS(1.0f - (setPoint.y / filteredData->rateData.y)), 10.0f, 500.0f); - filter_biquad_init(filterConfig.pitch_lpf_hz, &(lpfFilterStateRate.y)); - } - if (setPoint.z != 0.0f && oldSetPoint.z != setPoint.z) - { - filterConfig.yaw_lpf_hz = CONSTRAIN(BASE_LPF_HZ * ABS(1.0f - (setPoint.z / filteredData->rateData.z)), 10.0f, 500.0f); - filter_biquad_init(filterConfig.yaw_lpf_hz, &(lpfFilterStateRate.z)); - } - memcpy((uint32_t *)&oldSetPoint, (uint32_t *)&setPoint, sizeof(axisData_t)); } //no need to filter ACC is used in quaternions filteredData->accData.x = gyroAccData->x; @@ -95,4 +117,39 @@ void filter_data(volatile axisData_t *gyroRateData, volatile axisData_t *gyroAcc //should filter this filteredData->tempC = gyroTempData; -} \ No newline at end of file +} + +float pt1FilterGain(uint16_t f_cut, float dT) +{ + float RC = 1 / ( 2 * M_PI_FLOAT * f_cut); + return dT / (RC + dT); +} + +void pt1FilterInit(pt1Filter_t *filter, float k, float val) +{ + filter->state = val; + filter->k = k; +} + +float pt1FilterApply(pt1Filter_t *filter, float input) +{ + filter->state = filter->state + filter->k * (input - filter->state); + return filter->state; +} + +void filter_acc(volatile axisData_t *gyroAccData) +{ + if (!acc_filter_initialized) + { + acc_filter_initialized = true; + ax_filter.state = gyroAccData->x; + ay_filter.state = gyroAccData->y; + az_filter.state = gyroAccData->z; + } + else + { + gyroAccData->x = pt1FilterApply(&ax_filter, gyroAccData->x); + gyroAccData->y = pt1FilterApply(&ay_filter, gyroAccData->y); + gyroAccData->z = pt1FilterApply(&az_filter, gyroAccData->z); + } +} diff --git a/src/filter/filter.h b/src/filter/filter.h index a6894b9f..88886db1 100644 --- a/src/filter/filter.h +++ b/src/filter/filter.h @@ -19,13 +19,18 @@ typedef struct filter_config uint16_t i_roll_q; uint16_t i_pitch_q; uint16_t i_yaw_q; - uint16_t w; + uint16_t w; float roll_q; float pitch_q; float yaw_q; float pitch_lpf_hz; float roll_lpf_hz; float yaw_lpf_hz; + uint16_t acc_lpf_hz; + uint16_t i_roll_lpf_hz; + uint16_t i_pitch_lpf_hz; + uint16_t i_yaw_lpf_hz; + uint16_t sharpness; } filter_config_t; extern volatile filter_config_t filterConfig; @@ -35,3 +40,5 @@ extern volatile axisData_t setPoint; extern void allow_filter_init(void); extern void filter_init(void); extern void filter_data(volatile axisData_t* gyroRateData, volatile axisData_t* gyroAccData, float gyroTempData, filteredData_t* filteredData); + +void filter_acc(volatile axisData_t *gyroAccData); diff --git a/src/filter/kalman.c b/src/filter/kalman.c index a3b27f29..0624f84b 100644 --- a/src/filter/kalman.c +++ b/src/filter/kalman.c @@ -4,25 +4,25 @@ #include "filter.h" variance_t varStruct; -kalman_t kalmanFilterStateRate[3]; +kalman_t kalmanFilterStateRate[3]; - -void init_kalman(kalman_t *filter, float q) +void init_kalman(kalman_t *filter, float q, float sharpness) { memset(filter, 0, sizeof(kalman_t)); filter->q = q * 0.001f; //add multiplier to make tuning easier filter->r = 88.0f; //seeding R at 88.0f filter->p = 30.0f; //seeding P at 30.0f filter->e = 1.0f; + filter->s = sharpness * 0.01f; } void kalman_init(void) { setPointNew = 0; memset(&varStruct, 0, sizeof(varStruct)); - init_kalman(&kalmanFilterStateRate[ROLL], filterConfig.roll_q); - init_kalman(&kalmanFilterStateRate[PITCH], filterConfig.pitch_q); - init_kalman(&kalmanFilterStateRate[YAW], filterConfig.yaw_q); + init_kalman(&kalmanFilterStateRate[ROLL], filterConfig.roll_q, filterConfig.sharpness); + init_kalman(&kalmanFilterStateRate[PITCH], filterConfig.pitch_q, filterConfig.sharpness); + init_kalman(&kalmanFilterStateRate[YAW], filterConfig.yaw_q, filterConfig.sharpness); varStruct.inverseN = 1.0f/filterConfig.w; } @@ -30,77 +30,95 @@ void kalman_init(void) #pragma GCC optimize("O3") void update_kalman_covariance(volatile axisData_t *gyroRateData) { - varStruct.xWindow[ varStruct.windex] = gyroRateData->x; - varStruct.yWindow[ varStruct.windex] = gyroRateData->y; - varStruct.zWindow[ varStruct.windex] = gyroRateData->z; - - varStruct.xSumMean += varStruct.xWindow[ varStruct.windex]; - varStruct.ySumMean += varStruct.yWindow[ varStruct.windex]; - varStruct.zSumMean += varStruct.zWindow[ varStruct.windex]; - varStruct.xSumVar = varStruct.xSumVar + ( varStruct.xWindow[ varStruct.windex] * varStruct.xWindow[ varStruct.windex]); - varStruct.ySumVar = varStruct.ySumVar + ( varStruct.yWindow[ varStruct.windex] * varStruct.yWindow[ varStruct.windex]); - varStruct.zSumVar = varStruct.zSumVar + ( varStruct.zWindow[ varStruct.windex] * varStruct.zWindow[ varStruct.windex]); - varStruct.xySumCoVar = varStruct.xySumCoVar + ( varStruct.xWindow[ varStruct.windex] * varStruct.yWindow[ varStruct.windex]); - varStruct.xzSumCoVar = varStruct.xzSumCoVar + ( varStruct.xWindow[ varStruct.windex] * varStruct.zWindow[ varStruct.windex]); - varStruct.yzSumCoVar = varStruct.yzSumCoVar + ( varStruct.yWindow[ varStruct.windex] * varStruct.zWindow[ varStruct.windex]); - varStruct.windex++; + varStruct.xWindow[ varStruct.windex] = gyroRateData->x; + varStruct.yWindow[ varStruct.windex] = gyroRateData->y; + varStruct.zWindow[ varStruct.windex] = gyroRateData->z; + + varStruct.xSumMean += varStruct.xWindow[ varStruct.windex]; + varStruct.ySumMean += varStruct.yWindow[ varStruct.windex]; + varStruct.zSumMean += varStruct.zWindow[ varStruct.windex]; + + // calc varianceElement + float xvarianceElement = varStruct.xWindow[ varStruct.windex] - varStruct.xMean; + float yvarianceElement = varStruct.yWindow[ varStruct.windex] - varStruct.yMean; + float zvarianceElement = varStruct.zWindow[ varStruct.windex] - varStruct.zMean; + + xvarianceElement = xvarianceElement * xvarianceElement; + yvarianceElement = yvarianceElement * yvarianceElement; + zvarianceElement = zvarianceElement * zvarianceElement; + + varStruct.xSumVar += xvarianceElement; + varStruct.ySumVar += yvarianceElement; + varStruct.zSumVar += zvarianceElement; + + varStruct.xvarianceWindow[varStruct.windex] = xvarianceElement; + varStruct.yvarianceWindow[varStruct.windex] = yvarianceElement; + varStruct.zvarianceWindow[varStruct.windex] = zvarianceElement; + + varStruct.windex++; if ( varStruct.windex >= filterConfig.w) { varStruct.windex = 0; } - varStruct.xSumMean -= varStruct.xWindow[ varStruct.windex]; - varStruct.ySumMean -= varStruct.yWindow[ varStruct.windex]; - varStruct.zSumMean -= varStruct.zWindow[ varStruct.windex]; - varStruct.xSumVar = varStruct.xSumVar - ( varStruct.xWindow[ varStruct.windex] * varStruct.xWindow[ varStruct.windex]); - varStruct.ySumVar = varStruct.ySumVar - ( varStruct.yWindow[ varStruct.windex] * varStruct.yWindow[ varStruct.windex]); - varStruct.zSumVar = varStruct.zSumVar - ( varStruct.zWindow[ varStruct.windex] * varStruct.zWindow[ varStruct.windex]); - varStruct.xySumCoVar = varStruct.xySumCoVar - ( varStruct.xWindow[ varStruct.windex] * varStruct.yWindow[ varStruct.windex]); - varStruct.xzSumCoVar = varStruct.xzSumCoVar - ( varStruct.xWindow[ varStruct.windex] * varStruct.zWindow[ varStruct.windex]); - varStruct.yzSumCoVar = varStruct.yzSumCoVar - ( varStruct.yWindow[ varStruct.windex] * varStruct.zWindow[ varStruct.windex]); - - varStruct.xMean = varStruct.xSumMean * varStruct.inverseN; - varStruct.yMean = varStruct.ySumMean * varStruct.inverseN; - varStruct.zMean = varStruct.zSumMean * varStruct.inverseN; - - varStruct.xVar = ABS(varStruct.xSumVar * varStruct.inverseN - ( varStruct.xMean * varStruct.xMean)); - varStruct.yVar = ABS(varStruct.ySumVar * varStruct.inverseN - ( varStruct.yMean * varStruct.yMean)); - varStruct.zVar = ABS(varStruct.zSumVar * varStruct.inverseN - ( varStruct.zMean * varStruct.zMean)); - varStruct.xyCoVar = ABS(varStruct.xySumCoVar * varStruct.inverseN - ( varStruct.xMean * varStruct.yMean)); - varStruct.xzCoVar = ABS(varStruct.xzSumCoVar * varStruct.inverseN - ( varStruct.xMean * varStruct.zMean)); - varStruct.yzCoVar = ABS(varStruct.yzSumCoVar * varStruct.inverseN - ( varStruct.yMean * varStruct.zMean)); + + varStruct.xSumMean -= varStruct.xWindow[ varStruct.windex]; + varStruct.ySumMean -= varStruct.yWindow[ varStruct.windex]; + varStruct.zSumMean -= varStruct.zWindow[ varStruct.windex]; + varStruct.xSumVar -= varStruct.xvarianceWindow[varStruct.windex]; + varStruct.ySumVar -= varStruct.yvarianceWindow[varStruct.windex]; + varStruct.zSumVar -= varStruct.zvarianceWindow[varStruct.windex]; + + //New mean + varStruct.xMean = varStruct.xSumMean * varStruct.inverseN; + varStruct.yMean = varStruct.ySumMean * varStruct.inverseN; + varStruct.zMean = varStruct.zSumMean * varStruct.inverseN; + varStruct.xVar = varStruct.xSumVar * varStruct.inverseN; + varStruct.yVar = varStruct.ySumVar * varStruct.inverseN; + varStruct.zVar = varStruct.zSumVar * varStruct.inverseN; float squirt; - arm_sqrt_f32(varStruct.xVar + varStruct.xyCoVar + varStruct.xzCoVar, &squirt); + arm_sqrt_f32(varStruct.xVar, &squirt); kalmanFilterStateRate[ROLL].r = squirt * VARIANCE_SCALE; - arm_sqrt_f32(varStruct.yVar + varStruct.xyCoVar + varStruct.yzCoVar, &squirt); + arm_sqrt_f32(varStruct.yVar, &squirt); kalmanFilterStateRate[PITCH].r = squirt * VARIANCE_SCALE; - arm_sqrt_f32(varStruct.zVar + varStruct.yzCoVar + varStruct.xzCoVar, &squirt); - kalmanFilterStateRate[YAW].r = squirt * VARIANCE_SCALE; + arm_sqrt_f32(varStruct.zVar, &squirt); + kalmanFilterStateRate[YAW].r = squirt * VARIANCE_SCALE; } inline float kalman_process(kalman_t* kalmanState, volatile float input, volatile float target) { - //project the state ahead using acceleration - kalmanState->x += (kalmanState->x - kalmanState->lastX); - - //figure out how much to boost or reduce our error in the estimate based on setpoint target. - //this should be close to 0 as we approach the sepoint and really high the futher away we are from the setpoint. - //update last state - kalmanState->lastX = kalmanState->x; - - if (target != 0.0f) { - kalmanState->e = ABS(1.0f - (target/kalmanState->lastX)); - } else { - kalmanState->e = 1.0f; + //project the state ahead using acceleration + kalmanState->x += (kalmanState->x - kalmanState->lastX); + + //figure out how much to boost or reduce our error in the estimate based on setpoint target. + //this should be close to 0 as we approach the sepoint and really high the futher away we are from the setpoint. + //update last state + kalmanState->lastX = kalmanState->x; + + if (kalmanState->s != 0.0f) { + float average = fabsf(target + kalmanState->lastX) * 0.5f; + + if (average > 10.0f) + { + float error = fabsf(target - kalmanState->lastX); + float ratio = error / average; + kalmanState->e = kalmanState->s * powf(ratio, 3.0f); //"ratio" power 3 and multiply by a gain } - //prediction update - kalmanState->p = kalmanState->p + (kalmanState->q * kalmanState->e); + kalmanState->p = kalmanState->p + (kalmanState->q + kalmanState->e); - //measurement update - kalmanState->k = kalmanState->p / (kalmanState->p + kalmanState->r); - kalmanState->x += kalmanState->k * (input - kalmanState->x); - kalmanState->p = (1.0f - kalmanState->k) * kalmanState->p; - return kalmanState->x; + } else { + if (kalmanState->lastX != 0.0f) + { + kalmanState->e = fabsf(1.0f - (target / kalmanState->lastX)); + } + //prediction update + kalmanState->p = kalmanState->p + (kalmanState->q * kalmanState->e); + } + //measurement update + kalmanState->k = kalmanState->p / (kalmanState->p + kalmanState->r); + kalmanState->x += kalmanState->k * (input - kalmanState->x); + kalmanState->p = (1.0f - kalmanState->k) * kalmanState->p; + return kalmanState->x; } void kalman_update(volatile axisData_t* input, filteredData_t* output) diff --git a/src/filter/kalman.h b/src/filter/kalman.h index cdee0c2f..11f8569e 100644 --- a/src/filter/kalman.h +++ b/src/filter/kalman.h @@ -3,12 +3,12 @@ #include "gyro.h" #include "filter.h" -#define MAX_WINDOW_SIZE 1024 +#define MAX_WINDOW_SIZE 512 #define DEF_WINDOW_SIZE 32 #define MIN_WINDOW_SIZE 6 // #define VARIANCE_SCALE 0.001 -#define VARIANCE_SCALE 0.3333333f +#define VARIANCE_SCALE 0.67f typedef struct kalman { @@ -18,7 +18,8 @@ typedef struct kalman float k; //kalman gain float x; //state float lastX; //previous state - float e; + float e; //multiplier or adder to q + float s; //changes how dynamic e is } kalman_t; typedef struct variance @@ -26,14 +27,14 @@ typedef struct variance float xVar; float yVar; float zVar; - float xyCoVar; - float xzCoVar; - float yzCoVar; uint32_t windex; float xWindow[MAX_WINDOW_SIZE]; float yWindow[MAX_WINDOW_SIZE]; float zWindow[MAX_WINDOW_SIZE]; + float xvarianceWindow[MAX_WINDOW_SIZE]; + float yvarianceWindow[MAX_WINDOW_SIZE]; + float zvarianceWindow[MAX_WINDOW_SIZE]; float xSumMean; float ySumMean; @@ -46,9 +47,6 @@ typedef struct variance float xSumVar; float ySumVar; float zSumVar; - float xySumCoVar; - float xzSumCoVar; - float yzSumCoVar; float inverseN; } variance_t; diff --git a/src/gyro/gyro.c b/src/gyro/gyro.c index 77f89340..ff809ed3 100644 --- a/src/gyro/gyro.c +++ b/src/gyro/gyro.c @@ -276,6 +276,7 @@ void gyro_int_to_float(gyroFrame_t* gyroRxFrame) //RoomTemp_Offset = 25ºC //gyroTempMultiplier is gyro temp in C apply_gyro_acc_rotation(&rawAccData); + filter_acc(&rawAccData); } //f*f+f is one operation on FPU @@ -403,6 +404,7 @@ void increment_acc_tracker(void) void fire_spi_send_ready(void) { + static volatile uint8_t sendBuffer[60]; volatile uint8_t* memptr8 = (uint8_t*)&filteredData.rateData; volatile uint32_t* memptr32 = (uint32_t*)&filteredData.rateData; @@ -415,7 +417,6 @@ void fire_spi_send_ready(void) //everyother is 16KHz if (everyOther-- <= 0) { - append_crc_to_data_v( memptr32, (boardCommState.commMode >> 2)-1); everyOther = loopDivider; //reset khz counter //check if spi is done if not, return @@ -438,8 +439,12 @@ void fire_spi_send_ready(void) //this takes 0.78us to run cleanup_spi(BOARD_COMM_SPI, BOARD_COMM_TX_DMA, BOARD_COMM_RX_DMA, BOARD_COMM_SPI_RST_MSK); //reset sync //send the filtered data to the device + //copy send buffer + memcpy((uint8_t *)sendBuffer, (uint8_t *)memptr8, boardCommState.commMode); + //calc crc for send buffer + append_crc_to_data_v( (uint32_t *)sendBuffer, (boardCommState.commMode >> 2)-1); spiDoneFlag = 0; //flag for use during runtime to limit ISR overhead, might be able to remove this completely - spi_fire_dma(BOARD_COMM_SPI, BOARD_COMM_TX_DMA, BOARD_COMM_RX_DMA, &boardCommDmaInitStruct, (uint32_t *)&(boardCommState.bufferSize), memptr8, bcRxPtr); + spi_fire_dma(BOARD_COMM_SPI, BOARD_COMM_TX_DMA, BOARD_COMM_RX_DMA, &boardCommDmaInitStruct, (uint32_t *)&(boardCommState.bufferSize), sendBuffer, bcRxPtr); gpio_write_pin(BOARD_COMM_DATA_RDY_PORT, BOARD_COMM_DATA_RDY_PIN, 1); //a quick spike for EXTI gpio_write_pin(BOARD_COMM_DATA_RDY_PORT, BOARD_COMM_DATA_RDY_PIN, 0); //a quick spike for EXTI } @@ -461,4 +466,4 @@ void gyro_init(void) gyroCalibrationTrim.y = 0.0f; gyroCalibrationTrim.z = 0.0f; gyro_device_init(&gyro_read_done); -} \ No newline at end of file +} diff --git a/src/gyro/gyro_device.c b/src/gyro/gyro_device.c index a36f284c..ed5254de 100644 --- a/src/gyro/gyro_device.c +++ b/src/gyro/gyro_device.c @@ -3,7 +3,7 @@ #include "gyro_device.h" //multiple configs can go here, just need one right now -const gyro_device_config_t gyroConfig = {1, 0, INVENS_CONST_GYRO_FCB_32_8800, 0, INVENS_CONST_ACC_FCB_ENABLE, 8}; +const gyro_device_config_t gyroConfig = {1, 0, INVENS_CONST_GYRO_FCB_32_3600, 0, INVENS_CONST_ACC_FCB_ENABLE, 32}; gyroFrame_t gyroRxFrame; gyroFrame_t gyroTxFrame; diff --git a/src/stm32/includes.h b/src/stm32/includes.h index 0f06c583..84b1795a 100644 --- a/src/stm32/includes.h +++ b/src/stm32/includes.h @@ -3,6 +3,7 @@ //general C includes #include #include +#include #include #include diff --git a/src/version.h b/src/version.h index b1a0f73d..ba08b6da 100644 --- a/src/version.h +++ b/src/version.h @@ -3,5 +3,4 @@ #define HARDWARE_VERSION 101 #define BOOTLOADER_VERSION 101 -#define FIRMWARE_VERSION 110 - +#define FIRMWARE_VERSION 250 diff --git a/vendor/CMSIS_std/DSP_Lib/Source/TransformFunctions/arm_bitreversal2.S b/vendor/CMSIS_std/DSP_Lib/Source/TransformFunctions/arm_bitreversal2.S index 7211d0f4..08c5d759 100644 --- a/vendor/CMSIS_std/DSP_Lib/Source/TransformFunctions/arm_bitreversal2.S +++ b/vendor/CMSIS_std/DSP_Lib/Source/TransformFunctions/arm_bitreversal2.S @@ -1,8 +1,8 @@ ;/* ---------------------------------------------------------------------- ;* Copyright (C) 2010-2014 ARM Limited. All rights reserved. ;* -;* $Date: 12. March 2014 -;* $Revision: V1.4.4 +;* $Date: 19. March 2015 +;* $Revision: V.1.4.5 ;* ;* Project: CMSIS DSP Library ;* Title: arm_bitreversal2.S