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

Merge branch 'dev_fw_5_03' into tiltback_config

This commit is contained in:
Mitch Lustig 2021-08-19 13:27:31 -07:00
parent bec22c9f0f c34856b4a2
commit f91872bf40
35 changed files with 1628 additions and 57 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
CHANGELOG
View File

@ -32,6 +32,8 @@
* Added position PID-controller angle offset.
* Configurable PID controller rate.
* Added several AUX port modes.
* Added configurable safe start modes.
* Added fusion IMU filter.
=== FW 5.02 ===
* IMU calibration improvement.

2
applications/app_adc.c
View File

@ -116,7 +116,7 @@ static THD_FUNCTION(adc_thread, arg) {
}
// For safe start when fault codes occur
if (mc_interface_get_fault() != FAULT_CODE_NONE) {
if (mc_interface_get_fault() != FAULT_CODE_NONE && config.safe_start != SAFE_START_NO_FAULT) {
ms_without_power = 0;
}

2
applications/app_ppm.c
View File

@ -164,7 +164,7 @@ static THD_FUNCTION(ppm_thread, arg) {
}
}
continue;
} else if (mc_interface_get_fault() != FAULT_CODE_NONE){
} else if (mc_interface_get_fault() != FAULT_CODE_NONE && config.safe_start != SAFE_START_NO_FAULT){
pulses_without_power = 0;
}

2
conf_general.h
View File

@ -24,7 +24,7 @@
#define FW_VERSION_MAJOR 5
#define FW_VERSION_MINOR 03
// Set to 0 for building a release and iterate during beta test builds
#define FW_TEST_VERSION_NUMBER 46
#define FW_TEST_VERSION_NUMBER 47
#include "datatypes.h"

2
confgenerator.c
View File

