Non dynamic biquad/refactored sharpness

.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/[email protected]
+      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

.gitignore

@@ -5,3 +5,4 @@ redirection
 redirection.junk
 kalman/test
 .exe
+gcc-arm*

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
 ```

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),

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;
     }
-}
+}

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();
 }