diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml deleted file mode 100644 index 2b995a1b..00000000 --- a/.github/workflows/build.yml +++ /dev/null @@ -1,19 +0,0 @@ -name: Build IMU-F -on: [push, pull_request] -jobs: - build: - runs-on: ubuntu-latest - steps: - - uses: actions/checkout@v2 - - uses: fiam/arm-none-eabi-gcc@v1 - with: - release: '9-2019-q4' # The arm-none-eabi-gcc release to use. - - uses: actions/setup-python@v1 - - name: Make - run: | - ./make.py -T=F3 - - name: Upload artifcats - uses: actions/upload-artifact@v2-preview - with: - name: firmware - path: output/F3.bin diff --git a/README.md b/README.md index 69cab931..afefb884 100644 --- a/README.md +++ b/README.md @@ -13,3 +13,5 @@ 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 ``` +After getting everything that you need to compile, run this command to compile the IMUF +python make.py -T F3 \ No newline at end of file diff --git a/src/board_comm/board_comm.c b/src/board_comm/board_comm.c index 62aa1adf..50ad96ba 100644 --- a/src/board_comm/board_comm.c +++ b/src/board_comm/board_comm.c @@ -151,10 +151,10 @@ static void run_command(volatile imufCommand_t* command, volatile imufCommand_t* 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)(command->param10 >> 16); + 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 = 35; diff --git a/src/filter/biquad.c b/src/filter/biquad.c index 4d07db31..393cbfc8 100644 --- a/src/filter/biquad.c +++ b/src/filter/biquad.c @@ -23,9 +23,9 @@ void biquad_init(float filterCutFreq, biquad_axis_state_t *state, float refreshR switch (filterType) { case FILTER_TYPE_LOWPASS: - b0 = (1.0f - cs) * 0.5f; + b0 = (1.0f - cs) *0.5f; b1 = 1.0f - cs; - b2 = (1.0f - cs) * 0.5f; + b2 = (1.0f - cs) *0.5f; a0 = 1.0f + alpha; a1 = -2.0f * cs; a2 = 1.0f - alpha; @@ -41,12 +41,12 @@ void biquad_init(float filterCutFreq, biquad_axis_state_t *state, float refreshR } //don't let these states be used until they're updated __disable_irq(); - 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; + 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(); } @@ -78,4 +78,4 @@ float biquad_update(float sample, biquad_axis_state_t *state) return result; } -#pragma GCC pop_options +#pragma GCC pop_options \ No newline at end of file diff --git a/src/filter/filter.c b/src/filter/filter.c index 6955bd8e..578a18d7 100644 --- a/src/filter/filter.c +++ b/src/filter/filter.c @@ -6,25 +6,25 @@ volatile filter_config_t filterConfig = { - DEFAULT_ROLL_Q, - DEFAULT_PITCH_Q, - DEFAULT_YAW_Q, - MIN_WINDOW_SIZE, + 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)DEFAULT_ROLL_Q, + (float)DEFAULT_PITCH_Q, + (float)DEFAULT_YAW_Q, - (float)BASE_LPF_HZ, - (float)BASE_LPF_HZ, - (float)BASE_LPF_HZ, + (float)BASE_LPF_HZ, + (float)BASE_LPF_HZ, + (float)BASE_LPF_HZ, - 40.0f, + 40.0f, - BASE_LPF_HZ, - BASE_LPF_HZ, - BASE_LPF_HZ, - 100, + BASE_LPF_HZ, + BASE_LPF_HZ, + BASE_LPF_HZ, + 100, }; // PT1 Low Pass filter @@ -80,7 +80,7 @@ void filter_init(void) pt1FilterInit(&ay_filter, k, 0.0f); pt1FilterInit(&az_filter, k, 0.0f); - sharpness = filterConfig.sharpness / 250.0f; + sharpness = (float)filterConfig.sharpness / 250.0f; } void filter_data(volatile axisData_t *gyroRateData, volatile axisData_t *gyroAccData, float gyroTempData, filteredData_t *filteredData) @@ -112,9 +112,10 @@ void filter_data(volatile axisData_t *gyroRateData, volatile axisData_t *gyroAcc float errorMultiplierZ = ABS(setPoint.z - filteredData->rateData.z) * sharpness; // give a boost to the setpoint, used to caluclate the filter cutoff, based on the error and setpoint/gyrodata - errorMultiplierX = CONSTRAIN(errorMultiplierX * ABS(1.0f - (setPoint.x / filteredData->rateData.x)) + 1.0f, 1.0f, 50.0f); - errorMultiplierY = CONSTRAIN(errorMultiplierY * ABS(1.0f - (setPoint.y / filteredData->rateData.y)) + 1.0f, 1.0f, 50.0f); - errorMultiplierZ = CONSTRAIN(errorMultiplierZ * ABS(1.0f - (setPoint.z / filteredData->rateData.z)) + 1.0f, 1.0f, 50.0f); + + errorMultiplierX = CONSTRAIN(errorMultiplierX * ABS(1.0f - (setPoint.x / filteredData->rateData.x)) + 1.0f, 1.0f, 10.0f); + errorMultiplierY = CONSTRAIN(errorMultiplierY * ABS(1.0f - (setPoint.y / filteredData->rateData.y)) + 1.0f, 1.0f, 10.0f); + errorMultiplierZ = CONSTRAIN(errorMultiplierZ * ABS(1.0f - (setPoint.z / filteredData->rateData.z)) + 1.0f, 1.0f, 10.0f); if (setPointNew) @@ -122,17 +123,17 @@ void filter_data(volatile axisData_t *gyroRateData, volatile axisData_t *gyroAcc setPointNew = 0; if (setPoint.x != 0.0f && oldSetPoint.x != setPoint.x) { - filterConfig.roll_lpf_hz = CONSTRAIN(10.0f * ABS(1.0f - ((setPoint.x * errorMultiplierX) / filteredData->rateData.x)), 10.0f, 500.0f); + filterConfig.roll_lpf_hz = CONSTRAIN((float)filterConfig.i_roll_lpf_hz * ABS(1.0f - ((setPoint.x * errorMultiplierX) / 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(10.0f * ABS(1.0f - ((setPoint.y * errorMultiplierY) / filteredData->rateData.y)), 10.0f, 500.0f); + filterConfig.pitch_lpf_hz = CONSTRAIN((float)filterConfig.i_pitch_lpf_hz * ABS(1.0f - ((setPoint.y * errorMultiplierY) / 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(10.0f * ABS(1.0f - ((setPoint.z * errorMultiplierZ) / filteredData->rateData.z)), 10.0f, 500.0f); + filterConfig.yaw_lpf_hz = CONSTRAIN((float)filterConfig.i_yaw_lpf_hz * ABS(1.0f - ((setPoint.z * errorMultiplierZ) / 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)); diff --git a/src/filter/kalman.c b/src/filter/kalman.c index 34897274..30dcc88c 100644 --- a/src/filter/kalman.c +++ b/src/filter/kalman.c @@ -29,51 +29,42 @@ 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.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.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.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.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.xMean = varStruct.xSumMean * varStruct.inverseN; - varStruct.yMean = varStruct.ySumMean * varStruct.inverseN; - varStruct.zMean = varStruct.zSumMean * varStruct.inverseN; + 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.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)); 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); + arm_sqrt_f32(varStruct.zVar, &squirt); kalmanFilterStateRate[YAW].r = squirt * VARIANCE_SCALE; } diff --git a/src/filter/kalman.h b/src/filter/kalman.h index 78c681af..38310bad 100644 --- a/src/filter/kalman.h +++ b/src/filter/kalman.h @@ -26,9 +26,6 @@ typedef struct variance float xVar; float yVar; float zVar; - float xyCoVar; - float xzCoVar; - float yzCoVar; uint32_t windex; float xWindow[MAX_WINDOW_SIZE]; @@ -46,9 +43,6 @@ typedef struct variance float xSumVar; float ySumVar; float zSumVar; - float xySumCoVar; - float xzSumCoVar; - float yzSumCoVar; float inverseN; } variance_t; diff --git a/src/version.h b/src/version.h index 7b1a4c65..7f7ae610 100644 --- a/src/version.h +++ b/src/version.h @@ -3,4 +3,4 @@ #define HARDWARE_VERSION 101 #define BOOTLOADER_VERSION 101 -#define FIRMWARE_VERSION 227 +#define FIRMWARE_VERSION 230