@ -673,7 +673,7 @@ bool confgenerator_deserialize_appconf(const uint8_t *buffer, app_configuration
conf->app_balance_conf.tiltback_constant_erpm = buffer_get_uint16(buffer, &ind);
conf->app_balance_conf.tiltback_variable = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.tiltback_variable_max = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.noseangling_speed = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.noseangling_speed = buffer_get_float16(buffer, 100, &ind);
conf->app_balance_conf.startup_pitch_tolerance = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.startup_roll_tolerance = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.startup_speed = buffer_get_float32_auto(buffer, &ind);

2
confgenerator.h
View File

@ -9,7 +9,7 @@
// Constants
#define MCCONF_SIGNATURE 3706516163
#define APPCONF_SIGNATURE 2855199595
#define APPCONF_SIGNATURE 568603952
// Functions
int32_t confgenerator_serialize_mcconf(uint8_t *buffer, const mc_configuration *conf);

13
datatypes.h
View File

@ -510,6 +510,12 @@ typedef enum {
THR_EXP_POLY
} thr_exp_mode;
typedef enum {
SAFE_START_DISABLED = 0,
SAFE_START_REGULAR,
SAFE_START_NO_FAULT
} SAFE_START_MODE;
// PPM control types
typedef enum {
PPM_CTRL_TYPE_NONE = 0,
@ -532,7 +538,7 @@ typedef struct {
float pulse_end;
float pulse_center;
bool median_filter;
bool safe_start;
SAFE_START_MODE safe_start;
float throttle_exp;
float throttle_exp_brake;
thr_exp_mode throttle_exp_mode;
@ -585,7 +591,7 @@ typedef struct {
float voltage2_start;
float voltage2_end;
bool use_filter;
bool safe_start;
SAFE_START_MODE safe_start;
bool cc_button_inverted;
bool rev_button_inverted;
bool voltage_inverted;
@ -798,7 +804,8 @@ typedef enum {
typedef enum {
AHRS_MODE_MADGWICK = 0,
AHRS_MODE_MAHONY
AHRS_MODE_MAHONY,
AHRS_MODE_MADGWICK_FUSION
} AHRS_MODE;
typedef struct {

4
hwconf/drv8323s.h
View File

@ -35,6 +35,10 @@ void drv8323s_write_reg(int reg, int data);
#define HW_RESET_DRV_FAULTS() drv8323s_reset_faults()
#ifndef MCCONF_M_DRV8301_OC_MODE
#define MCCONF_M_DRV8301_OC_MODE DRV8301_OC_LATCH_SHUTDOWN // DRV8301 over current protection mode
#endif
// Defines
#define DRV8323S_FAULT_FET_LC_OC (1 << 0)
#define DRV8323S_FAULT_FET_HC_OC (1 << 1)

14
hwconf/hw_rd2.h
View File

@ -223,7 +223,7 @@
#define READ_HALL3() palReadPad(HW_HALL_ENC_GPIO3, HW_HALL_ENC_PIN3)
// Override dead time. See the stm32f4 reference manual for calculating this value.
#define HW_DEAD_TIME_NSEC 660.0
#define HW_DEAD_TIME_NSEC 1000.0
// Default setting overrides
#ifndef MCCONF_L_MAX_VOLTAGE
@ -240,7 +240,7 @@
#endif
#ifndef MCCONF_L_CURRENT_MAX
#define MCCONF_L_CURRENT_MAX 300.0
#define MCCONF_L_CURRENT_MAX 350.0
#endif
#ifndef MCCONF_L_CURRENT_MIN
#define MCCONF_L_CURRENT_MIN -60.0
@ -267,22 +267,22 @@
#define MCCONF_FOC_CURRENT_KP 0.0046
#endif
#ifndef MCCONF_FOC_CURRENT_KI
#define MCCONF_FOC_CURRENT_KI 6.6
#define MCCONF_FOC_CURRENT_KI 8.0
#endif
#ifndef MCCONF_FOC_F_SW
#define MCCONF_FOC_F_SW 20000.0
#define MCCONF_FOC_F_SW 30000.0
#endif
#ifndef MCCONF_FOC_MOTOR_L
#define MCCONF_FOC_MOTOR_L 2.32e-6
#endif
#ifndef MCCONF_FOC_MOTOR_R
#define MCCONF_FOC_MOTOR_R 0.0033
#define MCCONF_FOC_MOTOR_R 0.004
#endif
#ifndef MCCONF_FOC_MOTOR_FLUX_LINKAGE
#define MCCONF_FOC_MOTOR_FLUX_LINKAGE 0.00849
#define MCCONF_FOC_MOTOR_FLUX_LINKAGE 0.00805
#endif
#ifndef MCCONF_FOC_OBSERVER_GAIN
#define MCCONF_FOC_OBSERVER_GAIN 8.87e6
#define MCCONF_FOC_OBSERVER_GAIN 15.0e6
#endif
// Setup Info

24
hwconf/hw_stormcore_100d.c
View File

@ -61,20 +61,20 @@ void hw_init_gpio(void) {
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOE, ENABLE);
#ifdef HW_VER_IS_100D_V2
palSetPadMode(PHASE_FILTER_GPIO, PHASE_FILTER_PIN,
PAL_MODE_OUTPUT_PUSHPULL |
PAL_STM32_OSPEED_HIGHEST);
PHASE_FILTER_OFF();
palSetPadMode(PHASE_FILTER_GPIO_M2, PHASE_FILTER_PIN_M2,
PAL_MODE_OUTPUT_PUSHPULL |
PAL_STM32_OSPEED_HIGHEST);
PHASE_FILTER_OFF_M2();
palSetPadMode(PHASE_FILTER_GPIO, PHASE_FILTER_PIN,
PAL_MODE_OUTPUT_PUSHPULL |
PAL_STM32_OSPEED_HIGHEST);
PHASE_FILTER_OFF();
palSetPadMode(PHASE_FILTER_GPIO_M2, PHASE_FILTER_PIN_M2,
PAL_MODE_OUTPUT_PUSHPULL |
PAL_STM32_OSPEED_HIGHEST);
PHASE_FILTER_OFF_M2();
#endif
// LEDs
// LEDs
palSetPadMode(GPIOA, 8,
PAL_MODE_OUTPUT_PUSHPULL |
PAL_STM32_OSPEED_HIGHEST);
@ -516,24 +516,26 @@ static THD_FUNCTION(smart_switch_thread, arg) {
mc_interface_set_current(0);
mc_interface_lock();
int cts = 0;
//check if ADCS are active and working
while((ADC_Value[ADC_IND_V_BATT] < 1 || ADC_Value[ADC_IND_VIN_SENS] < 1) && (cts < 50)){
chThdSleepMilliseconds(100);
cts++;
}
cts = 0;
//Wait for precharge resistors to precharge bulk caps
while(((GET_BATT_VOLTAGE() - GET_INPUT_VOLTAGE()) > 8.0) && (cts < 50)){
chThdSleepMilliseconds(100);
cts++;
}
palSetPad(SWITCH_PRECHARGED_GPIO, SWITCH_PRECHARGED_PIN);
mc_interface_select_motor_thread(2);
mc_interface_unlock();
mc_interface_select_motor_thread(1);
mc_interface_unlock();
//Wait for other systems to boot up before proceeding
chThdSleepMilliseconds(2000);
break;
case SWITCH_HELD_AFTER_TURN_ON:
smart_switch_keep_on();
if(smart_switch_is_pressed()){
switch_state = SWITCH_HELD_AFTER_TURN_ON;
} else {

121
hwconf/hw_stormcore_100s.c
View File

@ -21,9 +21,15 @@
#include "hal.h"
#include "stm32f4xx_conf.h"
#include "utils.h"
#include "ledpwm.h"
#include "drv8323s.h"
#if defined (HW_VER_IS_100S_V2)
static THD_WORKING_AREA(switch_color_thread_wa, 128);
static THD_FUNCTION(switch_color_thread, arg);
static volatile float switch_bright = 1.0;
#endif
// I2C configuration
static const I2CConfig i2cfg = {
@ -40,6 +46,17 @@ void hw_init_gpio(void) {
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE);
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD, ENABLE);
#if defined (HW_VER_IS_100S_V2)
palSetPadMode(PHASE_FILTER_GPIO, PHASE_FILTER_PIN,
PAL_MODE_OUTPUT_PUSHPULL |
PAL_STM32_OSPEED_HIGHEST);
PHASE_FILTER_OFF();
palSetPadMode(SWITCH_LED_1_GPIO,SWITCH_LED_1_PIN, PAL_MODE_OUTPUT_OPENDRAIN | PAL_STM32_OSPEED_HIGHEST);
palSetPadMode(SWITCH_LED_2_GPIO,SWITCH_LED_2_PIN, PAL_MODE_OUTPUT_OPENDRAIN | PAL_STM32_OSPEED_HIGHEST);
palSetPadMode(SWITCH_LED_3_GPIO,SWITCH_LED_3_PIN, PAL_MODE_OUTPUT_OPENDRAIN | PAL_STM32_OSPEED_HIGHEST);
chThdCreateStatic(switch_color_thread_wa, sizeof(switch_color_thread_wa), LOWPRIO, switch_color_thread, NULL);
#endif
// LEDs
palSetPadMode(GPIOB, 0,
PAL_MODE_OUTPUT_PUSHPULL |
@ -244,4 +261,108 @@ void hw_try_restore_i2c(void) {
}
}
#if defined (HW_VER_IS_100S_V2)
static THD_FUNCTION(switch_color_thread, arg) {
(void)arg;
chRegSetThreadName("switch_color_thread");
float switch_red = 0.0;
float switch_green = 0.0;
float switch_blue = 0.0;
for(int i = 0; i < 400; i++) {
float angle = i*3.14/400.0;
float s,c;
utils_fast_sincos_better(angle, &s, &c);
switch_blue = 0.75* c*c;
ledpwm_set_intensity(LED_HW1,switch_bright*switch_blue);
utils_fast_sincos_better(angle + 3.14/3.0, &s, &c);
switch_green = 0.75* c*c;
ledpwm_set_intensity(LED_HW2,switch_bright*switch_green);
utils_fast_sincos_better(angle + 6.28/3.0, &s, &c);
switch_red = 0.75* c*c;
ledpwm_set_intensity(LED_HW3,switch_bright*switch_red);
chThdSleepMilliseconds(4);
}
float switch_red_old = switch_red_old;
float switch_green_old = switch_green;
float switch_blue_old = switch_blue;
float wh_left;
float left = mc_interface_get_battery_level(&wh_left);
if(left < 0.5){
float intense = utils_map(left,0.0, 0.5, 0.0, 1.0);
utils_truncate_number(&intense,0,1);
switch_blue = intense;
switch_red = 1.0-intense;
}else{
float intense = utils_map(left , 0.5, 1.0, 0.0, 1.0);
utils_truncate_number(&intense,0,1);
switch_green = intense;
switch_blue = 1.0-intense;
}
for(int i = 0; i < 100; i++) {
float red_now = utils_map((float) i,0.0, 100.0, switch_red_old, switch_red);
float blue_now = utils_map((float) i,0.0, 100.0, switch_blue_old, switch_blue);
float green_now = utils_map((float) i,0.0, 100.0, switch_green_old, switch_green);
ledpwm_set_intensity(LED_HW1, switch_bright*blue_now);
ledpwm_set_intensity(LED_HW2, switch_bright*green_now);
ledpwm_set_intensity(LED_HW3, switch_bright*red_now);
chThdSleepMilliseconds(2);
}
for (;;) {
mc_fault_code fault = mc_interface_get_fault();
mc_interface_select_motor_thread(2);
mc_fault_code fault2 = mc_interface_get_fault();
mc_interface_select_motor_thread(1);
if (fault != FAULT_CODE_NONE || fault2 != FAULT_CODE_NONE) {
ledpwm_set_intensity(LED_HW2, 0);
ledpwm_set_intensity(LED_HW1, 0);
for (int i = 0;i < (int)fault;i++) {
ledpwm_set_intensity(LED_HW3, 1.0);
chThdSleepMilliseconds(250);
ledpwm_set_intensity(LED_HW3, 0.0);
chThdSleepMilliseconds(250);
}
chThdSleepMilliseconds(500);
for (int i = 0;i < (int)fault2;i++) {
ledpwm_set_intensity(LED_HW3, 1.0);
chThdSleepMilliseconds(250);
ledpwm_set_intensity(LED_HW3, 0.0);
chThdSleepMilliseconds(250);
}
chThdSleepMilliseconds(500);
} else {
left = mc_interface_get_battery_level(&wh_left);
if(HW_SAMPLE_SHUTDOWN()){
switch_bright = 0.5;
}else{
switch_bright = 1.0;
}
if(left < 0.5){
float intense = utils_map(left,0.0, 0.5, 0.0, 1.0);
utils_truncate_number(&intense,0,1);
switch_blue = intense;
switch_red = 1.0-intense;
switch_green = 0;
}else{
float intense = utils_map(left , 0.5, 1.0, 0.0, 1.0);
utils_truncate_number(&intense,0,1);
switch_green = intense;
switch_blue = 1.0-intense;
switch_red = 0;
}
ledpwm_set_intensity(LED_HW1, switch_bright*switch_blue);
ledpwm_set_intensity(LED_HW2, switch_bright*switch_green);
ledpwm_set_intensity(LED_HW3, switch_bright*switch_red);
}
chThdSleepMilliseconds(20);
}
}
#endif

52
hwconf/hw_stormcore_100s.h
View File

@ -28,6 +28,48 @@
#define HW_HAS_DRV8323S
#define HW_HAS_3_SHUNTS
#if defined(HW_VER_IS_100S_V2)
#define SWITCH_LED_3_GPIO GPIOC
#define SWITCH_LED_3_PIN 15
#define SWITCH_LED_2_GPIO GPIOC
#define SWITCH_LED_2_PIN 14
#define SWITCH_LED_1_GPIO GPIOC
#define SWITCH_LED_1_PIN 13
#define LED_PWM1_ON() palClearPad(SWITCH_LED_1_GPIO,SWITCH_LED_1_PIN)
#define LED_PWM1_OFF() palSetPad(SWITCH_LED_1_GPIO,SWITCH_LED_1_PIN)
#define LED_PWM2_ON() palClearPad(SWITCH_LED_2_GPIO, SWITCH_LED_2_PIN)
#define LED_PWM2_OFF() palSetPad(SWITCH_LED_2_GPIO, SWITCH_LED_2_PIN)
#define LED_PWM3_ON() palClearPad(SWITCH_LED_3_GPIO, SWITCH_LED_3_PIN)
#define LED_PWM3_OFF() palSetPad(SWITCH_LED_3_GPIO, SWITCH_LED_3_PIN)
//#define HW_HAS_PHASE_FILTERS
#define PHASE_FILTER_GPIO GPIOA
#define PHASE_FILTER_PIN 15
#define PHASE_FILTER_ON() palSetPad(PHASE_FILTER_GPIO, PHASE_FILTER_PIN)
#define PHASE_FILTER_OFF() palClearPad(PHASE_FILTER_GPIO, PHASE_FILTER_PIN)
// Shutdown pin
#define HW_SHUTDOWN_GPIO GPIOB
#define HW_SHUTDOWN_PIN 2
#define HW_SWITCH_SENSE_GPIO GPIOB
#define HW_SWITCH_SENSE_PIN 12
#define HW_SHUTDOWN_HOLD_ON() palSetPad(HW_SHUTDOWN_GPIO, HW_SHUTDOWN_PIN)
#define HW_SHUTDOWN_HOLD_OFF() palClearPad(HW_SHUTDOWN_GPIO, HW_SHUTDOWN_PIN)
#define HW_SAMPLE_SHUTDOWN() palReadPad(HW_SWITCH_SENSE_GPIO, HW_SWITCH_SENSE_PIN)
// Hold shutdown pin early to wake up on short pulses
#define HW_EARLY_INIT() palSetPadMode(HW_SHUTDOWN_GPIO, HW_SHUTDOWN_PIN, PAL_MODE_OUTPUT_PUSHPULL); \
HW_SHUTDOWN_HOLD_ON(); \
palSetPadMode(HW_SWITCH_SENSE_GPIO, HW_SWITCH_SENSE_PIN, \
PAL_MODE_INPUT_PULLDOWN); \
palSetPadMode(PHASE_FILTER_GPIO, PHASE_FILTER_PIN, \
PAL_MODE_OUTPUT_PUSHPULL | \
PAL_STM32_OSPEED_HIGHEST); \
PHASE_FILTER_OFF()
#endif
#define DRV8323S_CUSTOM_SETTINGS() drv8323s_set_current_amp_gain(CURRENT_AMP_GAIN); \
drv8323s_write_reg(3,0x3af); \
drv8323s_write_reg(4,0x7af);
@ -101,12 +143,22 @@
#ifndef VIN_R2
#define VIN_R2 2200.0
#endif
#if defined(HW_VER_IS_100S_V2)
#ifndef CURRENT_AMP_GAIN
#define CURRENT_AMP_GAIN 20.0
#endif
#ifndef CURRENT_SHUNT_RES
#define CURRENT_SHUNT_RES 0.0005
#endif
#else
#ifndef CURRENT_AMP_GAIN
#define CURRENT_AMP_GAIN 10.0
#endif
#ifndef CURRENT_SHUNT_RES
#define CURRENT_SHUNT_RES 0.001
#endif
#endif
// Input voltage
#define GET_INPUT_VOLTAGE() ((V_REG / 4095.0) * (float)ADC_Value[ADC_IND_VIN_SENS] * ((VIN_R1 + VIN_R2) / VIN_R2))

11
hwconf/hw_stormcore_60d.c
View File

@ -61,7 +61,7 @@ void hw_init_gpio(void) {
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOE, ENABLE);
#ifdef HW_VER_IS_60D_PLUS
#if defined (HW_VER_IS_60D_PLUS) || defined (HW_VER_IS_60D_XS)
palSetPadMode(PHASE_FILTER_GPIO, PHASE_FILTER_PIN,
PAL_MODE_OUTPUT_PUSHPULL |
PAL_STM32_OSPEED_HIGHEST);
@ -367,7 +367,6 @@ static THD_FUNCTION(mux_thread, arg) {
void smart_switch_keep_on(void) {
palSetPad(SWITCH_OUT_GPIO, SWITCH_OUT_PIN);
}
void smart_switch_shut_down(void) {
@ -506,24 +505,26 @@ static THD_FUNCTION(smart_switch_thread, arg) {
mc_interface_set_current(0);
mc_interface_lock();
int cts = 0;
//check if ADCS are active and working
while((ADC_Value[ADC_IND_V_BATT] < 1 || ADC_Value[ADC_IND_VIN_SENS] < 1) && (cts < 50)){
chThdSleepMilliseconds(100);
cts++;
}
cts = 0;
//Wait for precharge resistors to precharge bulk caps
while(((GET_BATT_VOLTAGE() - GET_INPUT_VOLTAGE()) > 8.0) && (cts < 50)){
chThdSleepMilliseconds(100);
cts++;
}
palSetPad(SWITCH_PRECHARGED_GPIO, SWITCH_PRECHARGED_PIN);
mc_interface_select_motor_thread(2);
mc_interface_unlock();
mc_interface_select_motor_thread(1);
mc_interface_unlock();
//Wait for other systems to boot up before proceeding
chThdSleepMilliseconds(2000);
break;
case SWITCH_HELD_AFTER_TURN_ON:
smart_switch_keep_on();
if(smart_switch_is_pressed()){
switch_state = SWITCH_HELD_AFTER_TURN_ON;
} else {
@ -541,11 +542,11 @@ static THD_FUNCTION(smart_switch_thread, arg) {
if (millis_switch_pressed > SMART_SWITCH_MSECS_PRESSED_OFF) {
switch_state = SWITCH_SHUTTING_DOWN;
comm_can_shutdown(255);
}
break;
case SWITCH_SHUTTING_DOWN:
switch_bright = 0;
comm_can_shutdown(255);
smart_switch_shut_down();
chThdSleepMilliseconds(10000);
smart_switch_keep_on();

25
hwconf/hw_stormcore_60d.h
View File

@ -19,6 +19,8 @@
#ifdef HW_VER_IS_60D_PLUS
#define HW_NAME "STORMCORE_60D+"
#elif defined (HW_VER_IS_60D_XS)
#define HW_NAME "STORMCORE_60Dxs"
#else
#define HW_NAME "STORMCORE_60D"
#endif
@ -30,9 +32,15 @@
#define HW_HAS_DRV8323S // for idrive do 0x073b for reg 4 (LS) and 0x034b for reg 3 (HS)
#define HW_HAS_3_SHUNTS
#if defined (HW_VER_IS_60D_XS)
#define DRV8323S_CUSTOM_SETTINGS(); drv8323s_set_current_amp_gain(CURRENT_AMP_GAIN); \
drv8323s_write_reg(3,0x3aa); \
drv8323s_write_reg(4,0x7aa);
#else
#define DRV8323S_CUSTOM_SETTINGS(); drv8323s_set_current_amp_gain(CURRENT_AMP_GAIN); \
drv8323s_write_reg(3,0x3af); \
drv8323s_write_reg(4,0x7af);
#endif
@ -64,8 +72,8 @@
#define SMART_SWITCH_MSECS_PRESSED_OFF 2000
#ifdef HW_VER_IS_60D_PLUS
#define HW_HAS_PHASE_FILTERS
#if defined (HW_VER_IS_60D_PLUS) || defined (HW_VER_IS_60D_XS)
//#define HW_HAS_PHASE_FILTERS
#define PHASE_FILTER_GPIO GPIOE
#define PHASE_FILTER_PIN 1
#define PHASE_FILTER_GPIO_M2 GPIOE
@ -103,7 +111,7 @@
#define HW_UART_P_RX_PORT GPIOA
#define HW_UART_P_RX_PIN 10
#ifdef HW_VER_IS_60D_PLUS
#if defined (HW_VER_IS_60D_PLUS) || defined (HW_VER_IS_60D_XS)
//Pins for Third UART
#define HW_UART_3_BAUD 115200
#define HW_UART_3_DEV SD2
@ -116,10 +124,17 @@
// SPI for DRV8301
#ifdef HW_VER_IS_60D_XS
#define DRV8323S_MOSI_GPIO GPIOC
#define DRV8323S_MOSI_PIN 11
#define DRV8323S_MISO_GPIO GPIOC
#define DRV8323S_MISO_PIN 12
#else
#define DRV8323S_MOSI_GPIO GPIOC
#define DRV8323S_MOSI_PIN 12
#define DRV8323S_MISO_GPIO GPIOC
#define DRV8323S_MISO_PIN 11
#endif
#define DRV8323S_SCK_GPIO GPIOC
#define DRV8323S_SCK_PIN 10
#define DRV8323S_CS_GPIO GPIOC
@ -246,7 +261,7 @@
#define VIN_R2 2200.0
#endif
#ifdef HW_VER_IS_60D_PLUS
#if defined (HW_VER_IS_60D_PLUS) || defined (HW_VER_IS_60D_XS)
#ifndef CURRENT_AMP_GAIN
#define CURRENT_AMP_GAIN 20.0
#endif
@ -364,7 +379,7 @@
// NRF SWD
#ifdef HW_VER_IS_60D_PLUS
#if defined (HW_VER_IS_60D_PLUS) || defined (HW_VER_IS_60D_XS)
#define NRF5x_SWDIO_GPIO GPIOD
#define NRF5x_SWDIO_PIN 9
#define NRF5x_SWCLK_GPIO GPIOD

11
hwconf/hw_unity.c
View File

@ -364,17 +364,18 @@ static THD_FUNCTION(smart_switch_thread, arg) {
for (;;) {
switch (switch_state) {
case SWITCH_BOOTED:
ledpwm_set_intensity(LED_HW1, 0.6);
ledpwm_set_intensity(LED_HW1, 1.0);
switch_state = SWITCH_TURN_ON_DELAY_ACTIVE;
break;
case SWITCH_TURN_ON_DELAY_ACTIVE:
chThdSleepMilliseconds(500);
ledpwm_set_intensity(LED_HW1, 0.6);
switch_state = SWITCH_HELD_AFTER_TURN_ON;
smart_switch_keep_on();
ledpwm_set_intensity(LED_HW1, 1.0);
//Wait for other systems to boot up before proceeding
chThdSleepMilliseconds(3000);
break;
case SWITCH_HELD_AFTER_TURN_ON:
smart_switch_keep_on();
if(smart_switch_is_pressed()){
switch_state = SWITCH_HELD_AFTER_TURN_ON;
} else {
@ -384,10 +385,10 @@ static THD_FUNCTION(smart_switch_thread, arg) {
case SWITCH_TURNED_ON:
if (smart_switch_is_pressed()) {
millis_switch_pressed++;
ledpwm_set_intensity(LED_HW1, 1.0);
ledpwm_set_intensity(LED_HW1, 0.6);
} else {
millis_switch_pressed = 0;
ledpwm_set_intensity(LED_HW1, 0.6);
ledpwm_set_intensity(LED_HW1, 1.0);
}
if (millis_switch_pressed > SMART_SWITCH_MSECS_PRESSED_OFF) {

2
hwconf/hw_unity.h
View File

@ -54,7 +54,7 @@
#define SMART_SWITCH_MSECS_PRESSED_OFF 2000
#define HW_EARLY_INIT() smart_switch_pin_init(); smart_switch_thread_start();
#define HW_EARLY_INIT() smart_switch_pin_init(); LED_PWM1_ON(); smart_switch_thread_start();
// Pins for BLE UART
#define HW_UART_P_BAUD 250000

76
imu/Fusion/Examples/ExampleAhrs.c Normal file
View File

@ -0,0 +1,76 @@
#include "../Fusion.h"
#include <stdio.h>
FusionBias fusionBias;
FusionAhrs fusionAhrs;
float samplePeriod = 0.01f; // replace this value with actual sample period in seconds
FusionVector3 gyroscopeSensitivity = {
.axis.x = 1.0f,
.axis.y = 1.0f,
.axis.z = 1.0f,
}; // replace these values with actual sensitivity in degrees per second per lsb as specified in gyroscope datasheet
FusionVector3 accelerometerSensitivity = {
.axis.x = 1.0f,
.axis.y = 1.0f,
.axis.z = 1.0f,
}; // replace these values with actual sensitivity in g per lsb as specified in accelerometer datasheet
FusionVector3 hardIronBias = {
.axis.x = 0.0f,
.axis.y = 0.0f,
.axis.z = 0.0f,
}; // replace these values with actual hard-iron bias in uT if known
int main() {
// Initialise gyroscope bias correction algorithm
FusionBiasInitialise(&fusionBias, 0.5f, samplePeriod); // stationary threshold = 0.5 degrees per second
// Initialise AHRS algorithm
FusionAhrsInitialise(&fusionAhrs, 0.5f); // gain = 0.5
// Set optional magnetic field limits
FusionAhrsSetMagneticField(&fusionAhrs, 20.0f, 70.0f); // valid magnetic field range = 20 uT to 70 uT
// The contents of this do while loop should be called for each time new sensor measurements are available
do {
// Calibrate gyroscope
FusionVector3 uncalibratedGyroscope = {
.axis.x = 0.0f, /* replace this value with actual gyroscope x axis measurement in lsb */
.axis.y = 0.0f, /* replace this value with actual gyroscope y axis measurement in lsb */
.axis.z = 0.0f, /* replace this value with actual gyroscope z axis measurement in lsb */
};
FusionVector3 calibratedGyroscope = FusionCalibrationInertial(uncalibratedGyroscope, FUSION_ROTATION_MATRIX_IDENTITY, gyroscopeSensitivity, FUSION_VECTOR3_ZERO);
// Calibrate accelerometer
FusionVector3 uncalibratedAccelerometer = {
.axis.x = 0.0f, /* replace this value with actual accelerometer x axis measurement in lsb */
.axis.y = 0.0f, /* replace this value with actual accelerometer y axis measurement in lsb */
.axis.z = 1.0f, /* replace this value with actual accelerometer z axis measurement in lsb */
};
FusionVector3 calibratedAccelerometer = FusionCalibrationInertial(uncalibratedAccelerometer, FUSION_ROTATION_MATRIX_IDENTITY, accelerometerSensitivity, FUSION_VECTOR3_ZERO);
// Calibrate magnetometer
FusionVector3 uncalibratedMagnetometer = {
.axis.x = 0.5f, /* replace this value with actual magnetometer x axis measurement in uT */
.axis.y = 0.0f, /* replace this value with actual magnetometer y axis measurement in uT */
.axis.z = 0.0f, /* replace this value with actual magnetometer z axis measurement in uT */
};
FusionVector3 calibratedMagnetometer = FusionCalibrationMagnetic(uncalibratedMagnetometer, FUSION_ROTATION_MATRIX_IDENTITY, hardIronBias);
// Update gyroscope bias correction algorithm
calibratedGyroscope = FusionBiasUpdate(&fusionBias, calibratedGyroscope);
// Update AHRS algorithm
FusionAhrsUpdate(&fusionAhrs, calibratedGyroscope, calibratedAccelerometer, calibratedMagnetometer, samplePeriod);
// Print Euler angles
FusionEulerAngles eulerAngles = FusionQuaternionToEulerAngles(FusionAhrsGetQuaternion(&fusionAhrs));
printf("Roll = %0.1f, Pitch = %0.1f, Yaw = %0.1f\r\n", eulerAngles.angle.roll, eulerAngles.angle.pitch, eulerAngles.angle.yaw);
} while (false);
}

59
imu/Fusion/Examples/ExampleAhrsWithoutMagnetometer.c Normal file
View File

@ -0,0 +1,59 @@
#include "../Fusion.h"
#include <stdio.h>
FusionBias fusionBias;
FusionAhrs fusionAhrs;
float samplePeriod = 0.01f; // replace this value with actual sample period in seconds
FusionVector3 gyroscopeSensitivity = {
.axis.x = 1.0f,
.axis.y = 1.0f,
.axis.z = 1.0f,
}; // replace these values with actual sensitivity in degrees per second per lsb as specified in gyroscope datasheet
FusionVector3 accelerometerSensitivity = {
.axis.x = 1.0f,
.axis.y = 1.0f,
.axis.z = 1.0f,
}; // replace these values with actual sensitivity in g per lsb as specified in accelerometer datasheet
int main() {
// Initialise gyroscope bias correction algorithm
FusionBiasInitialise(&fusionBias, 0.5f, samplePeriod); // stationary threshold = 0.5 degrees per second
// Initialise AHRS algorithm
FusionAhrsInitialise(&fusionAhrs, 0.5f); // gain = 0.5
// The contents of this do while loop should be called for each time new sensor measurements are available
do {
// Calibrate gyroscope
FusionVector3 uncalibratedGyroscope = {
.axis.x = 0.0f, /* replace this value with actual gyroscope x axis measurement in lsb */
.axis.y = 0.0f, /* replace this value with actual gyroscope y axis measurement in lsb */
.axis.z = 0.0f, /* replace this value with actual gyroscope z axis measurement in lsb */
};
FusionVector3 calibratedGyroscope = FusionCalibrationInertial(uncalibratedGyroscope, FUSION_ROTATION_MATRIX_IDENTITY, gyroscopeSensitivity, FUSION_VECTOR3_ZERO);
// Calibrate accelerometer
FusionVector3 uncalibratedAccelerometer = {
.axis.x = 0.0f, /* replace this value with actual accelerometer x axis measurement in lsb */
.axis.y = 0.0f, /* replace this value with actual accelerometer y axis measurement in lsb */
.axis.z = 1.0f, /* replace this value with actual accelerometer z axis measurement in lsb */
};
FusionVector3 calibratedAccelerometer = FusionCalibrationInertial(uncalibratedAccelerometer, FUSION_ROTATION_MATRIX_IDENTITY, accelerometerSensitivity, FUSION_VECTOR3_ZERO);
// Update gyroscope bias correction algorithm
calibratedGyroscope = FusionBiasUpdate(&fusionBias, calibratedGyroscope);
// Update AHRS algorithm
FusionAhrsUpdateWithoutMagnetometer(&fusionAhrs, calibratedGyroscope, calibratedAccelerometer, samplePeriod);
// Print Euler angles
FusionEulerAngles eulerAngles = FusionQuaternionToEulerAngles(FusionAhrsGetQuaternion(&fusionAhrs));
printf("Roll = %0.1f, Pitch = %0.1f, Yaw = %0.1f\r\n", eulerAngles.angle.roll, eulerAngles.angle.pitch, eulerAngles.angle.yaw);
} while (false);
}

44
imu/Fusion/Examples/ExampleCompass.c Normal file
View File

@ -0,0 +1,44 @@
#include "../Fusion.h"
#include <stdio.h>
FusionVector3 accelerometerSensitivity = {
.axis.x = 1.0f,
.axis.y = 1.0f,
.axis.z = 1.0f,
}; // replace these values with actual sensitivity in g per lsb as specified in accelerometer datasheet
FusionVector3 hardIronBias = {
.axis.x = 0.0f,
.axis.y = 0.0f,
.axis.z = 0.0f,
}; // replace these values with actual hard-iron bias in uT if known
int main() {
// The contents of this do while loop should be called for each time new sensor measurements are available
do {
// Calibrate accelerometer
FusionVector3 uncalibratedAccelerometer = {
.axis.x = 0.0f, /* replace this value with actual accelerometer x axis measurement in lsb */
.axis.y = 0.0f, /* replace this value with actual accelerometer y axis measurement in lsb */
.axis.z = 1.0f, /* replace this value with actual accelerometer z axis measurement in lsb */
};
FusionVector3 calibratedAccelerometer = FusionCalibrationInertial(uncalibratedAccelerometer, FUSION_ROTATION_MATRIX_IDENTITY, accelerometerSensitivity, FUSION_VECTOR3_ZERO);
// Calibrate magnetometer
FusionVector3 uncalibratedMagnetometer = {
.axis.x = 0.5f, /* replace this value with actual magnetometer x axis measurement in uT */
.axis.y = 0.0f, /* replace this value with actual magnetometer y axis measurement in uT */
.axis.z = 0.0f, /* replace this value with actual magnetometer z axis measurement in uT */
};
FusionVector3 calibratedMagnetometer = FusionCalibrationMagnetic(uncalibratedMagnetometer, FUSION_ROTATION_MATRIX_IDENTITY, hardIronBias);
// Calculate heading
float heading = FusionCompassCalculateHeading(calibratedAccelerometer, calibratedMagnetometer);
// Print heading
printf("Heading = %0.1f\r\n", heading);
} while (false);
}

37
imu/Fusion/Fusion.h Normal file
View File

@ -0,0 +1,37 @@
/**
* @file Fusion.h
* @author Seb Madgwick
* @brief Main header file for the library. This is the only file that needs to
* be included when using the library.
*
* Fusion is an ANSI C99 compliment sensor fusion library for sensor arrays of
* gyroscopes, accelerometers, and magnetometers. Fusion was specifically
* developed for use with embedded systems and has been optimised for execution
* speed. The library includes modules for: attitude and heading reference
* system (AHRS) sensor fusion, gyroscope bias correction, and a tilt-
* compensated compass.
*/
#ifndef FUSION_H
#define FUSION_H
//------------------------------------------------------------------------------
// Includes
#ifdef __cplusplus
extern "C" {
#endif
#include "FusionAhrs.h"
#include "FusionBias.h"
#include "FusionCalibration.h"
#include "FusionCompass.h"
#include "FusionTypes.h"
#ifdef __cplusplus
}
#endif
#endif
//------------------------------------------------------------------------------
// End of file

290
imu/Fusion/FusionAhrs.c Normal file
View File

@ -0,0 +1,290 @@
/**
* @file FusionAhrs.c
* @author Seb Madgwick
* @brief The AHRS sensor fusion algorithm to combines gyroscope, accelerometer,
* and magnetometer measurements into a single measurement of orientation
* relative to the Earth (NWU convention).
*
* The algorithm behaviour is governed by a gain. A low gain will decrease the
* influence of the accelerometer and magnetometer so that the algorithm will
* better reject disturbances causes by translational motion and temporary
* magnetic distortions. However, a low gain will also increase the risk of
* drift due to gyroscope calibration errors. A typical gain value suitable for
* most applications is 0.5.
*
* The algorithm allows the application to define a minimum and maximum valid
* magnetic field magnitude. The algorithm will ignore magnetic measurements
* that fall outside of this range. This allows the algorithm to reject
* magnetic measurements that do not represent the direction of magnetic North.
* The typical magnitude of the Earth's magnetic field is between 20 uT and
* 70 uT.
*
* The algorithm can be used without a magnetometer. Measurements of
* orientation obtained using only gyroscope and accelerometer measurements
* can be expected to drift in the yaw component of orientation only. The
* application can reset the drift in yaw by setting the yaw to a specified
* angle at any time.
*
* The algorithm provides the measurement of orientation as a quaternion. The
* library includes functions for converting this quaternion to a rotation
* matrix and Euler angles.
*
* The algorithm also provides a measurement of linear acceleration and Earth
* acceleration. Linear acceleration is equal to the accelerometer measurement
* with the 1 g of gravity removed. Earth acceleration is a measurement of
* linear acceleration in the Earth coordinate frame.
*/
//------------------------------------------------------------------------------
// Includes
#include "FusionAhrs.h"
#include <float.h> // FLT_MAX
#include <math.h> // atan2f, cosf, sinf
//------------------------------------------------------------------------------
// Definitions
/**
* @brief Initialisation period (in seconds).
*/
#define INITIALISATION_PERIOD (3.0f)
//------------------------------------------------------------------------------
// Functions
/**
* @brief Initialises the AHRS algorithm structure.
* @param fusionAhrs AHRS algorithm structure.
* @param gain AHRS algorithm gain.
*/
void FusionAhrsInitialise(FusionAhrs * const fusionAhrs, const float gain, const float initialGain) {
fusionAhrs->gain = gain;
fusionAhrs->initialGain = initialGain;
fusionAhrs->minimumMagneticFieldSquared = 0.0f;
fusionAhrs->maximumMagneticFieldSquared = FLT_MAX;
fusionAhrs->quaternion = FUSION_QUATERNION_IDENTITY;
fusionAhrs->linearAcceleration = FUSION_VECTOR3_ZERO;
fusionAhrs->rampedGain = fusionAhrs->initialGain;
fusionAhrs->zeroYawPending = false;
}
/**
* @brief Sets the AHRS algorithm gain. The gain must be equal or greater than
* zero.
* @param gain AHRS algorithm gain.
*/
void FusionAhrsSetGain(FusionAhrs * const fusionAhrs, const float gain) {
fusionAhrs->gain = gain;
}
void FusionAhrsSetInitialGain(FusionAhrs * const fusionAhrs, const float initialGain) {
fusionAhrs->initialGain = initialGain;
}
/**
* @brief Sets the minimum and maximum valid magnetic field magnitudes in uT.
* @param fusionAhrs AHRS algorithm structure.
* @param minimumMagneticField Minimum valid magnetic field magnitude.
* @param maximumMagneticField Maximum valid magnetic field magnitude.
*/
void FusionAhrsSetMagneticField(FusionAhrs * const fusionAhrs, const float minimumMagneticField, const float maximumMagneticField) {
fusionAhrs->minimumMagneticFieldSquared = minimumMagneticField * minimumMagneticField;
fusionAhrs->maximumMagneticFieldSquared = maximumMagneticField * maximumMagneticField;
}
/**
* @brief Updates the AHRS algorithm. This function should be called for each
* new gyroscope measurement.
* @param fusionAhrs AHRS algorithm structure.
* @param gyroscope Gyroscope measurement in degrees per second.
* @param accelerometer Accelerometer measurement in g.
* @param magnetometer Magnetometer measurement in uT.
* @param samplePeriod Sample period in seconds. This is the difference in time
* between the current and previous gyroscope measurements.
*/
void FusionAhrsUpdate(FusionAhrs * const fusionAhrs, const FusionVector3 gyroscope, const FusionVector3 accelerometer, const FusionVector3 magnetometer, const float samplePeriod) {
#define Q fusionAhrs->quaternion.element // define shorthand label for more readable code
// Calculate feedback error
FusionVector3 halfFeedbackError = FUSION_VECTOR3_ZERO; // scaled by 0.5 to avoid repeated multiplications by 2
do {
// Abandon feedback calculation if accelerometer measurement invalid
if ((accelerometer.axis.x == 0.0f) && (accelerometer.axis.y == 0.0f) && (accelerometer.axis.z == 0.0f)) {
break;
}
// Calculate direction of gravity assumed by quaternion
const FusionVector3 halfGravity = {
.axis.x = Q.x * Q.z - Q.w * Q.y,
.axis.y = Q.w * Q.x + Q.y * Q.z,
.axis.z = Q.w * Q.w - 0.5f + Q.z * Q.z,
}; // equal to 3rd column of rotation matrix representation scaled by 0.5
// Calculate accelerometer feedback error
halfFeedbackError = FusionVectorCrossProduct(FusionVectorFastNormalise(accelerometer), halfGravity);
// Abandon magnetometer feedback calculation if magnetometer measurement invalid
const float magnetometerMagnitudeSquared = FusionVectorMagnitudeSquared(magnetometer);
if ((magnetometerMagnitudeSquared < fusionAhrs->minimumMagneticFieldSquared) || (magnetometerMagnitudeSquared > fusionAhrs->maximumMagneticFieldSquared)) {
break;
}
// Compute direction of 'magnetic west' assumed by quaternion
const FusionVector3 halfWest = {
.axis.x = Q.x * Q.y + Q.w * Q.z,
.axis.y = Q.w * Q.w - 0.5f + Q.y * Q.y,
.axis.z = Q.y * Q.z - Q.w * Q.x
}; // equal to 2nd column of rotation matrix representation scaled by 0.5
// Calculate magnetometer feedback error
halfFeedbackError = FusionVectorAdd(halfFeedbackError, FusionVectorCrossProduct(FusionVectorFastNormalise(FusionVectorCrossProduct(accelerometer, magnetometer)), halfWest));
} while (false);
// Ramp down gain until initialisation complete
if (fusionAhrs->gain == 0) {
fusionAhrs->rampedGain = 0; // skip initialisation if gain is zero
}
float feedbackGain = fusionAhrs->gain;
if (fusionAhrs->rampedGain > fusionAhrs->gain) {
fusionAhrs->rampedGain -= (fusionAhrs->initialGain - fusionAhrs->gain) * samplePeriod / INITIALISATION_PERIOD;
feedbackGain = fusionAhrs->rampedGain;
}
// Convert gyroscope to radians per second scaled by 0.5
FusionVector3 halfGyroscope = FusionVectorMultiplyScalar(gyroscope, 0.5f * FusionDegreesToRadians(1.0f));
// Apply feedback to gyroscope
halfGyroscope = FusionVectorAdd(halfGyroscope, FusionVectorMultiplyScalar(halfFeedbackError, feedbackGain));
// Integrate rate of change of quaternion
fusionAhrs->quaternion = FusionQuaternionAdd(fusionAhrs->quaternion, FusionQuaternionMultiplyVector(fusionAhrs->quaternion, FusionVectorMultiplyScalar(halfGyroscope, samplePeriod)));
// Normalise quaternion
fusionAhrs->quaternion = FusionQuaternionFastNormalise(fusionAhrs->quaternion);
// Calculate linear acceleration
const FusionVector3 gravity = {
.axis.x = 2.0f * (Q.x * Q.z - Q.w * Q.y),
.axis.y = 2.0f * (Q.w * Q.x + Q.y * Q.z),
.axis.z = 2.0f * (Q.w * Q.w - 0.5f + Q.z * Q.z),
}; // equal to 3rd column of rotation matrix representation
fusionAhrs->linearAcceleration = FusionVectorSubtract(accelerometer, gravity);
#undef Q // undefine shorthand label
}
/**
* @brief Updates the AHRS algorithm. This function should be called for each
* new gyroscope measurement.
* @param fusionAhrs AHRS algorithm structure.
* @param gyroscope Gyroscope measurement in degrees per second.
* @param accelerometer Accelerometer measurement in g.
* @param samplePeriod Sample period in seconds. This is the difference in time
* between the current and previous gyroscope measurements.
*/
void FusionAhrsUpdateWithoutMagnetometer(FusionAhrs * const fusionAhrs, const FusionVector3 gyroscope, const FusionVector3 accelerometer, const float samplePeriod) {
// Update AHRS algorithm
FusionAhrsUpdate(fusionAhrs, gyroscope, accelerometer, FUSION_VECTOR3_ZERO, samplePeriod);
// Zero yaw once initialisation complete
if (FusionAhrsIsInitialising(fusionAhrs) == true) {
fusionAhrs->zeroYawPending = true;
} else {
if (fusionAhrs->zeroYawPending == true) {
FusionAhrsSetYaw(fusionAhrs, 0.0f);
fusionAhrs->zeroYawPending = false;
}
}
}
/**
* @brief Gets the quaternion describing the sensor relative to the Earth.
* @param fusionAhrs AHRS algorithm structure.
* @return Quaternion describing the sensor relative to the Earth.
*/
FusionQuaternion FusionAhrsGetQuaternion(const FusionAhrs * const fusionAhrs) {
return FusionQuaternionConjugate(fusionAhrs->quaternion);
}
/**
* @brief Gets the linear acceleration measurement equal to the accelerometer
* measurement with the 1 g of gravity removed.
* @param fusionAhrs AHRS algorithm structure.
* @return Linear acceleration measurement.
*/
FusionVector3 FusionAhrsGetLinearAcceleration(const FusionAhrs * const fusionAhrs) {
return fusionAhrs->linearAcceleration;
}
/**
* @brief Gets the Earth acceleration measurement equal to linear acceleration
* in the Earth coordinate frame.
* @param fusionAhrs AHRS algorithm structure.
* @return Earth acceleration measurement.
*/
FusionVector3 FusionAhrsGetEarthAcceleration(const FusionAhrs * const fusionAhrs) {
#define Q fusionAhrs->quaternion.element // define shorthand labels for more readable code
#define A fusionAhrs->linearAcceleration.axis
const float qwqw = Q.w * Q.w; // calculate common terms to avoid repeated operations
const float qwqx = Q.w * Q.x;
const float qwqy = Q.w * Q.y;
const float qwqz = Q.w * Q.z;
const float qxqy = Q.x * Q.y;
const float qxqz = Q.x * Q.z;
const float qyqz = Q.y * Q.z;
const FusionVector3 earthAcceleration = {
.axis.x = 2.0f * ((qwqw - 0.5f + Q.x * Q.x) * A.x + (qxqy - qwqz) * A.y + (qxqz + qwqy) * A.z),
.axis.y = 2.0f * ((qxqy + qwqz) * A.x + (qwqw - 0.5f + Q.y * Q.y) * A.y + (qyqz - qwqx) * A.z),
.axis.z = 2.0f * ((qxqz - qwqy) * A.x + (qyqz + qwqx) * A.y + (qwqw - 0.5f + Q.z * Q.z) * A.z),
}; // transpose of a rotation matrix representation of the quaternion multiplied with the linear acceleration
return earthAcceleration;
#undef Q // undefine shorthand label
#undef A
}
/**
* @brief Reinitialise the AHRS algorithm.
* @param fusionAhrs AHRS algorithm structure.
*/
void FusionAhrsReinitialise(FusionAhrs * const fusionAhrs) {
fusionAhrs->quaternion = FUSION_QUATERNION_IDENTITY;
fusionAhrs->linearAcceleration = FUSION_VECTOR3_ZERO;
fusionAhrs->rampedGain = fusionAhrs->initialGain;
}
/**
* @brief Returns true while the AHRS algorithm is initialising.
* @param fusionAhrs AHRS algorithm structure.
* @return True while the AHRS algorithm is initialising.
*/
bool FusionAhrsIsInitialising(const FusionAhrs * const fusionAhrs) {
return fusionAhrs->rampedGain > fusionAhrs->gain;
}
/**
* @brief Sets the yaw component of the orientation measurement provided by the
* AHRS algorithm. This function can be used to reset drift in yaw when the
* AHRS algorithm is being used without a magnetometer.
* @param fusionAhrs AHRS algorithm structure.
* @param yaw Yaw angle in degrees.
*/
void FusionAhrsSetYaw(FusionAhrs * const fusionAhrs, const float yaw) {
#define Q fusionAhrs->quaternion.element // define shorthand label for more readable code
fusionAhrs->quaternion = FusionQuaternionNormalise(fusionAhrs->quaternion); // quaternion must be normalised accurately (approximation not sufficient)
const float inverseYaw = atan2f(Q.x * Q.y + Q.w * Q.z, Q.w * Q.w - 0.5f + Q.x * Q.x); // Euler angle of conjugate
const float halfInverseYawMinusOffset = 0.5f * (inverseYaw - FusionDegreesToRadians(yaw));
const FusionQuaternion inverseYawQuaternion = {
.element.w = cosf(halfInverseYawMinusOffset),
.element.x = 0.0f,
.element.y = 0.0f,
.element.z = -1.0f * sinf(halfInverseYawMinusOffset),
};
fusionAhrs->quaternion = FusionQuaternionMultiply(inverseYawQuaternion, fusionAhrs->quaternion);
#undef Q // undefine shorthand label
}
//------------------------------------------------------------------------------
// End of file

84
imu/Fusion/FusionAhrs.h Normal file
View File

@ -0,0 +1,84 @@
/**
* @file FusionAhrs.h
* @author Seb Madgwick
* @brief The AHRS sensor fusion algorithm to combines gyroscope, accelerometer,
* and magnetometer measurements into a single measurement of orientation
* relative to the Earth (NWU convention).
*
* The algorithm behaviour is governed by a gain. A low gain will decrease the
* influence of the accelerometer and magnetometer so that the algorithm will
* better reject disturbances causes by translational motion and temporary
* magnetic distortions. However, a low gain will also increase the risk of
* drift due to gyroscope calibration errors. A typical gain value suitable for
* most applications is 0.5.
*
* The algorithm allows the application to define a minimum and maximum valid
* magnetic field magnitude. The algorithm will ignore magnetic measurements
* that fall outside of this range. This allows the algorithm to reject
* magnetic measurements that do not represent the direction of magnetic North.
* The typical magnitude of the Earth's magnetic field is between 20 uT and
* 70 uT.
*
* The algorithm can be used without a magnetometer. Measurements of
* orientation obtained using only gyroscope and accelerometer measurements
* can be expected to drift in the yaw component of orientation only. The
* application can reset the drift in yaw by setting the yaw to a specified
* angle at any time.
*
* The algorithm provides the measurement of orientation as a quaternion. The
* library includes functions for converting this quaternion to a rotation
* matrix and Euler angles.
*
* The algorithm also provides a measurement of linear acceleration and Earth
* acceleration. Linear acceleration is equal to the accelerometer measurement
* with the 1 g of gravity removed. Earth acceleration is a measurement of
* linear acceleration in the Earth coordinate frame.
*/
#ifndef FUSION_AHRS_H
#define FUSION_AHRS_H
//------------------------------------------------------------------------------
// Includes
#include "FusionTypes.h"
#include <stdbool.h>
//------------------------------------------------------------------------------
// Definitions
/**
* @brief AHRS algorithm structure. Structure members are used internally and
* should not be used by the user application.
*/
typedef struct {
float gain;
float initialGain;
float minimumMagneticFieldSquared;
float maximumMagneticFieldSquared;
FusionQuaternion quaternion; // describes the Earth relative to the sensor
FusionVector3 linearAcceleration;
float rampedGain;
bool zeroYawPending;
} FusionAhrs;
//------------------------------------------------------------------------------
// Function prototypes
void FusionAhrsInitialise(FusionAhrs * const fusionAhrs, const float gain, const float initialGain);
void FusionAhrsSetGain(FusionAhrs * const fusionAhrs, const float gain);
void FusionAhrsSetInitialGain(FusionAhrs * const fusionAhrs, const float initialGain);
void FusionAhrsSetMagneticField(FusionAhrs * const fusionAhrs, const float minimumMagneticField, const float maximumMagneticField);
void FusionAhrsUpdate(FusionAhrs * const fusionAhrs, const FusionVector3 gyroscope, const FusionVector3 accelerometer, const FusionVector3 magnetometer, const float samplePeriod);
void FusionAhrsUpdateWithoutMagnetometer(FusionAhrs * const fusionAhrs, const FusionVector3 gyroscope, const FusionVector3 accelerometer, const float samplePeriod);
FusionQuaternion FusionAhrsGetQuaternion(const FusionAhrs * const fusionAhrs);
FusionVector3 FusionAhrsGetLinearAcceleration(const FusionAhrs * const fusionAhrs);
FusionVector3 FusionAhrsGetEarthAcceleration(const FusionAhrs * const fusionAhrs);
void FusionAhrsReinitialise(FusionAhrs * const fusionAhrs);
bool FusionAhrsIsInitialising(const FusionAhrs * const fusionAhrs);
void FusionAhrsSetYaw(FusionAhrs * const fusionAhrs, const float yaw);
#endif
//------------------------------------------------------------------------------
// End of file

89
imu/Fusion/FusionBias.c Normal file
View File

@ -0,0 +1,89 @@
/**
* @file FusionBias.c
* @author Seb Madgwick
* @brief The gyroscope bias correction algorithm achieves run-time calibration
* of the gyroscope bias. The algorithm will detect when the gyroscope is
* stationary for a set period of time and then begin to sample gyroscope
* measurements to calculate the bias as an average.
*/
//------------------------------------------------------------------------------
// Includes
#include "FusionBias.h"
#include "math.h" // fabs
//------------------------------------------------------------------------------
// Definitions
/**
* @brief Minimum stationary period (in seconds) after which the the algorithm
* becomes active and begins sampling gyroscope measurements.
*/
#define STATIONARY_PERIOD (5.0f)
/**
* @brief Corner frequency (in Hz) of the high-pass filter used to sample the
* gyroscope bias.
*/
#define CORNER_FREQUENCY (0.02f)
//------------------------------------------------------------------------------
// Functions
/**
* @brief Initialises the gyroscope bias correction algorithm.
* @param fusionBias FusionBias structure.
* @param threshold Gyroscope threshold (in degrees per second) below which the
* gyroscope is detected stationary.
* @param samplePeriod Nominal sample period (in seconds) corresponding the rate
* at which the application will update the algorithm.
*/
void FusionBiasInitialise(FusionBias * const fusionBias, const float threshold, const float samplePeriod) {
fusionBias->threshold = threshold;
fusionBias->filterCoefficient = (2.0f * M_PI * CORNER_FREQUENCY) * samplePeriod;
fusionBias->stationaryTimer = 0.0f;
fusionBias->gyroscopeBias = FUSION_VECTOR3_ZERO;
}
/**
* @brief Updates the gyroscope bias correction algorithm and returns the
* corrected gyroscope measurement.
* @param fusionBias FusionBias structure.
* @param gyroscope Gyroscope measurement in degrees per second.
* @param dt Nominal sample period (in seconds) corresponding the rate
* @return Corrected gyroscope measurement in degrees per second.
*/
FusionVector3 FusionBiasUpdate(FusionBias * const fusionBias, FusionVector3 gyroscope, float dt) {
// Subtract bias from gyroscope measurement
gyroscope = FusionVectorSubtract(gyroscope, fusionBias->gyroscopeBias);
// Reset stationary timer if gyroscope not stationary
if ((fabsf(gyroscope.axis.x) > fusionBias->threshold) || (fabsf(gyroscope.axis.y) > fusionBias->threshold) || (fabsf(gyroscope.axis.z) > fusionBias->threshold)) {
fusionBias->stationaryTimer = 0.0f;
return gyroscope;
}
// Increment stationary timer while gyroscope stationary
if (fusionBias->stationaryTimer < STATIONARY_PERIOD) {
fusionBias->stationaryTimer += dt;
return gyroscope;
}
// Adjust bias if stationary timer has elapsed
fusionBias->gyroscopeBias = FusionVectorAdd(fusionBias->gyroscopeBias, FusionVectorMultiplyScalar(gyroscope, fusionBias->filterCoefficient));
return gyroscope;
}
/**
* @brief Returns true if the gyroscope bias correction algorithm is active.
* @param fusionBias FusionBias structure.
* @return True if the gyroscope bias correction algorithm is active.
*/
bool FusionBiasIsActive(FusionBias * const fusionBias) {
return fusionBias->stationaryTimer >= STATIONARY_PERIOD;
}
//------------------------------------------------------------------------------
// End of file

43
imu/Fusion/FusionBias.h Normal file
View File

@ -0,0 +1,43 @@
/**
* @file FusionBias.h
* @author Seb Madgwick
* @brief The gyroscope bias correction algorithm achieves run-time calibration
* of the gyroscope bias. The algorithm will detect when the gyroscope is
* stationary for a set period of time and then begin to sample gyroscope
* measurements to calculate the bias as an average.
*/
#ifndef FUSION_BIAS_H
#define FUSION_BIAS_H
//------------------------------------------------------------------------------
// Includes
#include "FusionTypes.h"
#include <stdbool.h>
//------------------------------------------------------------------------------
// Definitions
/**
* @brief Gyroscope bias correction algorithm structure. Structure members are
* used internally and should not be used by the user application.
*/
typedef struct {
float threshold;
float filterCoefficient;
float stationaryTimer;
FusionVector3 gyroscopeBias;
} FusionBias;
//------------------------------------------------------------------------------
// Function prototypes
void FusionBiasInitialise(FusionBias * const fusionBias, const float threshold, const float samplePeriod);
FusionVector3 FusionBiasUpdate(FusionBias * const fusionBias, FusionVector3 gyroscope, float dt);
bool FusionBiasIsActive(FusionBias * const fusionBias);
#endif
//------------------------------------------------------------------------------
// End of file

49
imu/Fusion/FusionCalibration.h Normal file
View File

@ -0,0 +1,49 @@
/**
* @file FusionCalibration.h
* @author Seb Madgwick
* @brief Gyroscope, accelerometer, and magnetometer calibration model.
*
* Static inline implementations are used to optimise for increased execution
* speed.
*/
#ifndef FUSION_CALIBRATION_H
#define FUSION_CALIBRATION_H
//------------------------------------------------------------------------------
// Includes
#include "FusionTypes.h"
//------------------------------------------------------------------------------
// Inline functions
/**
* @brief Gyroscope and accelerometer calibration model.
* @param uncalibrated Uncalibrated gyroscope or accelerometer measurement in
* lsb.
* @param misalignment Misalignment matrix (may not be a true rotation matrix).
* @param sensitivity Sensitivity in g per lsb for an accelerometer and degrees
* per second per lsb for a gyroscope.
* @param bias Bias in lsb.
* @return Calibrated gyroscope or accelerometer measurement.
*/
static inline __attribute__((always_inline)) FusionVector3 FusionCalibrationInertial(const FusionVector3 uncalibrated, const FusionRotationMatrix misalignment, const FusionVector3 sensitivity, const FusionVector3 bias) {
return FusionRotationMatrixMultiplyVector(misalignment, FusionVectorHadamardProduct(FusionVectorSubtract(uncalibrated, bias), sensitivity));
}
/**
* @brief Magnetometer calibration model.
* @param magnetometer Uncalibrated magnetometer measurement in uT.
* @param softIronMatrix Soft-iron matrix (may not be a true rotation matrix).
* @param hardIronBias Hard-iron bias in uT.
* @return Calibrated magnetometer measurement.
*/
static inline __attribute__((always_inline)) FusionVector3 FusionCalibrationMagnetic(const FusionVector3 uncalibrated, const FusionRotationMatrix softIronMatrix, const FusionVector3 hardIronBias) {
return FusionVectorSubtract(FusionRotationMatrixMultiplyVector(softIronMatrix, uncalibrated), hardIronBias);
}
#endif
//------------------------------------------------------------------------------
// End of file

37
imu/Fusion/FusionCompass.c Normal file
View File

@ -0,0 +1,37 @@
/**
* @file FusionCompass.c
* @author Seb Madgwick
* @brief The tilt-compensated compass calculates an angular heading relative to
* magnetic north using accelerometer and magnetometer measurements (NWU
* convention).
*/
//------------------------------------------------------------------------------
// Includes
#include "FusionCompass.h"
#include <math.h> // atan2f
//------------------------------------------------------------------------------
// Functions
/**
* @brief Calculates the tilt-compensated heading relative to magnetic north.
* @param accelerometer Accelerometer measurement in any calibrated units.
* @param magnetometer Magnetometer measurement in any calibrated units.
* @return Heading angle in degrees.
*/
float FusionCompassCalculateHeading(const FusionVector3 accelerometer, const FusionVector3 magnetometer) {
// Compute direction of 'magnetic west' (Earth's y axis)
const FusionVector3 magneticWest = FusionVectorFastNormalise(FusionVectorCrossProduct(accelerometer, magnetometer));
// Compute direction of magnetic north (Earth's x axis)
const FusionVector3 magneticNorth = FusionVectorFastNormalise(FusionVectorCrossProduct(magneticWest, accelerometer));
// Calculate angular heading relative to magnetic north
return FusionRadiansToDegrees(atan2f(magneticWest.axis.x, magneticNorth.axis.x));
}
//------------------------------------------------------------------------------
// End of file

25
imu/Fusion/FusionCompass.h Normal file
View File

@ -0,0 +1,25 @@
/**
* @file FusionCompass.h
* @author Seb Madgwick
* @brief The tilt-compensated compass calculates an angular heading relative to
* magnetic north using accelerometer and magnetometer measurements (NWU
* convention).
*/
#ifndef FUSION_COMPASS_H
#define FUSION_COMPASS_H
//------------------------------------------------------------------------------
// Includes
#include "FusionTypes.h"
//------------------------------------------------------------------------------
// Function prototypes
float FusionCompassCalculateHeading(const FusionVector3 accelerometer, const FusionVector3 magnetometer);
#endif
//------------------------------------------------------------------------------
// End of file

470
imu/Fusion/FusionTypes.h Normal file
View File

@ -0,0 +1,470 @@
/**
* @file FusionTypes.h
* @author Seb Madgwick
* @brief Common types and their associated operations.
*
* Static inline implementations are used to optimise for increased execution
* speed.
*/
#ifndef FUSION_TYPES_H
#define FUSION_TYPES_H
//------------------------------------------------------------------------------
// Includes
#include <math.h> // M_PI, sqrtf, atan2f, asinf
#include <stdint.h> // int32_t
//------------------------------------------------------------------------------
// Definitions
/**
* @brief Three-dimensional spacial vector.
*/
typedef union {
float array[3];
struct {
float x;
float y;
float z;
} axis;
} FusionVector3;
/**
* @brief Quaternion. This library uses the conversion of placing the 'w'
* element as the first element. Other implementations may place the 'w'
* element as the last element.
*/
typedef union {
float array[4];
struct {
float w;
float x;
float y;
float z;
} element;
} FusionQuaternion;
/**
* @brief Rotation matrix in row-major order.
* See http://en.wikipedia.org/wiki/Row-major_order
*/
typedef union {
float array[9];
struct {
float xx;
float xy;
float xz;
float yx;
float yy;
float yz;
float zx;
float zy;
float zz;
} element;
} FusionRotationMatrix;
/**
* @brief Euler angles union. The Euler angles are in the Aerospace sequence
* also known as the ZYX sequence.
*/
typedef union {
float array[3];
struct {
float roll;
float pitch;
float yaw;
} angle;
} FusionEulerAngles;
/**
* @brief Zero-length vector definition.
*/
#define FUSION_VECTOR3_ZERO ((FusionVector3){ .array = {0.0f, 0.0f, 0.0f} })
/**
* @brief Quaternion identity definition to represent an aligned of
* orientation.
*/
#define FUSION_QUATERNION_IDENTITY ((FusionQuaternion){ .array = {1.0f, 0.0f, 0.0f, 0.0f} })
/**
* @brief Rotation matrix identity definition to represent an aligned of
* orientation.
*/
#define FUSION_ROTATION_MATRIX_IDENTITY ((FusionRotationMatrix){ .array = {1.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 1.0f} })
/**
* @brief Euler angles zero definition to represent an aligned of orientation.
*/
#define FUSION_EULER_ANGLES_ZERO ((FusionEulerAngles){ .array = {0.0f, 0.0f, 0.0f} })
/**
* @brief Definition of M_PI. Some compilers may not define this in math.h.
*/
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
//------------------------------------------------------------------------------
// Inline functions - Degrees and radians conversion
/**
* @brief Converts degrees to radians.
* @param degrees Degrees.
* @return Radians.
*/
static inline __attribute__((always_inline)) float FusionDegreesToRadians(const float degrees) {
return degrees * ((float) M_PI / 180.0f);
}
/**
* @brief Converts radians to degrees.
* @param radians Radians.
* @return Degrees.
*/
static inline __attribute__((always_inline)) float FusionRadiansToDegrees(const float radians) {
return radians * (180.0f / (float) M_PI);
}
//------------------------------------------------------------------------------
// Inline functions - Fast inverse square root
/**
* @brief Calculates the reciprocal of the square root.
* See http://en.wikipedia.org/wiki/Fast_inverse_square_root
* @param x Operand.
* @return Reciprocal of the square root of x.
*/
static inline __attribute__((always_inline)) float FusionFastInverseSqrt(const float x) {
if (x == 0.0) {
return 0.0;
}
return 1.0 / sqrtf(x);
}
//------------------------------------------------------------------------------
// Inline functions - Vector operations
/**
* @brief Adds two vectors.
* @param vectorA First vector of the operation.
* @param vectorB Second vector of the operation.
* @return Sum of vectorA and vectorB.
*/
static inline __attribute__((always_inline)) FusionVector3 FusionVectorAdd(const FusionVector3 vectorA, const FusionVector3 vectorB) {
FusionVector3 result;
result.axis.x = vectorA.axis.x + vectorB.axis.x;
result.axis.y = vectorA.axis.y + vectorB.axis.y;
result.axis.z = vectorA.axis.z + vectorB.axis.z;
return result;
}
/**
* @brief Subtracts two vectors.
* @param vectorA First vector of the operation.
* @param vectorB Second vector of the operation.
* @return vectorB subtracted from vectorA.
*/
static inline __attribute__((always_inline)) FusionVector3 FusionVectorSubtract(const FusionVector3 vectorA, const FusionVector3 vectorB) {
FusionVector3 result;
result.axis.x = vectorA.axis.x - vectorB.axis.x;
result.axis.y = vectorA.axis.y - vectorB.axis.y;
result.axis.z = vectorA.axis.z - vectorB.axis.z;
return result;
}
/**
* @brief Multiplies vector by a scalar.
* @param vector Vector to be multiplied.
* @param scalar Scalar to be multiplied.
* @return Vector multiplied by scalar.
*/
static inline __attribute__((always_inline)) FusionVector3 FusionVectorMultiplyScalar(const FusionVector3 vector, const float scalar) {
FusionVector3 result;
result.axis.x = vector.axis.x * scalar;
result.axis.y = vector.axis.y * scalar;
result.axis.z = vector.axis.z * scalar;
return result;
}
/**
* @brief Calculates the Hadamard product (element-wise multiplication) of two
* vectors.
* @param vectorA First vector of the operation.
* @param vectorB Second vector of the operation.
* @return Hadamard product of vectorA and vectorB.
*/
static inline __attribute__((always_inline)) FusionVector3 FusionVectorHadamardProduct(const FusionVector3 vectorA, const FusionVector3 vectorB) {
FusionVector3 result;
result.axis.x = vectorA.axis.x * vectorB.axis.x;
result.axis.y = vectorA.axis.y * vectorB.axis.y;
result.axis.z = vectorA.axis.z * vectorB.axis.z;
return result;
}
/**
* @brief Calculates the cross-product of two vectors.
* @param vectorA First vector of the operation.
* @param vectorB Second vector of the operation.
* @return Cross-product of vectorA and vectorB.
*/
static inline __attribute__((always_inline)) FusionVector3 FusionVectorCrossProduct(const FusionVector3 vectorA, const FusionVector3 vectorB) {
#define A vectorA.axis // define shorthand labels for more readable code
#define B vectorB.axis
FusionVector3 result;
result.axis.x = A.y * B.z - A.z * B.y;
result.axis.y = A.z * B.x - A.x * B.z;
result.axis.z = A.x * B.y - A.y * B.x;
return result;
#undef A // undefine shorthand labels
#undef B
}
/**
* @brief Calculates the vector magnitude squared.
* @param vector Vector of the operation.
* @return Vector magnitude squared.
*/
static inline __attribute__((always_inline)) float FusionVectorMagnitudeSquared(const FusionVector3 vector) {
#define V vector.axis // define shorthand label for more readable code
return V.x * V.x + V.y * V.y + V.z * V.z;
#undef V // undefine shorthand label
}
/**
* @brief Calculates the magnitude of a vector.
* @param vector Vector to be used in calculation.
* @return Vector magnitude.
*/
static inline __attribute__((always_inline)) float FusionVectorMagnitude(const FusionVector3 vector) {
return sqrtf(FusionVectorMagnitudeSquared(vector));
}
/**
* @brief Normalises a vector to be of unit magnitude.
* @param vector Vector to be normalised.
* @return Normalised vector.
*/
static inline __attribute__((always_inline)) FusionVector3 FusionVectorNormalise(const FusionVector3 vector) {
const float magnitudeReciprocal = 1.0f / sqrtf(FusionVectorMagnitudeSquared(vector));
return FusionVectorMultiplyScalar(vector, magnitudeReciprocal);
}
/**
* @brief Normalises a vector to be of unit magnitude using the fast inverse
* square root approximation.
* @param vector Vector to be normalised.
* @return Normalised vector.
*/
static inline __attribute__((always_inline)) FusionVector3 FusionVectorFastNormalise(const FusionVector3 vector) {
const float magnitudeReciprocal = FusionFastInverseSqrt(FusionVectorMagnitudeSquared(vector));
return FusionVectorMultiplyScalar(vector, magnitudeReciprocal);
}
//------------------------------------------------------------------------------
// Inline functions - Quaternion operations
/**
* @brief Adds two quaternions.
* @param quaternionA First quaternion of the operation.
* @param quaternionB Second quaternion of the operation.
* @return Sum of quaternionA and quaternionB.
*/
static inline __attribute__((always_inline)) FusionQuaternion FusionQuaternionAdd(const FusionQuaternion quaternionA, const FusionQuaternion quaternionB) {
FusionQuaternion result;
result.element.w = quaternionA.element.w + quaternionB.element.w;
result.element.x = quaternionA.element.x + quaternionB.element.x;
result.element.y = quaternionA.element.y + quaternionB.element.y;
result.element.z = quaternionA.element.z + quaternionB.element.z;
return result;
}
/**
* @brief Multiplies two quaternions.
* @param quaternionA First quaternion of the operation.
* @param quaternionB Second quaternion of the operation.
* @return quaternionA multiplied by quaternionB.
*/
static inline __attribute__((always_inline)) FusionQuaternion FusionQuaternionMultiply(const FusionQuaternion quaternionA, const FusionQuaternion quaternionB) {
#define A quaternionA.element // define shorthand labels for more readable code
#define B quaternionB.element
FusionQuaternion result;
result.element.w = A.w * B.w - A.x * B.x - A.y * B.y - A.z * B.z;
result.element.x = A.w * B.x + A.x * B.w + A.y * B.z - A.z * B.y;
result.element.y = A.w * B.y - A.x * B.z + A.y * B.w + A.z * B.x;
result.element.z = A.w * B.z + A.x * B.y - A.y * B.x + A.z * B.w;
return result;
#undef A // undefine shorthand labels
#undef B
}
/**
* @brief Multiplies quaternion by a vector. This is a normal quaternion
* multiplication where the vector is treated a quaternion with a 'w' element
* value of 0. The quaternion is post multiplied by the vector.
* @param quaternion Quaternion to be multiplied.
* @param vector Vector to be multiplied.
* @return Quaternion multiplied by vector.
*/
static inline __attribute__((always_inline)) FusionQuaternion FusionQuaternionMultiplyVector(const FusionQuaternion quaternion, const FusionVector3 vector) {
#define Q quaternion.element // define shorthand labels for more readable code
#define V vector.axis
FusionQuaternion result;
result.element.w = -Q.x * V.x - Q.y * V.y - Q.z * V.z;
result.element.x = Q.w * V.x + Q.y * V.z - Q.z * V.y;
result.element.y = Q.w * V.y - Q.x * V.z + Q.z * V.x;
result.element.z = Q.w * V.z + Q.x * V.y - Q.y * V.x;
return result;
#undef Q // undefine shorthand labels
#undef V
}
/**
* @brief Returns the quaternion conjugate.
* @param quaternion Quaternion to be conjugated.
* @return Conjugated quaternion.
*/
static inline __attribute__((always_inline)) FusionQuaternion FusionQuaternionConjugate(const FusionQuaternion quaternion) {
FusionQuaternion conjugate;
conjugate.element.w = quaternion.element.w;
conjugate.element.x = -1.0f * quaternion.element.x;
conjugate.element.y = -1.0f * quaternion.element.y;
conjugate.element.z = -1.0f * quaternion.element.z;
return conjugate;
}
/**
* @brief Normalises a quaternion to be of unit magnitude.
* @param quaternion Quaternion to be normalised.
* @return Normalised quaternion.
*/
static inline __attribute__((always_inline)) FusionQuaternion FusionQuaternionNormalise(const FusionQuaternion quaternion) {
#define Q quaternion.element // define shorthand label for more readable code
const float magnitudeReciprocal = 1.0f / sqrtf(Q.w * Q.w + Q.x * Q.x + Q.y * Q.y + Q.z * Q.z);
FusionQuaternion normalisedQuaternion;
normalisedQuaternion.element.w = Q.w * magnitudeReciprocal;
normalisedQuaternion.element.x = Q.x * magnitudeReciprocal;
normalisedQuaternion.element.y = Q.y * magnitudeReciprocal;
normalisedQuaternion.element.z = Q.z * magnitudeReciprocal;
return normalisedQuaternion;
#undef Q // undefine shorthand label
}
/**
* @brief Normalises a quaternion to be of unit magnitude using the fast inverse
* square root approximation.
* @param quaternion Quaternion to be normalised.
* @return Normalised quaternion.
*/
static inline __attribute__((always_inline)) FusionQuaternion FusionQuaternionFastNormalise(const FusionQuaternion quaternion) {
#define Q quaternion.element // define shorthand label for more readable code
const float magnitudeReciprocal = FusionFastInverseSqrt(Q.w * Q.w + Q.x * Q.x + Q.y * Q.y + Q.z * Q.z);
FusionQuaternion normalisedQuaternion;
normalisedQuaternion.element.w = Q.w * magnitudeReciprocal;
normalisedQuaternion.element.x = Q.x * magnitudeReciprocal;
normalisedQuaternion.element.y = Q.y * magnitudeReciprocal;
normalisedQuaternion.element.z = Q.z * magnitudeReciprocal;
return normalisedQuaternion;
#undef Q // undefine shorthand label
}
//------------------------------------------------------------------------------
// Inline functions - Rotation matrix operations
/**
* @brief Multiplies two rotation matrices.
* @param rotationMatrixA First rotation matrix of the operation.
* @param rotationMatrixB Second rotation matrix of the operation.
* @return rotationMatrixA with rotationMatrixB.
*/
static inline __attribute__((always_inline)) FusionRotationMatrix FusionRotationMatrixMultiply(const FusionRotationMatrix rotationMatrixA, const FusionRotationMatrix rotationMatrixB) {
#define A rotationMatrixA.element // define shorthand label for more readable code
#define B rotationMatrixB.element
FusionRotationMatrix result;
result.element.xx = A.xx * B.xx + A.xy * B.yx + A.xz * B.zx;
result.element.xy = A.xx * B.xy + A.xy * B.yy + A.xz * B.zy;
result.element.xz = A.xx * B.xz + A.xy * B.yz + A.xz * B.zz;
result.element.yx = A.yx * B.xx + A.yy * B.yx + A.yz * B.zx;
result.element.yy = A.yx * B.xy + A.yy * B.yy + A.yz * B.zy;
result.element.yz = A.yx * B.xz + A.yy * B.yz + A.yz * B.zz;
result.element.zx = A.zx * B.xx + A.zy * B.yx + A.zz * B.zx;
result.element.zy = A.zx * B.xy + A.zy * B.yy + A.zz * B.zy;
result.element.zz = A.zx * B.xz + A.zy * B.yz + A.zz * B.zz;
return result;
#undef A // undefine shorthand label
#undef B
}
/**
* @brief Multiplies rotation matrix with scalar.
* @param rotationMatrix Rotation matrix to be multiplied.
* @param vector Vector to be multiplied.
* @return Rotation matrix multiplied with scalar.
*/
static inline __attribute__((always_inline)) FusionVector3 FusionRotationMatrixMultiplyVector(const FusionRotationMatrix rotationMatrix, const FusionVector3 vector) {
#define R rotationMatrix.element // define shorthand label for more readable code
FusionVector3 result;
result.axis.x = R.xx * vector.axis.x + R.xy * vector.axis.y + R.xz * vector.axis.z;
result.axis.y = R.yx * vector.axis.x + R.yy * vector.axis.y + R.yz * vector.axis.z;
result.axis.z = R.zx * vector.axis.x + R.zy * vector.axis.y + R.zz * vector.axis.z;
return result;
#undef R // undefine shorthand label
}
//------------------------------------------------------------------------------
// Inline functions - Conversion operations
/**
* @brief Converts a quaternion to a rotation matrix.
* @param quaternion Quaternion to be converted.
* @return Rotation matrix.
*/
static inline __attribute__((always_inline)) FusionRotationMatrix FusionQuaternionToRotationMatrix(const FusionQuaternion quaternion) {
#define Q quaternion.element // define shorthand label for more readable code
const float qwqw = Q.w * Q.w; // calculate common terms to avoid repeated operations
const float qwqx = Q.w * Q.x;
const float qwqy = Q.w * Q.y;
const float qwqz = Q.w * Q.z;
const float qxqy = Q.x * Q.y;
const float qxqz = Q.x * Q.z;
const float qyqz = Q.y * Q.z;
FusionRotationMatrix rotationMatrix;
rotationMatrix.element.xx = 2.0f * (qwqw - 0.5f + Q.x * Q.x);
rotationMatrix.element.xy = 2.0f * (qxqy + qwqz);
rotationMatrix.element.xz = 2.0f * (qxqz - qwqy);
rotationMatrix.element.yx = 2.0f * (qxqy - qwqz);
rotationMatrix.element.yy = 2.0f * (qwqw - 0.5f + Q.y * Q.y);
rotationMatrix.element.yz = 2.0f * (qyqz + qwqx);
rotationMatrix.element.zx = 2.0f * (qxqz + qwqy);
rotationMatrix.element.zy = 2.0f * (qyqz - qwqx);
rotationMatrix.element.zz = 2.0f * (qwqw - 0.5f + Q.z * Q.z);
return rotationMatrix;
#undef Q // undefine shorthand label
}
/**
* @brief Converts a quaternion to Euler angles in degrees.
* @param quaternion Quaternion to be converted.
* @return Euler angles in degrees.
*/
static inline __attribute__((always_inline)) FusionEulerAngles FusionQuaternionToEulerAngles(const FusionQuaternion quaternion) {
#define Q quaternion.element // define shorthand label for more readable code
const float qwqwMinusHalf = Q.w * Q.w - 0.5f; // calculate common terms to avoid repeated operations
FusionEulerAngles eulerAngles;
eulerAngles.angle.roll = FusionRadiansToDegrees(atan2f(Q.y * Q.z - Q.w * Q.x, qwqwMinusHalf + Q.z * Q.z));
eulerAngles.angle.pitch = FusionRadiansToDegrees(-1.0f * asinf(2.0f * (Q.x * Q.z + Q.w * Q.y)));
eulerAngles.angle.yaw = FusionRadiansToDegrees(atan2f(Q.x * Q.y - Q.w * Q.z, qwqwMinusHalf + Q.x * Q.x));
return eulerAngles;
#undef Q // undefine shorthand label
}
#endif
//------------------------------------------------------------------------------
// End of file

3
imu/Fusion/LICENSE Normal file
View File

@ -0,0 +1,3 @@
This software is provided under the GNU General Public License.
https://www.gnu.org/licenses/gpl.html

25
imu/Fusion/README.md Normal file
View File

@ -0,0 +1,25 @@
# Fusion
Fusion is an ANSI C99 compliment sensor fusion library for sensor arrays of gyroscopes, accelerometers, and magnetometers. Fusion was specifically developed for use with embedded systems and has been optimised for execution speed. The library includes modules for: attitude and heading reference system (AHRS) sensor fusion, gyroscope bias correction, and a tilt-compensated compass.
## AHRS sensor fusion algorithm
The AHRS sensor fusion algorithm to combines gyroscope, accelerometer, and magnetometer measurements into a single measurement of orientation relative to the Earth (NWU convention).
The algorithm behaviour is governed by a gain. A low gain will decrease the influence of the accelerometer and magnetometer so that the algorithm will better reject disturbances causes by translational motion and temporary magnetic distortions. However, a low gain will also increase the risk of drift due to gyroscope calibration errors. A typical gain value suitable for most applications is 0.5.
The algorithm allows the application to define a minimum and maximum valid magnetic field magnitude. The algorithm will ignore magnetic measurements that fall outside of this range. This allows the algorithm to reject magnetic measurements that do not represent the direction of magnetic North. The typical magnitude of the Earth's magnetic field is between 20 uT and 70 uT.
The algorithm can be used without a magnetometer. Measurements of orientation obtained using only gyroscope and accelerometer measurements can be expected to drift in the yaw component of orientation only. The application can reset the drift in yaw by setting the yaw to a specified angle at any time.
The algorithm provides the measurement of orientation as a quaternion. The library includes functions for converting this quaternion to a rotation matrix and Euler angles.
The algorithm also provides a measurement of linear acceleration and Earth acceleration. Linear acceleration is equal to the accelerometer measurement with the 1 g of gravity removed. Earth acceleration is a measurement of linear acceleration in the Earth coordinate frame.
## Gyroscope bias correction algorithm
The gyroscope bias correction algorithm achieves run-time calibration of the gyroscope bias. The algorithm will detect when the gyroscope is stationary for a set period of time and then begin to sample gyroscope measurements to calculate the bias as an average.
## Tilt-compensated compass
The tilt-compensated compass calculates an angular heading relative to magnetic north using accelerometer and magnetometer measurements (NWU convention).

44
imu/imu.c
View File

@ -28,12 +28,14 @@
#include "bmi160_wrapper.h"
#include "lsm6ds3.h"
#include "utils.h"
#include "Fusion.h"
#include <math.h>
#include <string.h>
// Private variables
static ATTITUDE_INFO m_att;
static FusionAhrs m_fusionAhrs;
static float m_accel[3], m_gyro[3], m_mag[3];
static stkalign_t m_thd_work_area[THD_WORKING_AREA_SIZE(2048) / sizeof(stkalign_t)];
static i2c_bb_state m_i2c_bb;
@ -59,12 +61,7 @@ void imu_init(imu_config *set) {
memset(m_gyro_offset, 0, sizeof(m_gyro_offset));
imu_stop();
imu_ready = false;
init_time = chVTGetSystemTimeX();
ahrs_update_all_parameters(1.0, 10.0, 0.0, 2.0);
ahrs_init_attitude_info(&m_att);
imu_reset_orientation();
mpu9150_set_rate_hz(set->sample_rate_hz);
m_icm20948_state.rate_hz = set->sample_rate_hz;
@ -123,6 +120,14 @@ void imu_init(imu_config *set) {
terminal_gyro_info);
}
void imu_reset_orientation(void) {
imu_ready = false;
init_time = chVTGetSystemTimeX();
ahrs_init_attitude_info(&m_att);
FusionAhrsInitialise(&m_fusionAhrs, 2.0, 20.0);
ahrs_update_all_parameters(1.0, 10.0, 0.0, 2.0);
}
i2c_bb_state *imu_get_i2c(void) {
return &m_i2c_bb;
}
@ -417,7 +422,9 @@ void imu_get_calibration(float yaw, float *imu_cal) {
m_settings.gyro_offset_comp_fact[0] = backup_gyro_comp_x;
m_settings.gyro_offset_comp_fact[1] = backup_gyro_comp_y;
m_settings.gyro_offset_comp_fact[2] = backup_gyro_comp_z;
ahrs_init_attitude_info(&m_att);
FusionAhrsReinitialise(&m_fusionAhrs);
}
static void imu_read_callback(float *accel, float *gyro, float *mag) {
@ -431,6 +438,10 @@ static void imu_read_callback(float *accel, float *gyro, float *mag) {
m_settings.mahony_kp,
m_settings.mahony_ki,
m_settings.madgwick_beta);
FusionAhrsSetGain(&m_fusionAhrs, m_settings.madgwick_beta);
FusionAhrsSetInitialGain(&m_fusionAhrs, m_settings.madgwick_beta * 10.0);
imu_ready = true;
}
@ -498,12 +509,29 @@ static void imu_read_callback(float *accel, float *gyro, float *mag) {
gyro_rad[2] = m_gyro[2] * M_PI / 180.0;
switch (m_settings.mode) {
case (AHRS_MODE_MADGWICK):
case AHRS_MODE_MADGWICK:
ahrs_update_madgwick_imu(gyro_rad, m_accel, dt, (ATTITUDE_INFO *)&m_att);
break;
case (AHRS_MODE_MAHONY):
case AHRS_MODE_MAHONY:
ahrs_update_mahony_imu(gyro_rad, m_accel, dt, (ATTITUDE_INFO *)&m_att);
break;
case AHRS_MODE_MADGWICK_FUSION: {
FusionVector3 calibratedGyroscope = {
.axis.x = m_gyro[0],
.axis.y = m_gyro[1],
.axis.z = m_gyro[2],
};
FusionVector3 calibratedAccelerometer = {
.axis.x = m_accel[0],
.axis.y = m_accel[1],
.axis.z = m_accel[2],
};
FusionAhrsUpdateWithoutMagnetometer(&m_fusionAhrs, calibratedGyroscope, calibratedAccelerometer, dt);
m_att.q0 = m_fusionAhrs.quaternion.element.w;
m_att.q1 = m_fusionAhrs.quaternion.element.x;
m_att.q2 = m_fusionAhrs.quaternion.element.y;
m_att.q3 = m_fusionAhrs.quaternion.element.z;
} break;
}
}

1
imu/imu.h
View File

@ -26,6 +26,7 @@
#include "spi_bb.h"
void imu_init(imu_config *set);
void imu_reset_orientation(void);
i2c_bb_state *imu_get_i2c(void);
void imu_init_mpu9x50(stm32_gpio_t *sda_gpio, int sda_pin,
stm32_gpio_t *scl_gpio, int scl_pin);

8
imu/imu.mk
View File

@ -4,7 +4,11 @@ IMUSRC = imu/mpu9150.c \
imu/imu.c \
imu/BMI160_driver/bmi160.c \
imu/bmi160_wrapper.c \
imu/lsm6ds3.c
imu/lsm6ds3.c \
imu/Fusion/FusionAhrs.c \
imu/Fusion/FusionBias.c \
imu/Fusion/FusionCompass.c
IMUINC = imu \
imu/BMI160_driver
imu/BMI160_driver \
imu/Fusion

2
main.c
View File

@ -278,6 +278,8 @@ int main(void) {
shutdown_init();
#endif
imu_reset_orientation();
#ifdef BOOT_OK_GPIO
chThdSleepMilliseconds(500);
palSetPad(BOOT_OK_GPIO, BOOT_OK_PIN);

10
mcpwm_foc.c
View File

@ -150,7 +150,7 @@ typedef struct {
float m_hfi_plot_sample;
float m_phase_before;
float m_duty_filtered;
float m_duty_abs_filtered;
bool m_was_full_brake;
bool m_was_control_duty;
float m_duty_i_term;
@ -2637,8 +2637,8 @@ void mcpwm_foc_adc_int_handler(void *p, uint32_t flags) {
float iq_set_tmp = motor_now->m_iq_set;
motor_now->m_motor_state.max_duty = conf_now->l_max_duty;
UTILS_LP_FAST(motor_now->m_duty_filtered, motor_now->m_motor_state.duty_now, 0.1);
utils_truncate_number((float*)&motor_now->m_duty_filtered, -1.0, 1.0);
UTILS_LP_FAST(motor_now->m_duty_abs_filtered, fabsf(motor_now->m_motor_state.duty_now), 0.1);
utils_truncate_number_abs((float*)&motor_now->m_duty_abs_filtered, 1.0);
float duty_set = motor_now->m_duty_cycle_set;
bool control_duty = motor_now->m_control_mode == CONTROL_MODE_DUTY ||
@ -2650,7 +2650,7 @@ void mcpwm_foc_adc_int_handler(void *p, uint32_t flags) {
// applying active braking. Use a bit of hysteresis when leaving
// the shorted mode.
if (motor_now->m_control_mode == CONTROL_MODE_CURRENT_BRAKE &&
fabsf(motor_now->m_duty_filtered) < conf_now->l_min_duty * 1.5 &&
motor_now->m_duty_abs_filtered < conf_now->l_min_duty * 1.5 &&
(motor_now->m_motor_state.i_abs * (motor_now->m_was_full_brake ? 1.0 : 1.5)) <
fminf(fabsf(iq_set_tmp), fabsf(conf_now->l_current_min))) {
control_duty = true;
@ -3084,7 +3084,7 @@ static void run_fw(volatile motor_all_state_t *motor, float dt) {
motor->m_control_mode == CONTROL_MODE_SPEED ||
motor->m_i_fw_set > motor->m_conf->cc_min_current)) {
float fw_current_now = 0.0;
float duty_abs = fabsf(motor->m_duty_filtered);
float duty_abs = motor->m_duty_abs_filtered;
if (motor->m_conf->foc_fw_duty_start < 0.99 &&
duty_abs > motor->m_conf->foc_fw_duty_start * motor->m_conf->l_max_duty) {