Showing preview only (4,797K chars total). Download the full file or copy to clipboard to get everything.
Repository: suhetao/stm32f4_mpu9250
Branch: master
Commit: b86160c2b781
Files: 201
Total size: 4.5 MB
Directory structure:
gitextract_61j4xxir/
├── .gitignore
├── Algorithm/
│ ├── inc/
│ │ ├── CKF.h
│ │ ├── Control.h
│ │ ├── Double.h
│ │ ├── EKF.h
│ │ ├── INS_EKF.h
│ │ ├── PID.h
│ │ ├── Quaternion.h
│ │ ├── SRCKF.h
│ │ └── UKF.h
│ └── src/
│ ├── CKF.C
│ ├── Control.c
│ ├── EKF.c
│ ├── INS_EKF.c
│ ├── PID.c
│ ├── Quaternion.c
│ ├── SRCKF.c
│ └── UKF.c
├── Application/
│ ├── inc/
│ │ ├── stm32f4_common.h
│ │ ├── stm32f4_crc.h
│ │ ├── stm32f4_delay.h
│ │ ├── stm32f4_dmp.h
│ │ ├── stm32f4_exti.h
│ │ ├── stm32f4_gps.h
│ │ ├── stm32f4_mpu9250.h
│ │ ├── stm32f4_ms5611.h
│ │ ├── stm32f4_rcc.h
│ │ ├── stm32f4_serial.h
│ │ ├── stm32f4_string.h
│ │ ├── stm32f4_ublox.h
│ │ ├── stm32f4xx_conf.h
│ │ └── stm32f4xx_it.h
│ └── src/
│ ├── main.c
│ ├── stm32f4_delay.c
│ ├── stm32f4_exti.c
│ ├── stm32f4_gps.c
│ ├── stm32f4_mpu9250.c
│ ├── stm32f4_ms5611.c
│ ├── stm32f4_rcc.c
│ ├── stm32f4_serial.c
│ ├── stm32f4_string.c
│ ├── stm32f4_ublox.c
│ └── system_stm32f4xx.c
├── Common/
│ ├── inc/
│ │ └── Memory.h
│ └── src/
│ └── Memory.c
├── Data/
│ ├── inc/
│ │ ├── Fifo.h
│ │ └── Queue.h
│ └── src/
│ ├── Fifo.c
│ └── Queue.c
├── Drivers/
│ ├── inc/
│ │ ├── stm32f4_exti.h
│ │ ├── stm32f4_gpio.h
│ │ ├── stm32f4_spi.h
│ │ └── stm32f4_usart.h
│ └── src/
│ ├── stm32f4_exti.c
│ ├── stm32f4_gpio.c
│ ├── stm32f4_spi.c
│ └── stm32f4_usart.c
├── Gps/
│ ├── inc/
│ │ ├── Map.h
│ │ └── Nema.h
│ └── src/
│ ├── Map.c
│ └── Nema.c
├── LICENSE
├── Libraries/
│ ├── CMSIS/
│ │ ├── Device/
│ │ │ └── ST/
│ │ │ └── STM32F4xx/
│ │ │ ├── Include/
│ │ │ │ ├── stm32f4xx.h
│ │ │ │ └── system_stm32f4xx.h
│ │ │ ├── Release_Notes.html
│ │ │ └── Source/
│ │ │ └── Templates/
│ │ │ ├── TASKING/
│ │ │ │ └── cstart_thumb2.asm
│ │ │ ├── TrueSTUDIO/
│ │ │ │ ├── startup_stm32f401xx.s
│ │ │ │ ├── startup_stm32f40_41xxx.s
│ │ │ │ ├── startup_stm32f40xx.s
│ │ │ │ ├── startup_stm32f427_437xx.s
│ │ │ │ ├── startup_stm32f427xx.s
│ │ │ │ └── startup_stm32f429_439xx.s
│ │ │ ├── arm/
│ │ │ │ ├── startup_stm32f401xx.s
│ │ │ │ ├── startup_stm32f40_41xxx.s
│ │ │ │ ├── startup_stm32f40xx.s
│ │ │ │ ├── startup_stm32f427_437xx.s
│ │ │ │ ├── startup_stm32f427x.s
│ │ │ │ └── startup_stm32f429_439xx.s
│ │ │ ├── gcc_ride7/
│ │ │ │ ├── startup_stm32f401xx.s
│ │ │ │ ├── startup_stm32f40_41xxx.s
│ │ │ │ ├── startup_stm32f40xx.s
│ │ │ │ ├── startup_stm32f427_437xx.s
│ │ │ │ ├── startup_stm32f427x.s
│ │ │ │ └── startup_stm32f429_439xx.s
│ │ │ ├── iar/
│ │ │ │ ├── startup_stm32f401xx.s
│ │ │ │ ├── startup_stm32f40_41xxx.s
│ │ │ │ ├── startup_stm32f40xx.s
│ │ │ │ ├── startup_stm32f427_437xx.s
│ │ │ │ ├── startup_stm32f427x.s
│ │ │ │ └── startup_stm32f429_439xx.s
│ │ │ └── system_stm32f4xx.c
│ │ ├── Include/
│ │ │ ├── arm_common_tables.h
│ │ │ ├── arm_const_structs.h
│ │ │ ├── arm_math.h
│ │ │ ├── core_cm0.h
│ │ │ ├── core_cm0plus.h
│ │ │ ├── core_cm3.h
│ │ │ ├── core_cm4.h
│ │ │ ├── core_cm4_simd.h
│ │ │ ├── core_cmFunc.h
│ │ │ ├── core_cmInstr.h
│ │ │ ├── core_sc000.h
│ │ │ └── core_sc300.h
│ │ ├── README.txt
│ │ └── index.html
│ ├── STM32F4xx_StdPeriph_Driver/
│ │ ├── Release_Notes.html
│ │ ├── inc/
│ │ │ ├── misc.h
│ │ │ ├── stm32f4xx_adc.h
│ │ │ ├── stm32f4xx_can.h
│ │ │ ├── stm32f4xx_crc.h
│ │ │ ├── stm32f4xx_cryp.h
│ │ │ ├── stm32f4xx_dac.h
│ │ │ ├── stm32f4xx_dbgmcu.h
│ │ │ ├── stm32f4xx_dcmi.h
│ │ │ ├── stm32f4xx_dma.h
│ │ │ ├── stm32f4xx_dma2d.h
│ │ │ ├── stm32f4xx_exti.h
│ │ │ ├── stm32f4xx_flash.h
│ │ │ ├── stm32f4xx_fmc.h
│ │ │ ├── stm32f4xx_fsmc.h
│ │ │ ├── stm32f4xx_gpio.h
│ │ │ ├── stm32f4xx_hash.h
│ │ │ ├── stm32f4xx_i2c.h
│ │ │ ├── stm32f4xx_iwdg.h
│ │ │ ├── stm32f4xx_ltdc.h
│ │ │ ├── stm32f4xx_pwr.h
│ │ │ ├── stm32f4xx_rcc.h
│ │ │ ├── stm32f4xx_rng.h
│ │ │ ├── stm32f4xx_rtc.h
│ │ │ ├── stm32f4xx_sai.h
│ │ │ ├── stm32f4xx_sdio.h
│ │ │ ├── stm32f4xx_spi.h
│ │ │ ├── stm32f4xx_syscfg.h
│ │ │ ├── stm32f4xx_tim.h
│ │ │ ├── stm32f4xx_usart.h
│ │ │ └── stm32f4xx_wwdg.h
│ │ └── src/
│ │ ├── misc.c
│ │ ├── stm32f4xx_adc.c
│ │ ├── stm32f4xx_can.c
│ │ ├── stm32f4xx_crc.c
│ │ ├── stm32f4xx_cryp.c
│ │ ├── stm32f4xx_cryp_aes.c
│ │ ├── stm32f4xx_cryp_des.c
│ │ ├── stm32f4xx_cryp_tdes.c
│ │ ├── stm32f4xx_dac.c
│ │ ├── stm32f4xx_dbgmcu.c
│ │ ├── stm32f4xx_dcmi.c
│ │ ├── stm32f4xx_dma.c
│ │ ├── stm32f4xx_dma2d.c
│ │ ├── stm32f4xx_exti.c
│ │ ├── stm32f4xx_flash.c
│ │ ├── stm32f4xx_fmc.c
│ │ ├── stm32f4xx_fsmc.c
│ │ ├── stm32f4xx_gpio.c
│ │ ├── stm32f4xx_hash.c
│ │ ├── stm32f4xx_hash_md5.c
│ │ ├── stm32f4xx_hash_sha1.c
│ │ ├── stm32f4xx_i2c.c
│ │ ├── stm32f4xx_iwdg.c
│ │ ├── stm32f4xx_ltdc.c
│ │ ├── stm32f4xx_pwr.c
│ │ ├── stm32f4xx_rcc.c
│ │ ├── stm32f4xx_rng.c
│ │ ├── stm32f4xx_rtc.c
│ │ ├── stm32f4xx_sai.c
│ │ ├── stm32f4xx_sdio.c
│ │ ├── stm32f4xx_spi.c
│ │ ├── stm32f4xx_syscfg.c
│ │ ├── stm32f4xx_tim.c
│ │ ├── stm32f4xx_usart.c
│ │ └── stm32f4xx_wwdg.c
│ └── eMPL/
│ ├── dmpKey.h
│ ├── dmpmap.h
│ ├── inv_mpu.c
│ ├── inv_mpu.h
│ ├── inv_mpu_dmp_motion_driver.c
│ └── inv_mpu_dmp_motion_driver.h
├── Math/
│ ├── inc/
│ │ └── FastMath.h
│ └── src/
│ └── FastMath.c
├── Matrix/
│ ├── inc/
│ │ ├── DoubleMatrix.h
│ │ └── Matrix.h
│ └── src/
│ ├── DoubleMatrix.c
│ └── Matrix.c
├── Project/
│ ├── JLinkLog.txt
│ ├── JLinkSettings.ini
│ ├── stm32f4_dmp.uvopt
│ └── stm32f4_dmp.uvproj
├── README.md
├── miniAHRS/
│ ├── miniAHRS.c
│ └── miniAHRS.h
└── miniIMU/
├── FP/
│ ├── FP_Math.c
│ ├── FP_Math.h
│ ├── FP_Matrix.c
│ ├── FP_Matrix.h
│ ├── FP_miniIMU.c
│ └── FP_miniIMU.h
├── Usage.txt
├── miniIMU.c
├── miniIMU.h
├── miniMatrix.c
└── miniMatrix.h
================================================
FILE CONTENTS
================================================
================================================
FILE: .gitignore
================================================
# Object files
*.o
*.ko
*.obj
*.elf
# Precompiled Headers
*.gch
*.pch
# Libraries
*.lib
*.a
*.la
*.lo
# Shared objects (inc. Windows DLLs)
*.dll
*.so
*.so.*
*.dylib
# Executables
*.exe
*.out
*.app
*.i*86
*.x86_64
*.hex
================================================
FILE: Algorithm/inc/CKF.h
================================================
/*
The MIT License (MIT)
Copyright (c) 2015-? suhetao
Permission is hereby granted, free of charge, to any person obtaining a copy of
this software and associated documentation files (the "Software"), to deal in
the Software without restriction, including without limitation the rights to
use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
the Software, and to permit persons to whom the Software is furnished to do so,
subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
#ifndef _CKF_H
#define _CKF_H
#include "Matrix.h"
//7-state q0 q1 q2 q3 wx wy wz
#define CKF_STATE_DIM 7
//13-measurement q0 q1 q2 q3 ax ay az wx wy wz mx my mz
#define CKF_MEASUREMENT_DIM 13
#define CKF_CP_POINTS 14//(2 * CKF_STATE_DIM)
#define CKF_HALFPI 1.5707963267948966192313216916398f
#define CKF_PI 3.1415926535897932384626433832795f
#define CKF_TWOPI 6.283185307179586476925286766559f
#define CKF_TODEG(x) ((x) * 57.2957796f)
typedef struct CKF_FILTER_T{
//weights
float32_t W;
//Kesi
float32_t Kesi_f32[CKF_STATE_DIM * CKF_CP_POINTS];
float32_t iKesi_f32[CKF_STATE_DIM];
//state covariance
float32_t P_f32[CKF_STATE_DIM * CKF_STATE_DIM];
float32_t PX_f32[CKF_STATE_DIM * CKF_STATE_DIM];
float32_t PY_f32[CKF_MEASUREMENT_DIM * CKF_MEASUREMENT_DIM];
float32_t tmpPY_f32[CKF_MEASUREMENT_DIM * CKF_MEASUREMENT_DIM];
float32_t PXY_f32[CKF_STATE_DIM * CKF_MEASUREMENT_DIM];
float32_t tmpPXY_f32[CKF_STATE_DIM * CKF_MEASUREMENT_DIM];
float32_t Q_f32[CKF_STATE_DIM * CKF_STATE_DIM];
float32_t R_f32[CKF_MEASUREMENT_DIM * CKF_MEASUREMENT_DIM];
//cubature points
float32_t XCP_f32[CKF_STATE_DIM * CKF_CP_POINTS];
float32_t XminusCP_f32[CKF_STATE_DIM * CKF_CP_POINTS];
float32_t YSP_f32[CKF_MEASUREMENT_DIM * CKF_CP_POINTS];
//Kalman gain
float32_t K_f32[CKF_STATE_DIM * CKF_MEASUREMENT_DIM];
float32_t KT_f32[CKF_MEASUREMENT_DIM * CKF_STATE_DIM];
//state vector
float32_t X_f32[CKF_STATE_DIM];
float32_t XT_f32[CKF_STATE_DIM];
float32_t Xminus_f32[CKF_STATE_DIM];
float32_t XminusT_f32[CKF_STATE_DIM];
float32_t tmpX_f32[CKF_STATE_DIM];
float32_t tmpS_f32[CKF_STATE_DIM];
//measurement vector
float32_t Y_f32[CKF_MEASUREMENT_DIM];
float32_t YT_f32[CKF_MEASUREMENT_DIM];
float32_t Yminus_f32[CKF_MEASUREMENT_DIM];
float32_t YminusT_f32[CKF_MEASUREMENT_DIM];
float32_t tmpY_f32[CKF_MEASUREMENT_DIM];
//
arm_matrix_instance_f32 Kesi;
arm_matrix_instance_f32 iKesi;
arm_matrix_instance_f32 P;
arm_matrix_instance_f32 PX;
arm_matrix_instance_f32 PY;
arm_matrix_instance_f32 tmpPY;
arm_matrix_instance_f32 PXY;
arm_matrix_instance_f32 tmpPXY;
arm_matrix_instance_f32 Q;
arm_matrix_instance_f32 R;
//
arm_matrix_instance_f32 XCP;
arm_matrix_instance_f32 XminusCP;
arm_matrix_instance_f32 YCP;
//
arm_matrix_instance_f32 K;
arm_matrix_instance_f32 KT;
//
arm_matrix_instance_f32 X;
arm_matrix_instance_f32 XT;
arm_matrix_instance_f32 Xminus;
arm_matrix_instance_f32 XminusT;
arm_matrix_instance_f32 tmpX;
arm_matrix_instance_f32 tmpS;
arm_matrix_instance_f32 Y;
arm_matrix_instance_f32 YT;
arm_matrix_instance_f32 Yminus;
arm_matrix_instance_f32 YminusT;
arm_matrix_instance_f32 tmpY;
}CKF_Filter;
void CKF_New(CKF_Filter* ckf);
void CKF_Init(CKF_Filter* ckf, float32_t *q, float32_t *gyro);
void CKF_Update(CKF_Filter* ckf, float32_t *q, float32_t *gyro, float32_t *accel, float32_t *mag, float32_t dt);
void CKF_GetAngle(CKF_Filter* ckf, float32_t* rpy);
__inline void CKF_GetQ(CKF_Filter* ckf, float32_t* Q)
{
Q[0] = ckf->X_f32[0];
Q[1] = ckf->X_f32[1];
Q[2] = ckf->X_f32[2];
Q[3] = ckf->X_f32[3];
}
#endif
================================================
FILE: Algorithm/inc/Control.h
================================================
/*
The MIT License (MIT)
Copyright (c) 2015-? suhetao
Permission is hereby granted, free of charge, to any person obtaining a copy of
this software and associated documentation files (the "Software"), to deal in
the Software without restriction, including without limitation the rights to
use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
the Software, and to permit persons to whom the Software is furnished to do so,
subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
#ifndef _CONTROL_H_
#define _CONTROL_H_
#include "PID.h"
//Modelling, Identification and Control of a Quadrotor Helicopter
#define PI (3.1415926535897932384626433832795f)
#define PI_6 (0.52359877559829887307710723054658f)
#define MIN_ANG (-PI_6)
#define MAX_ANG PI_6
typedef struct CONTROLLERPARAMETER_T
{
float m; //mass of the quadrotor
float Ixx; //body moment of inertia around the x-axis
float Iyy; //body moment of inertia around the y-axis
float Izz; //body moment of inertia around the z-axis
float I[9]; //body inertia matrix
float d; //drag factor
float b; //thrust factor
float l; //center of quadrotor to center of propeller distance
float X;
float Y;
float Z;
PIDController PostionX;
PIDController PostionY;
PIDController PostionZ;
// not use yet
float g; //acceleration due to gravity
float n; //number of data acquired
float N; //gear box reduction ratio
float h; //PWM code vector
float R; //motor resistance
float Jtp; //total rotational moment of inertia around the propeller axis
float Ke; //electric motor constant
float Km; //mechanic motor constant
float L; //motor inductance
}QuadrotorParameter;
typedef enum TRAJECTORY_T{
X = 0,
Y = 1,
Z = 2,
PSI = 3
}Trajectory;
typedef enum EULER_T{
ROLL = 0,
PITCH = 1,
YAW = 2
}Euler;
void EulerConv(float* dt, float *deta);
void TorqueConv(float *eta, float *deta, float *dt);
void ForceConv(float *eta, float dz, float *df);
void TorqueInv(float *dt, float df, float *domega);
#endif
================================================
FILE: Algorithm/inc/Double.h
================================================
/*
The MIT License (MIT)
Copyright (c) 2015-? suhetao
Permission is hereby granted, free of charge, to any person obtaining a copy of
this software and associated documentation files (the "Software"), to deal in
the Software without restriction, including without limitation the rights to
use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
the Software, and to permit persons to whom the Software is furnished to do so,
subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
#ifndef _DOUBLE_H
#define _DOUBLE_H
//////////////////////////////////////////////////////////////////////////
//algorithm from the book
//<<Fast and Accurate Finite-Element Multigrid Solvers for PDE Simulations on GPU Clusters>>
//References:
//Emulated double precision Double single routine from nvidia developer zone
//////////////////////////////////////////////////////////////////////////
//define my own double struct
typedef struct
{
float hi;
float lo;
} Double;
//////////////////////////////////////////////////////////////////////////
//transmit double precision float to my own Double
__inline Double intToDouble(int A)
{
Double B;
B.hi = (float)A;
B.lo = 0.0f;
return B;
}
//
__inline Double floatToDouble(float A)
{
Double B;
B.hi = A;
B.lo = 0.0f;
return B;
}
//
__inline Double doubleToDouble(double A)
{
Double B;
B.hi = (float)A;
B.lo = (float)(A - (double)B.hi);
return B;
}
//transmit my own Double to double precision float
__inline double DoubleTodouble(Double B)
{
double A;
A = B.hi;
A += B.lo;
return A;
}
//addition: Double + Double
__inline Double DoubleAdd(Double A, Double B)
{
Double C;
float t1, t2, e;
//Compute high order sum and error
t1 = A.hi + B.hi;
e = t1 - A.hi;
//Compute low order term, including error and overflows
t2 = ((B.hi - e) + ( A.hi - (t1 - e))) + A.lo + B.lo;
//Normalise to get final result
C.hi = t1 + t2;
C.lo = t2 -(C.hi - t1);
return C;
}
//Subtraction: Double - Double
__inline Double DoubleSub(Double A, Double B)
{
Double C;
float t1, t2, e;
//Compute high order sub and error
t1 = A.hi - B.hi;
e = t1 - A.hi;
//Compute low order term, including error and overflows
t2 = ((-B.hi - e) + ( A.hi - (t1 - e))) + A.lo - B.lo;
//Normalise to get final result
C.hi = t1 + t2;
C.lo = t2 -(C.hi - t1);
return C;
}
//multiplication: Double * Double
__inline Double DoubleMul(Double A, Double B)
{
Double C;
float cona, conb, a1, a2, b1, b2;
float c11, c21, c2, e, t1, t2;
//Compute initial high order approximation and error
//If a fused multiply-add is available
//c11 = A.hi * B.hi;
//c21 = A.hi * B.hi - c11;
//If no fused multiply-add is available
cona = A.hi * 8193.0f;
conb = B.hi * 8193.0f;
a1 = cona - (cona - A.hi);
b1 = conb - (conb - B.hi);
a2 = A.hi - a1;
b2 = B.hi - b1;
c11 = A.hi * B.hi;
c21 = (((a1 * b1 - c11) + a1 * b2) + a2 * b1) + a2 * b2;
//Compute high order word of mixed term:
c2 = A.hi * B.lo + A.lo * B.hi;
//Compute (c11, c21) + c2 using Knuth's trick, including low order product
t1 = c11 + c2;
e = t1 - c11;
t2 = ((c2-e) + (c11 - (t1 -e))) + c21 + A.lo * B.lo;
//Normalise to get final result
C.hi = t1 + t2;
C.lo = t2 - ( C.hi - t1);
return C;
}
//divides: Double / Double
__inline Double DoubleDiv(Double A, Double B)
{
Double C;
float a1, a2, b1, b2, cona, conb, c11, c2, c21, e, s1, s2;
float t1, t2, t11, t12, t21, t22;
// Compute a DP approximation to the quotient.
s2 = 1.0f / B.hi;
s1 = A.hi * s2;
//This splits s1 and b.x into high-order and low-order words.
cona = s1 * 8193.0f;
conb = B.hi * 8193.0f;
a1 = cona - (cona - s1);
b1 = conb - (conb - B.hi);
a2 = s1 -a1;
b2 = B.hi - b1;
//Multiply s1 * dsb(1) using Dekker's method.
c11 = s1 * B.hi;
c21 = (((a1 * b1 - c11) + a1 * b2) + a2 * b1) + a2 * b2;
//Compute s1 * b.lo (only high-order word is needed).
c2 = s1 * B.lo;
//Compute (c11, c21) + c2 using Knuth's trick.
t1 = c11 + c2;
e = t1 - c11;
t2 = ((c2 - e) + (c11 - (t1 - e))) + c21;
//The result is t1 + t2, after normalization.
t12 = t1 + t2;
t22 = t2 - (t12 - t1);
//Compute dsa - (t12, t22) using Knuth's trick.
t11 = A.hi - t12;
e = t11 - A.hi;
t21 = ((-t12 - e) + (A.hi - (t11 - e))) + A.lo - t22;
//Compute high-order word of (t11, t21) and divide by b.hi.
s2 *= (t11 + t21);
//The result is s1 + s2, after normalization.
C.hi = s1 + s2;
C.lo = s2 - (C.hi - s1);
return C;
}
#endif
================================================
FILE: Algorithm/inc/EKF.h
================================================
/*
The MIT License (MIT)
Copyright (c) 2015-? suhetao
Permission is hereby granted, free of charge, to any person obtaining a copy of
this software and associated documentation files (the "Software"), to deal in
the Software without restriction, including without limitation the rights to
use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
the Software, and to permit persons to whom the Software is furnished to do so,
subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
#ifndef _EKF_H
#define _EKF_H
#include "Matrix.h"
//7-state q0 q1 q2 q3 wx wy wz
#define EKF_STATE_DIM 7
//13-measurement q0 q1 q2 q3 ax ay az wx wy wz mx my mz
#define EKF_MEASUREMENT_DIM 13
#define EKF_HALFPI 1.5707963267948966192313216916398f
#define EKF_PI 3.1415926535897932384626433832795f
#define EKF_TWOPI 6.283185307179586476925286766559f
#define EKF_TODEG(x) ((x) * 57.2957796f)
typedef struct EKF_FILTER_T{
//state covariance
float32_t P_f32[EKF_STATE_DIM * EKF_STATE_DIM];
float32_t Q_f32[EKF_STATE_DIM * EKF_STATE_DIM];
float32_t R_f32[EKF_MEASUREMENT_DIM * EKF_MEASUREMENT_DIM];
//Kalman gain
float32_t K_f32[EKF_STATE_DIM * EKF_MEASUREMENT_DIM];
float32_t KT_f32[EKF_MEASUREMENT_DIM * EKF_STATE_DIM];
//Measurement covariance
float32_t S_f32[EKF_MEASUREMENT_DIM * EKF_MEASUREMENT_DIM];
//The H matrix maps the measurement to the states
float32_t F_f32[EKF_STATE_DIM * EKF_STATE_DIM];
float32_t FT_f32[EKF_STATE_DIM * EKF_STATE_DIM];
float32_t H_f32[EKF_MEASUREMENT_DIM * EKF_STATE_DIM];
float32_t HT_f32[EKF_STATE_DIM * EKF_MEASUREMENT_DIM];
float32_t I_f32[EKF_STATE_DIM * EKF_STATE_DIM];
//state vector
float32_t X_f32[EKF_STATE_DIM];
//measurement vector
float32_t Y_f32[EKF_MEASUREMENT_DIM];
//
float32_t tmpP_f32[EKF_STATE_DIM * EKF_STATE_DIM];
float32_t tmpS_f32[EKF_MEASUREMENT_DIM * EKF_MEASUREMENT_DIM];
float32_t tmpX_f32[EKF_STATE_DIM];
float32_t tmpXX_f32[EKF_STATE_DIM * EKF_STATE_DIM];
float32_t tmpXXT_f32[EKF_STATE_DIM * EKF_STATE_DIM];
float32_t tmpXY_f32[EKF_STATE_DIM * EKF_MEASUREMENT_DIM];
float32_t tmpYX_f32[EKF_MEASUREMENT_DIM * EKF_STATE_DIM];
arm_matrix_instance_f32 P;
arm_matrix_instance_f32 Q;
arm_matrix_instance_f32 R;
arm_matrix_instance_f32 K;
arm_matrix_instance_f32 KT;
arm_matrix_instance_f32 S;
arm_matrix_instance_f32 F;
arm_matrix_instance_f32 FT;
arm_matrix_instance_f32 H;
arm_matrix_instance_f32 HT;
arm_matrix_instance_f32 I;
//
arm_matrix_instance_f32 X;
arm_matrix_instance_f32 Y;
//
arm_matrix_instance_f32 tmpP;
arm_matrix_instance_f32 tmpX;
arm_matrix_instance_f32 tmpYX;
arm_matrix_instance_f32 tmpXY;
arm_matrix_instance_f32 tmpXX;
arm_matrix_instance_f32 tmpXXT;
arm_matrix_instance_f32 tmpS;
}EKF_Filter;
void EKF_New(EKF_Filter* ekf);
void EKF_Init(EKF_Filter* ekf, float32_t *q, float32_t *gyro);
void EFK_Update(EKF_Filter* ekf, float32_t *q, float32_t *gyro, float32_t *accel, float32_t *mag, float32_t dt);
void EKF_GetAngle(EKF_Filter* ekf, float32_t* rpy);
__inline void EKF_GetQ(EKF_Filter* efk, float32_t* Q)
{
Q[0] = efk->X_f32[0];
Q[1] = efk->X_f32[1];
Q[2] = efk->X_f32[2];
Q[3] = efk->X_f32[3];
}
#endif
================================================
FILE: Algorithm/inc/INS_EKF.h
================================================
/*
The MIT License (MIT)
Copyright (c) 2015-? suhetao
Permission is hereby granted, free of charge, to any person obtaining a copy of
this software and associated documentation files (the "Software"), to deal in
the Software without restriction, including without limitation the rights to
use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
the Software, and to permit persons to whom the Software is furnished to do so,
subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
#ifndef _INS_EKF_H_
#define _INS_EKF_H_
#include "Matrix.h"
//16-state q0 q1 q2 q3 Pn Pe Alt Vn Ve Vd bwx bwy bwz bax bay baz
#define INS_EKF_STATE_DIM 16
//9-measurement mx my mz (3D magnetometer) Pn Pe Alt Vn Ve Vd
//unit vector pointing to MagNorth in body coords
//north pos, east pos, altitude
//north vel, east vel, down velocity
#define INS_EKF_MEASUREMENT_DIM 9
#define INS_EKF_HALFPI 1.5707963267948966192313216916398f
#define INS_EKF_PI 3.1415926535897932384626433832795f
#define INS_EKF_TWOPI 6.283185307179586476925286766559f
#define INS_EKF_TODEG(x) ((x) * 57.2957796f)
typedef struct INS_EKF_FILTER_T{
//state covariance
float32_t P_f32[INS_EKF_STATE_DIM * INS_EKF_STATE_DIM];
float32_t PX_f32[INS_EKF_STATE_DIM * INS_EKF_STATE_DIM];
float32_t PHT_f32[INS_EKF_STATE_DIM * INS_EKF_MEASUREMENT_DIM];
float32_t Q_f32[INS_EKF_STATE_DIM * INS_EKF_STATE_DIM];
float32_t R_f32[INS_EKF_MEASUREMENT_DIM * INS_EKF_MEASUREMENT_DIM];
float32_t K_f32[INS_EKF_STATE_DIM * INS_EKF_MEASUREMENT_DIM];
float32_t KH_f32[INS_EKF_STATE_DIM * INS_EKF_STATE_DIM];
float32_t KHP_f32[INS_EKF_STATE_DIM * INS_EKF_STATE_DIM];
float32_t KY_f32[INS_EKF_STATE_DIM];
float32_t F_f32[INS_EKF_STATE_DIM * INS_EKF_STATE_DIM];
float32_t FT_f32[INS_EKF_STATE_DIM * INS_EKF_STATE_DIM];
//The H matrix maps the measurement to the states
float32_t H_f32[INS_EKF_MEASUREMENT_DIM * INS_EKF_STATE_DIM];
float32_t HT_f32[INS_EKF_STATE_DIM * INS_EKF_MEASUREMENT_DIM];
float32_t HP_f32[INS_EKF_MEASUREMENT_DIM * INS_EKF_STATE_DIM];
float32_t S_f32[INS_EKF_MEASUREMENT_DIM * INS_EKF_MEASUREMENT_DIM];
float32_t SI_f32[INS_EKF_MEASUREMENT_DIM * INS_EKF_MEASUREMENT_DIM];
//state vector
float32_t X_f32[INS_EKF_STATE_DIM];
//measurement vector
float32_t Y_f32[INS_EKF_MEASUREMENT_DIM];
arm_matrix_instance_f32 P;
arm_matrix_instance_f32 PX;
arm_matrix_instance_f32 PHT;
arm_matrix_instance_f32 Q;
arm_matrix_instance_f32 R;
arm_matrix_instance_f32 K;
arm_matrix_instance_f32 KH;
arm_matrix_instance_f32 KHP;
arm_matrix_instance_f32 KY;
arm_matrix_instance_f32 F;
arm_matrix_instance_f32 FT;
arm_matrix_instance_f32 H;
arm_matrix_instance_f32 HT;
arm_matrix_instance_f32 HP;
arm_matrix_instance_f32 S;
arm_matrix_instance_f32 SI;
arm_matrix_instance_f32 X;
arm_matrix_instance_f32 Y;
float32_t declination;
float32_t gravity; //m/s^2
}INS_EKF_Filter;
void INS_EKF_New(INS_EKF_Filter* ins);
void INS_EKF_Init(INS_EKF_Filter* ins, float32_t *p, float32_t *v, float32_t *accel, float32_t *mag);
void INS_EFK_Update(INS_EKF_Filter* ins, float32_t *mag, float32_t *p, float32_t *v, float32_t *gyro, float32_t *accel, float32_t dt);
void INS_EKF_GetAngle(INS_EKF_Filter* ins, float32_t* rpy);
#endif
================================================
FILE: Algorithm/inc/PID.h
================================================
/*
The MIT License (MIT)
Copyright (c) 2015-? suhetao
Permission is hereby granted, free of charge, to any person obtaining a copy of
this software and associated documentation files (the "Software"), to deal in
the Software without restriction, including without limitation the rights to
use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
the Software, and to permit persons to whom the Software is furnished to do so,
subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
#ifndef _PID_H_
#define _PID_H_
typedef struct PIDCONTROLLER_T
{
float A0; // < The derived gain, A0 = Kp + Ki + Kd .
float A1; // < The derived gain, A1 = -Kp - 2Kd.
float A2; // < The derived gain, A2 = Kd .
float state[3]; // < The state array of length 3.
float Kp; // < The proportional gain.
float Ki; // < The integral gain.
float Kd; // < The derivative gain.
} PIDController;
__inline float PID_Calculate(PIDController *S, float in)
{
float out;
// y[n] = y[n-1] + A0 * x[n] + A1 * x[n-1] + A2 * x[n-2]
out = (S->A0 * in) + (S->A1 * S->state[0]) + (S->A2 * S->state[1]) + (S->state[2]);
// Update state
S->state[1] = S->state[0];
S->state[0] = in;
S->state[2] = out;
// return to application
return (out);
}
__inline void PID_Init(PIDController *S)
{
//Derived coefficient A0
S->A0 = S->Kp + S->Ki + S->Kd;
//Derived coefficient A1
S->A1 = (-S->Kp) - ((float) 2.0 * S->Kd);
//Derived coefficient A2
S->A2 = S->Kd;
S->state[0] = 0.0f;
S->state[1] = 0.0f;
S->state[2] = 0.0f;
}
__inline void PID_Reset(PIDController *S)
{
S->state[0] = 0.0f;
S->state[1] = 0.0f;
S->state[2] = 0.0f;
}
#endif
================================================
FILE: Algorithm/inc/Quaternion.h
================================================
/*
The MIT License (MIT)
Copyright (c) 2015-? suhetao
Permission is hereby granted, free of charge, to any person obtaining a copy of
this software and associated documentation files (the "Software"), to deal in
the Software without restriction, including without limitation the rights to
use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
the Software, and to permit persons to whom the Software is furnished to do so,
subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
#ifndef _QUATERNION_H_
#define _QUATERNION_H_
#include "FastMath.h"
__inline void Quaternion_Add(float *r, float *a, float *b)
{
r[0] = a[0] + b[0];
r[1] = a[1] + b[1];
r[2] = a[2] + b[2];
r[3] = a[3] + b[3];
}
__inline void Quaternion_Sub(float *r, float *a, float *b)
{
r[0] = a[0] - b[0];
r[1] = a[1] - b[1];
r[2] = a[2] - b[2];
r[3] = a[3] - b[3];
}
__inline void Quaternion_Multiply(float *r, float *a, float *b)
{
r[0] = a[0] * b[0] - a[1] * b[1] - a[2] * b[2] - a[3] * b[3];
r[1] = a[0] * b[1] + a[1] * b[0] + a[2] * b[3] - a[3] * b[2];
r[2] = a[0] * b[2] - a[1] * b[3] + a[2] * b[0] + a[3] * b[1];
r[3] = a[0] * b[3] + a[1] * b[2] - a[2] * b[1] + a[3] * b[0];
}
__inline void Quaternion_Conjugate(float *r, float *a)
{
r[0] = a[0];
r[1] = -a[1];
r[2] = -a[2];
r[3] = -a[3];
}
__inline void Quaternion_Scalar(float *r, float *q, float scalar)
{
r[0] = q[0] * scalar;
r[1] = q[1] * scalar;
r[2] = q[2] * scalar;
r[3] = q[3] * scalar;
}
void Quaternion_Normalize(float *q);
void Quaternion_FromEuler(float *q, float *rpy);
void Quaternion_ToEuler(float *q, float* rpy);
void Quaternion_FromRotationMatrix(float *R, float *Q);
void Quaternion_RungeKutta4(float *q, float *w, float dt, int normalize);
void Quaternion_From6AxisData(float* q, float *accel, float *mag);
#endif
================================================
FILE: Algorithm/inc/SRCKF.h
================================================
/*
The MIT License (MIT)
Copyright (c) 2015-? suhetao
Permission is hereby granted, free of charge, to any person obtaining a copy of
this software and associated documentation files (the "Software"), to deal in
the Software without restriction, including without limitation the rights to
use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
the Software, and to permit persons to whom the Software is furnished to do so,
subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
#ifndef _SRCKF_H
#define _SRCKF_H
#include "Matrix.h"
//7-state q0 q1 q2 q3 wbx wby wbz
#define SRCKF_STATE_DIM 7
//6-measurement ax ay az mx my mz
#define SRCKF_MEASUREMENT_DIM 6
#define SRCKF_CP_POINTS 14//(2 * SRCKF_STATE_DIM)
#define SRCKF_HALFPI 1.5707963267948966192313216916398f
#define SRCKF_PI 3.1415926535897932384626433832795f
#define SRCKF_TWOPI 6.283185307179586476925286766559f
#define SRCKF_TODEG(x) ((x) * 57.2957796f)
typedef struct SRCKF_FILTER_T{
//weights
float32_t W;
float32_t SW;
//Kesi
float32_t Kesi_f32[SRCKF_STATE_DIM * SRCKF_CP_POINTS];
float32_t iKesi_f32[SRCKF_STATE_DIM];
//state covariance
float32_t S_f32[SRCKF_STATE_DIM * SRCKF_STATE_DIM];
float32_t ST_f32[SRCKF_STATE_DIM * SRCKF_STATE_DIM];
//measurement covariance
float32_t SY_f32[SRCKF_MEASUREMENT_DIM * SRCKF_MEASUREMENT_DIM];
float32_t SYI_f32[SRCKF_MEASUREMENT_DIM * SRCKF_MEASUREMENT_DIM];
float32_t SYT_f32[SRCKF_MEASUREMENT_DIM * SRCKF_MEASUREMENT_DIM];
float32_t SYTI_f32[SRCKF_MEASUREMENT_DIM * SRCKF_MEASUREMENT_DIM];
//cross covariance
float32_t PXY_f32[SRCKF_STATE_DIM * SRCKF_MEASUREMENT_DIM];
float32_t tmpPXY_f32[SRCKF_STATE_DIM * SRCKF_MEASUREMENT_DIM];
//
float32_t SQ_f32[SRCKF_STATE_DIM * SRCKF_STATE_DIM];
float32_t SR_f32[SRCKF_MEASUREMENT_DIM * SRCKF_MEASUREMENT_DIM];
//cubature points
float32_t XCP_f32[SRCKF_STATE_DIM * SRCKF_CP_POINTS];
//propagated cubature points
float32_t XPCP_f32[SRCKF_STATE_DIM * SRCKF_CP_POINTS];
float32_t YPCP_f32[SRCKF_MEASUREMENT_DIM * SRCKF_CP_POINTS];
//centered matrix
float32_t XCPCM_f32[SRCKF_STATE_DIM * SRCKF_CP_POINTS];
float32_t tmpXCPCM_f32[SRCKF_STATE_DIM * SRCKF_CP_POINTS];
float32_t YCPCM_f32[SRCKF_MEASUREMENT_DIM * SRCKF_CP_POINTS];
float32_t YCPCMT_f32[SRCKF_CP_POINTS * SRCKF_MEASUREMENT_DIM];
float32_t XCM_f32[SRCKF_STATE_DIM * (SRCKF_CP_POINTS + SRCKF_STATE_DIM)];
float32_t YCM_f32[SRCKF_MEASUREMENT_DIM * (SRCKF_CP_POINTS + SRCKF_MEASUREMENT_DIM)];
float32_t XYCM_f32[SRCKF_STATE_DIM * (SRCKF_CP_POINTS + SRCKF_MEASUREMENT_DIM)];
//Kalman gain
float32_t K_f32[SRCKF_STATE_DIM * SRCKF_MEASUREMENT_DIM];
//state vector
float32_t X_f32[SRCKF_STATE_DIM];
float32_t tmpX_f32[SRCKF_STATE_DIM];
//measurement vector
float32_t Y_f32[SRCKF_MEASUREMENT_DIM];
float32_t tmpY_f32[SRCKF_MEASUREMENT_DIM];
//
//stuff
//Kesi
arm_matrix_instance_f32 Kesi;
arm_matrix_instance_f32 iKesi;
//state covariance
arm_matrix_instance_f32 S;
arm_matrix_instance_f32 ST;
//measurement covariance
arm_matrix_instance_f32 SY;
arm_matrix_instance_f32 SYI;
arm_matrix_instance_f32 SYT;
arm_matrix_instance_f32 SYTI;
//cross covariance
arm_matrix_instance_f32 PXY;
arm_matrix_instance_f32 tmpPXY;
//
arm_matrix_instance_f32 SQ;
arm_matrix_instance_f32 SR;
//cubature points
arm_matrix_instance_f32 XCP;
//propagated cubature points
arm_matrix_instance_f32 XPCP;
arm_matrix_instance_f32 YPCP;
//centered matrix
arm_matrix_instance_f32 XCPCM;
arm_matrix_instance_f32 tmpXCPCM;
arm_matrix_instance_f32 YCPCM;
arm_matrix_instance_f32 YCPCMT;
arm_matrix_instance_f32 XCM;
arm_matrix_instance_f32 YCM;
arm_matrix_instance_f32 XYCM;
//Kalman gain
arm_matrix_instance_f32 K;
//state vector
arm_matrix_instance_f32 X;
arm_matrix_instance_f32 tmpX;
//measurement vector
arm_matrix_instance_f32 Y;
arm_matrix_instance_f32 tmpY;
float32_t declination;
}SRCKF_Filter;
void SRCKF_New(SRCKF_Filter* srckf);
void SRCKF_Init(SRCKF_Filter* srckf, float32_t *accel, float32_t *mag);
void SRCKF_Update(SRCKF_Filter* srckf, float32_t *gyro, float32_t *accel, float32_t *mag, float32_t dt);
void SRCKF_GetAngle(SRCKF_Filter* srckf, float32_t* rpy);
__inline void SRCKF_GetQ(SRCKF_Filter* srckf, float32_t* Q)
{
Q[0] = srckf->X_f32[0];
Q[1] = srckf->X_f32[1];
Q[2] = srckf->X_f32[2];
Q[3] = srckf->X_f32[3];
}
#endif
================================================
FILE: Algorithm/inc/UKF.h
================================================
/*
The MIT License (MIT)
Copyright (c) 2015-? suhetao
Permission is hereby granted, free of charge, to any person obtaining a copy of
this software and associated documentation files (the "Software"), to deal in
the Software without restriction, including without limitation the rights to
use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
the Software, and to permit persons to whom the Software is furnished to do so,
subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
#ifndef _UKF_H
#define _UKF_H
#include "Matrix.h"
//7-state q0 q1 q2 q3 wx wy wz
#define UKF_STATE_DIM 7
//13-measurement q0 q1 q2 q3 ax ay az wx wy wz mx my mz
#define UKF_MEASUREMENT_DIM 13
#define UKF_SP_POINTS 15//(2 * UKF_STATE_DIM + 1)
#define UKF_HALFPI 1.5707963267948966192313216916398f
#define UKF_PI 3.1415926535897932384626433832795f
#define UKF_TWOPI 6.283185307179586476925286766559f
#define UKF_TODEG(x) ((x) * 57.2957796f)
typedef struct UKF_FILTER_T{
//scaling factor
float32_t gamma;
//weights for means
float32_t Wm0, Wmi;
//weights for covariance
float32_t Wc_f32[UKF_SP_POINTS * UKF_SP_POINTS];
//state covariance
float32_t P_f32[UKF_STATE_DIM * UKF_STATE_DIM];
float32_t PX_f32[UKF_STATE_DIM * UKF_STATE_DIM];
float32_t PY_f32[UKF_MEASUREMENT_DIM * UKF_MEASUREMENT_DIM];
float32_t tmpPY_f32[UKF_MEASUREMENT_DIM * UKF_MEASUREMENT_DIM];
float32_t PXY_f32[UKF_STATE_DIM * UKF_MEASUREMENT_DIM];
float32_t PXYT_f32[UKF_MEASUREMENT_DIM * UKF_STATE_DIM];
float32_t Q_f32[UKF_STATE_DIM * UKF_STATE_DIM];
float32_t R_f32[UKF_MEASUREMENT_DIM * UKF_MEASUREMENT_DIM];
//Sigma points
float32_t XSP_f32[UKF_STATE_DIM * UKF_SP_POINTS];
float32_t tmpXSP_f32[UKF_STATE_DIM * UKF_SP_POINTS];
float32_t tmpXSPT_f32[UKF_SP_POINTS * UKF_STATE_DIM];
float32_t tmpWcXSP_f32[UKF_STATE_DIM * UKF_SP_POINTS];
float32_t tmpWcYSP_f32[UKF_MEASUREMENT_DIM * UKF_SP_POINTS];
float32_t YSP_f32[UKF_MEASUREMENT_DIM * UKF_SP_POINTS];
float32_t tmpYSP_f32[UKF_MEASUREMENT_DIM * UKF_SP_POINTS];
float32_t tmpYSPT_f32[UKF_SP_POINTS * UKF_MEASUREMENT_DIM];
//Kalman gain
float32_t K_f32[UKF_STATE_DIM * UKF_MEASUREMENT_DIM];
float32_t KT_f32[UKF_MEASUREMENT_DIM * UKF_STATE_DIM];
//state vector
float32_t X_f32[UKF_STATE_DIM];
float32_t tmpX_f32[UKF_STATE_DIM];
//measurement vector
float32_t Y_f32[UKF_MEASUREMENT_DIM];
float32_t tmpY_f32[UKF_MEASUREMENT_DIM];
//
arm_matrix_instance_f32 Wc;
arm_matrix_instance_f32 P;
arm_matrix_instance_f32 PX;
arm_matrix_instance_f32 PY;
arm_matrix_instance_f32 tmpPY;
arm_matrix_instance_f32 PXY;
arm_matrix_instance_f32 PXYT;
arm_matrix_instance_f32 Q;
arm_matrix_instance_f32 R;
//
arm_matrix_instance_f32 XSP;
arm_matrix_instance_f32 tmpXSP;
arm_matrix_instance_f32 tmpXSPT;
arm_matrix_instance_f32 tmpWcXSP;
arm_matrix_instance_f32 tmpWcYSP;
arm_matrix_instance_f32 YSP;
arm_matrix_instance_f32 tmpYSP;
arm_matrix_instance_f32 tmpYSPT;
//
arm_matrix_instance_f32 K;
arm_matrix_instance_f32 KT;
//
arm_matrix_instance_f32 X;
arm_matrix_instance_f32 tmpX;
arm_matrix_instance_f32 Y;
arm_matrix_instance_f32 tmpY;
}UKF_Filter;
void UKF_New(UKF_Filter* UKF);
void UKF_Init(UKF_Filter* UKF, float32_t *q, float32_t *gyro);
void UKF_Update(UKF_Filter* UKF, float32_t *q, float32_t *gyro, float32_t *accel, float32_t *mag, float32_t dt);
void UKF_GetAngle(UKF_Filter* UKF, float32_t* rpy);
__inline void UKF_GetQ(UKF_Filter* ukf, float32_t* Q)
{
Q[0] = ukf->X_f32[0];
Q[1] = ukf->X_f32[1];
Q[2] = ukf->X_f32[2];
Q[3] = ukf->X_f32[3];
}
#endif
================================================
FILE: Algorithm/src/CKF.C
================================================
/*
The MIT License (MIT)
Copyright (c) 2015-? suhetao
Permission is hereby granted, free of charge, to any person obtaining a copy of
this software and associated documentation files (the "Software"), to deal in
the Software without restriction, including without limitation the rights to
use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
the Software, and to permit persons to whom the Software is furnished to do so,
subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
#include "CKF.h"
#include "FastMath.h"
#include "Quaternion.h"
#define USE_4TH_RUNGE_KUTTA
//////////////////////////////////////////////////////////////////////////
//all parameters below need to be tune including the weight
#define CKF_PQ_INITIAL 0.000001f
#define CKF_PW_INITIAL 0.000001f
#define CKF_QQ_INITIAL 0.0000045f
#define CKF_QW_INITIAL 0.0000025f
#define CKF_RQ_INITIAL 0.000001f
#define CKF_RA_INITIAL 0.07f
#define CKF_RW_INITIAL 0.0525f
#define CKF_RM_INITIAL 0.105f
//////////////////////////////////////////////////////////////////////////
//
void CKF_New(CKF_Filter* ckf)
{
float32_t *P = ckf->P_f32;
float32_t *Q = ckf->Q_f32;
float32_t *R = ckf->R_f32;
//////////////////////////////////////////////////////////////////////////
//initialise kesi
//generate the cubature point
float32_t kesi = (float32_t)CKF_STATE_DIM;
arm_matrix_instance_f32 KesiPuls, KesiMinu;
float32_t KesiPuls_f32[CKF_STATE_DIM * CKF_STATE_DIM],
KesiMinus_f32[CKF_STATE_DIM * CKF_STATE_DIM];
arm_sqrt_f32(kesi, &kesi);
arm_mat_init_f32(&KesiPuls, CKF_STATE_DIM, CKF_STATE_DIM, KesiPuls_f32);
arm_mat_zero_f32(&KesiPuls);
arm_mat_init_f32(&KesiMinu, CKF_STATE_DIM, CKF_STATE_DIM, KesiMinus_f32);
arm_mat_zero_f32(&KesiMinu);
arm_mat_identity_f32(&KesiPuls, kesi);
arm_mat_identity_f32(&KesiMinu, -kesi);
arm_mat_init_f32(&ckf->Kesi, CKF_STATE_DIM, CKF_CP_POINTS, ckf->Kesi_f32);
arm_mat_setsubmatrix_f32(&ckf->Kesi, &KesiPuls, 0, 0);
arm_mat_setsubmatrix_f32(&ckf->Kesi, &KesiMinu, 0, CKF_STATE_DIM);
arm_mat_init_f32(&ckf->iKesi, CKF_STATE_DIM, 1, ckf->iKesi_f32);
arm_mat_zero_f32(&ckf->iKesi);
//initialise weight
ckf->W = 1.0f / (float32_t)CKF_CP_POINTS;
//initialise P
arm_mat_init_f32(&ckf->P, CKF_STATE_DIM, CKF_STATE_DIM, ckf->P_f32);
arm_mat_zero_f32(&ckf->P);
P[0] = P[8] = P[16] = P[24] = CKF_PQ_INITIAL;
P[32] = P[40] = P[48] = CKF_PW_INITIAL;
arm_mat_init_f32(&ckf->PX, CKF_STATE_DIM, CKF_STATE_DIM, ckf->PX_f32);
arm_mat_init_f32(&ckf->PY, CKF_MEASUREMENT_DIM, CKF_MEASUREMENT_DIM, ckf->PY_f32);
arm_mat_init_f32(&ckf->tmpPY, CKF_MEASUREMENT_DIM, CKF_MEASUREMENT_DIM, ckf->tmpPY_f32);
arm_mat_init_f32(&ckf->PXY, CKF_STATE_DIM, CKF_MEASUREMENT_DIM, ckf->PXY_f32);
arm_mat_init_f32(&ckf->tmpPXY, CKF_STATE_DIM, CKF_MEASUREMENT_DIM, ckf->tmpPXY_f32);
//initialise Q
arm_mat_init_f32(&ckf->Q, CKF_STATE_DIM, CKF_STATE_DIM, ckf->Q_f32);
arm_mat_zero_f32(&ckf->Q);
Q[0] = Q[8] = Q[16] = Q[24] = CKF_QQ_INITIAL;
Q[32] = Q[40] = Q[48] = CKF_QW_INITIAL;
//initialise R
arm_mat_init_f32(&ckf->R, CKF_MEASUREMENT_DIM, CKF_MEASUREMENT_DIM, ckf->R_f32);
arm_mat_zero_f32(&ckf->R);
R[0] = R[14] = R[28] = R[42] = CKF_RQ_INITIAL;
R[56] = R[70] = R[84] = CKF_RA_INITIAL;
R[98] = R[112] = R[126] = CKF_RW_INITIAL;
R[140] = R[154] = R[168] = CKF_RM_INITIAL;
//other stuff
arm_mat_init_f32(&ckf->XCP, CKF_STATE_DIM, CKF_CP_POINTS, ckf->XCP_f32);
arm_mat_init_f32(&ckf->XminusCP, CKF_STATE_DIM, CKF_CP_POINTS, ckf->XminusCP_f32);
arm_mat_init_f32(&ckf->YCP, CKF_MEASUREMENT_DIM, CKF_CP_POINTS, ckf->YSP_f32);
//////////////////////////////////////////////////////////////////////////
arm_mat_init_f32(&ckf->K, CKF_STATE_DIM, CKF_MEASUREMENT_DIM, ckf->K_f32);
arm_mat_init_f32(&ckf->KT, CKF_MEASUREMENT_DIM, CKF_STATE_DIM, ckf->KT_f32);
//////////////////////////////////////////////////////////////////////////
arm_mat_init_f32(&ckf->X, CKF_STATE_DIM, 1, ckf->X_f32);
arm_mat_zero_f32(&ckf->X);
arm_mat_init_f32(&ckf->XT, 1, CKF_STATE_DIM, ckf->XT_f32);
arm_mat_zero_f32(&ckf->XT);
arm_mat_init_f32(&ckf->Xminus, CKF_STATE_DIM, 1, ckf->Xminus_f32);
arm_mat_zero_f32(&ckf->Xminus);
arm_mat_init_f32(&ckf->XminusT, 1, CKF_STATE_DIM, ckf->XminusT_f32);
arm_mat_zero_f32(&ckf->XminusT);
arm_mat_init_f32(&ckf->tmpX, CKF_STATE_DIM, 1, ckf->tmpX_f32);
arm_mat_zero_f32(&ckf->tmpX);
arm_mat_init_f32(&ckf->tmpS, CKF_STATE_DIM, 1, ckf->tmpS_f32);
arm_mat_zero_f32(&ckf->tmpS);
//////////////////////////////////////////////////////////////////////////
arm_mat_init_f32(&ckf->Y, CKF_MEASUREMENT_DIM, 1, ckf->Y_f32);
arm_mat_zero_f32(&ckf->Y);
arm_mat_init_f32(&ckf->YT, 1, CKF_MEASUREMENT_DIM, ckf->YT_f32);
arm_mat_zero_f32(&ckf->YT);
arm_mat_init_f32(&ckf->Yminus, CKF_MEASUREMENT_DIM, 1, ckf->Yminus_f32);
arm_mat_zero_f32(&ckf->Yminus);
arm_mat_init_f32(&ckf->YminusT, 1, CKF_MEASUREMENT_DIM, ckf->YminusT_f32);
arm_mat_zero_f32(&ckf->YminusT);
arm_mat_init_f32(&ckf->tmpY, CKF_MEASUREMENT_DIM, 1, ckf->tmpY_f32);
arm_mat_zero_f32(&ckf->tmpY);
//////////////////////////////////////////////////////////////////////////
}
void CKF_Init(CKF_Filter* ckf, float32_t *q, float32_t *gyro)
{
float32_t *X = ckf->X_f32;
float32_t norm;
//initialise quaternion state
X[0] = q[0];
X[1] = q[1];
X[2] = q[2];
X[3] = q[3];
norm = FastSqrtI(X[0] * X[0] + X[1] * X[1] + X[2] * X[2] + X[3] * X[3]);
X[0] *= norm;
X[1] *= norm;
X[2] *= norm;
X[3] *= norm;
//initialise gyro state
X[4] = gyro[0];
X[5] = gyro[1];
X[6] = gyro[2];
}
void CKF_Update(CKF_Filter* ckf, float32_t *q, float32_t *gyro, float32_t *accel, float32_t *mag, float32_t dt)
{
int col, row;
float32_t norm;
#ifndef USE_4TH_RUNGE_KUTTA
float32_t halfdx, halfdy, halfdz;
float32_t halfdt = 0.5f * dt;
#endif
//////////////////////////////////////////////////////////////////////////
float32_t q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
//
float32_t hx, hy, hz;
float32_t bx, bz;
float32_t _2mx, _2my, _2mz;
//
float32_t *X = ckf->X_f32, *Y = ckf->Y_f32;
float32_t *tmpX = ckf->tmpX_f32, *tmpY = ckf->tmpY_f32;
float32_t *iKesi = ckf->iKesi_f32;
float32_t *Xminus = ckf->Xminus_f32;
float32_t *Yminus = ckf->Yminus_f32;
float32_t *tmpS = ckf->tmpS_f32;
float32_t tmpQ[4];
//////////////////////////////////////////////////////////////////////////
//time update
//evaluate the cholesky factor
arm_mat_chol_f32(&ckf->P);
arm_mat_remainlower_f32(&ckf->P);
//evaluate the cubature points
arm_mat_getcolumn_f32(&ckf->Kesi, iKesi, 0);
arm_mat_mult_f32(&ckf->P, &ckf->iKesi, &ckf->tmpX);
arm_mat_add_f32(&ckf->tmpX, &ckf->X, &ckf->tmpX);
arm_mat_setcolumn_f32(&ckf->XCP, tmpX, 0);
//
//evaluate the propagated cubature points
#ifdef USE_4TH_RUNGE_KUTTA
tmpQ[0] = 0;
tmpQ[1] = tmpX[4];
tmpQ[2] = tmpX[5];
tmpQ[3] = tmpX[6];
Quaternion_RungeKutta4(tmpX, tmpQ, dt, 1);
#else
halfdx = halfdt * tmpX[4];
halfdy = halfdt * tmpX[5];
halfdz = halfdt * tmpX[6];
//
tmpQ[0] = tmpX[0];
tmpQ[1] = tmpX[1];
tmpQ[2] = tmpX[2];
tmpQ[3] = tmpX[3];
//model prediction
//simple way, pay attention!!!
//according to the actual gyroscope output
//and coordinate system definition
tmpX[0] = tmpQ[0] + (halfdx * tmpQ[1] + halfdy * tmpQ[2] + halfdz * tmpQ[3]);
tmpX[1] = tmpQ[1] - (halfdx * tmpQ[0] + halfdy * tmpQ[3] - halfdz * tmpQ[2]);
tmpX[2] = tmpQ[2] + (halfdx * tmpQ[3] - halfdy * tmpQ[0] - halfdz * tmpQ[1]);
tmpX[3] = tmpQ[3] - (halfdx * tmpQ[2] - halfdy * tmpQ[1] + halfdz * tmpQ[0]);
//////////////////////////////////////////////////////////////////////////
//re-normalize quaternion
norm = FastSqrtI(tmpX[0] * tmpX[0] + tmpX[1] * tmpX[1] + tmpX[2] * tmpX[2] + tmpX[3] * tmpX[3]);
tmpX[0] *= norm;
tmpX[1] *= norm;
tmpX[2] *= norm;
tmpX[3] *= norm;
//
#endif
arm_mat_setcolumn_f32(&ckf->XminusCP, tmpX, 0);
for(row = 0; row < CKF_STATE_DIM; row++){
tmpS[row] = tmpX[row];
}
for(col = 1; col < CKF_CP_POINTS; col++){
//evaluate the cubature points
arm_mat_getcolumn_f32(&ckf->Kesi, iKesi, col);
arm_mat_mult_f32(&ckf->P, &ckf->iKesi, &ckf->tmpX);
arm_mat_add_f32(&ckf->tmpX, &ckf->X, &ckf->tmpX);
arm_mat_setcolumn_f32(&ckf->XCP, tmpX, col);
//
//evaluate the propagated cubature points
#ifdef USE_4TH_RUNGE_KUTTA
tmpQ[0] = 0;
tmpQ[1] = tmpX[4];
tmpQ[2] = tmpX[5];
tmpQ[3] = tmpX[6];
Quaternion_RungeKutta4(tmpX, tmpQ, dt, 1);
#else
halfdx = halfdt * tmpX[4];
halfdy = halfdt * tmpX[5];
halfdz = halfdt * tmpX[6];
//
tmpQ[0] = tmpX[0];
tmpQ[1] = tmpX[1];
tmpQ[2] = tmpX[2];
tmpQ[3] = tmpX[3];
//model prediction
//simple way, pay attention!!!
//according to the actual gyroscope output
//and coordinate system definition
tmpX[0] = tmpQ[0] + (halfdx * tmpQ[1] + halfdy * tmpQ[2] + halfdz * tmpQ[3]);
tmpX[1] = tmpQ[1] - (halfdx * tmpQ[0] + halfdy * tmpQ[3] - halfdz * tmpQ[2]);
tmpX[2] = tmpQ[2] + (halfdx * tmpQ[3] - halfdy * tmpQ[0] - halfdz * tmpQ[1]);
tmpX[3] = tmpQ[3] - (halfdx * tmpQ[2] - halfdy * tmpQ[1] + halfdz * tmpQ[0]);
//////////////////////////////////////////////////////////////////////////
//re-normalize quaternion
norm = FastSqrtI(tmpX[0] * tmpX[0] + tmpX[1] * tmpX[1] + tmpX[2] * tmpX[2] + tmpX[3] * tmpX[3]);
tmpX[0] *= norm;
tmpX[1] *= norm;
tmpX[2] *= norm;
tmpX[3] *= norm;
#endif
//
arm_mat_setcolumn_f32(&ckf->XminusCP, tmpX, col);
for(row = 0; row < CKF_STATE_DIM; row++){
tmpS[row] += tmpX[row];
}
}
//estimate the predicted state
arm_mat_scale_f32(&ckf->tmpS, ckf->W, &ckf->X);
//estimate the predicted error covariance
arm_mat_getcolumn_f32(&ckf->XminusCP, Xminus, 0);
arm_mat_trans_f32(&ckf->Xminus, &ckf->XminusT);
arm_mat_mult_f32(&ckf->Xminus, &ckf->XminusT, &ckf->P);
for(col = 1; col < CKF_CP_POINTS; col++){
arm_mat_getcolumn_f32(&ckf->XminusCP, Xminus, col);
arm_mat_trans_f32(&ckf->Xminus, &ckf->XminusT);
arm_mat_mult_f32(&ckf->Xminus, &ckf->XminusT, &ckf->PX);
arm_mat_add_f32(&ckf->P, &ckf->PX, &ckf->P);
}
arm_mat_scale_f32(&ckf->P, ckf->W, &ckf->P);
arm_mat_trans_f32(&ckf->X, &ckf->XT);
arm_mat_mult_f32(&ckf->X, &ckf->XT, &ckf->PX);
arm_mat_sub_f32(&ckf->P, &ckf->PX, &ckf->P);
arm_mat_add_f32(&ckf->P, &ckf->Q, &ckf->P);
//////////////////////////////////////////////////////////////////////////
//measurement update
//evaluate the cholesky factor
arm_mat_chol_f32(&ckf->P);
arm_mat_remainlower_f32(&ckf->P);
//evaluate the cubature points
arm_mat_getcolumn_f32(&ckf->Kesi, iKesi, 0);
arm_mat_mult_f32(&ckf->P, &ckf->iKesi, &ckf->tmpX);
arm_mat_add_f32(&ckf->tmpX, &ckf->X, &ckf->tmpX);
arm_mat_setcolumn_f32(&ckf->XCP, tmpX, 0);
//normalize accel and mag
norm = FastSqrtI(accel[0] * accel[0] + accel[1] * accel[1] + accel[2] * accel[2]);
accel[0] *= norm;
accel[1] *= norm;
accel[2] *= norm;
//////////////////////////////////////////////////////////////////////////
norm = FastSqrtI(mag[0] * mag[0] + mag[1] * mag[1] + mag[2] * mag[2]);
mag[0] *= norm;
mag[1] *= norm;
mag[2] *= norm;
_2mx = 2.0f * mag[0];
_2my = 2.0f * mag[1];
_2mz = 2.0f * mag[2];
//auxiliary variables to avoid repeated arithmetic
//
q0q0 = tmpX[0] * tmpX[0];
q0q1 = tmpX[0] * tmpX[1];
q0q2 = tmpX[0] * tmpX[2];
q0q3 = tmpX[0] * tmpX[3];
q1q1 = tmpX[1] * tmpX[1];
q1q2 = tmpX[1] * tmpX[2];
q1q3 = tmpX[1] * tmpX[3];
q2q2 = tmpX[2] * tmpX[2];
q2q3 = tmpX[2] * tmpX[3];
q3q3 = tmpX[3] * tmpX[3];
//reference direction of earth's magnetic field
hx = _2mx * (0.5f - q2q2 - q3q3) + _2my * (q1q2 - q0q3) + _2mz *(q1q3 + q0q2);
hy = _2mx * (q1q2 + q0q3) + _2my * (0.5f - q1q1 - q3q3) + _2mz * (q2q3 - q0q1);
hz = _2mx * (q1q3 - q0q2) + _2my * (q2q3 + q0q1) + _2mz *(0.5f - q1q1 - q2q2);
arm_sqrt_f32(hx * hx + hy * hy, &bx);
bz = hz;
tmpY[0] = tmpX[0];
tmpY[1] = tmpX[1];
tmpY[2] = tmpX[2];
tmpY[3] = tmpX[3];
tmpY[4] = 2.0f * (q1q3 - q0q2);
tmpY[5] = 2.0f * (q2q3 + q0q1);
tmpY[6] = -1.0f + 2.0f * (q0q0 + q3q3);
tmpY[7] = tmpX[4];
tmpY[8] = tmpX[5];
tmpY[9] = tmpX[6];
tmpY[10] = bx * (1.0f - 2.0f * (q2q2 + q3q3)) + bz * ( 2.0f * (q1q3 - q0q2));
tmpY[11] = bx * (2.0f * (q1q2 - q0q3)) + bz * (2.0f * (q2q3 + q0q1));
tmpY[12] = bx * (2.0f * (q1q3 + q0q2)) + bz * (1.0f - 2.0f * (q1q1 + q2q2));
arm_mat_setcolumn_f32(&ckf->YCP, tmpY, 0);
for(row = 0; row < CKF_MEASUREMENT_DIM; row++){
Y[row] = tmpY[row];
}
for(col = 1; col < CKF_CP_POINTS; col++){
//evaluate the cubature points
arm_mat_getcolumn_f32(&ckf->Kesi, iKesi, col);
arm_mat_mult_f32(&ckf->P, &ckf->iKesi, &ckf->tmpX);
arm_mat_add_f32(&ckf->tmpX, &ckf->X, &ckf->tmpX);
arm_mat_setcolumn_f32(&ckf->XCP, tmpX, col);
//auxiliary variables to avoid repeated arithmetic
//
q0q0 = tmpX[0] * tmpX[0];
q0q1 = tmpX[0] * tmpX[1];
q0q2 = tmpX[0] * tmpX[2];
q0q3 = tmpX[0] * tmpX[3];
q1q1 = tmpX[1] * tmpX[1];
q1q2 = tmpX[1] * tmpX[2];
q1q3 = tmpX[1] * tmpX[3];
q2q2 = tmpX[2] * tmpX[2];
q2q3 = tmpX[2] * tmpX[3];
q3q3 = tmpX[3] * tmpX[3];
//reference direction of earth's magnetic field
hx = _2mx * (0.5f - q2q2 - q3q3) + _2my * (q1q2 - q0q3) + _2mz *(q1q3 + q0q2);
hy = _2mx * (q1q2 + q0q3) + _2my * (0.5f - q1q1 - q3q3) + _2mz * (q2q3 - q0q1);
hz = _2mx * (q1q3 - q0q2) + _2my * (q2q3 + q0q1) + _2mz *(0.5f - q1q1 - q2q2);
arm_sqrt_f32(hx * hx + hy * hy, &bx);
bz = hz;
tmpY[0] = tmpX[0];
tmpY[1] = tmpX[1];
tmpY[2] = tmpX[2];
tmpY[3] = tmpX[3];
tmpY[4] = 2.0f * (q1q3 - q0q2);
tmpY[5] = 2.0f * (q2q3 + q0q1);
tmpY[6] = -1.0f + 2.0f * (q0q0 + q3q3);
tmpY[7] = tmpX[4];
tmpY[8] = tmpX[5];
tmpY[9] = tmpX[6];
tmpY[10] = bx * (1.0f - 2.0f * (q2q2 + q3q3)) + bz * ( 2.0f * (q1q3 - q0q2));
tmpY[11] = bx * (2.0f * (q1q2 - q0q3)) + bz * (2.0f * (q2q3 + q0q1));
tmpY[12] = bx * (2.0f * (q1q3 + q0q2)) + bz * (1.0f - 2.0f * (q1q1 + q2q2));
arm_mat_setcolumn_f32(&ckf->YCP, tmpY, col);
for(row = 0; row < CKF_MEASUREMENT_DIM; row++){
Y[row] += tmpY[row];
}
}
//estimate the predicted measurement
arm_mat_scale_f32(&ckf->Y, ckf->W, &ckf->Y);
//estimate the innovation covariance matrix
arm_mat_getcolumn_f32(&ckf->YCP, Yminus, 0);
arm_mat_trans_f32(&ckf->Yminus, &ckf->YminusT);
arm_mat_mult_f32(&ckf->Yminus, &ckf->YminusT, &ckf->PY);
for(col = 1; col < CKF_CP_POINTS; col++){
arm_mat_getcolumn_f32(&ckf->YCP, Yminus, col);
arm_mat_trans_f32(&ckf->Yminus, &ckf->YminusT);
arm_mat_mult_f32(&ckf->Yminus, &ckf->YminusT, &ckf->tmpPY);
arm_mat_add_f32(&ckf->PY, &ckf->tmpPY, &ckf->PY);
}
arm_mat_scale_f32(&ckf->PY, ckf->W, &ckf->PY);
arm_mat_trans_f32(&ckf->Y, &ckf->YT);
arm_mat_mult_f32(&ckf->Y, &ckf->YT, &ckf->tmpPY);
arm_mat_sub_f32(&ckf->PY, &ckf->tmpPY, &ckf->PY);
arm_mat_add_f32(&ckf->PY, &ckf->R, &ckf->PY);
//estimate the cross-covariance matrix
arm_mat_getcolumn_f32(&ckf->XCP, Xminus, 0);
arm_mat_getcolumn_f32(&ckf->YCP, Yminus, 0);
arm_mat_trans_f32(&ckf->Yminus, &ckf->YminusT);
arm_mat_mult_f32(&ckf->Xminus, &ckf->YminusT, &ckf->PXY);
arm_mat_scale_f32(&ckf->PXY, ckf->W, &ckf->PXY);
for(col = 1; col < CKF_CP_POINTS; col++){
arm_mat_getcolumn_f32(&ckf->XCP, Xminus, col);
arm_mat_getcolumn_f32(&ckf->YCP, Yminus, col);
arm_mat_trans_f32(&ckf->Yminus, &ckf->YminusT);
arm_mat_mult_f32(&ckf->Xminus, &ckf->YminusT, &ckf->tmpPXY);
arm_mat_scale_f32(&ckf->tmpPXY, ckf->W, &ckf->tmpPXY);
arm_mat_add_f32(&ckf->PXY, &ckf->tmpPXY, &ckf->PXY);
}
//arm_mat_scale_f32(&ckf->PXY, ckf->W, &ckf->PXY);
arm_mat_trans_f32(&ckf->Y, &ckf->YT);
arm_mat_mult_f32(&ckf->X, &ckf->YT, &ckf->tmpPXY);
arm_mat_sub_f32(&ckf->PXY, &ckf->tmpPXY, &ckf->PXY);
//estimate the kalman gain
//K = PXY * inv(PY);
arm_mat_inverse_f32(&ckf->PY, &ckf->tmpPY);
arm_mat_mult_f32(&ckf->PXY, &ckf->tmpPY, &ckf->K);
//estimate the updated state
//X = X + K*(z - Y);
Y[0] = q[0] - Y[0];
Y[1] = q[1] - Y[1];
Y[2] = q[2] - Y[2];
Y[3] = q[3] - Y[3];
//
Y[4] = accel[0] - Y[4];
Y[5] = accel[1] - Y[5];
Y[6] = accel[2] - Y[6];
Y[7] = gyro[0] - Y[7];
Y[8] = gyro[1] - Y[8];
Y[9] = gyro[2] - Y[9];
//////////////////////////////////////////////////////////////////////////
Y[10] = mag[0] - Y[10];
Y[11] = mag[1] - Y[11];
Y[12] = mag[2] - Y[12];
arm_mat_mult_f32(&ckf->K, &ckf->Y, &ckf->tmpX);
arm_mat_add_f32(&ckf->X, &ckf->tmpX, &ckf->X);
//normalize quaternion
norm = FastSqrtI(X[0] * X[0] + X[1] * X[1] + X[2] * X[2] + X[3] * X[3]);
X[0] *= norm;
X[1] *= norm;
X[2] *= norm;
X[3] *= norm;
//estimate the corresponding error covariance
//the default tuning parameters don't work properly
//must tune the P,Q,R and calibrate the sensor data
//P = P - K * PY * K'
arm_mat_trans_f32(&ckf->K, &ckf->KT);
arm_mat_mult_f32(&ckf->K, &ckf->PY, &ckf->PXY);
arm_mat_mult_f32(&ckf->PXY, &ckf->KT, &ckf->PX);
arm_mat_sub_f32(&ckf->P, &ckf->PX, &ckf->P);
}
void CKF_GetAngle(CKF_Filter* ckf, float32_t* rpy)
{
float32_t R[3][3];
float32_t *X = ckf->X_f32;
//Z-Y-X
R[0][0] = 2.0f * (X[0] * X[0] + X[1] * X[1]) - 1.0f;
R[0][1] = 2.0f * (X[1] * X[2] + X[0] * X[3]);
R[0][2] = 2.0f * (X[1] * X[3] - X[0] * X[2]);
//R[1][0] = 2.0f * (X[1] * X[2] - X[0] * X[3]);
//R[1][1] = 2.0f * (X[0] * X[0] + X[2] * X[2]) - 1.0f;
R[1][2] = 2.0f * (X[2] * X[3] + X[0] * X[1]);
//R[2][0] = 2.0f * (X[1] * X[3] + X[0] * X[2]);
//R[2][1] = 2.0f * (X[2] * X[3] - X[0] * X[1]);
R[2][2] = 2.0f * (X[0] * X[0] + X[3] * X[3]) - 1.0f;
//roll
rpy[0] = FastAtan2(R[1][2], R[2][2]);
if (rpy[0] == CKF_PI)
rpy[0] = -CKF_PI;
//pitch
if (R[0][2] >= 1.0f)
rpy[1] = -CKF_HALFPI;
else if (R[0][2] <= -1.0f)
rpy[1] = CKF_HALFPI;
else
rpy[1] = FastAsin(-R[0][2]);
//yaw
rpy[2] = FastAtan2(R[0][1], R[0][0]);
if (rpy[2] < 0.0f){
rpy[2] += CKF_TWOPI;
}
if (rpy[2] > CKF_TWOPI){
rpy[2] = 0.0f;
}
rpy[0] = CKF_TODEG(rpy[0]);
rpy[1] = CKF_TODEG(rpy[1]);
rpy[2] = CKF_TODEG(rpy[2]);
}
================================================
FILE: Algorithm/src/Control.c
================================================
/*
The MIT License (MIT)
Copyright (c) 2015-? suhetao
Permission is hereby granted, free of charge, to any person obtaining a copy of
this software and associated documentation files (the "Software"), to deal in
the Software without restriction, including without limitation the rights to
use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
the Software, and to permit persons to whom the Software is furnished to do so,
subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
#include "Control.h"
#include "FastMath.h"
int Matrix_Inv3x3(float* A)
{
// det = a11(a33a22-a32a23)-a21(a33a12-a32a13)+a31(a23a12-a22a13)
// det = a00(a22a11-a21a12)-a10(a22a01-a21a02)+a20(a12a01-a11a02)
float det;
float M[9];
// Invert the matrix
/*
| a11 a12 a13 |-1 | a33a22-a32a23 -(a33a12-a32a13) a23a12-a22a13 |
| a21 a22 a23 | = 1/DET * | -(a33a21-a31a23) a33a11-a31a13 -(a23a11-a21a13) |
| a31 a32 a33 | | a32a21-a31a22 -(a32a11-a31a12) a22a11-a21a12 |
| a00 a01 a02 |-1 | a22a11-a21a12 -(a22a01-a21a02) a12a01-a11a02 |
| a10 a11 a12 | = 1/DET * | -(a22a10-a20a12) a22a00-a20a02 -(a12a00-a10a02) |
| a20 a21 a22 | | a21a10-a20a11 -(a21a00-a20a01) a11a00-a10a01 |
*/
det = A[0] * (A[8] * A[4] - A[7] * A[5]) -
A[3] * (A[8] * A[1] - A[7] * A[2]) +
A[6] * (A[5] * A[1] - A[4] * A[2]);
// Row 1
// M[0] = (a22a11-a21a12)/det;
M[0] = (A[8] * A[4] - A[7] * A[5]) / det;
// M[1] = -(a22a01-a21a02)/det;
M[1] = -(A[8] * A[1] - A[7] * A[2]) / det;
// M[2] = (a12a01-a11a02)/det;
M[2] = (A[5] * A[1] - A[4] * A[2]) / det;
// Row 2
// M[3] = -(a22a10-a20a12)/det;
M[3] = -(A[8] * A[3] - A[6] * A[5]) / det;
// M[4] = (a22a00-a20a02)/det;
M[4] = (A[8] * A[0] - A[6] * A[2]) / det;
// M[5] = -(a12a00-a10a02)/det;
M[5] = -(A[5] * A[0] - A[3] * A[2]) / det;
// Row 3
// M[6] = (a21a10-a20a11)/det;
M[6] = (A[7] * A[3] - A[6] * A[4]) / det;
// M[7] = -(a21a00-a20a01)/det;
M[7] = -(A[7] * A[0] - A[6] * A[1]) / det;
// M[8] = (a11a00-a10a01)/det;
M[8] = (A[4] * A[0] - A[3] * A[1]) / det;
A[0] = M[0]; A[1] = M[1]; A[2] = M[2];
A[3] = M[3]; A[4] = M[4]; A[5] = M[5];
A[6] = M[6]; A[7] = M[7]; A[8] = M[8];
return 1;
}
//sweep
void Matrix_Inv(float *A, int n)
{
float d;
int i, j, k;
int kn, kk;
int ln, lk;
for (k = 0; k < n; ++k){
kn = k * n;
kk = kn + k;
d = 1.0f / A[kk];
A[kk] = d;
for (i = 0; i < n; ++i){
if (i != k){
A[kn + i] *= -d;
}
}
for (i = 0; i < n; ++i){
if ( i != k){
A[i * n + k] *= d;
}
}
for (i = 0; i < n; ++i){
if (i != k){
ln = i * n;
lk = ln + k;
for (j = 0; j < n; ++j){
if (j != k ){
A[ln + j] += A[lk] * A[kn + j] / d;
}
}
}
}
}
}
static QuadrotorParameter gQuadrotorParameter = {
0,
};
void EulerConv(float* dt, float *deta)
{
float dx = dt[X];
float dy = dt[Y];
float dz = dt[Z];
float psi = dt[PSI];
float d;
float spsi, cpsi;
FastSinCos(psi, &spsi, &cpsi);
d = FastSqrt(dx * dx + dy * dy + dz * dz);
if (-0.001f < d && d < 0.001f){
deta[ROLL] = 0;
}
else{
deta[ROLL] = FastAsin((dx * spsi - dy * cpsi)/d);
}
deta[PITCH] = FastAtan2(dx * cpsi + dy * spsi, dz);
if(deta[ROLL] < MIN_ANG){
deta[ROLL] = MIN_ANG;
}
else if(deta[ROLL] > MAX_ANG){
deta[ROLL] = MAX_ANG;
}
if(deta[PITCH] < MIN_ANG){
deta[PITCH] = MIN_ANG;
}
else if(deta[PITCH] > MAX_ANG){
deta[PITCH] = MAX_ANG;
}
deta[YAW] = psi;
}
void TorqueConv(float *eta, float *deta, float *dt)
{
float sphi, cphi;
float stht, ctht;
float spsi, cpsi;
float C[9];
float sum[9];
float* I = gQuadrotorParameter.I;
FastSinCos(eta[ROLL], &sphi, &cphi);
FastSinCos(eta[PITCH], &stht, &ctht);
FastSinCos(eta[YAW], &spsi, &cpsi);
C[0] = 1; C[1] = 0; C[2] = -stht;
C[3] = 0; C[4] = cphi; C[5] = sphi * ctht;
C[6] = 0; C[7] = -sphi; C[8] = cphi * ctht;
//torque = I * C * euler;
sum[0] = I[0] * C[0] + I[1] * C[3] + I[2] * C[6];
sum[1] = I[0] * C[1] + I[1] * C[4] + I[2] * C[7];
sum[2] = I[0] * C[2] + I[1] * C[5] + I[2] * C[8];
sum[3] = I[3] * C[0] + I[4] * C[3] + I[5] * C[6];
sum[4] = I[3] * C[1] + I[4] * C[4] + I[5] * C[7];
sum[5] = I[3] * C[2] + I[4] * C[5] + I[5] * C[8];
sum[6] = I[6] * C[0] + I[7] * C[3] + I[8] * C[6];
sum[7] = I[6] * C[1] + I[7] * C[4] + I[8] * C[7];
sum[8] = I[6] * C[2] + I[7] * C[5] + I[8] * C[8];
dt[0] = sum[0] * deta[ROLL] + sum[1] * deta[PITCH] + sum[2] * deta[YAW];
dt[1] = sum[0] * deta[ROLL] + sum[1] * deta[PITCH] + sum[2] * deta[YAW];
dt[2] = sum[0] * deta[ROLL] + sum[1] * deta[PITCH] + sum[2] * deta[YAW];
}
void ForceConv(float *eta, float dz, float *df)
{
float sphi, cphi;
float stht, ctht;
float spsi, cpsi;
float R[9];
float m = gQuadrotorParameter.m;
FastSinCos(eta[ROLL], &sphi, &cphi);
FastSinCos(eta[PITCH], &stht, &ctht);
FastSinCos(eta[YAW], &spsi, &cpsi);
R[0] = cpsi * ctht; R[1] = cpsi * stht * sphi - spsi * cphi; R[2] = cpsi * stht * cphi + spsi * sphi;
R[3] = spsi * ctht; R[4] = spsi * stht * sphi + cpsi * cphi; R[5] = spsi * stht * cphi - cpsi * sphi;
R[6] = -stht; R[7] = ctht * sphi; R[8] = ctht * cphi;
Matrix_Inv3x3(R);
//u = {0, 0, dz};
//d = m * inv(R) * u;
//fd = d[2];
R[8] *= m;
*df = R[8] * dz;
}
void TorqueInv(float *dt, float df, float *domega)
{
float T[16];
float u[4];
float b = gQuadrotorParameter.b;
float d = gQuadrotorParameter.d;
float l = gQuadrotorParameter.l;
T[0] = b; T[1] = b; T[2] = b; T[3] = b;
T[4] = 0; T[5] = -l * b; T[6] = 0; T[7] = l * b;
T[8] = -l * b; T[9] = 0; T[10] = l * b; T[11] = 0;
T[12] = -d; T[13] = d; T[14] = -d; T[15] = d;
u[0] = df; u[1] = dt[0]; u[2] = dt[1]; u[3] = dt[2];
//omega = inv(T) * u;
Matrix_Inv(T, 4);
domega[0] = T[0] * u[0] + T[1] * u[1] + T[2] * u[2] + T[3] * u[3];
domega[1] = T[4] * u[0] + T[5] * u[1] + T[6] * u[2] + T[7] * u[3];
domega[2] = T[8] * u[0] + T[9] * u[1] + T[10] * u[2] + T[11] * u[3];
domega[3] = T[12] * u[0] + T[13] * u[1] + T[14] * u[2] + T[15] * u[3];
}
// Newton-Euler model
void QuadrotorControl(float* task, float *q, float *u)
{
task[0] = PID_Calculate(&gQuadrotorParameter.PostionX, gQuadrotorParameter.X - task[0]);
task[1] = PID_Calculate(&gQuadrotorParameter.PostionY, gQuadrotorParameter.Y - task[1]);
task[2] = PID_Calculate(&gQuadrotorParameter.PostionZ, gQuadrotorParameter.Z - task[2]);
//EulerConv(
// quadrotor linear acceleration along zE WRT E-frame
// quadrotor angular acceleration around x2 WRT E-frame (roll)
// theta quadrotor angular acceleration around y1 WRT E-frame (pitch)
// quadrotor angular acceleration around zE WRT E-frame (yaw)
// The first one shows how the quadrotor accelerates according to
// the basic movement commands given.
// The second system of equations explains how the basic movements are related
// to the propellers squared speed.
// The third equation takes into accounts the motors dynamics and shows the
// relation between propellers speed and motors voltage.
}
================================================
FILE: Algorithm/src/EKF.c
================================================
/*
The MIT License (MIT)
Copyright (c) 2015-? suhetao
Permission is hereby granted, free of charge, to any person obtaining a copy of
this software and associated documentation files (the "Software"), to deal in
the Software without restriction, including without limitation the rights to
use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
the Software, and to permit persons to whom the Software is furnished to do so,
subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
#include "EKF.h"
#include "FastMath.h"
#include "Quaternion.h"
#define USE_4TH_RUNGE_KUTTA
//////////////////////////////////////////////////////////////////////////
//all parameters below need to be tune
#define EKF_PQ_INITIAL 0.000001f
#define EKF_PW_INITIAL 0.000010f
#define EKF_QQ_INITIAL 0.000045f
#define EKF_QW_INITIAL 0.00025f
#define EKF_RQ_INITIAL 0.000001f
#define EKF_RA_INITIAL 0.07f
#define EKF_RW_INITIAL 0.525f
#define EKF_RM_INITIAL 0.105f
//////////////////////////////////////////////////////////////////////////
void EKF_New(EKF_Filter* ekf)
{
float32_t *H = ekf->H_f32;
float32_t *P = ekf->P_f32;
float32_t *Q = ekf->Q_f32;
float32_t *R = ekf->R_f32;
//////////////////////////////////////////////////////////////////////////
arm_mat_init_f32(&ekf->P, EKF_STATE_DIM, EKF_STATE_DIM, ekf->P_f32);
arm_mat_zero_f32(&ekf->P);
P[0] = P[8] = P[16] = P[24] = EKF_PQ_INITIAL;
P[32] = P[40] = P[48] = EKF_PW_INITIAL;
//
arm_mat_init_f32(&ekf->Q, EKF_STATE_DIM, EKF_STATE_DIM, ekf->Q_f32);
arm_mat_zero_f32(&ekf->Q);
Q[0] = Q[8] = Q[16] = Q[24] = EKF_QQ_INITIAL;
Q[32] = Q[40] = Q[48] = EKF_QW_INITIAL;
//
arm_mat_init_f32(&ekf->R, EKF_MEASUREMENT_DIM, EKF_MEASUREMENT_DIM, ekf->R_f32);
arm_mat_zero_f32(&ekf->R);
R[0] = R[14] = R[28] = R[42] = EKF_RQ_INITIAL;
R[56] = R[70] = R[84] = EKF_RA_INITIAL;
R[98] = R[112] = R[126] = EKF_RW_INITIAL;
R[140] = R[154] = R[168] = EKF_RM_INITIAL;
//////////////////////////////////////////////////////////////////////////
arm_mat_init_f32(&ekf->K, EKF_STATE_DIM, EKF_MEASUREMENT_DIM, ekf->K_f32);
arm_mat_init_f32(&ekf->KT, EKF_MEASUREMENT_DIM, EKF_STATE_DIM, ekf->KT_f32);
arm_mat_init_f32(&ekf->S, EKF_MEASUREMENT_DIM, EKF_MEASUREMENT_DIM, ekf->S_f32);
//
arm_mat_init_f32(&ekf->F, EKF_STATE_DIM, EKF_STATE_DIM, ekf->F_f32);
arm_mat_zero_f32(&ekf->F);
arm_mat_identity_f32(&ekf->F, 1.0f);
//
arm_mat_init_f32(&ekf->FT, EKF_STATE_DIM, EKF_STATE_DIM, ekf->FT_f32);
//
arm_mat_init_f32(&ekf->H, EKF_MEASUREMENT_DIM, EKF_STATE_DIM, H);
arm_mat_zero_f32(&ekf->H);
H[0] = H[8] = H[16] = H[24] = 1.0f; //q row 0~3, col 0~3
H[53] = H[61] = H[69] = 1.0f; //w row 7~9, col 4~6
//
arm_mat_init_f32(&ekf->HT, EKF_STATE_DIM, EKF_MEASUREMENT_DIM, ekf->HT_f32);
//////////////////////////////////////////////////////////////////////////
arm_mat_init_f32(&ekf->I, EKF_STATE_DIM, EKF_STATE_DIM, ekf->I_f32);
arm_mat_zero_f32(&ekf->I);
arm_mat_identity_f32(&ekf->I, 1.0f);
//////////////////////////////////////////////////////////////////////////
arm_mat_init_f32(&ekf->X, EKF_STATE_DIM, 1, ekf->X_f32);
arm_mat_zero_f32(&ekf->X);
//////////////////////////////////////////////////////////////////////////
arm_mat_init_f32(&ekf->Y, EKF_MEASUREMENT_DIM, 1, ekf->Y_f32);
arm_mat_zero_f32(&ekf->Y);
//////////////////////////////////////////////////////////////////////////
//
arm_mat_init_f32(&ekf->tmpP, EKF_STATE_DIM, EKF_STATE_DIM, ekf->tmpP_f32);
arm_mat_init_f32(&ekf->tmpX, EKF_STATE_DIM, 1, ekf->tmpX_f32);
arm_mat_init_f32(&ekf->tmpYX, EKF_MEASUREMENT_DIM, EKF_STATE_DIM, ekf->tmpYX_f32);
arm_mat_init_f32(&ekf->tmpXY, EKF_STATE_DIM, EKF_MEASUREMENT_DIM, ekf->tmpXY_f32);
arm_mat_init_f32(&ekf->tmpXX, EKF_STATE_DIM, EKF_STATE_DIM, ekf->tmpXX_f32);
arm_mat_init_f32(&ekf->tmpXXT, EKF_STATE_DIM, EKF_STATE_DIM, ekf->tmpXXT_f32);
arm_mat_init_f32(&ekf->tmpS, EKF_MEASUREMENT_DIM, EKF_MEASUREMENT_DIM, ekf->tmpS_f32);
//////////////////////////////////////////////////////////////////////////
}
void EKF_Init(EKF_Filter* ekf, float32_t *q, float32_t *gyro)
{
float32_t *X = ekf->X_f32;
float32_t norm;
X[0] = q[0];
X[1] = q[1];
X[2] = q[2];
X[3] = q[3];
norm = FastSqrtI(X[0] * X[0] + X[1] * X[1] + X[2] * X[2] + X[3] * X[3]);
X[0] *= norm;
X[1] *= norm;
X[2] *= norm;
X[3] *= norm;
X[4] = gyro[0];
X[5] = gyro[1];
X[6] = gyro[2];
}
void EFK_Update(EKF_Filter* ekf, float32_t *q, float32_t *gyro, float32_t *accel, float32_t *mag, float32_t dt)
{
float32_t norm;
float32_t h[EKF_MEASUREMENT_DIM];
float32_t halfdx, halfdy, halfdz;
float32_t neghalfdx, neghalfdy, neghalfdz;
float32_t halfdtq0, halfdtq1, neghalfdtq1, halfdtq2, neghalfdtq2, halfdtq3, neghalfdtq3;
float32_t halfdt = 0.5f * dt;
#ifdef USE_4TH_RUNGE_KUTTA
float32_t tmpW[4];
#endif
//////////////////////////////////////////////////////////////////////////
float32_t q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
float32_t _2q0,_2q1,_2q2,_2q3;
float32_t q0, q1, q2, q3;
//
float32_t hx, hy, hz;
float32_t bx, bz;
float32_t _2mx, _2my, _2mz;
//
float32_t *H = ekf->H_f32, *F = ekf->F_f32;
float32_t *X = ekf->X_f32, *Y = ekf->Y_f32;
//////////////////////////////////////////////////////////////////////////
halfdx = halfdt * X[4];
neghalfdx = -halfdx;
halfdy = halfdt * X[5];
neghalfdy = -halfdy;
halfdz = halfdt * X[6];
neghalfdz = -halfdz;
//
q0 = X[0];
q1 = X[1];
q2 = X[2];
q3 = X[3];
halfdtq0 = halfdt * q0;
halfdtq1 = halfdt * q1;
neghalfdtq1 = -halfdtq1;
halfdtq2 = halfdt * q2;
neghalfdtq2 = -halfdtq2;
halfdtq3 = halfdt * q3;
neghalfdtq3 = -halfdtq3;
//F[0] = 1.0f;
F[1] = neghalfdx;
F[2] = neghalfdy;
F[3] = neghalfdz;
F[4] = neghalfdtq1;
F[5] = neghalfdtq2;
F[6] = neghalfdtq3;
F[7] = halfdx;
//F[8] = 1.0f;
F[9] = neghalfdz;
F[10] = halfdy;
F[11] = halfdtq0;
F[12] = halfdtq3;
F[13] = neghalfdtq2;
F[14] = halfdy;
F[15] = halfdz;
//F[16] = 1.0f;
F[17] = neghalfdx;
F[18] = neghalfdtq3;
F[19] = halfdtq0;
F[20] = neghalfdtq1;
F[21] = halfdz;
F[22] = neghalfdy;
F[23] = halfdx;
//F[24] = 1.0f;
F[25] = halfdtq2;
F[26] = neghalfdtq1;
F[27] = halfdtq0;
//model prediction
//simple way, pay attention!!!
//according to the actual gyroscope output
//and coordinate system definition
#ifdef USE_4TH_RUNGE_KUTTA
tmpW[0] = 0;
tmpW[1] = X[4];
tmpW[2] = X[5];
tmpW[3] = X[6];
Quaternion_RungeKutta4(X, tmpW, dt, 1);
#else
X[0] = q0 - (halfdx * q1 + halfdy * q2 + halfdz * q3);
X[1] = q1 + (halfdx * q0 + halfdy * q3 - halfdz * q2);
X[2] = q2 - (halfdx * q3 - halfdy * q0 - halfdz * q1);
X[3] = q3 + (halfdx * q2 - halfdy * q1 + halfdz * q0);
//////////////////////////////////////////////////////////////////////////
//Re-normalize Quaternion
norm = FastSqrtI(X[0] * X[0] + X[1] * X[1] + X[2] * X[2] + X[3] * X[3]);
X[0] *= norm;
X[1] *= norm;
X[2] *= norm;
X[3] *= norm;
#endif
//X covariance matrix update based on model
//P = F*P*F' + Q;
arm_mat_trans_f32(&ekf->F, &ekf->FT);
arm_mat_mult_f32(&ekf->F, &ekf->P, &ekf->tmpP);
arm_mat_mult_f32(&ekf->tmpP, &ekf->FT, &ekf->P);
arm_mat_add_f32(&ekf->P, &ekf->Q, &ekf->tmpP);
//////////////////////////////////////////////////////////////////////////
//model and measurement differences
//normalize acc and mag measurements
norm = FastSqrtI(accel[0] * accel[0] + accel[1] * accel[1] + accel[2] * accel[2]);
accel[0] *= norm;
accel[1] *= norm;
accel[2] *= norm;
//////////////////////////////////////////////////////////////////////////
norm = FastSqrtI(mag[0] * mag[0] + mag[1] * mag[1] + mag[2] * mag[2]);
mag[0] *= norm;
mag[1] *= norm;
mag[2] *= norm;
//Auxiliary variables to avoid repeated arithmetic
_2q0 = 2.0f * X[0];
_2q1 = 2.0f * X[1];
_2q2 = 2.0f * X[2];
_2q3 = 2.0f * X[3];
//
q0q0 = X[0] * X[0];
q0q1 = X[0] * X[1];
q0q2 = X[0] * X[2];
q0q3 = X[0] * X[3];
q1q1 = X[1] * X[1];
q1q2 = X[1] * X[2];
q1q3 = X[1] * X[3];
q2q2 = X[2] * X[2];
q2q3 = X[2] * X[3];
q3q3 = X[3] * X[3];
_2mx = 2.0f * mag[0];
_2my = 2.0f * mag[1];
_2mz = 2.0f * mag[2];
//Reference direction of Earth's magnetic field
hx = _2mx * (0.5f - q2q2 - q3q3) + _2my * (q1q2 - q0q3) + _2mz *(q1q3 + q0q2);
hy = _2mx * (q1q2 + q0q3) + _2my * (0.5f - q1q1 - q3q3) + _2mz * (q2q3 - q0q1);
hz = _2mx * (q1q3 - q0q2) + _2my * (q2q3 + q0q1) + _2mz *(0.5f - q1q1 - q2q2);
arm_sqrt_f32(hx * hx + hy * hy, &bx);
bz = hz;
h[0] = X[0];
h[1] = X[1];
h[2] = X[2];
h[3] = X[3];
h[4] = 2.0f * (q1q3 - q0q2);
h[5] = 2.0f * (q2q3 + q0q1);
h[6] = -1.0f + 2.0f * (q0q0 + q3q3);
h[7] = X[4];
h[8] = X[5];
h[9] = X[6];
h[10] = bx * (1.0f - 2.0f * (q2q2 + q3q3)) + bz * ( 2.0f * (q1q3 - q0q2));
h[11] = bx * (2.0f * (q1q2 - q0q3)) + bz * (2.0f * (q2q3 + q0q1));
h[12] = bx * (2.0f * (q1q3 + q0q2)) + bz * (1.0f - 2.0f * (q1q1 + q2q2));
/////////////////////////////////////////////////////////////////////////
//The H matrix maps the measurement to the states 13 x 7
//row started from 0 to 12, col started from 0 to 6
//row 4, col 0~3
H[28] = -_2q2; H[29] = _2q3; H[30] = -_2q0; H[31] = _2q1;
//row 5, col 0~3
H[35] = _2q1; H[36] = _2q0; H[37] = _2q3; H[38] = _2q2;
//row 6, col 0~3
H[42] = _2q0; H[43] = -_2q1; H[44] = -_2q2; H[45] = _2q3;
//row 10, col 0~3
H[70] = bx * _2q0 - bz * _2q2;
H[71] = bx * _2q1 + bz * _2q3;
H[72] = -bx * _2q2 - bz * _2q0;
H[73] = bz * _2q1 - bx * _2q3;
//row 11, col 0~3
H[77] = bz * _2q1 - bx * _2q3;
H[78] = bx * _2q2 + bz * _2q0;
H[79] = bx * _2q1 + bz * _2q3;
H[80] = bz * _2q2 - bx * _2q0;
//row 12, col 0~3
H[84] = bx * _2q2 + bz * _2q0;
H[85] = bx * _2q3 - bz * _2q1;
H[86] = bx * _2q0 - bz * _2q2;
H[87] = bx * _2q1 + bz * _2q3;
//
//y = z - h;
Y[0] = q[0] - h[0];
Y[1] = q[1] - h[1];
Y[2] = q[2] - h[2];
Y[3] = q[3] - h[3];
//
Y[4] = accel[0] - h[4];
Y[5] = accel[1] - h[5];
Y[6] = accel[2] - h[6];
Y[7] = gyro[0] - h[7];
Y[8] = gyro[1] - h[8];
Y[9] = gyro[2] - h[9];
//////////////////////////////////////////////////////////////////////////
Y[10] = mag[0] - h[10];
Y[11] = mag[1] - h[11];
Y[12] = mag[2] - h[12];
//////////////////////////////////////////////////////////////////////////
//Measurement covariance update
//S = H*P*H' + R;
arm_mat_trans_f32(&ekf->H, &ekf->HT);
arm_mat_mult_f32(&ekf->H, &ekf->tmpP, &ekf->tmpYX);
arm_mat_mult_f32(&ekf->tmpYX, &ekf->HT, &ekf->S);
arm_mat_add_f32(&ekf->S, &ekf->R, &ekf->tmpS);
//Calculate Kalman gain
//K = P*H'/S;
arm_mat_inverse_f32(&ekf->tmpS, &ekf->S);
arm_mat_mult_f32(&ekf->tmpP, &ekf->HT, &ekf->tmpXY);
arm_mat_mult_f32(&ekf->tmpXY, &ekf->S, &ekf->K);
//Corrected model prediction
//S = S + K*y;
arm_mat_mult_f32(&ekf->K, &ekf->Y, &ekf->tmpX);
arm_mat_add_f32(&ekf->X, &ekf->tmpX, &ekf->X);
//normalize quaternion
norm = FastSqrtI(X[0] * X[0] + X[1] * X[1] + X[2] * X[2] + X[3] * X[3]);
X[0] *= norm;
X[1] *= norm;
X[2] *= norm;
X[3] *= norm;
//Update state covariance with new knowledge
//option: P = P - K*H*P or P = (I - K*H)*P*(I - K*H)' + K*R*K'
#if 0
//P = P - K*H*P
//simple but it can't ensure the matrix will be a positive definite matrix
arm_mat_mult_f32(&ekf->K, &ekf->H, &ekf->tmpXX);
arm_mat_mult_f32(&ekf->tmpXX, &ekf->tmpP, &ekf->P);
arm_mat_sub_f32(&ekf->tmpP, &ekf->P, &ekf->P);
#else
//P=(I - K*H)*P*(I - K*H)' + K*R*K'
arm_mat_mult_f32(&ekf->K, &ekf->H, &ekf->tmpXX);
arm_mat_sub_f32(&ekf->I, &ekf->tmpXX, &ekf->tmpXX);
arm_mat_trans_f32(&ekf->tmpXX, &ekf->tmpXXT);
arm_mat_mult_f32(&ekf->tmpXX, &ekf->tmpP, &ekf->P);
arm_mat_mult_f32(&ekf->P, &ekf->tmpXXT, &ekf->tmpP);
arm_mat_trans_f32(&ekf->K, &ekf->KT);
arm_mat_mult_f32(&ekf->K, &ekf->R, &ekf->tmpXY);
arm_mat_mult_f32(&ekf->tmpXY, &ekf->KT, &ekf->tmpXX);
arm_mat_add_f32(&ekf->tmpP, &ekf->tmpXX, &ekf->P);
#endif
}
void EKF_GetAngle(EKF_Filter* ekf, float32_t* rpy)
{
float32_t R[3][3];
float32_t *X = ekf->X_f32;
//Z-Y-X
R[0][0] = 2.0f * (X[0] * X[0] + X[1] * X[1]) - 1.0f;
R[0][1] = 2.0f * (X[1] * X[2] + X[0] * X[3]);
R[0][2] = 2.0f * (X[1] * X[3] - X[0] * X[2]);
//R[1][0] = 2.0f * (X[1] * X[2] - X[0] * X[3]);
//R[1][1] = 2.0f * (X[0] * X[0] + X[2] * X[2]) - 1.0f;
R[1][2] = 2.0f * (X[2] * X[3] + X[0] * X[1]);
//R[2][0] = 2.0f * (X[1] * X[3] + X[0] * X[2]);
//R[2][1] = 2.0f * (X[2] * X[3] - X[0] * X[1]);
R[2][2] = 2.0f * (X[0] * X[0] + X[3] * X[3]) - 1.0f;
//roll
rpy[0] = FastAtan2(R[1][2], R[2][2]);
if (rpy[0] == EKF_PI)
rpy[0] = -EKF_PI;
//pitch
if (R[0][2] >= 1.0f)
rpy[1] = -EKF_HALFPI;
else if (R[0][2] <= -1.0f)
rpy[1] = EKF_HALFPI;
else
rpy[1] = FastAsin(-R[0][2]);
//yaw
rpy[2] = FastAtan2(R[0][1], R[0][0]);
if (rpy[2] < 0.0f){
rpy[2] += EKF_TWOPI;
}
if (rpy[2] > EKF_TWOPI){
rpy[2] = 0.0f;
}
rpy[0] = EKF_TODEG(rpy[0]);
rpy[1] = EKF_TODEG(rpy[1]);
rpy[2] = EKF_TODEG(rpy[2]);
}
================================================
FILE: Algorithm/src/INS_EKF.c
================================================
/*
The MIT License (MIT)
Copyright (c) 2015-? suhetao
Permission is hereby granted, free of charge, to any person obtaining a copy of
this software and associated documentation files (the "Software"), to deal in
the Software without restriction, including without limitation the rights to
use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
the Software, and to permit persons to whom the Software is furnished to do so,
subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
#include "INS_EKF.h"
#include "FastMath.h"
#include "Quaternion.h"
#define USE_4TH_RUNGE_KUTTA
//////////////////////////////////////////////////////////////////////////
//all parameters below need to be tune
#define INS_EKF_PQ_INITIAL 0.1f //init quaternion (NED-to-body) uncertainty
#define INS_EKF_PP_INITIAL 100.0f //init North-East-Alt position uncertainties, m
#define INS_EKF_PV_INITIAL 10.0f //init NED velocity uncertainties, m/s
#define INS_EKF_PW_INITIAL 0.01f //init XYZ gyro bias uncertainties, rad/s
#define INS_EKF_PA_INITIAL 0.1f //init XYZ accel bias uncertainties, m/s^2
#define INS_EKF_QQ_INITIAL 0.001f //quaternion process noise
#define INS_EKF_QP_INITIAL 0.0f //position process noise, m
#define INS_EKF_QV_INITIAL 2.0f //velocity process noise, m/s
#define INS_EKF_QW_INITIAL 0.000001f //gyro bias process noise, rad/s
#define INS_EKF_QA_INITIAL 0.000001f //accel bias process noise, m/s^2
#define INS_EKF_RM_INITIAL 0.025f
#define INS_EKF_RP_INITIAL 2.0f
#define INS_EKF_RV_INITIAL 1.0f
#define INS_EKF_GRAVITY 9.81f //local gravity m/s^2
//local magnetic declination
#define INS_EKF_DECLINATION (47.888f / 180.0f * INS_EKF_PI) //47.888f DIP in shenzhen
//////////////////////////////////////////////////////////////////////////
void INS_EKF_New(INS_EKF_Filter* ins)
{
float32_t *F = ins->F_f32;
float32_t *H = ins->H_f32;
float32_t *P = ins->P_f32;
float32_t *Q = ins->Q_f32;
float32_t *R = ins->R_f32;
//////////////////////////////////////////////////////////////////////////
arm_mat_init_f32(&ins->P, INS_EKF_STATE_DIM, INS_EKF_STATE_DIM, ins->P_f32);
arm_mat_zero_f32(&ins->P);
//quaternion
P[0] = P[17] = P[34] = P[51] = INS_EKF_PQ_INITIAL;
//position
P[68] = P[85] = P[102] = INS_EKF_PP_INITIAL;
//velocity
P[119] = P[136] = P[153] = INS_EKF_PV_INITIAL;
//gyro bias
P[170] = P[187] = P[204] = INS_EKF_PW_INITIAL;
//accel bias
P[221] = P[238] = P[255] = INS_EKF_PA_INITIAL;
arm_mat_init_f32(&ins->PX, INS_EKF_STATE_DIM, INS_EKF_STATE_DIM, ins->PX_f32);
arm_mat_init_f32(&ins->PHT, INS_EKF_STATE_DIM, INS_EKF_MEASUREMENT_DIM, ins->PHT_f32);
arm_mat_init_f32(&ins->Q, INS_EKF_STATE_DIM, INS_EKF_STATE_DIM, ins->Q_f32);
arm_mat_zero_f32(&ins->Q);
//quaternion
Q[0] = Q[17] = Q[34] = Q[51] = INS_EKF_QQ_INITIAL;
//position
Q[68] = Q[85] = Q[102] = INS_EKF_QP_INITIAL;
//velocity
Q[119] = Q[136] = Q[153] = INS_EKF_QV_INITIAL;
//gyro bias
Q[170] = Q[187] = Q[204] = INS_EKF_QW_INITIAL;
//accel bias
Q[221] = Q[238] = Q[255] = INS_EKF_QA_INITIAL;
arm_mat_init_f32(&ins->R, INS_EKF_MEASUREMENT_DIM, INS_EKF_MEASUREMENT_DIM, ins->R_f32);
arm_mat_zero_f32(&ins->R);
R[0] = R[10] = R[20] = INS_EKF_RM_INITIAL;
R[30] = R[40] = R[50] = INS_EKF_RP_INITIAL;
R[60] = R[70] = R[80] = INS_EKF_RV_INITIAL;
//////////////////////////////////////////////////////////////////////////
arm_mat_init_f32(&ins->K, INS_EKF_STATE_DIM, INS_EKF_MEASUREMENT_DIM, ins->K_f32);
arm_mat_init_f32(&ins->KH, INS_EKF_STATE_DIM, INS_EKF_STATE_DIM, ins->KH_f32);
arm_mat_init_f32(&ins->KHP, INS_EKF_STATE_DIM, INS_EKF_STATE_DIM, ins->KHP_f32);
arm_mat_init_f32(&ins->KY, INS_EKF_STATE_DIM, 1, ins->KY_f32);
arm_mat_init_f32(&ins->F, INS_EKF_STATE_DIM, INS_EKF_STATE_DIM, ins->F_f32);
arm_mat_zero_f32(&ins->F);
arm_mat_identity_f32(&ins->F, 1.0f);
F[71] = 1.0f; F[88] = 1.0f; F[105] = -1.0f;
//
arm_mat_init_f32(&ins->FT, INS_EKF_STATE_DIM, INS_EKF_STATE_DIM, ins->FT_f32);
//
arm_mat_init_f32(&ins->H, INS_EKF_MEASUREMENT_DIM, INS_EKF_STATE_DIM, H);
arm_mat_zero_f32(&ins->H);
H[52] = 1.0f; H[69] = 1.0f; H[86] = 1.0f; H[103] = 1.0f; H[120] = 1.0f; H[137] = 1.0f;
arm_mat_init_f32(&ins->HT, INS_EKF_STATE_DIM, INS_EKF_MEASUREMENT_DIM, ins->HT_f32);
arm_mat_init_f32(&ins->HP, INS_EKF_MEASUREMENT_DIM, INS_EKF_STATE_DIM, ins->HP_f32);
//////////////////////////////////////////////////////////////////////////
arm_mat_init_f32(&ins->S, INS_EKF_MEASUREMENT_DIM, INS_EKF_MEASUREMENT_DIM, ins->S_f32);
arm_mat_init_f32(&ins->SI, INS_EKF_MEASUREMENT_DIM, INS_EKF_MEASUREMENT_DIM, ins->SI_f32);
arm_mat_init_f32(&ins->X, INS_EKF_STATE_DIM, 1, ins->X_f32);
arm_mat_zero_f32(&ins->X);
//////////////////////////////////////////////////////////////////////////
arm_mat_init_f32(&ins->Y, INS_EKF_MEASUREMENT_DIM, 1, ins->Y_f32);
arm_mat_zero_f32(&ins->Y);
//////////////////////////////////////////////////////////////////////////
//
}
void INS_EKF_Init(INS_EKF_Filter* ins, float32_t *p, float32_t *v, float32_t *accel, float32_t *mag)
{
float32_t *X = ins->X_f32;
// local variables
float32_t norma, normm, normx, normy;
float32_t declination;
//3x3 rotation matrix
float32_t R[9];
// place the un-normalized gravity and geomagnetic vectors into
// the rotation matrix z and x axes
R[2] = accel[0]; R[5] = accel[1]; R[8] = accel[2];
R[0] = mag[0]; R[3] = mag[1]; R[6] = mag[2];
// set y vector to vector product of z and x vectors
R[1] = R[5] * R[6] - R[8] * R[3];
R[4] = R[8] * R[0] - R[2] * R[6];
R[7] = R[2] * R[3] - R[5] * R[0];
// set x vector to vector product of y and z vectors
R[0] = R[4] * R[8] - R[7] * R[5];
R[3] = R[7] * R[2] - R[1] * R[8];
R[6] = R[1] * R[5] - R[4] * R[2];
// calculate the vector moduli invert
norma = FastSqrtI(accel[0] * accel[0] + accel[1] * accel[1] + accel[2] * accel[2]);
normm = FastSqrtI(mag[0] * mag[0] + mag[1] * mag[1] + mag[2] * mag[2]);
normx = FastSqrtI(R[0] * R[0] + R[3] * R[3] + R[6] * R[6]);
normy = FastSqrtI(R[1] * R[1] + R[4] * R[4] + R[7] * R[7]);
// normalize the rotation matrix
// normalize x axis
R[0] *= normx; R[3] *= normx; R[6] *= normx;
// normalize y axis
R[1] *= normy; R[4] *= normy; R[7] *= normy;
// normalize z axis
R[2] *= norma; R[5] *= norma; R[8] *= norma;
//////////////////////////////////////////////////////////////////////////
//estimate the declination
declination = (accel[0] * mag[0] + accel[1] * mag[1] + accel[2] * mag[2]) * norma * normm;
ins->declination = FastAsin(declination);
//ins->declination = INS_EKF_DECLINATION;
ins->gravity = INS_EKF_GRAVITY;
Quaternion_FromRotationMatrix(R, X);
X[4] = p[0]; X[5] = p[1]; X[6] = p[2];
X[7] = v[0]; X[8] = v[1]; X[9] = v[2];
}
//////////////////////////////////////////////////////////////////////////
//additional input for calculate F and H with gyroscope, accelerometer and delta time
void INS_EFK_Update(INS_EKF_Filter* ins, float32_t *mag, float32_t *p, float32_t *v, float32_t *gyro, float32_t *accel, float32_t dt)
{
float32_t *F = ins->F_f32;
float32_t *H = ins->H_f32;
float32_t *X = ins->X_f32;
float32_t *Y = ins->Y_f32;
//////////////////////////////////////////////////////////////////////////
float32_t q0, q1, q2, q3; //w x y z
//float32_t halfq0,halfq1,halfq2,halfq3;
float32_t halfdtq0,halfdtq1,halfdtq2,halfdtq3;
float32_t /*q0q0,*/ q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
float32_t _2q0, _2q1, _2q2, _2q3;
float32_t /*_4q0,*/ _4q1, _4q2, _4q3;
float32_t halfgx, halfgy, halfgz;
//float32_t halfdgx, halfdgy, halfdgz;
float32_t dax, day, daz;
float32_t ax, ay, az;
float32_t dtdvx, dtdvy, dtdvz;
float32_t halfdt = 0.5f * dt;
//////////////////////////////////////////////////////////////////////////
float32_t Cbn[9];//Cnb[9];
float32_t sdeclination, cdeclination;
//stuff
float32_t norm;
//////////////////////////////////////////////////////////////////////////
//halfdgx = 0.5f * (gyro[0] - X[10]);
//halfdgy = 0.5f * (gyro[1] - X[11]);
//halfdgz = 0.5f * (gyro[2] - X[12]);
halfgx = halfdt * (gyro[0] - X[10]);
halfgy = halfdt * (gyro[1] - X[11]);
halfgz = halfdt * (gyro[2] - X[12]);
//
dax = accel[0] - X[13];
day = accel[1] - X[14];
daz = accel[2] - X[15];
//
dtdvx = dax * dt; dtdvy = day * dt; dtdvz = daz * dt;
/////////////////////////////////////////////////////////////////////////
q0 = X[0]; q1 = X[1]; q2 = X[2]; q3 = X[3];
//halfq0 = 0.5f * q0; halfq1 = 0.5f * q1; halfq2 = 0.5f * q2; halfq3 = 0.5f * q3;
halfdtq0 = halfdt * q0; halfdtq1 = halfdt * q1; halfdtq2 = halfdt * q2; halfdtq3 = halfdt * q3;
/*q0q0 = q0 * q0;*/ q0q1 = q0 * q1; q0q2 = q0 * q2; q0q3 = q0 * q3;
q1q1 = q1 * q1; q1q2 = q1 * q2; q1q3 = q1 * q3;
q2q2 = q2 * q2; q2q3 = q2 * q3;
q3q3 = q3 * q3;
_2q0 = 2.0f * q0; _2q1 = 2.0f * q1; _2q2 = 2.0f * q2; _2q3 = 2.0f * q3;
/*_4q0 = 4.0f * q0;*/ _4q1 = 4.0f * q1; _4q2 = 4.0f * q2; _4q3 = 4.0f * q3;
//////////////////////////////////////////////////////////////////////////
//accel b frame to reference frame
Cbn[0] = 1.0f - 2.0f *(q2q2 + q3q3); Cbn[1] = 2.0f *(q1q2 - q0q3); Cbn[2] = 2.0f * (q1q3 + q0q2);
Cbn[3] = 2.0f * (q1q2 + q0q3); Cbn[4] = 1.0f - 2.0f * (q1q1 + q3q3); Cbn[5] = 2.0f * (q2q3 - q0q1);
Cbn[6] = 2.0f * (q1q3 - q0q2); Cbn[7] = 2.0f *(q2q3 + q0q1); Cbn[8] = 1.0f - 2.0f * (q1q1 + q2q2);
ax = Cbn[0] * dax + Cbn[1] * day + Cbn[2] * daz;
ay = Cbn[3] * dax + Cbn[4] * day + Cbn[5] * daz;
az = Cbn[6] * dax + Cbn[7] * day + Cbn[8] * daz;
az = az + ins->gravity;
//////////////////////////////////////////////////////////////////////////
//X = X + dX * dt;
//quaternion
X[0] = q0 - halfgx * q1 - halfgy * q2 - halfgz * q3;
X[1] = q1 + halfgx * q0 - halfgy * q3 + halfgz * q2;
X[2] = q2 + halfgx * q3 + halfgy * q0 - halfgz * q1;
X[3] = q3 - halfgx * q2 + halfgy * q1 + halfgz * q0;
//North-East-Alt position
X[4] = X[4] + X[7] * dt;
X[5] = X[5] + X[8] * dt;
X[6] = X[6] - X[9] * dt;
//NED velocity
X[7] = X[7] + ax * dt;
X[8] = X[8] + ay * dt;
X[9] = X[9] + az * dt;
//////////////////////////////////////////////////////////////////////////
//calculate linearized state dynamics, f = d(xdot)/dx
/*
F[0] = 0.0f; F[1] = -halfdgx; F[2] = -halfdgy; F[3] = -halfdgz; F[4] = 0; F[5] = 0; F[6] = 0; F[7] = 0; F[8] = 0; F[9] = 0; F[10] = halfq1; F[11] = halfq2; F[12] = halfq3; F[13] = 0; F[14] = 0; F[15] = 0;
F[16] = halfdgx; F[17] = 0; F[18] = halfdgz; F[19] = -halfdgy; F[20] = 0; F[21] = 0; F[22] = 0; F[23] = 0; F[24] = 0; F[25] = 0; F[26] = -halfq0; F[27] = halfq3; F[28] = -halfq2; F[29] = 0; F[30] = 0; F[31] = 0;
F[32] = halfdgy; F[33] = -halfdgz; F[34] = 0; F[35] = halfdgx; F[36] = 0; F[37] = 0; F[38] = 0; F[39] = 0; F[40] = 0; F[41] = 0; F[42] = -halfq3; F[43] = -halfq0; F[44] = halfq1; F[45] = 0; F[46] = 0; F[47] = 0;
F[48] = halfdgz; F[49] = halfdgy; F[50] = -halfdgx; F[51] = 0; F[52] = 0; F[53] = 0; F[54] = 0; F[55] = 0; F[56] = 0; F[57] = 0; F[58] = halfq2; F[59] = -halfq1; F[60] = -halfq0; F[61] = 0; F[62] = 0; F[63] = 0;
F[64] = 0; F[65] = 0; F[66] = 0; F[67] = 0; F[68] = 0; F[69] = 0; F[70] = 0; F[71] = 1.0f; F[72] = 0; F[73] = 0; F[74] = 0; F[75] = 0; F[76] = 0; F[77] = 0; F[78] = 0; F[79] = 0;
F[80] = 0; F[81] = 0; F[82] = 0; F[83] = 0; F[84] = 0; F[85] = 0; F[86] = 0; F[87] = 0; F[88] = 1.0f; F[89] = 0; F[90] = 0; F[91] = 0; F[92] = 0; F[93] = 0; F[94] = 0; F[95] = 0;
F[96] = 0; F[97] = 0; F[98] = 0; F[99] = 0; F[100] = 0; F[101] = 0; F[102] = 0; F[103] = 0; F[104] = 0; F[105] = -1.0f; F[106] = 0; F[107] = 0; F[108] = 0; F[109] = 0; F[110] = 0; F[111] = 0;
F[112] = -_2q3 * day + _2q2 * daz; F[113] = _2q2 * day + _2q3 * daz; F[114] = -_4q2 * dax + _2q1 * day + _2q0 * daz; F[115] = -_2q0 * day - _4q3 * dax + _2q1 * daz; F[116] = 0; F[117] = 0; F[118] = 0; F[119] = 0; F[120] = 0; F[121] = 0; F[122] = 0; F[123] = 0; F[124] = 0; F[125] = -Cbn[0]; F[126] = -Cbn[1]; F[127] = -Cbn[2];
F[128] = -_2q1 * daz + _2q3 * dax; F[129] = -_4q1 * day + _2q2 * dax - _2q0 * daz; F[130] = _2q1 * dax + _2q3 * daz; F[131] = -_4q3 * day + _2q0 * dax + _2q2 * daz; F[132] = 0; F[133] = 0; F[134] = 0; F[135] = 0; F[136] = 0; F[137] = 0; F[138] = 0; F[139] = 0; F[140] = 0; F[141] = -Cbn[3]; F[142] = -Cbn[4]; F[143] = -Cbn[5];
F[144] = -_2q2 * dax + _2q1 * day; F[145] = -_4q1 * daz + _2q3 * dax + _2q0 * day; F[146] = -_2q0 * dax + _2q3 * day - _4q2 * daz; F[147] = _2q1 * dax + _2q2 * day; F[148] = 0; F[149] = 0; F[150] = 0; F[151] = 0; F[152] = 0; F[153] = 0; F[154] = 0; F[155] = 0; F[156] = 0; F[157] = -Cbn[6]; F[158] = -Cbn[7]; F[159] = -Cbn[8];
F[160] = 0; F[161] = 0; F[162] = 0; F[163] = 0; F[164] = 0; F[165] = 0; F[166] = 0; F[167] = 0; F[168] = 0; F[169] = 0; F[170] = 0; F[171] = 0; F[172] = 0; F[173] = 0; F[174] = 0; F[175] = 0;
F[176] = 0; F[177] = 0; F[178] = 0; F[179] = 0; F[180] = 0; F[181] = 0; F[182] = 0; F[183] = 0; F[184] = 0; F[185] = 0; F[186] = 0; F[187] = 0; F[188] = 0; F[189] = 0; F[190] = 0; F[191] = 0;
F[192] = 0; F[193] = 0; F[194] = 0; F[195] = 0; F[196] = 0; F[197] = 0; F[198] = 0; F[199] = 0; F[200] = 0; F[201] = 0; F[202] = 0; F[203] = 0; F[204] = 0; F[205] = 0; F[206] = 0; F[207] = 0;
F[208] = 0; F[209] = 0; F[210] = 0; F[211] = 0; F[212] = 0; F[213] = 0; F[214] = 0; F[215] = 0; F[216] = 0; F[217] = 0; F[218] = 0; F[219] = 0; F[220] = 0; F[221] = 0; F[222] = 0; F[223] = 0;
F[224] = 0; F[225] = 0; F[226] = 0; F[227] = 0; F[228] = 0; F[229] = 0; F[230] = 0; F[231] = 0; F[232] = 0; F[233] = 0; F[234] = 0; F[235] = 0; F[236] = 0; F[237] = 0; F[238] = 0; F[239] = 0;
F[240] = 0; F[241] = 0; F[242] = 0; F[243] = 0; F[244] = 0; F[245] = 0; F[246] = 0; F[247] = 0; F[248] = 0; F[249] = 0; F[250] = 0; F[251] = 0; F[252] = 0; F[253] = 0; F[254] = 0; F[255] = 0;
*/
//////////////////////////////////////////////////////////////////////////
//convert from linearized continuous-domain state model (F,Q(t)), into discrete-domain state transformation (FT[k] & Q[k])
//simple take two items from taylor series or you can try another way
//FT[k] = (I + F'* dt)' = I' + (F' * dt)' = I + F * dt;
//F[0] = F[17] = F[34] = F[51] = F[68] = F[85] = F[102] = F[119] = F[136] = F[153] = F[170] = F[187] = F[204] = F[221] = F[238] = F[255] = 1.0f;
F[1] = -halfgx; F[2] = -halfgy; F[3] = -halfgz;F[10] = halfdtq1; F[11] = halfdtq2; F[12] = halfdtq3;
F[16] = halfgx; F[18] = halfgz; F[19] = -halfgy; F[26] = -halfdtq0; F[27] = halfdtq3; F[28] = -halfdtq2;
F[32] = halfgy; F[33] = -halfgz; F[35] = halfgx; F[42] = -halfdtq3; F[43] = -halfdtq0; F[44] = halfdtq1;
F[48] = halfgz; F[49] = halfgy; F[50] = -halfgx; F[58] = halfdtq2; F[59] = -halfdtq1; F[60] = -halfdtq0;
F[112] = -_2q3 * dtdvy + _2q2 * dtdvz; F[113] = _2q2 * dtdvy + _2q3 * dtdvz; F[114] = -_4q2 * dtdvx + _2q1 * dtdvy + _2q0 * dtdvz; F[115] = -_2q0 * dtdvy - _4q3 * dtdvx + _2q1 * dtdvz; F[125] = -Cbn[0] * dt; F[126] = -Cbn[1] * dt; F[127] = -Cbn[2] * dt;
F[128] = -_2q1 * dtdvz + _2q3 * dtdvx; F[129] = -_4q1 * dtdvy + _2q2 * dtdvx - _2q0 * dtdvz; F[130] = _2q1 * dtdvx + _2q3 * dtdvz; F[131] = -_4q3 * dtdvy + _2q0 * dtdvx + _2q2 * dtdvz; F[141] = -Cbn[3] * dt; F[142] = -Cbn[4] * dt; F[143] = -Cbn[5] * dt;
F[144] = -_2q2 * dtdvx + _2q1 * dtdvy; F[145] = -_4q1 * dtdvz + _2q3 * dtdvx + _2q0 * dtdvy; F[146] = -_2q0 * dtdvx + _2q3 * dtdvy - _4q2 * dtdvz; F[147] = _2q1 * dtdvx + _2q2 * dtdvy; F[157] = -Cbn[6] * dt; F[158] = -Cbn[7] * dt; F[159] = -Cbn[8] * dt;
//
//Q[k] = FT[k] * inv(FT[k]) * Q(t)
//////////////////////////////////////////////////////////////////////////
//P = F * P * F' + Q[k]
arm_mat_trans_f32(&ins->F, &ins->FT);
arm_mat_mult_f32(&ins->F, &ins->P, &ins->PX);
arm_mat_mult_f32(&ins->PX, &ins->FT, &ins->P);
arm_mat_add_f32(&ins->P, &ins->Q, &ins->P);
//////////////////////////////////////////////////////////////////////////
q0 = X[0]; q1 = X[1]; q2 = X[2]; q3 = X[3];
_2q0 = 2.0f * q0; _2q1 = 2.0f * q1; _2q2 = 2.0f * q2; _2q3 = 2.0f * q3;
/*_4q0 = 4.0f * q0;*/ _4q1 = 4.0f * q1; _4q2 = 4.0f * q2; _4q3 = 4.0f * q3;
//////////////////////////////////////////////////////////////////////////
//magnetic declination
FastSinCos(ins->declination, &sdeclination, &cdeclination);
//////////////////////////////////////////////////////////////////////////
//Cnb[0] = 1.0f - 2.0f *(q2 * q2 + q3 * q3); Cnb[1] = 2.0f * (q1 * q2 + q3 * q0); Cnb[2] = 2.0f * (q1 * q3 - q2 * q0);
//Cnb[3] = 2.0f * (q1 * q2 - q3 * q0); Cnb[4] = 1.0f - 2.0f *(q1 * q1 + q3 * q3); Cnb[5] = 2.0f * (q2 * q3 + q1 * q0);
//Cnb[6] = 2.0f * (q1 * q3 + q2 * q0); Cnb[7] = 2.0f * (q2 * q3 - q1 * q0); Cnb[8] = 1.0f - 2.0f * (q1 * q1 + q2 * q2);
//Cmn[0] = cdeclination; Cmn[1] = -sdeclination; Cmn[2] = 0;
//Cmn[3] = sdeclination; Cmn[4] = cdeclination; Cmn[5] = 0;
//Cmn[6] = 0; Cmn[7] = 0; Cmn[8] = 1.0f;
//mn[3] = {1.0, 0, 0}; 3D mag ned unit calculate by Cnb * Cmn * mn; (in body)
Y[0] = (1.0f - 2.0f *(q2 * q2 + q3 * q3)) * cdeclination + 2.0f * (q1 * q2 + q3 * q0) * sdeclination;
Y[1] = 2.0f * (q1 * q2 - q3 * q0) * cdeclination + (1.0f - 2.0f *(q1 * q1 + q3 * q3)) * sdeclination;
Y[2] = 2.0f * (q1 * q3 + q2 * q0) * cdeclination + 2.0f * (q2 * q3 - q1 * q0) * sdeclination;
//postion
Y[3] = X[4]; Y[4] = X[5]; Y[5] = X[6];
//velocity
Y[6] = X[7]; Y[7] = X[8]; Y[8] = X[9];
H[0] = _2q3 * sdeclination; H[1] = _2q2 * sdeclination; H[2] = _2q1 * sdeclination - _4q2 * cdeclination; H[3] = _2q0 * sdeclination - _4q3 * cdeclination; /*H[4] = 0; H[5] = 0; H[6] = 0; H[7] = 0; H[8] = 0; H[9] = 0; H[10] = 0; H[11] = 0; H[12] = 0; H[13] = 0; H[14] = 0; H[15] = 0;*/
H[16] = -_2q3 * cdeclination; H[17] = _2q2 * cdeclination - _4q1 * sdeclination; H[18] = _2q1 * cdeclination; H[19] = - _2q0 * cdeclination - _4q3* sdeclination; /*H[20] = 0; H[21] = 0; H[22] = 0; H[23] = 0; H[24] = 0; H[25] = 0; H[26] = 0; H[27] = 0; H[28] = 0; H[29] = 0; H[30] = 0; H[31] = 0;*/
H[32] = _2q2 * cdeclination - _2q1 * sdeclination; H[33] = _2q3 * cdeclination - _2q0 * sdeclination; H[34] = _2q0 * cdeclination + _2q3 * sdeclination; H[35] = _2q1 * cdeclination + _2q2 * sdeclination; /*H[36] = 0; H[37] = 0; H[38] = 0; H[39] = 0; H[40] = 0; H[41] = 0; H[42] = 0; H[43] = 0; H[44] = 0; H[45] = 0; H[46] = 0; H[47] = 0;*/
/*H[48] = 0; H[49] = 0; H[50] = 0; H[51] = 0; H[52] = 1.0f; H[53] = 0; H[54] = 0; H[55] = 0; H[56] = 0; H[57] = 0; H[58] = 0; H[59] = 0; H[60] = 0; H[61] = 0; H[62] = 0; H[63] = 0;*/
/*H[64] = 0; H[65] = 0; H[66] = 0; H[67] = 0; H[68] = 0; H[69] = 1.0f; H[70] = 0; H[71] = 0; H[72] = 0; H[73] = 0; H[74] = 0; H[75] = 0; H[76] = 0; H[77] = 0; H[78] = 0; H[79] = 0;*/
/*H[80] = 0; H[81] = 0; H[82] = 0; H[83] = 0; H[84] = 0; H[85] = 0; H[86] = 1.0f; H[87] = 0; H[88] = 0; H[89] = 0; H[90] = 0; H[91] = 0; H[92] = 0; H[93] = 0; H[94] = 0; H[95] = 0;*/
/*H[96] = 0; H[97] = 0; H[98] = 0; H[99] = 0; H[100] = 0; H[101] = 0; H[102] = 0; H[103] = 1.0f; H[104] = 0; H[105] = 0; H[106] = 0; H[107] = 0; H[108] = 0; H[109] = 0; H[110] = 0; H[111] = 0;*/
/*H[112] = 0; H[113] = 0; H[114] = 0; H[115] = 0; H[116] = 0; H[117] = 0; H[118] = 0; H[119] = 0; H[120] = 1.0f; H[121] = 0; H[122] = 0; H[123] = 0; H[124] = 0; H[125] = 0; H[126] = 0; H[127] = 0;*/
/*H[128] = 0; H[129] = 0; H[130] = 0; H[131] = 0; H[132] = 0; H[133] = 0; H[134] = 0; H[135] = 0; H[136] = 0; H[137] = 1.0f; H[138] = 0; H[139] = 0; H[140] = 0; H[141] = 0; H[142] = 0; H[143] = 0;*/
//////////////////////////////////////////////////////////////////////////
//K = (P * H') / (H * P * H'+ R)
//S = H * P * H' + R;
arm_mat_trans_f32(&ins->H, &ins->HT);
arm_mat_mult_f32(&ins->H, &ins->P, &ins->HP);
arm_mat_mult_f32(&ins->HP, &ins->HT, &ins->S);
arm_mat_add_f32(&ins->S, &ins->R, &ins->S);
//calculate Kalman gain
//K = P * H'/ S;
arm_mat_inverse_f32(&ins->S, &ins->SI);
arm_mat_mult_f32(&ins->P, &ins->HT, &ins->PHT);
arm_mat_mult_f32(&ins->PHT, &ins->SI, &ins->K);
//////////////////////////////////////////////////////////////////////////
//
norm = FastSqrtI(mag[0] * mag[0] + mag[1] * mag[1] + mag[2] * mag[2]);
mag[0] *= norm; mag[1] *= norm; mag[2] *= norm;
//unit vector pointing to magnetic North in body coords
Y[0] = mag[0] - Y[0]; Y[1] = mag[1] - Y[1]; Y[2] = mag[2] - Y[2];
//north pos, east pos, altitude
Y[3] = p[0] - Y[3]; Y[4] = p[1] - Y[4]; Y[5] = p[2] - Y[5];
//north vel, east vel, down velocity
Y[6] = v[0] - Y[6]; Y[7] = v[1] - Y[7]; Y[8] = v[2] - Y[8];
//X = X + K * y
arm_mat_mult_f32(&ins->K, &ins->Y, &ins->KY);
arm_mat_add_f32(&ins->X, &ins->KY, &ins->X);
//////////////////////////////////////////////////////////////////////////
//P = (I - K * H) * P
//P = P - K * H * P
arm_mat_mult_f32(&ins->K, &ins->H, &ins->KH);
arm_mat_mult_f32(&ins->KH, &ins->P, &ins->KHP);
arm_mat_sub_f32(&ins->P, &ins->KHP, &ins->P);
//////////////////////////////////////////////////////////////////////////
//normalize quaternion
norm = FastSqrtI(X[0] * X[0] + X[1] * X[1] + X[2] * X[2] + X[3] * X[3]);
X[0] *= norm;
X[1] *= norm;
X[2] *= norm;
X[3] *= norm;
}
void INS_EKF_GetAngle(INS_EKF_Filter* ins, float32_t* rpy)
{
float32_t Cnb[9];
float32_t *X = ins->X_f32;
//Cnb
Cnb[0] = 2.0f * (X[0] * X[0] + X[1] * X[1]) - 1.0f;
Cnb[1] = 2.0f * (X[1] * X[2] + X[0] * X[3]);
Cnb[2] = 2.0f * (X[1] * X[3] - X[0] * X[2]);
//Cnb[3] = 2.0f * (X[1] * X[2] - X[0] * X[3]);
//Cnb[4] = 2.0f * (X[0] * X[0] + X[2] * X[2]) - 1.0f;
Cnb[5] = 2.0f * (X[2] * X[3] + X[0] * X[1]);
//Cnb[6] = 2.0f * (X[1] * X[3] + X[0] * X[2]);
//Cnb[7] = 2.0f * (X[2] * X[3] - X[0] * X[1]);
Cnb[8] = 2.0f * (X[0] * X[0] + X[3] * X[3]) - 1.0f;
//roll
rpy[0] = FastAtan2(Cnb[5], Cnb[8]);
if (rpy[0] == INS_EKF_PI)
rpy[0] = -INS_EKF_PI;
//pitch
if (Cnb[2] >= 1.0f)
rpy[1] = -INS_EKF_HALFPI;
else if (Cnb[2] <= -1.0f)
rpy[1] = INS_EKF_HALFPI;
else
rpy[1] = FastAsin(-Cnb[2]);
//yaw
rpy[2] = FastAtan2(Cnb[1], Cnb[0]);
if (rpy[2] < 0.0f){
rpy[2] += INS_EKF_TWOPI;
}
if (rpy[2] > INS_EKF_TWOPI){
rpy[2] = 0.0f;
}
rpy[0] = INS_EKF_TODEG(rpy[0]);
rpy[1] = INS_EKF_TODEG(rpy[1]);
rpy[2] = INS_EKF_TODEG(rpy[2]);
}
================================================
FILE: Algorithm/src/PID.c
================================================
/*
The MIT License (MIT)
Copyright (c) 2015-? suhetao
Permission is hereby granted, free of charge, to any person obtaining a copy of
this software and associated documentation files (the "Software"), to deal in
the Software without restriction, including without limitation the rights to
use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
the Software, and to permit persons to whom the Software is furnished to do so,
subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
#include "PID.h"
================================================
FILE: Algorithm/src/Quaternion.c
================================================
/*
The MIT License (MIT)
Copyright (c) 2015-? suhetao
Permission is hereby granted, free of charge, to any person obtaining a copy of
this software and associated documentation files (the "Software"), to deal in
the Software without restriction, including without limitation the rights to
use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
the Software, and to permit persons to whom the Software is furnished to do so,
subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
#include "Quaternion.h"
#include "FastMath.h"
void Quaternion_Normalize(float *q)
{
float norm = FastSqrtI(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
q[0] *= norm;
q[1] *= norm;
q[2] *= norm;
q[3] *= norm;
}
void Quaternion_FromEuler(float *q, float *rpy)
{
float sPhi2, cPhi2; // sin(phi/2) and cos(phi/2)
float sThe2, cThe2; // sin(theta/2) and cos(theta/2)
float sPsi2, cPsi2; // sin(psi/2) and cos(psi/2)
// calculate sines and cosines
FastSinCos(0.5f * rpy[0], &sPhi2, &cPhi2);
FastSinCos(0.5f * rpy[1], &sThe2, &cThe2);
FastSinCos(0.5f * rpy[2], &sPsi2, &cPsi2);
// compute the quaternion elements
q[0] = cPsi2 * cThe2 * cPhi2 + sPsi2 * sThe2 * sPhi2;
q[1] = cPsi2 * cThe2 * sPhi2 - sPsi2 * sThe2 * cPhi2;
q[2] = cPsi2 * sThe2 * cPhi2 + sPsi2 * cThe2 * sPhi2;
q[3] = sPsi2 * cThe2 * cPhi2 - cPsi2 * sThe2 * sPhi2;
}
void Quaternion_ToEuler(float *q, float* rpy)
{
float R[3][3];
//Z-Y-X
R[0][0] = 2.0f * (q[0] * q[0] + q[1] * q[1]) - 1.0f;
R[0][1] = 2.0f * (q[1] * q[2] + q[0] * q[3]);
R[0][2] = 2.0f * (q[1] * q[3] - q[0] * q[2]);
//R[1][0] = 2.0f * (q[1] * q[2] - q[0] * q[3]);
//R[1][1] = 2.0f * (q[0] * q[0] + q[2] * q[2]) - 1.0f;
R[1][2] = 2.0f * (q[2] * q[3] + q[0] * q[1]);
//R[2][0] = 2.0f * (q[1] * q[3] + q[0] * q[2]);
//R[2][1] = 2.0f * (q[2] * q[3] - q[0] * q[1]);
R[2][2] = 2.0f * (q[0] * q[0] + q[3] * q[3]) - 1.0f;
//roll
rpy[0] = FastAtan2(R[1][2], R[2][2]);
if (rpy[0] == PI)
rpy[0] = -PI_2;
//pitch
if (R[0][2] >= 1.0f)
rpy[1] = -PI_2;
else if (R[0][2] <= -1.0f)
rpy[1] = PI_2;
else
rpy[1] = FastAsin(-R[0][2]);
//yaw
rpy[2] = FastAtan2(R[0][1], R[0][0]);
if (rpy[2] < 0.0f){
rpy[2] += _2_PI;
}
if (rpy[2] > _2_PI){
rpy[2] = 0.0f;
}
//rpy[0] = RADTODEG(rpy[0]);
//rpy[1] = RADTODEG(rpy[1]);
//rpy[2] = RADTODEG(rpy[2]);
}
void Quaternion_FromRotationMatrix(float *R, float *Q)
{
#if 0
// calculate the trace of the matrix
float trace = R[0] + R[4] + R[8];
float s;
if(trace > 0){
s = 0.5f * FastSqrt(trace + 1.0f);
Q[0] = 0.25f / s;
Q[1] = (R[7] - R[5]) * s;
Q[2] = (R[2] - R[6]) * s;
Q[3] = (R[3] - R[1]) * s;
}
else{
if(R[0] > R[4] && R[0] > R[8] ){
s = 0.5f * FastSqrtI(1.0f + R[0] - R[4] - R[8]);
Q[0] = (R[7] - R[5]) * s;
Q[1] = 0.25f / s;
Q[2] = (R[1] + R[3]) * s;
Q[3] = (R[2] + R[6]) * s;
}
else if(R[4] > R[8]) {
s = 0.5f * FastSqrtI(1.0f + R[4] - R[0] - R[8]);
Q[0] = (R[2] - R[6]) * s;
Q[1] = (R[1] + R[3]) * s;
Q[2] = 0.25f / s;
Q[3] = (R[5] + R[7]) * s;
}
else{
s = 0.5f * FastSqrtI(1.0f + R[8] - R[0] - R[4]);
Q[0] = (R[3] - R[1]) * s;
Q[1] = (R[2] + R[6]) * s;
Q[2] = (R[5] + R[7]) * s;
Q[3] = 0.25f / s;
}
}
#else
// get the instantaneous orientation quaternion
float fq0sq; // q0^2
float recip4q0; // 1/4q0
float fmag; // quaternion magnitude
#define SMALLQ0 0.01F // limit where rounding errors may appear
// get q0^2 and q0
fq0sq = 0.25f * (1.0f + R[0] + R[4] + R[8]);
Q[0] = (float)FastSqrt(FastAbs(fq0sq));
// normal case when q0 is not small meaning rotation angle not near 180 deg
if (Q[0] > SMALLQ0){
// calculate q1 to q3
recip4q0 = 0.25F / Q[0];
Q[1] = recip4q0 * (R[5] - R[7]);
Q[2] = recip4q0 * (R[6] - R[2]);
Q[3] = recip4q0 * (R[1] - R[3]);
}
else{
// special case of near 180 deg corresponds to nearly symmetric matrix
// which is not numerically well conditioned for division by small q0
// instead get absolute values of q1 to q3 from leading diagonal
Q[1] = FastSqrt(FastAbs(0.5f * (1.0f + R[0]) - fq0sq));
Q[2] = FastSqrt(FastAbs(0.5f * (1.0f + R[4]) - fq0sq));
Q[3] = FastSqrt(FastAbs(0.5f * (1.0f + R[8]) - fq0sq));
// first assume q1 is positive and ensure q2 and q3 are consistent with q1
if ((R[1] + R[3]) < 0.0f){
// q1*q2 < 0 so q2 is negative
Q[2] = -Q[2];
if ((R[5] + R[7]) > 0.0f){
// q1*q2 < 0 and q2*q3 > 0 so q3 is also both negative
Q[3] = -Q[3];
}
}
else if ((R[1] + R[3]) > 0.0f){
if ((R[5] + R[7]) < 0.0f){
// q1*q2 > 0 and q2*q3 < 0 so q3 is negative
Q[3] = -Q[3];
}
}
// negate the vector components if q1 should be negative
if ((R[5] - R[7]) < 0.0f){
Q[1] = -Q[1];
Q[2] = -Q[2];
Q[3] = -Q[3];
}
}
// finally re-normalize
fmag = FastSqrtI(Q[0] * Q[0] + Q[1] * Q[1] + Q[2] * Q[2] + Q[3] * Q[3]);
Q[0] *= fmag;
Q[1] *= fmag;
Q[2] *= fmag;
Q[3] *= fmag;
#endif
}
void Quaternion_RungeKutta4(float *q, float *w, float dt, int normalize)
{
float half = 0.5f;
float two = 2.0f;
float qw[4], k2[4], k3[4], k4[4];
float tmpq[4], tmpk[4];
//qw = q * w * half;
Quaternion_Multiply(qw, q, w);
Quaternion_Scalar(qw, qw, half);
//k2 = (q + qw * dt * half) * w * half;
Quaternion_Scalar(tmpk, qw, dt * half);
Quaternion_Add(tmpk, q, tmpk);
Quaternion_Multiply(k2, tmpk, w);
Quaternion_Scalar(k2, k2, half);
//k3 = (q + k2 * dt * half) * w * half;
Quaternion_Scalar(tmpk, k2, dt * half);
Quaternion_Add(tmpk, q, tmpk);
Quaternion_Multiply(k3, tmpk, w);
Quaternion_Scalar(k3, k3, half);
//k4 = (q + k3 * dt) * w * half;
Quaternion_Scalar(tmpk, k3, dt);
Quaternion_Add(tmpk, q, tmpk);
Quaternion_Multiply(k4, tmpk, w);
Quaternion_Scalar(k4, k4, half);
//q += (qw + k2 * two + k3 * two + k4) * (dt / 6);
Quaternion_Scalar(tmpk, k2, two);
Quaternion_Add(tmpq, qw, tmpk);
Quaternion_Scalar(tmpk, k3, two);
Quaternion_Add(tmpq, tmpq, tmpk);
Quaternion_Add(tmpq, tmpq, k4);
Quaternion_Scalar(tmpq, tmpq, dt / 6.0f);
Quaternion_Add(q, q, tmpq);
if (normalize){
Quaternion_Normalize(q);
}
}
void Quaternion_From6AxisData(float* q, float *accel, float *mag)
{
// local variables
float norma, normx, normy;
//float normm;
//3x3 rotation matrix
float R[9];
// place the un-normalized gravity and geomagnetic vectors into
// the rotation matrix z and x axes
R[2] = accel[0]; R[5] = accel[1]; R[8] = accel[2];
R[0] = mag[0]; R[3] = mag[1]; R[6] = mag[2];
// set y vector to vector product of z and x vectors
R[1] = R[5] * R[6] - R[8] * R[3];
R[4] = R[8] * R[0] - R[2] * R[6];
R[7] = R[2] * R[3] - R[5] * R[0];
// set x vector to vector product of y and z vectors
R[0] = R[4] * R[8] - R[7] * R[5];
R[3] = R[7] * R[2] - R[1] * R[8];
R[6] = R[1] * R[5] - R[4] * R[2];
// calculate the vector moduli invert
norma = FastSqrtI(accel[0] * accel[0] + accel[1] * accel[1] + accel[2] * accel[2]);
//normm = FastSqrtI(mag[0] * mag[0] + mag[1] * mag[1] + mag[2] * mag[2]);
normx = FastSqrtI(R[0] * R[0] + R[3] * R[3] + R[6] * R[6]);
normy = FastSqrtI(R[1] * R[1] + R[4] * R[4] + R[7] * R[7]);
// normalize the rotation matrix
// normalize x axis
R[0] *= normx; R[3] *= normx; R[6] *= normx;
// normalize y axis
R[1] *= normy; R[4] *= normy; R[7] *= normy;
// normalize z axis
R[2] *= norma; R[5] *= norma; R[8] *= norma;
Quaternion_FromRotationMatrix(R, q);
}
================================================
FILE: Algorithm/src/SRCKF.c
================================================
/*
The MIT License (MIT)
Copyright (c) 2015-? suhetao
Permission is hereby granted, free of charge, to any person obtaining a copy of
this software and associated documentation files (the "Software"), to deal in
the Software without restriction, including without limitation the rights to
use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
the Software, and to permit persons to whom the Software is furnished to do so,
subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
#include "SRCKF.h"
#include "FastMath.h"
#include "Quaternion.h"
//////////////////////////////////////////////////////////////////////////
//all parameters below need to be tune
#define SRCKF_PQ_INITIAL 0.000001f
#define SRCKF_PW_INITIAL 0.000001f
#define SRCKF_QQ_INITIAL 0.0000045f
#define SRCKF_QW_INITIAL 0.0000025f
#define SRCKF_RA_INITIAL 0.07f
#define SRCKF_RM_INITIAL 0.105f
//////////////////////////////////////////////////////////////////////////
//
void SRCKF_New(SRCKF_Filter* srckf)
{
float32_t kesi;
arm_matrix_instance_f32 KesiPuls, KesiMinu;
float32_t KesiPuls_f32[SRCKF_STATE_DIM * SRCKF_STATE_DIM], KesiMinus_f32[SRCKF_STATE_DIM * SRCKF_STATE_DIM];
float32_t *S = srckf->S_f32;
float32_t *SQ = srckf->SQ_f32;
float32_t *SR = srckf->SR_f32;
//////////////////////////////////////////////////////////////////////////
//initialise weight
srckf->W = 1.0f / (float32_t)SRCKF_CP_POINTS;
arm_sqrt_f32(srckf->W, &srckf->SW);
//initialise kesi
//generate the cubature point
kesi = (float32_t)SRCKF_STATE_DIM;
arm_sqrt_f32(kesi, &kesi);
arm_mat_init_f32(&KesiPuls, SRCKF_STATE_DIM, SRCKF_STATE_DIM, KesiPuls_f32);
arm_mat_zero_f32(&KesiPuls);
arm_mat_init_f32(&KesiMinu, SRCKF_STATE_DIM, SRCKF_STATE_DIM, KesiMinus_f32);
arm_mat_zero_f32(&KesiMinu);
arm_mat_identity_f32(&KesiPuls, kesi);
arm_mat_identity_f32(&KesiMinu, -kesi);
arm_mat_init_f32(&srckf->Kesi, SRCKF_STATE_DIM, SRCKF_CP_POINTS, srckf->Kesi_f32);
arm_mat_setsubmatrix_f32(&srckf->Kesi, &KesiPuls, 0, 0);
arm_mat_setsubmatrix_f32(&srckf->Kesi, &KesiMinu, 0, SRCKF_STATE_DIM);
arm_mat_init_f32(&srckf->iKesi, SRCKF_STATE_DIM, 1, srckf->iKesi_f32);
arm_mat_zero_f32(&srckf->iKesi);
//initialise state covariance
arm_mat_init_f32(&srckf->S, SRCKF_STATE_DIM, SRCKF_STATE_DIM, srckf->S_f32);
arm_mat_zero_f32(&srckf->S);
S[0] = S[8] = S[16] = S[24] = SRCKF_PQ_INITIAL;
S[32] = S[40] = S[48] = SRCKF_PW_INITIAL;
arm_mat_init_f32(&srckf->ST, SRCKF_STATE_DIM, SRCKF_STATE_DIM, srckf->ST_f32);
//initialise measurement covariance
arm_mat_init_f32(&srckf->SY, SRCKF_MEASUREMENT_DIM, SRCKF_MEASUREMENT_DIM, srckf->SY_f32);
arm_mat_init_f32(&srckf->SYI, SRCKF_MEASUREMENT_DIM, SRCKF_MEASUREMENT_DIM, srckf->SYI_f32);
arm_mat_init_f32(&srckf->SYT, SRCKF_MEASUREMENT_DIM, SRCKF_MEASUREMENT_DIM, srckf->SYT_f32);
arm_mat_init_f32(&srckf->SYTI, SRCKF_MEASUREMENT_DIM, SRCKF_MEASUREMENT_DIM, srckf->SYTI_f32);
arm_mat_init_f32(&srckf->PXY, SRCKF_STATE_DIM, SRCKF_MEASUREMENT_DIM, srckf->PXY_f32);
arm_mat_init_f32(&srckf->tmpPXY, SRCKF_STATE_DIM, SRCKF_MEASUREMENT_DIM, srckf->tmpPXY_f32);
//initialise SQ
arm_mat_init_f32(&srckf->SQ, SRCKF_STATE_DIM, SRCKF_STATE_DIM, srckf->SQ_f32);
arm_mat_zero_f32(&srckf->SQ);
SQ[0] = SQ[8] = SQ[16] = SQ[24] = SRCKF_QQ_INITIAL;
SQ[32] = SQ[40] = SQ[48] = SRCKF_QW_INITIAL;
//initialise SR
arm_mat_init_f32(&srckf->SR, SRCKF_MEASUREMENT_DIM, SRCKF_MEASUREMENT_DIM, srckf->SR_f32);
arm_mat_zero_f32(&srckf->SR);
SR[0] = SR[7] = SR[14] = SRCKF_RA_INITIAL;
SR[21] = SR[28] = SR[35] = SRCKF_RM_INITIAL;
//other stuff
//cubature points
arm_mat_init_f32(&srckf->XCP, SRCKF_STATE_DIM, SRCKF_CP_POINTS, srckf->XCP_f32);
//propagated cubature points
arm_mat_init_f32(&srckf->XPCP, SRCKF_STATE_DIM, SRCKF_CP_POINTS, srckf->XPCP_f32);
arm_mat_init_f32(&srckf->YPCP, SRCKF_MEASUREMENT_DIM, SRCKF_CP_POINTS, srckf->YPCP_f32);
arm_mat_init_f32(&srckf->XCPCM, SRCKF_STATE_DIM, SRCKF_CP_POINTS, srckf->XCPCM_f32);
arm_mat_init_f32(&srckf->tmpXCPCM, SRCKF_STATE_DIM, SRCKF_CP_POINTS, srckf->tmpXCPCM_f32);
arm_mat_init_f32(&srckf->YCPCM, SRCKF_MEASUREMENT_DIM, SRCKF_CP_POINTS, srckf->YCPCM_f32);
arm_mat_init_f32(&srckf->YCPCMT, SRCKF_CP_POINTS, SRCKF_MEASUREMENT_DIM, srckf->YCPCMT_f32);
arm_mat_init_f32(&srckf->XCM, SRCKF_STATE_DIM, SRCKF_CP_POINTS + SRCKF_STATE_DIM, srckf->XCM_f32);
//initialise fill SQ
arm_mat_setsubmatrix_f32(&srckf->XCM, &srckf->SQ, 0, SRCKF_CP_POINTS);
arm_mat_init_f32(&srckf->YCM, SRCKF_MEASUREMENT_DIM, SRCKF_CP_POINTS + SRCKF_MEASUREMENT_DIM, srckf->YCM_f32);
//initialise fill SR
arm_mat_setsubmatrix_f32(&srckf->YCM, &srckf->SR, 0, SRCKF_CP_POINTS);
arm_mat_init_f32(&srckf->XYCM, SRCKF_STATE_DIM, SRCKF_CP_POINTS + SRCKF_MEASUREMENT_DIM, srckf->XYCM_f32);
//Kalman gain
arm_mat_init_f32(&srckf->K, SRCKF_STATE_DIM, SRCKF_MEASUREMENT_DIM, srckf->K_f32);
//////////////////////////////////////////////////////////////////////////
//state vector
arm_mat_init_f32(&srckf->X, SRCKF_STATE_DIM, 1, srckf->X_f32);
arm_mat_zero_f32(&srckf->X);
arm_mat_init_f32(&srckf->tmpX, SRCKF_STATE_DIM, 1, srckf->tmpX_f32);
arm_mat_zero_f32(&srckf->tmpX);
//measurement vector
arm_mat_init_f32(&srckf->Y, SRCKF_MEASUREMENT_DIM, 1, srckf->Y_f32);
arm_mat_zero_f32(&srckf->Y);
arm_mat_init_f32(&srckf->tmpY, SRCKF_MEASUREMENT_DIM, 1, srckf->tmpY_f32);
arm_mat_zero_f32(&srckf->tmpY);
}
void SRCKF_Init(SRCKF_Filter* srckf, float32_t *accel, float32_t *mag)
{
float32_t *X = srckf->X_f32;
// local variables
float32_t norma, normx, normy;
//3x3 rotation matrix
float32_t R[9];
// place the un-normalized gravity and geomagnetic vectors into
// the rotation matrix z and x axes
R[2] = accel[0]; R[5] = accel[1]; R[8] = accel[2];
R[0] = mag[0]; R[3] = mag[1]; R[6] = mag[2];
// set y vector to vector product of z and x vectors
R[1] = R[5] * R[6] - R[8] * R[3];
R[4] = R[8] * R[0] - R[2] * R[6];
R[7] = R[2] * R[3] - R[5] * R[0];
// set x vector to vector product of y and z vectors
R[0] = R[4] * R[8] - R[7] * R[5];
R[3] = R[7] * R[2] - R[1] * R[8];
R[6] = R[1] * R[5] - R[4] * R[2];
// calculate the vector moduli invert
norma = FastSqrtI(accel[0] * accel[0] + accel[1] * accel[1] + accel[2] * accel[2]);
normx = FastSqrtI(R[0] * R[0] + R[3] * R[3] + R[6] * R[6]);
normy = FastSqrtI(R[1] * R[1] + R[4] * R[4] + R[7] * R[7]);
// normalize the rotation matrix
// normalize x axis
R[0] *= normx; R[3] *= normx; R[6] *= normx;
// normalize y axis
R[1] *= normy; R[4] *= normy; R[7] *= normy;
// normalize z axis
R[2] *= norma; R[5] *= norma; R[8] *= norma;
Quaternion_FromRotationMatrix(R, X);
}
void SRCKF_Update(SRCKF_Filter* srckf, float32_t *gyro, float32_t *accel, float32_t *mag, float32_t dt)
{
//////////////////////////////////////////////////////////////////////////
float32_t q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
//
float32_t hx, hy, hz;
float32_t bx, bz;
float32_t _2mx, _2my, _2mz;
//
int col;
float32_t dQ[4];
float32_t norm;
float32_t *X = srckf->X_f32, *Y = srckf->Y_f32;
float32_t *tmpX = srckf->tmpX_f32, *tmpY = srckf->tmpY_f32;
float32_t *iKesi = srckf->iKesi_f32;
dQ[0] = 0.0f;
dQ[1] = (gyro[0] - X[4]);
dQ[2] = (gyro[1] - X[5]);
dQ[3] = (gyro[2] - X[6]);
//////////////////////////////////////////////////////////////////////////
//time update
for(col = 0; col < SRCKF_CP_POINTS; col++){
//evaluate the cubature points
arm_mat_getcolumn_f32(&srckf->Kesi, iKesi, col);
arm_mat_mult_f32(&srckf->S, &srckf->iKesi, &srckf->tmpX);
arm_mat_add_f32(&srckf->tmpX, &srckf->X, &srckf->tmpX);
arm_mat_setcolumn_f32(&srckf->XCP, tmpX, col);
//evaluate the propagated cubature points
Quaternion_RungeKutta4(tmpX, dQ, dt, 1);
arm_mat_setcolumn_f32(&srckf->XPCP, tmpX, col);
}
//estimate the predicted state
arm_mat_cumsum_f32(&srckf->XPCP, tmpX, X);
arm_mat_scale_f32(&srckf->X, srckf->W, &srckf->X);
//estimate the square-root factor of the predicted error covariance
for(col = 0; col < SRCKF_CP_POINTS; col++){
arm_mat_getcolumn_f32(&srckf->XPCP, tmpX, col);
arm_mat_sub_f32(&srckf->tmpX, &srckf->X, &srckf->tmpX);
arm_mat_scale_f32(&srckf->tmpX, srckf->SW, &srckf->tmpX);
arm_mat_setcolumn_f32(&srckf->XCM, tmpX, col);
}
//XCM[XPCP, SQ], SQ fill already
arm_mat_qr_decompositionT_f32(&srckf->XCM, &srckf->ST);
arm_mat_trans_f32(&srckf->ST, &srckf->S);
//////////////////////////////////////////////////////////////////////////
//measurement update
//normalize accel and magnetic
norm = FastSqrtI(accel[0] * accel[0] + accel[1] * accel[1] + accel[2] * accel[2]);
accel[0] *= norm;
accel[1] *= norm;
accel[2] *= norm;
//////////////////////////////////////////////////////////////////////////
norm = FastSqrtI(mag[0] * mag[0] + mag[1] * mag[1] + mag[2] * mag[2]);
mag[0] *= norm;
mag[1] *= norm;
mag[2] *= norm;
_2mx = 2.0f * mag[0];
_2my = 2.0f * mag[1];
_2mz = 2.0f * mag[2];
for(col = 0; col < SRCKF_CP_POINTS; col++){
//evaluate the cubature points
arm_mat_getcolumn_f32(&srckf->Kesi, iKesi, col);
arm_mat_mult_f32(&srckf->S, &srckf->iKesi, &srckf->tmpX);
arm_mat_add_f32(&srckf->tmpX, &srckf->X, &srckf->tmpX);
arm_mat_setcolumn_f32(&srckf->XCP, tmpX, col);
//evaluate the propagated cubature points
//auxiliary variables to avoid repeated arithmetic
q0q0 = tmpX[0] * tmpX[0];
q0q1 = tmpX[0] * tmpX[1];
q0q2 = tmpX[0] * tmpX[2];
q0q3 = tmpX[0] * tmpX[3];
q1q1 = tmpX[1] * tmpX[1];
q1q2 = tmpX[1] * tmpX[2];
q1q3 = tmpX[1] * tmpX[3];
q2q2 = tmpX[2] * tmpX[2];
q2q3 = tmpX[2] * tmpX[3];
q3q3 = tmpX[3] * tmpX[3];
//reference direction of earth's magnetic field
hx = _2mx * (0.5f - q2q2 - q3q3) + _2my * (q1q2 - q0q3) + _2mz *(q1q3 + q0q2);
hy = _2mx * (q1q2 + q0q3) + _2my * (0.5f - q1q1 - q3q3) + _2mz * (q2q3 - q0q1);
hz = _2mx * (q1q3 - q0q2) + _2my * (q2q3 + q0q1) + _2mz *(0.5f - q1q1 - q2q2);
arm_sqrt_f32(hx * hx + hy * hy, &bx);
bz = hz;
tmpY[0] = 2.0f * (q1q3 - q0q2);
tmpY[1] = 2.0f * (q2q3 + q0q1);
tmpY[2] = -1.0f + 2.0f * (q0q0 + q3q3);
tmpY[3] = bx * (1.0f - 2.0f * (q2q2 + q3q3)) + bz * ( 2.0f * (q1q3 - q0q2));
tmpY[4] = bx * (2.0f * (q1q2 - q0q3)) + bz * (2.0f * (q2q3 + q0q1));
tmpY[5] = bx * (2.0f * (q1q3 + q0q2)) + bz * (1.0f - 2.0f * (q1q1 + q2q2));
arm_mat_setcolumn_f32(&srckf->YPCP, tmpY, col);
}
//estimate the predicted measurement
arm_mat_cumsum_f32(&srckf->YPCP, tmpY, Y);
arm_mat_scale_f32(&srckf->Y, srckf->W, &srckf->Y);
//estimate the square-root of the innovation covariance matrix
for(col = 0; col < SRCKF_CP_POINTS; col++){
arm_mat_getcolumn_f32(&srckf->YPCP, tmpY, col);
arm_mat_sub_f32(&srckf->tmpY, &srckf->Y, &srckf->tmpY);
arm_mat_scale_f32(&srckf->tmpY, srckf->SW, &srckf->tmpY);
arm_mat_setcolumn_f32(&srckf->YCPCM, tmpY, col);
arm_mat_setcolumn_f32(&srckf->YCM, tmpY, col);
}
//YCM[YPCP, SR], SR fill already
arm_mat_qr_decompositionT_f32(&srckf->YCM, &srckf->SYT);
//estimate the cross-covariance matrix
for(col = 0; col < SRCKF_CP_POINTS; col++){
arm_mat_getcolumn_f32(&srckf->XCP, tmpX, col);
arm_mat_sub_f32(&srckf->tmpX, &srckf->X, &srckf->tmpX);
arm_mat_scale_f32(&srckf->tmpX, srckf->SW, &srckf->tmpX);
arm_mat_setcolumn_f32(&srckf->XCPCM, tmpX, col);
}
arm_mat_trans_f32(&srckf->YCPCM, &srckf->YCPCMT);
arm_mat_mult_f32(&srckf->XCPCM, &srckf->YCPCMT, &srckf->PXY);
//estimate the kalman gain
arm_mat_trans_f32(&srckf->SYT, &srckf->SY);
arm_mat_inverse_f32(&srckf->SYT, &srckf->SYTI);
arm_mat_inverse_f32(&srckf->SY, &srckf->SYI);
arm_mat_mult_f32(&srckf->PXY, &srckf->SYTI, &srckf->tmpPXY);
arm_mat_mult_f32(&srckf->tmpPXY, &srckf->SYI, &srckf->K);
//estimate the updated state
Y[0] = accel[0] - Y[0];
Y[1] = accel[1] - Y[1];
Y[2] = accel[2] - Y[2];
Y[3] = mag[0] - Y[3];
Y[4] = mag[1] - Y[4];
Y[5] = mag[2] - Y[5];
arm_mat_mult_f32(&srckf->K, &srckf->Y, &srckf->tmpX);
arm_mat_add_f32(&srckf->X, &srckf->tmpX, &srckf->X);
//normalize quaternion
norm = FastSqrtI(X[0] * X[0] + X[1] * X[1] + X[2] * X[2] + X[3] * X[3]);
X[0] *= norm;
X[1] *= norm;
X[2] *= norm;
X[3] *= norm;
//estimate the square-root factor of the corresponding error covariance
arm_mat_mult_f32(&srckf->K, &srckf->YCPCM, &srckf->tmpXCPCM);
arm_mat_sub_f32(&srckf->XCPCM, &srckf->tmpXCPCM, &srckf->XCPCM);
arm_mat_setsubmatrix_f32(&srckf->XYCM, &srckf->XCPCM, 0, 0);
arm_mat_mult_f32(&srckf->K, &srckf->SR, &srckf->tmpPXY);
arm_mat_setsubmatrix_f32(&srckf->XYCM, &srckf->tmpPXY, 0, SRCKF_CP_POINTS);
arm_mat_qr_decompositionT_f32(&srckf->XYCM, &srckf->ST);
arm_mat_trans_f32(&srckf->ST, &srckf->S);
}
void SRCKF_GetAngle(SRCKF_Filter* srckf, float32_t* rpy)
{
float32_t R[3][3];
float32_t *X = srckf->X_f32;
//Z-Y-X
R[0][0] = 2.0f * (X[0] * X[0] + X[1] * X[1]) - 1.0f;
R[0][1] = 2.0f * (X[1] * X[2] + X[0] * X[3]);
R[0][2] = 2.0f * (X[1] * X[3] - X[0] * X[2]);
//R[1][0] = 2.0f * (X[1] * X[2] - X[0] * X[3]);
//R[1][1] = 2.0f * (X[0] * X[0] + X[2] * X[2]) - 1.0f;
R[1][2] = 2.0f * (X[2] * X[3] + X[0] * X[1]);
//R[2][0] = 2.0f * (X[1] * X[3] + X[0] * X[2]);
//R[2][1] = 2.0f * (X[2] * X[3] - X[0] * X[1]);
R[2][2] = 2.0f * (X[0] * X[0] + X[3] * X[3]) - 1.0f;
//roll
rpy[0] = FastAtan2(R[1][2], R[2][2]);
if (rpy[0] == SRCKF_PI)
rpy[0] = -SRCKF_PI;
//pitch
if (R[0][2] >= 1.0f)
rpy[1] = -SRCKF_HALFPI;
else if (R[0][2] <= -1.0f)
rpy[1] = SRCKF_HALFPI;
else
rpy[1] = FastAsin(-R[0][2]);
//yaw
rpy[2] = FastAtan2(R[0][1], R[0][0]);
if (rpy[2] < 0.0f){
rpy[2] += SRCKF_TWOPI;
}
if (rpy[2] > SRCKF_TWOPI){
rpy[2] = 0.0f;
}
rpy[0] = SRCKF_TODEG(rpy[0]);
rpy[1] = SRCKF_TODEG(rpy[1]);
rpy[2] = SRCKF_TODEG(rpy[2]);
}
================================================
FILE: Algorithm/src/UKF.c
================================================
/*
The MIT License (MIT)
Copyright (c) 2015-? suhetao
Permission is hereby granted, free of charge, to any person obtaining a copy of
this software and associated documentation files (the "Software"), to deal in
the Software without restriction, including without limitation the rights to
use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
the Software, and to permit persons to whom the Software is furnished to do so,
subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
#include "UKF.h"
#include "FastMath.h"
#include "Quaternion.h"
#define USE_4TH_RUNGE_KUTTA
//////////////////////////////////////////////////////////////////////////
//all parameters below need to be tune
#define UKF_PQ_INITIAL 0.000001f
#define UKF_PW_INITIAL 0.000001f
#define UKF_QQ_INITIAL 0.0000045f
#define UKF_QW_INITIAL 0.0000025f
#define UKF_RQ_INITIAL 0.000001f
#define UKF_RA_INITIAL 0.07f
#define UKF_RW_INITIAL 0.0525f
#define UKF_RM_INITIAL 0.105f
#define UKF_alpha (1.0f)
#define UKF_beta (2.0f)
#define UKF_kappa (-1.0f)
//////////////////////////////////////////////////////////////////////////
//
static void UKF_GenerateSigmaPoints(UKF_Filter* ukf)
{
int cols = 0;
//state
float32_t *X = ukf->X_f32;
float32_t *tmpX = ukf->tmpX_f32;
//need to tune!!!
//covariance
//c*chol(P)'
arm_mat_chol_f32(&ukf->P);
arm_mat_scale_f32(&ukf->P, ukf->gamma, &ukf->P);
//////////////////////////////////////////////////////////////////////////
//generate sigma points
arm_mat_setcolumn_f32(&ukf->XSP, X, cols);
for(cols = 1; cols <= UKF_STATE_DIM; cols++){
arm_mat_getcolumn_f32(&ukf->P, tmpX, cols - 1);
arm_mat_add_f32(&ukf->X, &ukf->tmpX, &ukf->tmpX);
arm_mat_setcolumn_f32(&ukf->XSP, tmpX, cols);
}
for(cols = UKF_STATE_DIM + 1; cols < UKF_SP_POINTS; cols++){
arm_mat_getcolumn_f32(&ukf->P, tmpX, cols - 8/*UKF_STATE_DIM + 1*/);
arm_mat_sub_f32(&ukf->X, &ukf->tmpX, &ukf->tmpX);
arm_mat_setcolumn_f32(&ukf->XSP, tmpX, cols);
}
}
void UKF_New(UKF_Filter* ukf)
{
float32_t *P = ukf->P_f32;
float32_t *Q = ukf->Q_f32;
float32_t *R = ukf->R_f32;
float32_t *Wc = ukf->Wc_f32;
//scaling factor
float32_t lambda = UKF_alpha * UKF_alpha *((float32_t)UKF_STATE_DIM + UKF_kappa) - (float32_t)UKF_STATE_DIM;
float32_t gamma = (float32_t)UKF_STATE_DIM + lambda;
//weights for means
ukf->Wm0 = lambda / gamma;
ukf->Wmi = 0.5f / gamma;
//weights for covariance
arm_mat_init_f32(&ukf->Wc, UKF_SP_POINTS, UKF_SP_POINTS, ukf->Wc_f32);
arm_mat_identity_f32(&ukf->Wc, ukf->Wmi);
Wc[0] = ukf->Wm0 + (1.0f - UKF_alpha * UKF_alpha + UKF_beta);
//scaling factor need to tune!
arm_sqrt_f32(gamma, &ukf->gamma);
//////////////////////////////////////////////////////////////////////////
//initialise P
arm_mat_init_f32(&ukf->P, UKF_STATE_DIM, UKF_STATE_DIM, ukf->P_f32);
arm_mat_identity_f32(&ukf->P, 1.0f);
P[0] = P[8] = P[16] = P[24] = UKF_PQ_INITIAL;
P[32] = P[40] = P[48] = UKF_PW_INITIAL;
arm_mat_init_f32(&ukf->PX, UKF_STATE_DIM, UKF_STATE_DIM, ukf->PX_f32);
arm_mat_init_f32(&ukf->PY, UKF_MEASUREMENT_DIM, UKF_MEASUREMENT_DIM, ukf->PY_f32);
arm_mat_init_f32(&ukf->tmpPY, UKF_MEASUREMENT_DIM, UKF_MEASUREMENT_DIM, ukf->tmpPY_f32);
arm_mat_init_f32(&ukf->PXY, UKF_STATE_DIM, UKF_MEASUREMENT_DIM, ukf->PXY_f32);
arm_mat_init_f32(&ukf->PXYT, UKF_MEASUREMENT_DIM, UKF_STATE_DIM, ukf->PXYT_f32);
//initialise Q
arm_mat_init_f32(&ukf->Q, UKF_STATE_DIM, UKF_STATE_DIM, ukf->Q_f32);
Q[0] = Q[8] = Q[16] = Q[24] = UKF_QQ_INITIAL;
Q[32] = Q[40] = Q[48] = UKF_QW_INITIAL;
//initialise R
arm_mat_init_f32(&ukf->R, UKF_MEASUREMENT_DIM, UKF_MEASUREMENT_DIM, ukf->R_f32);
R[0] = R[14] = R[28] = R[42] = UKF_RQ_INITIAL;
R[56] = R[70] = R[84] = UKF_RA_INITIAL;
R[98] = R[112] = R[126] = UKF_RW_INITIAL;
R[140] = R[154] = R[168] = UKF_RM_INITIAL;
arm_mat_init_f32(&ukf->XSP, UKF_STATE_DIM, UKF_SP_POINTS, ukf->XSP_f32);
arm_mat_init_f32(&ukf->tmpXSP, UKF_STATE_DIM, UKF_SP_POINTS, ukf->tmpXSP_f32);
arm_mat_init_f32(&ukf->tmpXSPT, UKF_SP_POINTS, UKF_STATE_DIM, ukf->tmpXSPT_f32);
arm_mat_init_f32(&ukf->tmpWcXSP, UKF_STATE_DIM, UKF_SP_POINTS, ukf->tmpWcXSP_f32);
arm_mat_init_f32(&ukf->tmpWcYSP, UKF_MEASUREMENT_DIM, UKF_SP_POINTS, ukf->tmpWcYSP_f32);
arm_mat_init_f32(&ukf->YSP, UKF_MEASUREMENT_DIM, UKF_SP_POINTS, ukf->YSP_f32);
arm_mat_init_f32(&ukf->tmpYSP, UKF_MEASUREMENT_DIM, UKF_SP_POINTS, ukf->tmpYSP_f32);
arm_mat_init_f32(&ukf->tmpYSPT, UKF_SP_POINTS, UKF_MEASUREMENT_DIM, ukf->tmpYSPT_f32);
//////////////////////////////////////////////////////////////////////////
arm_mat_init_f32(&ukf->K, UKF_STATE_DIM, UKF_MEASUREMENT_DIM, ukf->K_f32);
arm_mat_init_f32(&ukf->KT, UKF_MEASUREMENT_DIM, UKF_STATE_DIM, ukf->KT_f32);
//////////////////////////////////////////////////////////////////////////
arm_mat_init_f32(&ukf->X, UKF_STATE_DIM, 1, ukf->X_f32);
arm_mat_zero_f32(&ukf->X);
arm_mat_init_f32(&ukf->tmpX, UKF_STATE_DIM, 1, ukf->tmpX_f32);
arm_mat_zero_f32(&ukf->tmpX);
//////////////////////////////////////////////////////////////////////////
arm_mat_init_f32(&ukf->Y, UKF_MEASUREMENT_DIM, 1, ukf->Y_f32);
arm_mat_zero_f32(&ukf->Y);
arm_mat_init_f32(&ukf->tmpY, UKF_MEASUREMENT_DIM, 1, ukf->tmpY_f32);
arm_mat_zero_f32(&ukf->tmpY);
//////////////////////////////////////////////////////////////////////////
}
void UKF_Init(UKF_Filter* ukf, float32_t *q, float32_t *gyro)
{
float32_t *X = ukf->X_f32;
float32_t norm;
X[0] = q[0];
X[1] = q[1];
X[2] = q[2];
X[3] = q[3];
norm = FastSqrtI(X[0] * X[0] + X[1] * X[1] + X[2] * X[2] + X[3] * X[3]);
X[0] *= norm;
X[1] *= norm;
X[2] *= norm;
X[3] *= norm;
X[4] = gyro[0];
X[5] = gyro[1];
X[6] = gyro[2];
}
void UKF_Update(UKF_Filter* ukf, float32_t *q, float32_t *gyro, float32_t *accel, float32_t *mag, float32_t dt)
{
int col, row;
float32_t norm;
#ifndef USE_4TH_RUNGE_KUTTA
float32_t halfdx, halfdy, halfdz;
float32_t halfdt = 0.5f * dt;
#endif
//////////////////////////////////////////////////////////////////////////
float32_t q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
//
float32_t hx, hy, hz;
float32_t bx, bz;
float32_t _2mx, _2my, _2mz;
//
float32_t *X = ukf->X_f32, *Y = ukf->Y_f32;
float32_t *tmpX = ukf->tmpX_f32, *tmpY = ukf->tmpY_f32;
float32_t tmpQ[4];
//////////////////////////////////////////////////////////////////////////
//calculate sigma points
UKF_GenerateSigmaPoints(ukf);
//time update
//unscented transformation of process
arm_mat_getcolumn_f32(&ukf->XSP, tmpX, 0);
#ifdef USE_4TH_RUNGE_KUTTA
tmpQ[0] = 0;
tmpQ[1] = tmpX[4];
tmpQ[2] = tmpX[5];
tmpQ[3] = tmpX[6];
Quaternion_RungeKutta4(tmpX, tmpQ, dt, 1);
#else
//
halfdx = halfdt * tmpX[4];
halfdy = halfdt * tmpX[5];
halfdz = halfdt * tmpX[6];
//
tmpQ[0] = tmpX[0];
tmpQ[1] = tmpX[1];
tmpQ[2] = tmpX[2];
tmpQ[3] = tmpX[3];
//model prediction
//simple way, pay attention!!!
//according to the actual gyroscope output
//and coordinate system definition
tmpX[0] = tmpQ[0] + (halfdx * tmpQ[1] + halfdy * tmpQ[2] + halfdz * tmpQ[3]);
tmpX[1] = tmpQ[1] - (halfdx * tmpQ[0] + halfdy * tmpQ[3] - halfdz * tmpQ[2]);
tmpX[2] = tmpQ[2] + (halfdx * tmpQ[3] - halfdy * tmpQ[0] - halfdz * tmpQ[1]);
tmpX[3] = tmpQ[3] - (halfdx * tmpQ[2] - halfdy * tmpQ[1] + halfdz * tmpQ[0]);
//////////////////////////////////////////////////////////////////////////
//Re-normalize Quaternion
norm = FastSqrtI(tmpX[0] * tmpX[0] + tmpX[1] * tmpX[1] + tmpX[2] * tmpX[2] + tmpX[3] * tmpX[3]);
tmpX[0] *= norm;
tmpX[1] *= norm;
tmpX[2] *= norm;
tmpX[3] *= norm;
#endif
//
arm_mat_setcolumn_f32(&ukf->XSP, tmpX, 0);
for(row = 0; row < UKF_STATE_DIM; row++){
X[row] = ukf->Wm0 * tmpX[row];
}
for(col = 1; col < UKF_SP_POINTS; col++){
//
arm_mat_getcolumn_f32(&ukf->XSP, tmpX, col);
//
#ifdef USE_4TH_RUNGE_KUTTA
tmpQ[0] = 0;
tmpQ[1] = tmpX[4];
tmpQ[2] = tmpX[5];
tmpQ[3] = tmpX[6];
Quaternion_RungeKutta4(tmpX, tmpQ, dt, 1);
#else
halfdx = halfdt * tmpX[4];
halfdy = halfdt * tmpX[5];
halfdz = halfdt * tmpX[6];
//
tmpQ[0] = tmpX[0];
tmpQ[1] = tmpX[1];
tmpQ[2] = tmpX[2];
tmpQ[3] = tmpX[3];
//model prediction
//simple way, pay attention!!!
//according to the actual gyroscope output
//and coordinate system definition
tmpX[0] = tmpQ[0] + (halfdx * tmpQ[1] + halfdy * tmpQ[2] + halfdz * tmpQ[3]);
tmpX[1] = tmpQ[1] - (halfdx * tmpQ[0] + halfdy * tmpQ[3] - halfdz * tmpQ[2]);
tmpX[2] = tmpQ[2] + (halfdx * tmpQ[3] - halfdy * tmpQ[0] - halfdz * tmpQ[1]);
tmpX[3] = tmpQ[3] - (halfdx * tmpQ[2] - halfdy * tmpQ[1] + halfdz * tmpQ[0]);
//////////////////////////////////////////////////////////////////////////
//re-normalize quaternion
norm = FastSqrtI(tmpX[0] * tmpX[0] + tmpX[1] * tmpX[1] + tmpX[2] * tmpX[2] + tmpX[3] * tmpX[3]);
tmpX[0] *= norm;
tmpX[1] *= norm;
tmpX[2] *= norm;
tmpX[3] *= norm;
#endif
//
arm_mat_setcolumn_f32(&ukf->XSP, tmpX, col);
for(row = 0; row < UKF_STATE_DIM; row++){
X[row] += ukf->Wmi * tmpX[row];
}
}
for(col = 0; col < UKF_SP_POINTS; col++){
arm_mat_getcolumn_f32(&ukf->XSP, tmpX, col);
arm_mat_sub_f32(&ukf->tmpX, &ukf->X, &ukf->tmpX);
arm_mat_setcolumn_f32(&ukf->tmpXSP, tmpX, col);
}
arm_mat_trans_f32(&ukf->tmpXSP, &ukf->tmpXSPT);
arm_mat_mult_f32(&ukf->tmpXSP, &ukf->Wc, &ukf->tmpWcXSP);
arm_mat_mult_f32(&ukf->tmpWcXSP, &ukf->tmpXSPT, &ukf->PX);
arm_mat_add_f32(&ukf->PX, &ukf->Q, &ukf->PX);
//////////////////////////////////////////////////////////////////////////
//recalculate sigma points
//UKF_GenerateSigmaPoints(ukf);
//measurement update
//unscented transformation of measurments
//////////////////////////////////////////////////////////////////////////
//Normalize accel and mag
norm = FastSqrtI(accel[0] * accel[0] + accel[1] * accel[1] + accel[2] * accel[2]);
accel[0] *= norm;
accel[1] *= norm;
accel[2] *= norm;
//////////////////////////////////////////////////////////////////////////
norm = FastSqrtI(mag[0] * mag[0] + mag[1] * mag[1] + mag[2] * mag[2]);
mag[0] *= norm;
mag[1] *= norm;
mag[2] *= norm;
_2mx = 2.0f * mag[0];
_2my = 2.0f * mag[1];
_2mz = 2.0f * mag[2];
arm_mat_getcolumn_f32(&ukf->XSP, tmpX, 0);
//Auxiliary variables to avoid repeated arithmetic
//
q0q0 = tmpX[0] * tmpX[0];
q0q1 = tmpX[0] * tmpX[1];
q0q2 = tmpX[0] * tmpX[2];
q0q3 = tmpX[0] * tmpX[3];
q1q1 = tmpX[1] * tmpX[1];
q1q2 = tmpX[1] * tmpX[2];
q1q3 = tmpX[1] * tmpX[3];
q2q2 = tmpX[2] * tmpX[2];
q2q3 = tmpX[2] * tmpX[3];
q3q3 = tmpX[3] * tmpX[3];
//Reference direction of Earth's magnetic field
hx = _2mx * (0.5f - q2q2 - q3q3) + _2my * (q1q2 - q0q3) + _2mz *(q1q3 + q0q2);
hy = _2mx * (q1q2 + q0q3) + _2my * (0.5f - q1q1 - q3q3) + _2mz * (q2q3 - q0q1);
hz = _2mx * (q1q3 - q0q2) + _2my * (q2q3 + q0q1) + _2mz *(0.5f - q1q1 - q2q2);
arm_sqrt_f32(hx * hx + hy * hy, &bx);
bz = hz;
tmpY[0] = tmpX[0];
tmpY[1] = tmpX[1];
tmpY[2] = tmpX[2];
tmpY[3] = tmpX[3];
tmpY[4] = 2.0f * (q1q3 - q0q2);
tmpY[5] = 2.0f * (q2q3 + q0q1);
tmpY[6] = -1.0f + 2.0f * (q0q0 + q3q3);
tmpY[7] = tmpX[4];
tmpY[8] = tmpX[5];
tmpY[9] = tmpX[6];
tmpY[10] = bx * (1.0f - 2.0f * (q2q2 + q3q3)) + bz * ( 2.0f * (q1q3 - q0q2));
tmpY[11] = bx * (2.0f * (q1q2 - q0q3)) + bz * (2.0f * (q2q3 + q0q1));
tmpY[12] = bx * (2.0f * (q1q3 + q0q2)) + bz * (1.0f - 2.0f * (q1q1 + q2q2));
arm_mat_setcolumn_f32(&ukf->YSP, tmpY, 0);
for(row = 0; row < UKF_MEASUREMENT_DIM; row++){
Y[row] = ukf->Wm0 * tmpY[row];
}
for(col = 1; col < UKF_SP_POINTS; col++){
arm_mat_getcolumn_f32(&ukf->XSP, tmpX, col);
//Auxiliary variables to avoid repeated arithmetic
//
q0q0 = tmpX[0] * tmpX[0];
q0q1 = tmpX[0] * tmpX[1];
q0q2 = tmpX[0] * tmpX[2];
q0q3 = tmpX[0] * tmpX[3];
q1q1 = tmpX[1] * tmpX[1];
q1q2 = tmpX[1] * tmpX[2];
q1q3 = tmpX[1] * tmpX[3];
q2q2 = tmpX[2] * tmpX[2];
q2q3 = tmpX[2] * tmpX[3];
q3q3 = tmpX[3] * tmpX[3];
//Reference direction of Earth's magnetic field
hx = _2mx * (0.5f - q2q2 - q3q3) + _2my * (q1q2 - q0q3) + _2mz *(q1q3 + q0q2);
hy = _2mx * (q1q2 + q0q3) + _2my * (0.5f - q1q1 - q3q3) + _2mz * (q2q3 - q0q1);
hz = _2mx * (q1q3 - q0q2) + _2my * (q2q3 + q0q1) + _2mz *(0.5f - q1q1 - q2q2);
arm_sqrt_f32(hx * hx + hy * hy, &bx);
bz = hz;
tmpY[0] = tmpX[0];
tmpY[1] = tmpX[1];
tmpY[2] = tmpX[2];
tmpY[3] = tmpX[3];
tmpY[4] = 2.0f * (q1q3 - q0q2);
tmpY[5] = 2.0f * (q2q3 + q0q1);
tmpY[6] = -1.0f + 2.0f * (q0q0 + q3q3);
tmpY[7] = tmpX[4];
tmpY[8] = tmpX[5];
tmpY[9] = tmpX[6];
tmpY[10] = bx * (1.0f - 2.0f * (q2q2 + q3q3)) + bz * ( 2.0f * (q1q3 - q0q2));
tmpY[11] = bx * (2.0f * (q1q2 - q0q3)) + bz * (2.0f * (q2q3 + q0q1));
tmpY[12] = bx * (2.0f * (q1q3 + q0q2)) + bz * (1.0f - 2.0f * (q1q1 + q2q2));
arm_mat_setcolumn_f32(&ukf->YSP, tmpY, col);
for(row = 0; row < UKF_MEASUREMENT_DIM; row++){
Y[row] += ukf->Wmi * tmpY[row];
}
}
for(col = 0; col < UKF_SP_POINTS; col++){
arm_mat_getcolumn_f32(&ukf->YSP, tmpY, col);
arm_mat_sub_f32(&ukf->tmpY, &ukf->Y, &ukf->tmpY);
arm_mat_setcolumn_f32(&ukf->tmpYSP, tmpY, col);
}
arm_mat_trans_f32(&ukf->tmpYSP, &ukf->tmpYSPT);
arm_mat_mult_f32(&ukf->tmpYSP, &ukf->Wc, &ukf->tmpWcYSP);
arm_mat_mult_f32(&ukf->tmpWcYSP, &ukf->tmpYSPT, &ukf->PY);
arm_mat_add_f32(&ukf->PY, &ukf->R, &ukf->PY);
//transformed cross-covariance
arm_mat_mult_f32(&ukf->tmpWcXSP, &ukf->tmpYSPT, &ukf->PXY);
//calculate kalman gate
//K = PXY * inv(PY);
arm_mat_inverse_f32(&ukf->PY, &ukf->tmpPY);
arm_mat_mult_f32(&ukf->PXY, &ukf->tmpPY, &ukf->K);
//state update
//X = X + K*(z - Y);
Y[0] = q[0] - Y[0];
Y[1] = q[1] - Y[1];
Y[2] = q[2] - Y[2];
Y[3] = q[3] - Y[3];
//
Y[4] = accel[0] - Y[4];
Y[5] = accel[1] - Y[5];
Y[6] = accel[2] - Y[6];
Y[7] = gyro[0] - Y[7];
Y[8] = gyro[1] - Y[8];
Y[9] = gyro[2] - Y[9];
//////////////////////////////////////////////////////////////////////////
Y[10] = mag[0] - Y[10];
Y[11] = mag[1] - Y[11];
Y[12] = mag[2] - Y[12];
arm_mat_mult_f32(&ukf->K, &ukf->Y, &ukf->tmpX);
arm_mat_add_f32(&ukf->X, &ukf->tmpX, &ukf->X);
//normalize quaternion
norm = FastSqrtI(X[0] * X[0] + X[1] * X[1] + X[2] * X[2] + X[3] * X[3]);
X[0] *= norm;
X[1] *= norm;
X[2] *= norm;
X[3] *= norm;
//covariance update
#if 0
//the default tuning parameters don't work properly
//so that use the simple update method below
//original update method
//P = PX - K * PY * K'
arm_mat_trans_f32(&ukf->K, &ukf->KT);
arm_mat_mult_f32(&ukf->K, &ukf->PY, &ukf->PXY);
arm_mat_mult_f32(&ukf->PXY, &ukf->KT, &ukf->P);
arm_mat_sub_f32(&ukf->PX, &ukf->P, &ukf->P);
#else
//must tune the P,Q,R
//simple update method
//P = PX - K * PXY'
arm_mat_trans_f32(&ukf->PXY, &ukf->PXYT);
arm_mat_mult_f32(&ukf->K, &ukf->PXYT, &ukf->P);
arm_mat_sub_f32(&ukf->PX, &ukf->P, &ukf->P);
#endif
}
void UKF_GetAngle(UKF_Filter* ukf, float32_t* rpy)
{
float32_t R[3][3];
float32_t *X = ukf->X_f32;
//Z-Y-X
R[0][0] = 2.0f * (X[0] * X[0] + X[1] * X[1]) - 1.0f;
R[0][1] = 2.0f * (X[1] * X[2] + X[0] * X[3]);
R[0][2] = 2.0f * (X[1] * X[3] - X[0] * X[2]);
//R[1][0] = 2.0f * (X[1] * X[2] - X[0] * X[3]);
//R[1][1] = 2.0f * (X[0] * X[0] + X[2] * X[2]) - 1.0f;
R[1][2] = 2.0f * (X[2] * X[3] + X[0] * X[1]);
//R[2][0] = 2.0f * (X[1] * X[3] + X[0] * X[2]);
//R[2][1] = 2.0f * (X[2] * X[3] - X[0] * X[1]);
R[2][2] = 2.0f * (X[0] * X[0] + X[3] * X[3]) - 1.0f;
//roll
rpy[0] = FastAtan2(R[1][2], R[2][2]);
if (rpy[0] == UKF_PI)
rpy[0] = -UKF_PI;
//pitch
if (R[0][2] >= 1.0f)
rpy[1] = -UKF_HALFPI;
else if (R[0][2] <= -1.0f)
rpy[1] = UKF_HALFPI;
else
rpy[1] = FastAsin(-R[0][2]);
//yaw
rpy[2] = FastAtan2(R[0][1], R[0][0]);
if (rpy[2] < 0.0f){
rpy[2] += UKF_TWOPI;
}
if (rpy[2] > UKF_TWOPI){
rpy[2] = 0.0f;
}
rpy[0] = UKF_TODEG(rpy[0]);
rpy[1] = UKF_TODEG(rpy[1]);
rpy[2] = UKF_TODEG(rpy[2]);
}
================================================
FILE: Application/inc/stm32f4_common.h
================================================
/*
The MIT License (MIT)
Copyright (c) 2015-? suhetao
Permission is hereby granted, free of charge, to any person obtaining a copy of
this software and associated documentation files (the "Software"), to deal in
the Software without restriction, including without limitation the rights to
use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
the Software, and to permit persons to whom the Software is furnished to do so,
subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
#ifndef _STM32F4_COMMON_H
#define _STM32F4_COMMON_H
#include "stm32f4xx.h"
#endif
================================================
FILE: Application/inc/stm32f4_crc.h
================================================
/*
The MIT License (MIT)
Copyright (c) 2015-? suhetao
Permission is hereby granted, free of charge, to any person obtaining a copy of
this software and associated documentation files (the "Software"), to deal in
the Software without restriction, including without limitation the rights to
use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
the Software, and to permit persons to whom the Software is furnished to do so,
subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
#ifndef __STM32F4_CRC_H
#define __STM32F4_CRC_H
// Includes
#include "stm32f4xx.h"
u16 CRC_Cal16(u8 *puchMsg, u16 usDataLen);
#endif
================================================
FILE: Application/inc/stm32f4_delay.h
================================================
/*
The MIT License (MIT)
Copyright (c) 2015-? suhetao
Permission is hereby granted, free of charge, to any person obtaining a copy of
this software and associated documentation files (the "Software"), to deal in
the Software without restriction, including without limitation the rights to
use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
the Software, and to permit persons to whom the Software is furnished to do so,
subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
#ifndef STM32F4_DELAY_H
#define STM32F4_DELAY_H
#include "stm32f4xx.h"
void Delay_Init(void);
void Delay_Ms(u32 ms);
void Delay_Us(u32 value);
int Get_Ms(unsigned long *count);
u32 Micros(void);
u32 Millis(void);
#endif
================================================
FILE: Application/inc/stm32f4_dmp.h
================================================
/*
The MIT License (MIT)
Copyright (c) 2015-? suhetao
Permission is hereby granted, free of charge, to any person obtaining a copy of
this software and associated documentation files (the "Software"), to deal in
the Software without restriction, including without limitation the rights to
use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
the Software, and to permit persons to whom the Software is furnished to do so,
subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
#define DEFAULT_MPU_HZ (200)
__inline unsigned short inv_row_2_scale(const signed char *row)
{
unsigned short b;
if (row[0] > 0)
b = 0;
else if (row[0] < 0)
b = 4;
else if (row[1] > 0)
b = 1;
else if (row[1] < 0)
b = 5;
else if (row[2] > 0)
b = 2;
else if (row[2] < 0)
b = 6;
else
b = 7; // error
return b;
}
static __inline unsigned short inv_orientation_matrix_to_scalar(const signed char *mtx){
unsigned short scalar;
/*
XYZ 010_001_000 Identity Matrix
XZY 001_010_000
YXZ 010_000_001
YZX 000_010_001
ZXY 001_000_010
ZYX 000_001_010
*/
scalar = inv_row_2_scale(mtx);
scalar |= inv_row_2_scale(mtx + 3) << 3;
scalar |= inv_row_2_scale(mtx + 6) << 6;
return scalar;
}
================================================
FILE: Application/inc/stm32f4_exti.h
================================================
/*
The MIT License (MIT)
Copyright (c) 2015-? suhetao
Permission is hereby granted, free of charge, to any person obtaining a copy of
this software and associated documentation files (the "Software"), to deal in
the Software without restriction, including without limitation the rights to
use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
the Software, and to permit persons to whom the Software is furnished to do so,
subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
#ifndef _STM32F4_EXTI_H
#define _STM32F4_EXTI_H
#include "stm32f4xx.h"
//Define the Interrupt pin
#define INTERRUPT_PIN GPIO_Pin_8
#define INTERRUPT_GPIO_PORT GPIOB
#define INTERRUPT_GPIO_CLK RCC_AHB1Periph_GPIOB
#define INTERRUPT_EXTI_LINE EXTI_Line8
#define INTERRUPT_EXTI_PORT_SOURCE EXTI_PortSourceGPIOB
#define INTERRUPT_EXTI_PIN_SOURCE GPIO_PinSource8
#define INTERRUPT_EDGE EXTI_Trigger_Rising
#define INTERRUPT_EXTI_IRQN EXTI9_5_IRQn
#define INTERRUPT_EXTI_PREEMPTION_PRIORITY 14
#define INTERRUPT_EXTI_SUB_PRIORITY 0
void Interrupt_Init(void);
u8 Interrupt_GetState(void);
#endif
================================================
FILE: Application/inc/stm32f4_gps.h
================================================
/*
The MIT License (MIT)
Copyright (c) 2015-? suhetao
Permission is hereby granted, free of charge, to any person obtaining a copy of
this software and associated documentation files (the "Software"), to deal in
the Software without restriction, including without limitation the rights to
use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
the Software, and to permit persons to whom the Software is furnished to do so,
subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
#ifndef _STM32F4_GPS_H
#define _STM32F4_GPS_H
#endif
================================================
FILE: Application/inc/stm32f4_mpu9250.h
================================================
/*
The MIT License (MIT)
Copyright (c) 2015-? suhetao
Permission is hereby granted, free of charge, to any person obtaining a copy of
this software and associated documentation files (the "Software"), to deal in
the Software without restriction, including without limitation the rights to
use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
the Software, and to permit persons to whom the Software is furnished to do so,
subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
#ifndef _STM32F4_MPU9250_H
#define _STM32F4_MPU9250_H
#include "stm32f4xx.h"
//////////////////////////////////////////////////////////////////////////
//Register Map for Gyroscope and Accelerometer
#define MPU9250_SELF_TEST_X_GYRO 0x00
#define MPU9250_SELF_TEST_Y_GYRO 0x01
#define MPU9250_SELF_TEST_Z_GYRO 0x02
#define MPU9250_SELF_TEST_X_ACCEL 0x0D
#define MPU9250_SELF_TEST_Y_ACCEL 0x0E
#define MPU9250_SELF_TEST_Z_ACCEL 0x0F
#define MPU9250_XG_OFFSET_H 0x13
#define MPU9250_XG_OFFSET_L 0x14
#define MPU9250_YG_OFFSET_H 0x15
#define MPU9250_YG_OFFSET_L 0x16
#define MPU9250_ZG_OFFSET_H 0x17
#define MPU9250_ZG_OFFSET_L 0x18
#define MPU9250_SMPLRT_DIV 0x19
#define MPU9250_CONFIG 0x1A
#define MPU9250_GYRO_CONFIG 0x1B
#define MPU9250_ACCEL_CONFIG 0x1C
#define MPU9250_ACCEL_CONFIG2 0x1D
#define MPU9250_LP_ACCEL_ODR 0x1E
#define MPU9250_WOM_THR 0x1F
#define MPU9250_FIFO_EN 0x23
#define MPU9250_I2C_MST_CTRL 0x24
#define MPU9250_I2C_SLV0_ADDR 0x25
#define MPU9250_I2C_SLV0_REG 0x26
#define MPU9250_I2C_SLV0_CTRL 0x27
#define MPU9250_I2C_SLV1_ADDR 0x28
#define MPU9250_I2C_SLV1_REG 0x29
#define MPU9250_I2C_SLV1_CTRL 0x2A
#define MPU9250_I2C_SLV2_ADDR 0x2B
#define MPU9250_I2C_SLV2_REG 0x2C
#define MPU9250_I2C_SLV2_CTRL 0x2D
#define MPU9250_I2C_SLV3_ADDR 0x2E
#define MPU9250_I2C_SLV3_REG 0x2F
#define MPU9250_I2C_SLV3_CTRL 0x30
#define MPU9250_I2C_SLV4_ADDR 0x31
#define MPU9250_I2C_SLV4_REG 0x32
#define MPU9250_I2C_SLV4_DO 0x33
#define MPU9250_I2C_SLV4_CTRL 0x34
#define MPU9250_I2C_SLV4_DI 0x35
#define MPU9250_I2C_MST_STATUS 0x36
#define MPU9250_INT_PIN_CFG 0x37
#define MPU9250_INT_ENABLE 0x38
#define MPU9250_INT_STATUS 0x3A
#define MPU9250_ACCEL_XOUT_H 0x3B
#define MPU9250_ACCEL_XOUT_L 0x3C
#define MPU9250_ACCEL_YOUT_H 0x3D
#define MPU9250_ACCEL_YOUT_L 0x3E
#define MPU9250_ACCEL_ZOUT_H 0x3F
#define MPU9250_ACCEL_ZOUT_L 0x40
#define MPU9250_TEMP_OUT_H 0x41
#define MPU9250_TEMP_OUT_L 0x42
#define MPU9250_GYRO_XOUT_H 0x43
#define MPU9250_GYRO_XOUT_L 0x44
#define MPU9250_GYRO_YOUT_H 0x45
#define MPU9250_GYRO_YOUT_L 0x46
#define MPU9250_GYRO_ZOUT_H 0x47
#define MPU9250_GYRO_ZOUT_L 0x48
#define MPU9250_EXT_SENS_DATA_00 0x49
#define MPU9250_EXT_SENS_DATA_01 0x4A
#define MPU9250_EXT_SENS_DATA_02 0x4B
#define MPU9250_EXT_SENS_DATA_03 0x4C
#define MPU9250_EXT_SENS_DATA_04 0x4D
#define MPU9250_EXT_SENS_DATA_05 0x4E
#define MPU9250_EXT_SENS_DATA_06 0x4F
#define MPU9250_EXT_SENS_DATA_07 0x50
#define MPU9250_EXT_SENS_DATA_08 0x51
#define MPU9250_EXT_SENS_DATA_09 0x52
#define MPU9250_EXT_SENS_DATA_10 0x53
#define MPU9250_EXT_SENS_DATA_11 0x54
#define MPU9250_EXT_SENS_DATA_12 0x55
#define MPU9250_EXT_SENS_DATA_13 0x56
#define MPU9250_EXT_SENS_DATA_14 0x57
#define MPU9250_EXT_SENS_DATA_15 0x58
#define MPU9250_EXT_SENS_DATA_16 0x59
#define MPU9250_EXT_SENS_DATA_17 0x5A
#define MPU9250_EXT_SENS_DATA_18 0x5B
#define MPU9250_EXT_SENS_DATA_19 0x5C
#define MPU9250_EXT_SENS_DATA_20 0x5D
#define MPU9250_EXT_SENS_DATA_21 0x5E
#define MPU9250_EXT_SENS_DATA_22 0x5F
#define MPU9250_EXT_SENS_DATA_23 0x60
#define MPU9250_I2C_SLV0_DO 0x63
#define MPU9250_I2C_SLV1_DO 0x64
#define MPU9250_I2C_SLV2_DO 0x65
#define MPU9250_I2C_SLV3_DO 0x66
#define MPU9250_I2C_MST_DELAY_CTRL 0x67
#define MPU9250_SIGNAL_PATH_RESET 0x68
#define MPU9250_MOT_DETECT_CTRL 0x69
#define MPU9250_USER_CTRL 0x6A
#define MPU9250_PWR_MGMT_1 0x6B
#define MPU9250_PWR_MGMT_2 0x6C
#define MPU9250_FIFO_COUNTH 0x72
#define MPU9250_FIFO_COUNTL 0x73
#define MPU9250_FIFO_R_W 0x74
#define MPU9250_WHO_AM_I 0x75
#define MPU9250_XA_OFFSET_H 0x77
#define MPU9250_XA_OFFSET_L 0x78
#define MPU9250_YA_OFFSET_H 0x7A
#define MPU9250_YA_OFFSET_L 0x7B
#define MPU9250_ZA_OFFSET_H 0x7D
#define MPU9250_ZA_OFFSET_L 0x7E
//
#define MPU9250_I2C_READ 0x80
//Magnetometer register maps
#define MPU9250_AK8963_WIA 0x00
#define MPU9250_AK8963_INFO 0x01
#define MPU9250_AK8963_ST1 0x02
#define MPU9250_AK8963_XOUT_L 0x03
#define MPU9250_AK8963_XOUT_H 0x04
#define MPU9250_AK8963_YOUT_L 0x05
#define MPU9250_AK8963_YOUT_H 0x06
#define MPU9250_AK8963_ZOUT_L 0x07
#define MPU9250_AK8963_ZOUT_H 0x08
#define MPU9250_AK8963_ST2 0x09
#define MPU9250_AK8963_CNTL 0x0A
#define MPU9250_AK8963_CNTL2 0x0B
#define MPU9250_AK8963_RSV 0x0B //DO NOT ACCESS <MPU9250_AK8963_CNTL2>
#define MPU9250_AK8963_ASTC 0x0C
#define MPU9250_AK8963_TS1 0x0D //DO NOT ACCESS
#define MPU9250_AK8963_TS2 0x0E //DO NOT ACCESS
#define MPU9250_AK8963_I2CDIS 0x0F
#define MPU9250_AK8963_ASAX 0x10
#define MPU9250_AK8963_ASAY 0x11
#define MPU9250_AK8963_ASAZ 0x12
#define MPU9250_AK8963_I2C_ADDR 0x0C
#define MPU9250_AK8963_POWER_DOWN 0x10
#define MPU9250_AK8963_FUSE_ROM_ACCESS 0x1F
#define MPU9250_AK8963_SINGLE_MEASUREMENT 0x11
#define MPU9250_AK8963_CONTINUOUS_MEASUREMENT 0x16 //MODE 2
#define MPU9250_AK8963_DATA_READY (0x01)
#define MPU9250_AK8963_DATA_OVERRUN (0x02)
#define MPU9250_AK8963_OVERFLOW (0x80)
#define MPU9250_AK8963_DATA_ERROR (0x40)
#define MPU9250_AK8963_CNTL2_SRST 0x01
//
#define MPU9250_I2C_SLV4_EN 0x80
#define MPU9250_I2C_SLV4_DONE 0x40
#define MPU9250_I2C_SLV4_NACK 0x10
//
#define MPU9250_I2C_IF_DIS (0x10)
#define MPU9250_I2C_MST_EN (0x20)
#define MPU9250_FIFO_RST (0x04)
#define MPU9250_FIFO_ENABLE (0x40)
//
#define MPU9250_RESET 0x80
#define MPU9250_CLOCK_MASK 0xF8
#define MPU9250_CLOCK_INTERNAL 0x00
#define MPU9250_CLOCK_PLL 0x01
#define MPU9250_CLOCK_PLLGYROZ 0x03
#define MPU9250_FS_SEL_MASK 0xE7
#define MPU9250_SLEEP_MASK 0x40
//
#define MPU9250_XYZ_GYRO 0xC7
#define MPU9250_XYZ_ACCEL 0xF8
//
#define MPU9250_RAW_RDY_EN (0x01)
#define MPU9250_RAW_DATA_RDY_INT (0x01)
#define MPU9250_FIFO_OVERFLOW (0x10)
//
#define MPU9250_INT_ANYRD_2CLEAR (0x10)
#define MPU9250_LATCH_INT_EN (0x20)
//
#define MPU9250_MAX_FIFO (1024)
#define MPU9250_FIFO_SIZE_1024 (0x40)
#define MPU9250_FIFO_SIZE_2048 (0x80)
#define MPU9250_FIFO_SIZE_4096 (0xC0)
#define MPU9250_TEMP_OUT (0x80)
#define MPU9250_GYRO_XOUT (0x40)
#define MPU9250_GYRO_YOUT (0x20)
#define MPU9250_GYRO_ZOUT (0x10)
#define MPU9250_ACCEL (0x08)
//
#define SMPLRT_DIV 0
#define MPU9250_SPIx_ADDR 0x00
//////////////////////////////////////////////////////////////////////////
//
enum MPU9250_GYRO_DLPF {
MPU9250_GYRO_DLPF_250HZ = 0,
MPU9250_GYRO_DLPF_184HZ,
MPU9250_GYRO_DLPF_92HZ,
MPU9250_GYRO_DLPF_41HZ,
MPU9250_GYRO_DLPF_20HZ,
MPU9250_GYRO_DLPF_10HZ,
MPU9250_GYRO_DLPF_5HZ,
MPU9250_GYRO_DLPF_3600HZ,
NUM_GYRO_DLPF
};
enum MPU9250_GYRO_FSR {
MPU9250_FSR_250DPS = 0,
MPU9250_FSR_500DPS,
MPU9250_FSR_1000DPS,
MPU9250_FSR_2000DPS,
MPU9250_NUM_GYRO_FSR
};
enum MPU9250_ACCEL_DLPF {
MPU9250_ACCEL_DLPF_460HZ = 0,
MPU9250_ACCEL_DLPF_184HZ,
MPU9250_ACCEL_DLPF_92HZ,
MPU9250_ACCEL_DLPF_41HZ,
MPU9250_ACCEL_DLPF_20HZ,
MPU9250_ACCEL_DLPF_10HZ,
MPU9250_ACCEL_DLPF_5HZ,
MPU9250_ACCEL_DLPF_460HZ2,
MPU9250_NUM_ACCEL_DLPF
};
enum MPU9250_ACCEL_FSR {
MPU9250_FSR_2G = 0,
MPU9250_FSR_4G,
MPU9250_FSR_8G,
MPU9250_FSR_16G,
MPU9250_NUM_ACCEL_FSR
};
enum MPU9250_CLK {
MPU9250_CLK_INTERNAL = 0,
MPU9250_CLK_PLL,
MPU9250_NUM_CLK
};
//////////////////////////////////////////////////////////////////////////
//low hardware functions
void MPU9250_Init(void);
u8 MPU9250_SPIx_SendByte(u8 data);
void MPU9250_SPIx_SetDivisor(u16 data);
//
u8 MPU9250_SPIx_Read(u8 addr, u8 reg_addr);
int MPU9250_SPIx_Reads(u8 addr, u8 reg_addr, u8 len, u8* data);
int MPU9250_SPIx_Write(u8 addr, u8 reg_addr, u8 data);
int MPU9250_SPIx_Writes(u8 addr, u8 reg_addr, u8 len, u8* data);
//global function use for eMPL
int MPU9250_AK8963_SPIx_Read(u8 akm_addr, u8 reg_addr, u8* data);
int MPU9250_AK8963_SPIx_Reads(u8 akm_addr, u8 reg_addr, u8 len, u8* data);
int MPU9250_AK8963_SPIx_Write(u8 akm_addr, u8 reg_addr, u8 data);
int MPU9250_AK8963_SPIx_Writes(u8 akm_addr, u8 reg_addr, u8 len, u8* data);
//data functions
void MPU9250_Get9AxisRawData(short *accel, short * gyro, short * mag);
void MPU9250_Get6AxisRawData(short* accel, short* gyro);
void MPU9250_Get3AxisAccelRawData(short * accel);
void MPU9250_Get3AxisGyroRawData(short* gyro);
void MPU9250_Get3AxisMagnetRawData(short *mag);
void MPU9250_GetTemperatureRawData(long *temperature);
int MPU9250_IsDataReady(void);
#endif
================================================
FILE: Application/inc/stm32f4_ms5611.h
================================================
/*
The MIT License (MIT)
Copyright (c) 2015-? suhetao
Permission is hereby granted, free of charge, to any person obtaining a copy of
this software and associated documentation files (the "Software"), to deal in
the Software without restriction, including without limitation the rights to
use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
the Software, and to permit persons to whom the Software is furnished to do so,
subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
#ifndef STM32F4_MS5611_H
#define STM32F4_MS5611_H
#include "stm32f4xx.h"
//
#define MS5611_RESET 0x1E
#define MS5611_READ_ADC 0x00
#define MS5611_READ 0x00
//OSR 256 0.60 mSec conversion time (1666.67 Hz)
//OSR 512 1.17 mSec conversion time ( 854.70 Hz)
//OSR 1024 2.28 mSec conversion time ( 357.14 Hz)
//OSR 2048 4.54 mSec conversion time ( 220.26 Hz)
//OSR 4096 9.04 mSec conversion time ( 110.62 Hz)
#define D1_OSR_256 0x40
#define D1_OSR_512 0x42
#define D1_OSR_1024 0x44
#define D1_OSR_2048 0x46
#define D1_OSR_4096 0x48
#define D2_OSR_256 0x50
#define D2_OSR_512 0x52
#define D2_OSR_1024 0x54
#define D2_OSR_2048 0x56
#define D2_OSR_4096 0x58
#define MS5611_READ_PROM_RSV 0xA0 //0xA0 to 0xAE
#define MS5611_READ_PROM_C1 0xA2
#define MS5611_READ_PROM_C2 0xA4
#define MS5611_READ_PROM_C3 0xA6
#define MS5611_READ_PROM_C4 0xA8
#define MS5611_READ_PROM_C5 0xAA
#define MS5611_READ_PROM_C6 0xAC
#define MS5611_READ_PROM_CRC 0xAE
u8 MS5611_SPIx_SendByte(u8);
void MS5611_Init(void);
void MS5611_GetTemperatureAndPressure(s32* TEMP, s32 *P);
#endif
================================================
FILE: Application/inc/stm32f4_rcc.h
================================================
/*
The MIT License (MIT)
Copyright (c) 2015-? suhetao
Permission is hereby granted, free of charge, to any person obtaining a copy of
this software and associated documentation files (the "Software"), to deal in
the Software without restriction, including without limitation the rights to
use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
the Software, and to permit persons to whom the Software is furnished to do so,
subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
#ifndef _STM32F4_RCC_H
#define _STM32F4_RCC_H
#include "stm32f4xx.h"
typedef struct PLL_PARAMS_T
{
uint32_t PLLM;
uint32_t PLLN;
uint32_t PLLP;
uint32_t PLLQ;
}
PLL_PARAMS;
typedef void (*RCC_AXXPeriphClockCmd)(uint32_t RCC_AXXPeriph, FunctionalState NewState);
void RCC_SystemCoreClockUpdate(PLL_PARAMS params);
#endif
================================================
FILE: Application/inc/stm32f4_serial.h
================================================
/*
The MIT License (MIT)
Copyright (c) 2015-? suhetao
Permission is hereby granted, free of charge, to any person obtaining a copy of
this software and associated documentation files (the "Software"), to deal in
the Software without restriction, including without limitation the rights to
use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
the Software, and to permit persons to whom the Software is furnished to do so,
subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
#ifndef _STM32F4_SERIAL_H
#define _STM32F4_SERIAL_H
#include "stm32f4xx.h"
#define PACKET_LENGTH (46)
//low function
void Serial_Init(void);
void Serial_SendByte(uint8_t byte);
void Serial_SendBytes(uint8_t* buffer, uint8_t length);
//
void Serial_Upload(short accel[3], short gyro[3], short compass[3], long quat[4], long temperature, long pressure);
#endif
================================================
FILE: Application/inc/stm32f4_string.h
================================================
#ifndef _STM32F4_STRING_H_
#define _STM32F4_STRING_H_
#include "stm32f4xx.h"
void FastMemCpy(uint8_t* dest, uint8_t* src, uint16_t size);
#endif
================================================
FILE: Application/inc/stm32f4_ublox.h
================================================
/*
The MIT License (MIT)
Copyright (c) 2015-? suhetao
Permission is hereby granted, free of charge, to any person obtaining a copy of
this software and associated documentation files (the "Software"), to deal in
the Software without restriction, including without limitation the rights to
use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
the Software, and to permit persons to whom the Software is furnished to do so,
subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
#ifndef _STM32F4_UBLOX_H
#define _STM32F4_UBLOX_H
#include "stm32f4xx.h"
#include "Nema.h"
#define UBLOX_DEFAULT_BAUDRATE (9600)
#define UBLOX_DEFAULT_TX_BUFFERSIZE (64)
#define UBLOX_DEFAULT_RX_BUFFERSIZE (512)
#define UBLOX_DEFAULT_PARSER_MAXSIZE (128)
typedef struct UBLOX_PARSERBUFF
{
s8 *Data;
u16 Size;
u16 Need;
u16 Left;
}Ublox_ParserBuff;
void Ublox_Init(void);
void Ublox_GetMessage(void);
void Ublox_GetPostion(double *x, double *y, double *z);
#endif
================================================
FILE: Application/inc/stm32f4xx_conf.h
================================================
/**
******************************************************************************
* @file stm32f4xx_conf.h
* @author MCD Application Team
* @version V1.0.0
* @date 19-September-2011
* @brief Library configuration file.
******************************************************************************
* @attention
*
* THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
* WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE
* TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY
* DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING
* FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE
* CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
*
* <h2><center>© COPYRIGHT 2011 STMicroelectronics</center></h2>
******************************************************************************
*/
/* Define to prevent recursive inclusion -------------------------------------*/
#ifndef __STM32F4xx_CONF_H
#define __STM32F4xx_CONF_H
#if defined (HSE_VALUE)
/* Redefine the HSE value; it's equal to 12 MHz on the STM32F4-DISCOVERY Kit */
#undef HSE_VALUE
#define HSE_VALUE ((uint32_t)12000000)
#endif /* HSE_VALUE */
/* Includes ------------------------------------------------------------------*/
/* Uncomment the line below to enable peripheral header file inclusion */
// #include "stm32f4xx_adc.h"
// #include "stm32f4xx_can.h"
//#include "stm32f4xx_crc.h"
// #include "stm32f4xx_cryp.h"
// #include "stm32f4xx_dac.h"
// #include "stm32f4xx_dbgmcu.h"
// #include "stm32f4xx_dcmi.h"
#include "stm32f4xx_dma.h"
#include "stm32f4xx_exti.h"
#include "stm32f4xx_flash.h"
// #include "stm32f4xx_fsmc.h"
// #include "stm32f4xx_hash.h"
#include "stm32f4xx_gpio.h"
//#include "stm32f4xx_i2c.h"
// #include "stm32f4xx_iwdg.h"
// #include "stm32f4xx_pwr.h"
#include "stm32f4xx_rcc.h"
// #include "stm32f4xx_rng.h"
// #include "stm32f4xx_rtc.h"
// #include "stm32f4xx_sdio.h"
#include "stm32f4xx_spi.h"
#include "stm32f4xx_syscfg.h"
//#include "stm32f4xx_tim.h"
#include "stm32f4xx_usart.h"
// #include "stm32f4xx_wwdg.h"
#include "misc.h" /* High level functions for NVIC and SysTick (add-on to CMSIS functions) */
/* Exported types ------------------------------------------------------------*/
/* Exported constants --------------------------------------------------------*/
/* If an external clock source is used, then the value of the following define
should be set to the value of the external clock source, else, if no external
clock is used, keep this define commented */
/*#define I2S_EXTERNAL_CLOCK_VAL 12288000 */ /* Value of the external clock in Hz */
/* Uncomment the line below to expanse the "assert_param" macro in the
Standard Peripheral Library drivers code */
/* #define USE_FULL_ASSERT 1 */
/* Exported macro ------------------------------------------------------------*/
#ifdef USE_FULL_ASSERT
/**
* @brief The assert_param macro is used for function's parameters check.
* @param expr: If expr is false, it calls assert_failed function
* which reports the name of the source file and the source
* line number of the call that failed.
* If expr is true, it returns no value.
* @retval None
*/
#define assert_param(expr) ((expr) ? (void)0 : assert_failed((uint8_t *)__FILE__, __LINE__))
/* Exported functions ------------------------------------------------------- */
void assert_failed(uint8_t* file, uint32_t line);
#else
#define assert_param(expr) ((void)0)
#endif /* USE_FULL_ASSERT */
#endif /* __STM32F4xx_CONF_H */
/******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/
================================================
FILE: Application/inc/stm32f4xx_it.h
================================================
/**
******************************************************************************
* @file USART/USART_TwoBoards/USART_DataExchangeDMA/stm32f4xx_it.h
* @author MCD Application Team
* @version V1.4.0
* @date 04-August-2014
* @brief This file contains the headers of the interrupt handlers.
******************************************************************************
* @attention
*
* <h2><center>© COPYRIGHT 2013 STMicroelectronics</center></h2>
*
* Licensed under MCD-ST Liberty SW License Agreement V2, (the "License");
* You may not use this file except in compliance with the License.
* You may obtain a copy of the License at:
*
* http://www.st.com/software_license_agreement_liberty_v2
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
******************************************************************************
*/
/* Define to prevent recursive inclusion -------------------------------------*/
#ifndef __STM32F4xx_IT_H
#define __STM32F4xx_IT_H
/* Includes ------------------------------------------------------------------*/
#include "stm32f4xx.h"
/* Exported types ------------------------------------------------------------*/
/* Exported constants --------------------------------------------------------*/
/* Exported macro ------------------------------------------------------------*/
/* Exported functions ------------------------------------------------------- */
void NMI_Handler(void);
void HardFault_Handler(void);
void MemManage_Handler(void);
void BusFault_Handler(void);
void UsageFault_Handler(void);
void SVC_Handler(void);
void DebugMon_Handler(void);
void PendSV_Handler(void);
void SysTick_Handler(void);
#endif /* __STM32F4xx_IT_H */
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
================================================
FILE: Application/src/main.c
================================================
/*
The MIT License (MIT)
Copyright (c) 2015-? suhetao
Permission is hereby granted, free of charge, to any person obtaining a copy of
this software and associated documentation files (the "Software"), to deal in
the Software without restriction, including without limitation the rights to
use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
the Software, and to permit persons to whom the Software is furnished to do so,
subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
//////////////////////////////////////////////////////////////////////////
//header
#include "stm32f4_delay.h"
#include "stm32f4_serial.h"
#include "stm32f4_rcc.h"
#include "stm32f4_mpu9250.h"
#include "stm32f4_ms5611.h"
#include "stm32f4_ublox.h"
//////////////////////////////////////////////////////////////////////////
//
#include "Quaternion.h"
//////////////////////////////////////////////////////////////////////////
//dmp
//#define USE_DMP
#include "stm32f4_dmp.h"
#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h"
//////////////////////////////////////////////////////////////////////////
//uncomment one
//#define USE_EKF
//#define USE_UKF
//#define USE_CKF
#define USE_SRCKF
//#define USE_9AXIS_EKF
//for doctor's mini Quadrotor
//#define USE_6AXIS_EKF
//#define USE_6AXIS_FP_EKF
//////////////////////////////////////////////////////////////////////////
#ifdef USE_EKF
#include "EKF.h"
#elif defined USE_UKF
#include "UKF.h"
#elif defined USE_CKF
#include "CKF.h"
#elif defined USE_SRCKF
#include "SRCKF.H"
#elif defined USE_6AXIS_EKF
#include "miniIMU.h"
#elif defined USE_6AXIS_FP_EKF
#include "FP_miniIMU.h"
#elif defined USE_9AXIS_EKF
#include "miniAHRS.h"
#endif
int main(void)
{
//PLL_M PLL_N PLL_P PLL_Q
//PLL_PARAMS pFreq120M = {12, 240, 2, 5};
PLL_PARAMS pFreq128M = {12, 256, 2, 6};
//PLL_PARAMS pFreq168M = {12, 336, 2, 7};
s8 gyro_orientation[9] = {
0, 1, 0,
1, 0, 0,
0, 0, 1
};
s32 s32Result;
struct int_param_s pInitParam = {0};
u8 u8AccelFsr = 0;
u16 u16GyroRate = 0;
u16 u16GyroFsr = 0;
//u16 u16DmpFeatures = DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO |
//DMP_FEATURE_GYRO_CAL;
u16 u16DmpFeatures = DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO;
s16 s16Gyro[3] = {0}, s16Accel[3] = {0}, s16Mag[3] = {0};
float fRealGyro[3] = {0}, fRealAccel[3] = {0};
#if !defined USE_6AXIS_EKF && !defined USE_6AXIS_FP_EKF
float fRealMag[3] = {0}, fRealQ[4] = {0};
#endif
s16 s16Sensors = 0;
u8 u8More = 0;
long lQuat[4] = {0};
unsigned long ulTimeStamp = 0;
float fRPY[3] = {0};
float fQ[4] = {0};
//position
double dX, dY, dZ;
//
s32 s32Temperature = 0, s32Pressure = 0;
float fAltNow = 0.0f, fAltLast = 0.0f;
//
#ifdef USE_EKF
EKF_Filter ekf;
#elif defined USE_UKF
UKF_Filter ukf;
#elif defined USE_CKF
CKF_Filter ckf;
#elif defined USE_SRCKF
SRCKF_Filter srckf;
#endif
unsigned long ulNowTime = 0;
unsigned long ulLastTime = 0;
unsigned long ulSendTime = 0;
unsigned long ulGPSUpdateTime = 0;
float fDeltaTime = 0.0f;
u32 u32KFState = 0;
//Reduced frequency
//128 / 4 = 32Mhz APB1, 32/32 = 1MHz SPI Clock
//1Mhz SPI Clock for read/write
RCC_SystemCoreClockUpdate(pFreq128M);
Delay_Init();
MPU9250_Init();
MS5611_Init();
Ublox_Init();
#ifdef USE_EKF
//Create a new EKF object;
EKF_New(&ekf);
#elif defined USE_UKF
//Create a new UKF object;
UKF_New(&ukf);
#elif defined USE_CKF
//Create a new CKF object;
CKF_New(&ckf);
#elif defined USE_SRCKF
SRCKF_New(&srckf);
#endif
#ifdef USE_DMP
//////////////////////////////////////////////////////////////////////////
//Init DMP
s32Result = mpu_init(&pInitParam);
s32Result = mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS);
s32Result = mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);
s32Result = mpu_set_sample_rate(DEFAULT_MPU_HZ);
s32Result = mpu_get_sample_rate(&u16GyroRate);
s32Result = mpu_get_gyro_fsr(&u16GyroFsr);
s32Result = mpu_get_accel_fsr(&u8AccelFsr);
s32Result = dmp_load_motion_driver_firmware();
//
s32Result = dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation));
//
s32Result = dmp_enable_feature(u16DmpFeatures);
s32Result = dmp_set_fifo_rate(DEFAULT_MPU_HZ);
s32Result = mpu_set_dmp_state(1);
#endif
//////////////////////////////////////////////////////////////////////////
//Recover frequency
//why DMP fifo must be reset when it overflows.
//SPI write operation occur, when you reset DMP fifo,
//but it can' write at 20Mhz SPI Clock? Fix me!
//RCC_SystemCoreClockUpdate(pFreq168M);
//Delay_Init();
//MPU9250_Init();
//42Mhz APB1, 42/2 = 21 MHz SPI Clock
//MPU9250_SPIx_SetDivisor(SPI_BaudRatePrescaler_2);
Serial_Init();
for(;;){
////
Get_Ms(&ulNowTime);
if (MPU9250_IsDataReady()){
#ifdef USE_DMP
s32Result = dmp_read_fifo(s16Gyro, s16Accel, lQuat, &ulTimeStamp, &s16Sensors, &u8More);
if(s32Result < 0){
continue;
}
//because 20Mhz SPI Clock satisfy MPU9250 with read condition
//so that you can't use I2C Master mode read/write from SPI at 20Mhz SPI Clock
//and dmp fifo can't use for Magnetometer, but you can use this function below
s32Result = mpu_get_compass_reg(s16Mag, &ulTimeStamp);
#else
MPU9250_Get9AxisRawData(s16Accel, s16Gyro, s16Mag);
#endif
//must calibrate gryo, accel, magnetic data
//ned coordinate system
//todo
fRealGyro[0] = DEGTORAD(s16Gyro[0]) * 0.06097560975609756097560975609756f;
fRealGyro[1] = DEGTORAD(s16Gyro[1]) * 0.06097560975609756097560975609756f;
fRealGyro[2] = DEGTORAD(s16Gyro[2]) * 0.06097560975609756097560975609756f;
fRealAccel[0] = s16Accel[0] / 16384.0f;
fRealAccel[1] = s16Accel[1] / 16384.0f;
fRealAccel[2] = s16Accel[2] / 16384.0f;
#if !defined USE_6AXIS_EKF && !defined USE_6AXIS_FP_EKF
if(!s32Result){
fRealMag[0] = s16Mag[0];
fRealMag[1] = s16Mag[1];
fRealMag[2] = s16Mag[2];
}
#ifdef USE_DMP
//q30 to float
fRealQ[0] = (float)lQuat[0] / 1073741824.0f;
fRealQ[1] = (float)lQuat[1] / 1073741824.0f;
fRealQ[2] = (float)lQuat[2] / 1073741824.0f;
fRealQ[3] = (float)lQuat[3] / 1073741824.0f;
#else
Quaternion_From6AxisData(fRealQ, fRealAccel, fRealMag);
#endif
#endif
if(!u32KFState){
if(s32Result < 0){
continue;
}
#ifdef USE_EKF
EKF_Init(&ekf, fRealQ, fRealGyro);
#elif defined USE_UKF
UKF_Init(&ukf, fRealQ, fRealGyro);
#elif defined USE_CKF
CKF_Init(&ckf, fRealQ, fRealGyro);
#elif defined USE_SRCKF
SRCKF_Init(&srckf, fRealAccel, fRealMag);
#elif defined USE_6AXIS_EKF
EKF_IMUInit(fRealAccel, fRealGyro);
#elif defined USE_6AXIS_FP_EKF
FP_EKF_IMUInit(fRealAccel, fRealGyro);
#elif defined USE_9AXIS_EKF
EKF_AHRSInit(fRealAccel, fRealMag);
#endif
ulLastTime = ulNowTime;
ulSendTime = ulNowTime;
u32KFState = 1;
}
else{
fDeltaTime = 0.001f * (float)(ulNowTime - ulLastTime);
#ifdef USE_EKF
EFK_Update(&ekf, fRealQ, fRealGyro, fRealAccel, fRealMag, fDeltaTime);
#elif defined USE_UKF
UKF_Update(&ukf, fRealQ, fRealGyro, fRealAccel, fRealMag, fDeltaTime);
#elif defined USE_CKF
CKF_Update(&ckf, fRealQ, fRealGyro, fRealAccel, fRealMag, fDeltaTime);
#elif defined USE_SRCKF
SRCKF_Update(&srckf, fRealGyro, fRealAccel, fRealMag, fDeltaTime);
#elif defined USE_6AXIS_EKF
EKF_IMUUpdate(fRealGyro, fRealAccel, fDeltaTime);
#elif defined USE_6AXIS_FP_EKF
FP_EKF_IMUUpdate(fRealGyro, fRealAccel, fDeltaTime);
#elif defined USE_9AXIS_EKF
EKF_AHRSUpdate(fRealGyro, fRealAccel, fRealMag, fDeltaTime);
#endif
}
#ifdef USE_EKF
EKF_GetAngle(&ekf, fRPY);
EKF_GetQ(&ekf, fQ);
#elif defined USE_UKF
UKF_GetAngle(&ukf, fRPY);
UKF_GetQ(&ukf, fQ);
#elif defined USE_CKF
CKF_GetAngle(&ckf, fRPY);
CKF_GetQ(&ckf, fQ);
#elif defined USE_SRCKF
SRCKF_GetAngle(&srckf, fRPY);
SRCKF_GetQ(&srckf, fQ);
#elif defined USE_6AXIS_EKF
EKF_IMUGetAngle(fRPY);
EKF_IMUGetQ(fQ);
#elif defined USE_6AXIS_FP_EKF
FP_EKF_IMUGetAngle(fRPY);
FP_EKF_IMUGetQ(fQ);
#elif defined USE_9AXIS_EKF
EKF_AHRSGetAngle(fRPY);
EKF_AHRSGetQ(fQ);
#endif
//transmit Quaternion float format to Q31
lQuat[0] = (long)(fQ[0] * 2147483648.0f);
lQuat[1] = (long)(fQ[1] * 2147483648.0f);
lQuat[2] = (long)(fQ[2] * 2147483648.0f);
lQuat[3] = (long)(fQ[3] * 2147483648.0f);
//todo
ulLastTime = ulNowTime;
}
//////////////////////////////////////////////////////////////////////////
//ublox default 250ms up
//1000 / 100 = 10HZ
if(ulNowTime - ulGPSUpdateTime > 99){
Ublox_GetMessage();
Ublox_GetPostion(&dX, &dY, &dZ);
ulGPSUpdateTime = ulNowTime;
}
//////////////////////////////////////////////////////////////////////////
//transmit the gyro, accel, mag, quat roll pitch yaw to anywhere
//1000 / 10 = 100HZ
if(ulNowTime - ulSendTime > 9){
MS5611_GetTemperatureAndPressure(&s32Temperature, &s32Pressure);
//fRealTemperature = (float)s32Temperature * 0.01f;
//////////////////////////////////////////////////////////////////////////
//simple Low pass filter, K = 0.125f
fAltLast = 44330.8f - 4946.54f * FastPow((float)s32Pressure, 0.1902632f);
fAltNow = fAltNow * 0.875f + fAltLast * 0.125f;
//////////////////////////////////////////////////////////////////////////
Serial_Upload(s16Accel, s16Gyro, s16Mag, lQuat, s32Temperature, s32Pressure);
ulSendTime = ulNowTime;
}
}
}
================================================
FILE: Application/src/stm32f4_delay.c
================================================
/*
The MIT License (MIT)
Copyright (c) 2015-? suhetao
Permission is hereby granted, free of charge, to any person obtaining a copy of
this software and associated documentation files (the "Software"), to deal in
the Software without restriction, including without limitation the rights to
use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
the Software, and to permit persons to whom the Software is furnished to do so,
subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
#include"stm32f4_delay.h"
//Cycles per microsecond
static __IO uint32_t us_ticks = 0;
static __IO uint32_t uptime_ticks = 0;
static __IO uint32_t cycle_ticks = 0;
void Delay_Init()
{
RCC_ClocksTypeDef RCC_Clocks;
RCC_GetClocksFreq(&RCC_Clocks);
us_ticks = RCC_Clocks.SYSCLK_Frequency / 1000000u;
//enable DWT access
CoreDebug->DEMCR &= ~CoreDebug_DEMCR_TRCENA_Msk;
CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;
//enable the CPU cycle counter
DWT->CTRL &= ~DWT_CTRL_CYCCNTENA_Msk;
DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk;
//Reset counter
DWT->CYCCNT = 0u;
if(SysTick_Config(RCC_Clocks.HCLK_Frequency / 1000u)){
while (1); // Handle Error
}
}
__inline uint32_t Millis(void)
{
return uptime_ticks;
}
u32 Micros(void)
{
register uint32_t old_cycle, cycle, timeMs;
do{
timeMs = __LDREXW(&uptime_ticks);
cycle = DWT->CYCCNT;
old_cycle = cycle_ticks;
}
while ( __STREXW( timeMs , &uptime_ticks ) );
return (timeMs * 1000) + (cycle - old_cycle) / us_ticks;
}
void Delay_Ms(u32 ms)
{
while (ms--){
Delay_Us(1000);
}
}
void Delay_Us(u32 us)
{
uint32_t elapsed = 0;
uint32_t elapsed_us = 0;
uint32_t lastCount = DWT->CYCCNT;
register uint32_t current_count = DWT->CYCCNT;
for (;;) {
current_count = DWT->CYCCNT;
elapsed += current_count - lastCount;
lastCount = current_count;
elapsed_us = elapsed / us_ticks;
if (elapsed_us >= us){
break;
}
us -= elapsed_us;
elapsed %= us_ticks;
}
}
int Get_Ms(unsigned long *count)
{
count[0] = uptime_ticks;
return 0;
}
void SysTick_Handler(void)
{
cycle_ticks = DWT->CYCCNT;
uptime_ticks++;
}
================================================
FILE: Application/src/stm32f4_exti.c
================================================
/*
The MIT License (MIT)
Copyright (c) 2015-? suhetao
Permission is hereby granted, free of charge, to any person obtaining a copy of
this software and associated documentation files (the "Software"), to deal in
the Software without restriction, including without limitation the rights to
use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
the Software, and to permit persons to whom the Software is furnished to do so,
subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
#include "stm32f4_exti.h"
#include "stm32f4_mpu9250.h"
static __IO u8 gu8InterruptState = 0;
void Interrupt_Init()
{
GPIO_InitTypeDef GPIO_InitStructure;
EXTI_InitTypeDef EXTI_InitStructure;
NVIC_InitTypeDef NVIC_InitStructure;
//Enable MPU9250int GPIO clocks
RCC_AHB1PeriphClockCmd(INTERRUPT_GPIO_CLK, ENABLE);
//Enable SYSCFG clock
RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE);
//Configure MPU9250int pin as input floating
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
GPIO_InitStructure.GPIO_Pin = INTERRUPT_PIN;
GPIO_Init(INTERRUPT_GPIO_PORT, &GPIO_InitStructure);
//Connect MPU9250int EXTI Line to MPU9250int GPIO Pin
SYSCFG_EXTILineConfig(INTERRUPT_EXTI_PORT_SOURCE, INTERRUPT_EXTI_PIN_SOURCE);
//Configure MPU9250int EXTI line
EXTI_InitStructure.EXTI_Line = INTERRUPT_EXTI_LINE;
EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt;
EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Rising;
EXTI_InitStructure.EXTI_LineCmd = ENABLE;
EXTI_Init(&EXTI_InitStructure);
//Enable and set MPU9250int EXTI Interrupt priority
NVIC_InitStructure.NVIC_IRQChannel = INTERRUPT_EXTI_IRQN;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = INTERRUPT_EXTI_PREEMPTION_PRIORITY;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = INTERRUPT_EXTI_SUB_PRIORITY;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
}
void EXTI9_5_IRQHandler(void)
{
gu8InterruptState = 1;
EXTI_ClearITPendingBit(EXTI_Line8);
}
u8 Interrupt_GetState(void){
u8 u8State = gu8InterruptState;
gu8InterruptState = 0;
return u8State;
}
================================================
FILE: Application/src/stm32f4_gps.c
================================================
/*
The MIT License (MIT)
Copyright (c) 2015-? suhetao
Permission is hereby granted, free of charge, to any person obtaining a copy of
this software and associated documentation files (the "Software"), to deal in
the Software without restriction, including without limitation the rights to
use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
the Software, and to permit persons to whom the Software is furnished to do so,
subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
#include "stm32f4_gps.h"
================================================
FILE: Application/src/stm32f4_mpu9250.c
================================================
/*
The MIT License (MIT)
Copyright (c) 2015-? suhetao
Permission is hereby granted, free of charge, to any person obtaining a copy of
this software and associated documentation files (the "Software"), to deal in
the Software without restriction, including without limitation the rights to
use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
the Software, and to permit persons to whom the Software is furnished to do so,
subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
#include "stm32f4_mpu9250.h"
#include "stm32f4_exti.h"
#include "stm32f4_spi.h"
#include "stm32f4_delay.h"
//////////////////////////////////////////////////////////////////////////
//
static s16 MPU9250_AK8963_ASA[3] = {0, 0, 0};
//////////////////////////////////////////////////////////////////////////
//basic SPI driver for MPU9250
static SPI_Driver mMPU9250 = {
SPI2, RCC_APB1PeriphClockCmd, RCC_APB1Periph_SPI2,
GPIOB, RCC_AHB1PeriphClockCmd, RCC_AHB1Periph_GPIOB,
GPIOB, RCC_AHB1PeriphClockCmd, RCC_AHB1Periph_GPIOB, GPIO_Pin_12,
GPIO_Pin_13, GPIO_Pin_14, GPIO_Pin_15,
GPIO_PinSource13, GPIO_PinSource14, GPIO_PinSource15,
#ifdef SPIx_USE_DMA
#endif
{
SPI_Direction_2Lines_FullDuplex, SPI_Mode_Master, SPI_DataSize_8b,
SPI_CPOL_High, SPI_CPHA_2Edge, SPI_NSS_Soft, SPI_BaudRatePrescaler_32,
SPI_FirstBit_MSB, 7
},
GPIO_AF_SPI2
};
static SPI_Driver* pMPU9250 = &mMPU9250;
//
static EXTI_Driver mMPU9250INT= {
GPIOB, RCC_AHB1PeriphClockCmd, RCC_AHB1Periph_GPIOB, GPIO_Pin_8,
EXTI_PortSourceGPIOB, EXTI_PinSource8,
{
EXTI_Line8, EXTI_Mode_Interrupt, EXTI_Trigger_Rising, ENABLE
},
{
EXTI9_5_IRQn, 14, 0, ENABLE
}
};
static EXTI_Driver* pMPU9250INT = &mMPU9250INT;
//////////////////////////////////////////////////////////////////////////
//
#define MPU9250_SPIx_SendByte(byte) SPIx_SendByte(pMPU9250, byte);
#define MPU9250_SPIx_SetDivisor(divisor) SPIx_SetDivisor(pMPU9250, divisor);
//////////////////////////////////////////////////////////////////////////
//init
void MPU9250_Init(void)
{
u8 data = 0, state = 0;
uint8_t response[3] = {0, 0, 0};
//Lower level hardware Init
SPIx_Init(pMPU9250);
EXTIx_Init(pMPU9250INT);
//////////////////////////////////////////////////////////////////////////
//MPU9250 Reset
MPU9250_SPIx_Write(MPU9250_SPIx_ADDR, MPU9250_PWR_MGMT_1, MPU9250_RESET);
Delay_Ms(100);
//MPU9250 Set Clock Source
MPU9250_SPIx_Write(MPU9250_SPIx_ADDR, MPU9250_PWR_MGMT_1, MPU9250_CLOCK_PLLGYROZ);
Delay_Ms(1);
//MPU9250 Set Interrupt
MPU9250_SPIx_Write(MPU9250_SPIx_ADDR, MPU9250_INT_PIN_CFG, MPU9250_INT_ANYRD_2CLEAR);
Delay_Ms(1);
MPU9250_SPIx_Write(MPU9250_SPIx_ADDR, MPU9250_INT_ENABLE, ENABLE);
Delay_Ms(1);
//MPU9250 Set Sensors
MPU9250_SPIx_Write(MPU9250_SPIx_ADDR, MPU9250_PWR_MGMT_2, MPU9250_XYZ_GYRO & MPU9250_XYZ_ACCEL);
Delay_Ms(1);
//MPU9250 Set SampleRate
//SAMPLE_RATE = Internal_Sample_Rate / (1 + SMPLRT_DIV)
MPU9250_SPIx_Write(MPU9250_SPIx_ADDR, MPU9250_SMPLRT_DIV, SMPLRT_DIV);
Delay_Ms(1);
//MPU9250 Set Full Scale Gyro Range
//Fchoice_b[1:0] = [00] enable DLPF
MPU9250_SPIx_Write(MPU9250_SPIx_ADDR, MPU9250_GYRO_CONFIG, (MPU9250_FSR_2000DPS << 3));
Delay_Ms(1);
//MPU9250 Set Full Scale Accel Range PS:2G
MPU9250_SPIx_Write(MPU9250_SPIx_ADDR, MPU9250_ACCEL_CONFIG, (MPU9250_FSR_2G << 3));
Delay_Ms(1);
//MPU9250 Set Accel DLPF
data = MPU9250_SPIx_Read(MPU9250_SPIx_ADDR, MPU9250_ACCEL_CONFIG2);
data |= MPU9250_ACCEL_DLPF_41HZ;
Delay_Ms(1);
MPU9250_SPIx_Write(MPU9250_SPIx_ADDR, MPU9250_ACCEL_CONFIG2, data);
Delay_Ms(1);
//MPU9250 Set Gyro DLPF
MPU9250_SPIx_Write(MPU9250_SPIx_ADDR, MPU9250_CONFIG, MPU9250_GYRO_DLPF_41HZ);
Delay_Ms(1);
//MPU9250 Set SPI Mode
state = MPU9250_SPIx_Read(MPU9250_SPIx_ADDR, MPU9250_USER_CTRL);
Delay_Ms(1);
MPU9250_SPIx_Write(MPU9250_SPIx_ADDR, MPU9250_USER_CTRL, state | MPU9250_I2C_IF_DIS);
Delay_Ms(1);
state = MPU9250_SPIx_Read(MPU9250_SPIx_ADDR, MPU9250_USER_CTRL);
Delay_Ms(1);
MPU9250_SPIx_Write(MPU9250_SPIx_ADDR, MPU9250_USER_CTRL, state | MPU9250_I2C_MST_EN);
Delay_Ms(1);
//////////////////////////////////////////////////////////////////////////
//AK8963 Setup
//reset AK8963
MPU9250_AK8963_SPIx_Write(MPU9250_AK8963_I2C_ADDR, MPU9250_AK8963_CNTL2, MPU9250_AK8963_CNTL2_SRST);
Delay_Ms(2);
MPU9250_AK8963_SPIx_Write(MPU9250_AK8963_I2C_ADDR, MPU9250_AK8963_CNTL, MPU9250_AK8963_POWER_DOWN);
Delay_Ms(1);
MPU9250_AK8963_SPIx_Write(MPU9250_AK8963_I2C_ADDR, MPU9250_AK8963_CNTL, MPU9250_AK8963_FUSE_ROM_ACCESS);
Delay_Ms(1);
//
//AK8963 get calibration data
MPU9250_AK8963_SPIx_Reads(MPU9250_AK8963_I2C_ADDR, MPU9250_AK8963_ASAX, 3, response);
//AK8963_SENSITIVITY_SCALE_FACTOR
//AK8963_ASA[i++] = (s16)((data - 128.0f) / 256.0f + 1.0f) ;
MPU9250_AK8963_ASA[0] = (s16)(response[0]) + 128;
MPU9250_AK8963_ASA[1] = (s16)(response[1]) + 128;
MPU9250_AK8963_ASA[2] = (s16)(response[2]) + 128;
Delay_Ms(1);
MPU9250_AK8963_SPIx_Write(MPU9250_AK8963_I2C_ADDR, MPU9250_AK8963_CNTL, MPU9250_AK8963_POWER_DOWN);
Delay_Ms(1);
//
MPU9250_SPIx_Write(MPU9250_SPIx_ADDR, MPU9250_I2C_MST_CTRL, 0x5D);
Delay_Ms(1);
MPU9250_SPIx_Write(MPU9250_SPIx_ADDR, MPU9250_I2C_SLV0_ADDR, MPU9250_AK8963_I2C_ADDR | MPU9250_I2C_READ);
Delay_Ms(1);
MPU9250_SPIx_Write(MPU9250_SPIx_ADDR, MPU9250_I2C_SLV0_REG, MPU9250_AK8963_ST1);
Delay_Ms(1);
MPU9250_SPIx_Write(MPU9250_SPIx_ADDR, MPU9250_I2C_SLV0_CTRL, 0x88);
Delay_Ms(1);
//
MPU9250_AK8963_SPIx_Write(MPU9250_AK8963_I2C_ADDR, MPU9250_AK8963_CNTL, MPU9250_AK8963_CONTINUOUS_MEASUREMENT);
Delay_Ms(1);
//
MPU9250_SPIx_Write(MPU9250_SPIx_ADDR, MPU9250_I2C_SLV4_CTRL, 0x09);
Delay_Ms(1);
//
MPU9250_SPIx_Write(MPU9250_SPIx_ADDR, MPU9250_I2C_MST_DELAY_CTRL, 0x81);
Delay_Ms(100);
}
int MPU9250_SPIx_Write(u8 addr, u8 reg_addr, u8 data){
Chip_Select(pMPU9250);
MPU9250_SPIx_SendByte(reg_addr);
MPU9250_SPIx_SendByte(data);
Chip_DeSelect(pMPU9250);
return 0;
}
int MPU9250_SPIx_Writes(u8 addr, u8 reg_addr, u8 len, u8* data){
u32 i = 0;
Chip_Select(pMPU9250);
MPU9250_SPIx_SendByte(reg_addr);
while(i < len){
MPU9250_SPIx_SendByte(data[i++]);
}
Chip_DeSelect(pMPU9250);
return 0;
}
u8 MPU9250_SPIx_Read(u8 addr, u8 reg_addr)
{
u8 dummy = 0;
u8 data = 0;
Chip_Select(pMPU9250);
MPU9250_SPIx_SendByte(0x80 | reg_addr);
data = MPU9250_SPIx_SendByte(dummy);
Chip_DeSelect(pMPU9250);
return data;
}
int MPU9250_SPIx_Reads(u8 addr, u8 reg_addr, u8 len, u8* data){
u32 i = 0;
u8 dummy = 0x00;
Chip_Select(pMPU9250);
MPU9250_SPIx_SendByte(MPU9250_I2C_READ | reg_addr);
while(i < len){
data[i++] = MPU9250_SPIx_SendByte(dummy);
}
Chip_DeSelect(pMPU9250);
return 0;
}
int MPU9250_AK8963_SPIx_Read(u8 akm_addr, u8 reg_addr, u8* data) {
u8 status = 0;
u32 timeout = 0;
MPU9250_SPIx_Writes(MPU9250_SPIx_ADDR, MPU9250_I2C_SLV4_REG, 1, ®_addr);
Delay_Ms(1);
reg_addr = akm_addr | MPU9250_I2C_READ;
MPU9250_SPIx_Writes(MPU9250_SPIx_ADDR, MPU9250_I2C_SLV4_ADDR, 1, ®_addr);
Delay_Ms(1);
reg_addr = MPU9250_I2C_SLV4_EN;
MPU9250_SPIx_Writes(MPU9250_SPIx_ADDR, MPU9250_I2C_SLV4_CTRL, 1, ®_addr);
Delay_Ms(1);
do {
if (timeout++ > 50){
return -2;
}
MPU9250_SPIx_Reads(MPU9250_SPIx_ADDR, MPU9250_I2C_MST_STATUS, 1, &status);
Delay_Ms(1);
} while ((status & MPU9250_I2C_SLV4_DONE) == 0);
MPU9250_SPIx_Reads(MPU9250_SPIx_ADDR, MPU9250_I2C_SLV4_DI, 1, data);
return 0;
}
int MPU9250_AK8963_SPIx_Reads(u8 akm_addr, u8 reg_addr, u8 len, u8* data){
u8 index = 0;
u8 status = 0;
u32 timeout = 0;
u8 tmp = 0;
tmp = akm_addr | MPU9250_I2C_READ;
MPU9250_SPIx_Writes(MPU9250_SPIx_ADDR, MPU9250_I2C_SLV4_ADDR, 1, &tmp);
Delay_Ms(1);
while(index < len){
tmp = reg_addr + index;
MPU9250_SPIx_Writes(MPU9250_SPIx_ADDR, MPU9250_I2C_SLV4_REG, 1, &tmp);
Delay_Ms(1);
tmp = MPU9250_I2C_SLV4_EN;
MPU9250_SPIx_Writes(MPU9250_SPIx_ADDR, MPU9250_I2C_SLV4_CTRL, 1, &tmp);
Delay_Ms(1);
do {
if (timeout++ > 50){
return -2;
}
MPU9250_SPIx_Reads(MPU9250_SPIx_ADDR, MPU9250_I2C_MST_STATUS, 1, &status);
Delay_Ms(2);
} while ((status & MPU9250_I2C_SLV4_DONE) == 0);
MPU9250_SPIx_Reads(MPU9250_SPIx_ADDR, MPU9250_I2C_SLV4_DI, 1, data + index);
Delay_Ms(1);
index++;
}
return 0;
}
int MPU9250_AK8963_SPIx_Write(u8 akm_addr, u8 reg_addr, u8 data)
{
u32 timeout = 0;
uint8_t status = 0;
u8 tmp = 0;
tmp = akm_addr;
MPU9250_SPIx_Writes(MPU9250_SPIx_ADDR, MPU9250_I2C_SLV4_ADDR, 1, &tmp);
Delay_Ms(1);
tmp = reg_addr;
MPU9250_SPIx_Writes(MPU9250_SPIx_ADDR, MPU9250_I2C_SLV4_REG, 1, &tmp);
Delay_Ms(1);
tmp = data;
MPU9250_SPIx_Writes(MPU9250_SPIx_ADDR, MPU9250_I2C_SLV4_DO, 1, &tmp);
Delay_Ms(1);
tmp = MPU9250_I2C_SLV4_EN;
MPU9250_SPIx_Writes(MPU9250_SPIx_ADDR, MPU9250_I2C_SLV4_CTRL, 1, &tmp);
Delay_Ms(1);
do {
if (timeout++ > 50)
return -2;
MPU9250_SPIx_Reads(MPU9250_SPIx_ADDR, MPU9250_I2C_MST_STATUS, 1, &status);
Delay_Ms(1);
} while ((status & MPU9250_I2C_SLV4_DONE) == 0);
if (status & MPU9250_I2C_SLV4_NACK)
return -3;
return 0;
}
int MPU9250_AK8963_SPIx_Writes(u8 akm_addr, u8 reg_addr, u8 len, u8* data)
{
u32 timeout = 0;
uint8_t status = 0;
u8 tmp = 0;
u8 index = 0;
tmp = akm_addr;
MPU9250_SPIx_Writes(MPU9250_SPIx_ADDR, MPU9250_I2C_SLV4_ADDR, 1, &tmp);
Delay_Ms(1);
while(index < len){
tmp = reg_addr + index;
MPU9250_SPIx_Writes(MPU9250_SPIx_ADDR, MPU9250_I2C_SLV4_REG, 1, &tmp);
Delay_Ms(1);
MPU9250_SPIx_Writes(MPU9250_SPIx_ADDR, MPU9250_I2C_SLV4_DO, 1, data + index);
Delay_Ms(1);
tmp = MPU9250_I2C_SLV4_EN;
MPU9250_SPIx_Writes(MPU9250_SPIx_ADDR, MPU9250_I2C_SLV4_CTRL, 1, &tmp);
Delay_Ms(1);
do {
if (timeout++ > 50)
return -2;
MPU9250_SPIx_Reads(MPU9250_SPIx_ADDR, MPU9250_I2C_MST_STATUS, 1, &status);
Delay_Ms(1);
} while ((status & MPU9250_I2C_SLV4_DONE) == 0);
if (status & MPU9250_I2C_SLV4_NACK)
return -3;
index++;
}
return 0;
}
//////////////////////////////////////////////////////////////////////////
//
void MPU9250_Get9AxisRawData(short *accel, short * gyro, short * mag)
{
u8 data[22];
MPU9250_SPIx_Reads(MPU9250_SPIx_ADDR, MPU9250_ACCEL_XOUT_H, 22, data);
accel[0] = (data[0] << 8) | data[1];
accel[1] = (data[2] << 8) | data[3];
accel[2] = (data[4] << 8) | data[5];
gyro[0] = (data[8] << 8) | data[9];
gyro[1] = (data[10] << 8) | data[11];
gyro[2] = (data[12] << 8) | data[13];
if (!(data[14] & MPU9250_AK8963_DATA_READY) || (data[14] & MPU9250_AK8963_DATA_OVERRUN)){
return;
}
if (data[21] & MPU9250_AK8963_OVERFLOW){
return;
}
mag[0] = (data[16] << 8) | data[15];
mag[1] = (data[18] << 8) | data[17];
mag[2] = (data[20] << 8) | data[19];
//ned x,y,z
mag[0] = ((long)mag[0] * MPU9250_AK8963_ASA[0]) >> 8;
mag[1] = ((long)mag[1] * MPU9250_AK8963_ASA[1]) >> 8;
mag[2] = ((long)mag[2] * MPU9250_AK8963_ASA[2]) >> 8;
}
//////////////////////////////////////////////////////////////////////////
//
void MPU9250_Get6AxisRawData(short *accel, short * gyro)
{
u8 data[14];
MPU9250_SPIx_Reads(MPU9250_SPIx_ADDR, MPU9250_ACCEL_XOUT_H, 14, data);
accel[0] = (data[0] << 8) | data[1];
accel[1] = (data[2] << 8) | data[3];
accel[2] = (data[4] << 8) | data[5];
gyro[0] = (data[8] << 8) | data[9];
gyro[1] = (data[10] << 8) | data[11];
gyro[2] = (data[12] << 8) | data[13];
}
//////////////////////////////////////////////////////////////////////////
//
void MPU9250_Get3AxisAccelRawData(short * accel)
{
u8 data[6];
MPU9250_SPIx_Reads(MPU9250_SPIx_ADDR, MPU9250_ACCEL_XOUT_H, 6, data);
accel[0] = (data[0] << 8) | data[1];
accel[1] = (data[2] << 8) | data[3];
accel[2] = (data[4] << 8) | data[5];
}
//////////////////////////////////////////////////////////////////////////
//
void MPU9250_Get3AxisGyroRawData(short * gyro)
{
u8 data[6];
MPU9250_SPIx_Reads(MPU9250_SPIx_ADDR, MPU9250_GYRO_XOUT_H, 6, data);
gyro[0] = (data[0] << 8) | data[1];
gyro[1] = (data[2] << 8) | data[3];
gyro[2] = (data[4] << 8) | data[5];
}
////////////////////////////////////////////////////////////////////////
gitextract_61j4xxir/
├── .gitignore
├── Algorithm/
│ ├── inc/
│ │ ├── CKF.h
│ │ ├── Control.h
│ │ ├── Double.h
│ │ ├── EKF.h
│ │ ├── INS_EKF.h
│ │ ├── PID.h
│ │ ├── Quaternion.h
│ │ ├── SRCKF.h
│ │ └── UKF.h
│ └── src/
│ ├── CKF.C
│ ├── Control.c
│ ├── EKF.c
│ ├── INS_EKF.c
│ ├── PID.c
│ ├── Quaternion.c
│ ├── SRCKF.c
│ └── UKF.c
├── Application/
│ ├── inc/
│ │ ├── stm32f4_common.h
│ │ ├── stm32f4_crc.h
│ │ ├── stm32f4_delay.h
│ │ ├── stm32f4_dmp.h
│ │ ├── stm32f4_exti.h
│ │ ├── stm32f4_gps.h
│ │ ├── stm32f4_mpu9250.h
│ │ ├── stm32f4_ms5611.h
│ │ ├── stm32f4_rcc.h
│ │ ├── stm32f4_serial.h
│ │ ├── stm32f4_string.h
│ │ ├── stm32f4_ublox.h
│ │ ├── stm32f4xx_conf.h
│ │ └── stm32f4xx_it.h
│ └── src/
│ ├── main.c
│ ├── stm32f4_delay.c
│ ├── stm32f4_exti.c
│ ├── stm32f4_gps.c
│ ├── stm32f4_mpu9250.c
│ ├── stm32f4_ms5611.c
│ ├── stm32f4_rcc.c
│ ├── stm32f4_serial.c
│ ├── stm32f4_string.c
│ ├── stm32f4_ublox.c
│ └── system_stm32f4xx.c
├── Common/
│ ├── inc/
│ │ └── Memory.h
│ └── src/
│ └── Memory.c
├── Data/
│ ├── inc/
│ │ ├── Fifo.h
│ │ └── Queue.h
│ └── src/
│ ├── Fifo.c
│ └── Queue.c
├── Drivers/
│ ├── inc/
│ │ ├── stm32f4_exti.h
│ │ ├── stm32f4_gpio.h
│ │ ├── stm32f4_spi.h
│ │ └── stm32f4_usart.h
│ └── src/
│ ├── stm32f4_exti.c
│ ├── stm32f4_gpio.c
│ ├── stm32f4_spi.c
│ └── stm32f4_usart.c
├── Gps/
│ ├── inc/
│ │ ├── Map.h
│ │ └── Nema.h
│ └── src/
│ ├── Map.c
│ └── Nema.c
├── LICENSE
├── Libraries/
│ ├── CMSIS/
│ │ ├── Device/
│ │ │ └── ST/
│ │ │ └── STM32F4xx/
│ │ │ ├── Include/
│ │ │ │ ├── stm32f4xx.h
│ │ │ │ └── system_stm32f4xx.h
│ │ │ ├── Release_Notes.html
│ │ │ └── Source/
│ │ │ └── Templates/
│ │ │ ├── TASKING/
│ │ │ │ └── cstart_thumb2.asm
│ │ │ ├── TrueSTUDIO/
│ │ │ │ ├── startup_stm32f401xx.s
│ │ │ │ ├── startup_stm32f40_41xxx.s
│ │ │ │ ├── startup_stm32f40xx.s
│ │ │ │ ├── startup_stm32f427_437xx.s
│ │ │ │ ├── startup_stm32f427xx.s
│ │ │ │ └── startup_stm32f429_439xx.s
│ │ │ ├── arm/
│ │ │ │ ├── startup_stm32f401xx.s
│ │ │ │ ├── startup_stm32f40_41xxx.s
│ │ │ │ ├── startup_stm32f40xx.s
│ │ │ │ ├── startup_stm32f427_437xx.s
│ │ │ │ ├── startup_stm32f427x.s
│ │ │ │ └── startup_stm32f429_439xx.s
│ │ │ ├── gcc_ride7/
│ │ │ │ ├── startup_stm32f401xx.s
│ │ │ │ ├── startup_stm32f40_41xxx.s
│ │ │ │ ├── startup_stm32f40xx.s
│ │ │ │ ├── startup_stm32f427_437xx.s
│ │ │ │ ├── startup_stm32f427x.s
│ │ │ │ └── startup_stm32f429_439xx.s
│ │ │ ├── iar/
│ │ │ │ ├── startup_stm32f401xx.s
│ │ │ │ ├── startup_stm32f40_41xxx.s
│ │ │ │ ├── startup_stm32f40xx.s
│ │ │ │ ├── startup_stm32f427_437xx.s
│ │ │ │ ├── startup_stm32f427x.s
│ │ │ │ └── startup_stm32f429_439xx.s
│ │ │ └── system_stm32f4xx.c
│ │ ├── Include/
│ │ │ ├── arm_common_tables.h
│ │ │ ├── arm_const_structs.h
│ │ │ ├── arm_math.h
│ │ │ ├── core_cm0.h
│ │ │ ├── core_cm0plus.h
│ │ │ ├── core_cm3.h
│ │ │ ├── core_cm4.h
│ │ │ ├── core_cm4_simd.h
│ │ │ ├── core_cmFunc.h
│ │ │ ├── core_cmInstr.h
│ │ │ ├── core_sc000.h
│ │ │ └── core_sc300.h
│ │ ├── README.txt
│ │ └── index.html
│ ├── STM32F4xx_StdPeriph_Driver/
│ │ ├── Release_Notes.html
│ │ ├── inc/
│ │ │ ├── misc.h
│ │ │ ├── stm32f4xx_adc.h
│ │ │ ├── stm32f4xx_can.h
│ │ │ ├── stm32f4xx_crc.h
│ │ │ ├── stm32f4xx_cryp.h
│ │ │ ├── stm32f4xx_dac.h
│ │ │ ├── stm32f4xx_dbgmcu.h
│ │ │ ├── stm32f4xx_dcmi.h
│ │ │ ├── stm32f4xx_dma.h
│ │ │ ├── stm32f4xx_dma2d.h
│ │ │ ├── stm32f4xx_exti.h
│ │ │ ├── stm32f4xx_flash.h
│ │ │ ├── stm32f4xx_fmc.h
│ │ │ ├── stm32f4xx_fsmc.h
│ │ │ ├── stm32f4xx_gpio.h
│ │ │ ├── stm32f4xx_hash.h
│ │ │ ├── stm32f4xx_i2c.h
│ │ │ ├── stm32f4xx_iwdg.h
│ │ │ ├── stm32f4xx_ltdc.h
│ │ │ ├── stm32f4xx_pwr.h
│ │ │ ├── stm32f4xx_rcc.h
│ │ │ ├── stm32f4xx_rng.h
│ │ │ ├── stm32f4xx_rtc.h
│ │ │ ├── stm32f4xx_sai.h
│ │ │ ├── stm32f4xx_sdio.h
│ │ │ ├── stm32f4xx_spi.h
│ │ │ ├── stm32f4xx_syscfg.h
│ │ │ ├── stm32f4xx_tim.h
│ │ │ ├── stm32f4xx_usart.h
│ │ │ └── stm32f4xx_wwdg.h
│ │ └── src/
│ │ ├── misc.c
│ │ ├── stm32f4xx_adc.c
│ │ ├── stm32f4xx_can.c
│ │ ├── stm32f4xx_crc.c
│ │ ├── stm32f4xx_cryp.c
│ │ ├── stm32f4xx_cryp_aes.c
│ │ ├── stm32f4xx_cryp_des.c
│ │ ├── stm32f4xx_cryp_tdes.c
│ │ ├── stm32f4xx_dac.c
│ │ ├── stm32f4xx_dbgmcu.c
│ │ ├── stm32f4xx_dcmi.c
│ │ ├── stm32f4xx_dma.c
│ │ ├── stm32f4xx_dma2d.c
│ │ ├── stm32f4xx_exti.c
│ │ ├── stm32f4xx_flash.c
│ │ ├── stm32f4xx_fmc.c
│ │ ├── stm32f4xx_fsmc.c
│ │ ├── stm32f4xx_gpio.c
│ │ ├── stm32f4xx_hash.c
│ │ ├── stm32f4xx_hash_md5.c
│ │ ├── stm32f4xx_hash_sha1.c
│ │ ├── stm32f4xx_i2c.c
│ │ ├── stm32f4xx_iwdg.c
│ │ ├── stm32f4xx_ltdc.c
│ │ ├── stm32f4xx_pwr.c
│ │ ├── stm32f4xx_rcc.c
│ │ ├── stm32f4xx_rng.c
│ │ ├── stm32f4xx_rtc.c
│ │ ├── stm32f4xx_sai.c
│ │ ├── stm32f4xx_sdio.c
│ │ ├── stm32f4xx_spi.c
│ │ ├── stm32f4xx_syscfg.c
│ │ ├── stm32f4xx_tim.c
│ │ ├── stm32f4xx_usart.c
│ │ └── stm32f4xx_wwdg.c
│ └── eMPL/
│ ├── dmpKey.h
│ ├── dmpmap.h
│ ├── inv_mpu.c
│ ├── inv_mpu.h
│ ├── inv_mpu_dmp_motion_driver.c
│ └── inv_mpu_dmp_motion_driver.h
├── Math/
│ ├── inc/
│ │ └── FastMath.h
│ └── src/
│ └── FastMath.c
├── Matrix/
│ ├── inc/
│ │ ├── DoubleMatrix.h
│ │ └── Matrix.h
│ └── src/
│ ├── DoubleMatrix.c
│ └── Matrix.c
├── Project/
│ ├── JLinkLog.txt
│ ├── JLinkSettings.ini
│ ├── stm32f4_dmp.uvopt
│ └── stm32f4_dmp.uvproj
├── README.md
├── miniAHRS/
│ ├── miniAHRS.c
│ └── miniAHRS.h
└── miniIMU/
├── FP/
│ ├── FP_Math.c
│ ├── FP_Math.h
│ ├── FP_Matrix.c
│ ├── FP_Matrix.h
│ ├── FP_miniIMU.c
│ └── FP_miniIMU.h
├── Usage.txt
├── miniIMU.c
├── miniIMU.h
├── miniMatrix.c
└── miniMatrix.h
SYMBOL INDEX (1619 symbols across 130 files)
FILE: Algorithm/inc/CKF.h
type CKF_Filter (line 41) | typedef struct CKF_FILTER_T{
function CKF_GetQ (line 114) | __inline void CKF_GetQ(CKF_Filter* ckf, float32_t* Q)
FILE: Algorithm/inc/Control.h
type QuadrotorParameter (line 36) | typedef struct CONTROLLERPARAMETER_T
type Trajectory (line 67) | typedef enum TRAJECTORY_T{
type Euler (line 75) | typedef enum EULER_T{
FILE: Algorithm/inc/Double.h
type Double (line 34) | typedef struct
function Double (line 41) | __inline Double intToDouble(int A)
function Double (line 51) | __inline Double floatToDouble(float A)
function Double (line 61) | __inline Double doubleToDouble(double A)
function DoubleTodouble (line 71) | __inline double DoubleTodouble(Double B)
function Double (line 82) | __inline Double DoubleAdd(Double A, Double B)
function Double (line 100) | __inline Double DoubleSub(Double A, Double B)
function Double (line 118) | __inline Double DoubleMul(Double A, Double B)
function Double (line 152) | __inline Double DoubleDiv(Double A, Double B)
FILE: Algorithm/inc/EKF.h
type EKF_Filter (line 39) | typedef struct EKF_FILTER_T{
function EKF_GetQ (line 97) | __inline void EKF_GetQ(EKF_Filter* efk, float32_t* Q)
FILE: Algorithm/inc/INS_EKF.h
type INS_EKF_Filter (line 43) | typedef struct INS_EKF_FILTER_T{
FILE: Algorithm/inc/PID.h
type PIDController (line 27) | typedef struct PIDCONTROLLER_T
function PID_Calculate (line 38) | __inline float PID_Calculate(PIDController *S, float in)
function PID_Init (line 53) | __inline void PID_Init(PIDController *S)
function PID_Reset (line 69) | __inline void PID_Reset(PIDController *S)
FILE: Algorithm/inc/Quaternion.h
function Quaternion_Add (line 29) | __inline void Quaternion_Add(float *r, float *a, float *b)
function Quaternion_Sub (line 37) | __inline void Quaternion_Sub(float *r, float *a, float *b)
function Quaternion_Multiply (line 45) | __inline void Quaternion_Multiply(float *r, float *a, float *b)
function Quaternion_Conjugate (line 53) | __inline void Quaternion_Conjugate(float *r, float *a)
function Quaternion_Scalar (line 61) | __inline void Quaternion_Scalar(float *r, float *q, float scalar)
FILE: Algorithm/inc/SRCKF.h
type SRCKF_Filter (line 41) | typedef struct SRCKF_FILTER_T{
function SRCKF_GetQ (line 133) | __inline void SRCKF_GetQ(SRCKF_Filter* srckf, float32_t* Q)
FILE: Algorithm/inc/UKF.h
type UKF_Filter (line 41) | typedef struct UKF_FILTER_T{
function UKF_GetQ (line 109) | __inline void UKF_GetQ(UKF_Filter* ukf, float32_t* Q)
FILE: Algorithm/src/CKF.C
function CKF_New (line 43) | void CKF_New(CKF_Filter* ckf)
function CKF_Init (line 134) | void CKF_Init(CKF_Filter* ckf, float32_t *q, float32_t *gyro)
function CKF_Update (line 157) | void CKF_Update(CKF_Filter* ckf, float32_t *q, float32_t *gyro, float32_...
function CKF_GetAngle (line 478) | void CKF_GetAngle(CKF_Filter* ckf, float32_t* rpy)
FILE: Algorithm/src/Control.c
function Matrix_Inv3x3 (line 27) | int Matrix_Inv3x3(float* A)
function Matrix_Inv (line 75) | void Matrix_Inv(float *A, int n)
function EulerConv (line 118) | void EulerConv(float* dt, float *deta)
function TorqueConv (line 156) | void TorqueConv(float *eta, float *deta, float *dt)
function ForceConv (line 190) | void ForceConv(float *eta, float dz, float *df)
function TorqueInv (line 215) | void TorqueInv(float *dt, float df, float *domega)
function QuadrotorControl (line 241) | void QuadrotorControl(float* task, float *q, float *u)
FILE: Algorithm/src/EKF.c
function EKF_New (line 43) | void EKF_New(EKF_Filter* ekf)
function EKF_Init (line 105) | void EKF_Init(EKF_Filter* ekf, float32_t *q, float32_t *gyro)
function EFK_Update (line 126) | void EFK_Update(EKF_Filter* ekf, float32_t *q, float32_t *gyro, float32_...
function EKF_GetAngle (line 380) | void EKF_GetAngle(EKF_Filter* ekf, float32_t* rpy)
FILE: Algorithm/src/INS_EKF.c
function INS_EKF_New (line 53) | void INS_EKF_New(INS_EKF_Filter* ins)
function INS_EKF_Init (line 127) | void INS_EKF_Init(INS_EKF_Filter* ins, float32_t *p, float32_t *v, float...
function INS_EFK_Update (line 174) | void INS_EFK_Update(INS_EKF_Filter* ins, float32_t *mag, float32_t *p, f...
function INS_EKF_GetAngle (line 364) | void INS_EKF_GetAngle(INS_EKF_Filter* ins, float32_t* rpy)
FILE: Algorithm/src/Quaternion.c
function Quaternion_Normalize (line 27) | void Quaternion_Normalize(float *q)
function Quaternion_FromEuler (line 36) | void Quaternion_FromEuler(float *q, float *rpy)
function Quaternion_ToEuler (line 54) | void Quaternion_ToEuler(float *q, float* rpy)
function Quaternion_FromRotationMatrix (line 92) | void Quaternion_FromRotationMatrix(float *R, float *Q)
function Quaternion_RungeKutta4 (line 183) | void Quaternion_RungeKutta4(float *q, float *w, float dt, int normalize)
function Quaternion_From6AxisData (line 222) | void Quaternion_From6AxisData(float* q, float *accel, float *mag)
FILE: Algorithm/src/SRCKF.c
function SRCKF_New (line 40) | void SRCKF_New(SRCKF_Filter* srckf)
function SRCKF_Init (line 126) | void SRCKF_Init(SRCKF_Filter* srckf, float32_t *accel, float32_t *mag)
function SRCKF_Update (line 160) | void SRCKF_Update(SRCKF_Filter* srckf, float32_t *gyro, float32_t *accel...
function SRCKF_GetAngle (line 310) | void SRCKF_GetAngle(SRCKF_Filter* srckf, float32_t* rpy)
FILE: Algorithm/src/UKF.c
function UKF_GenerateSigmaPoints (line 47) | static void UKF_GenerateSigmaPoints(UKF_Filter* ukf)
function UKF_New (line 74) | void UKF_New(UKF_Filter* ukf)
function UKF_Init (line 143) | void UKF_Init(UKF_Filter* ukf, float32_t *q, float32_t *gyro)
function UKF_Update (line 164) | void UKF_Update(UKF_Filter* ukf, float32_t *q, float32_t *gyro, float32_...
function UKF_GetAngle (line 438) | void UKF_GetAngle(UKF_Filter* ukf, float32_t* rpy)
FILE: Application/inc/stm32f4_dmp.h
function inv_row_2_scale (line 26) | __inline unsigned short inv_row_2_scale(const signed char *row)
function inv_orientation_matrix_to_scalar (line 46) | static __inline unsigned short inv_orientation_matrix_to_scalar(const si...
FILE: Application/inc/stm32f4_mpu9250.h
type MPU9250_GYRO_DLPF (line 218) | enum MPU9250_GYRO_DLPF {
type MPU9250_GYRO_FSR (line 230) | enum MPU9250_GYRO_FSR {
type MPU9250_ACCEL_DLPF (line 238) | enum MPU9250_ACCEL_DLPF {
type MPU9250_ACCEL_FSR (line 250) | enum MPU9250_ACCEL_FSR {
type MPU9250_CLK (line 258) | enum MPU9250_CLK {
FILE: Application/inc/stm32f4_rcc.h
type PLL_PARAMS (line 29) | typedef struct PLL_PARAMS_T
FILE: Application/inc/stm32f4_ublox.h
type Ublox_ParserBuff (line 35) | typedef struct UBLOX_PARSERBUFF
FILE: Application/src/main.c
function main (line 72) | int main(void)
FILE: Application/src/stm32f4_delay.c
function Delay_Init (line 31) | void Delay_Init()
function Millis (line 51) | __inline uint32_t Millis(void)
function u32 (line 56) | u32 Micros(void)
function Delay_Ms (line 69) | void Delay_Ms(u32 ms)
function Delay_Us (line 76) | void Delay_Us(u32 us)
function Get_Ms (line 97) | int Get_Ms(unsigned long *count)
function SysTick_Handler (line 103) | void SysTick_Handler(void)
FILE: Application/src/stm32f4_exti.c
function Interrupt_Init (line 29) | void Interrupt_Init()
function EXTI9_5_IRQHandler (line 64) | void EXTI9_5_IRQHandler(void)
function u8 (line 70) | u8 Interrupt_GetState(void){
FILE: Application/src/stm32f4_mpu9250.c
function MPU9250_Init (line 71) | void MPU9250_Init(void)
function MPU9250_SPIx_Write (line 164) | int MPU9250_SPIx_Write(u8 addr, u8 reg_addr, u8 data){
function MPU9250_SPIx_Writes (line 172) | int MPU9250_SPIx_Writes(u8 addr, u8 reg_addr, u8 len, u8* data){
function u8 (line 183) | u8 MPU9250_SPIx_Read(u8 addr, u8 reg_addr)
function MPU9250_SPIx_Reads (line 195) | int MPU9250_SPIx_Reads(u8 addr, u8 reg_addr, u8 len, u8* data){
function MPU9250_AK8963_SPIx_Read (line 208) | int MPU9250_AK8963_SPIx_Read(u8 akm_addr, u8 reg_addr, u8* data) {
function MPU9250_AK8963_SPIx_Reads (line 232) | int MPU9250_AK8963_SPIx_Reads(u8 akm_addr, u8 reg_addr, u8 len, u8* data){
function MPU9250_AK8963_SPIx_Write (line 263) | int MPU9250_AK8963_SPIx_Write(u8 akm_addr, u8 reg_addr, u8 data)
function MPU9250_AK8963_SPIx_Writes (line 294) | int MPU9250_AK8963_SPIx_Writes(u8 akm_addr, u8 reg_addr, u8 len, u8* data)
function MPU9250_Get9AxisRawData (line 329) | void MPU9250_Get9AxisRawData(short *accel, short * gyro, short * mag)
function MPU9250_Get6AxisRawData (line 359) | void MPU9250_Get6AxisRawData(short *accel, short * gyro)
function MPU9250_Get3AxisAccelRawData (line 374) | void MPU9250_Get3AxisAccelRawData(short * accel)
function MPU9250_Get3AxisGyroRawData (line 385) | void MPU9250_Get3AxisGyroRawData(short * gyro)
function MPU9250_Get3AxisMagnetRawData (line 396) | void MPU9250_Get3AxisMagnetRawData(short *mag)
function MPU9250_GetTemperatureRawData (line 417) | void MPU9250_GetTemperatureRawData(long *temperature)
function MPU9250_IsDataReady (line 426) | int MPU9250_IsDataReady(void)
function EXTI9_5_IRQHandler (line 435) | void EXTI9_5_IRQHandler(void)
FILE: Application/src/stm32f4_ms5611.c
function MS5611_Reset (line 50) | void MS5611_Reset(SPI_Driver *MS5611)
function u16 (line 60) | u16 MS5611_SPIx_ReadWord(SPI_Driver *MS5611, u8 addr)
function MS5611_SPIx_ReadADC (line 77) | void MS5611_SPIx_ReadADC(SPI_Driver *MS5611, u8 osr, u32* value)
function MS5611_ReadPROM (line 101) | void MS5611_ReadPROM(SPI_Driver *MS5611)
type u64 (line 122) | typedef uint64_t u64;
type s64 (line 123) | typedef int64_t s64;
function MS5611_GetTemperatureAndPressure (line 125) | void MS5611_GetTemperatureAndPressure(s32* T, s32 *P)
function MS5611_Cal (line 165) | void MS5611_Cal(s32* T, s32 *P)
function u8 (line 205) | u8 MS5611_CRC4(u32 n_prom[])
function MS5611_Init (line 236) | void MS5611_Init(void)
FILE: Application/src/stm32f4_rcc.c
function RCC_SystemCoreClockUpdate (line 26) | void RCC_SystemCoreClockUpdate(PLL_PARAMS params)
FILE: Application/src/stm32f4_serial.c
function Serial_Init (line 50) | void Serial_Init(void)
function Serial_SendByte (line 55) | void Serial_SendByte(uint8_t byte)
function Serial_SendBytes (line 60) | void Serial_SendBytes(uint8_t* buffer, uint8_t length)
function Serial_Upload (line 69) | void Serial_Upload(short accel[3], short gyro[3], short compass[3], long...
function USART1_IRQHandler (line 170) | void USART1_IRQHandler(void)
function DMA2_Stream7_IRQHandler (line 190) | void DMA2_Stream7_IRQHandler(void)
FILE: Application/src/stm32f4_string.c
function exit (line 56) | exit
FILE: Application/src/stm32f4_ublox.c
function Ublox_Init (line 73) | void Ublox_Init(void)
function Ublox_SendBytes (line 82) | void Ublox_SendBytes(uint8_t* buffer, uint8_t length)
function Ublox_ParserMessage (line 91) | void Ublox_ParserMessage()
function Ublox_GetMessage (line 107) | void Ublox_GetMessage()
function Ublox_GetPostion (line 137) | void Ublox_GetPostion(double *x, double *y, double *z)
function UART4_IRQHandler (line 146) | void UART4_IRQHandler(void)
function DMA1_Stream4_IRQHandler (line 169) | void DMA1_Stream4_IRQHandler(void)
FILE: Application/src/system_stm32f4xx.c
function SystemInit (line 214) | void SystemInit(void)
function SystemCoreClockUpdate (line 291) | void SystemCoreClockUpdate(void)
function SetSysClock (line 347) | static void SetSysClock(void)
function SystemInit_ExtMemCtl (line 434) | void SystemInit_ExtMemCtl(void)
FILE: Data/inc/Fifo.h
type Fifo (line 31) | typedef struct FIFO_T
FILE: Data/inc/Queue.h
type Buff (line 33) | typedef struct Buff_T
type Queue (line 39) | typedef struct QUEUE_T
function s32 (line 47) | __inline s32 Queue_IsFull(PQueue queue)
function u16 (line 52) | __inline u16 Queue_Size(PQueue queue)
function s32 (line 57) | __inline s32 Queue_IsEmpty(PQueue queue)
FILE: Data/src/Fifo.c
function Fifo_Init (line 27) | void Fifo_Init(Fifo* fifo, u8 *buff, u16 len)
function u16 (line 34) | u16 Fifo_Get(Fifo* fifo, u8 *buff, u16 len)
function Fifo_Put (line 45) | void Fifo_Put(Fifo *fifo, u8 *buff, u16 len)
FILE: Data/src/Queue.c
function s32 (line 27) | s32 Queue_Enqueue(PQueue queue, s8* string, u16 len)
function s32 (line 42) | s32 Queue_Dequeue(PQueue queue, Buff* buff)
FILE: Drivers/inc/stm32f4_exti.h
type EXTI_Driver (line 30) | typedef struct EXTI_DRIVER_T
FILE: Drivers/inc/stm32f4_gpio.h
type GPIO_Driver (line 32) | typedef struct GPIO_DRIVER_T
function GPIOx_SetLow (line 42) | __inline void GPIOx_SetLow(GPIO_Driver* GPIOx)
function GPIOx_SetHigh (line 47) | __inline void GPIOx_SetHigh(GPIO_Driver* GPIOx){
FILE: Drivers/inc/stm32f4_spi.h
type SPI_Driver (line 34) | typedef struct SPI_DRIVER_T
function Chip_Select (line 76) | __inline void Chip_Select(SPI_Driver* SPIx)
function Chip_DeSelect (line 81) | __inline void Chip_DeSelect(SPI_Driver* SPIx){
FILE: Drivers/inc/stm32f4_usart.h
type USART_Driver (line 38) | typedef struct USART_DRIVER_T
FILE: Drivers/src/stm32f4_exti.c
function EXTIx_Init (line 26) | void EXTIx_Init(EXTI_Driver* EXTIx)
FILE: Drivers/src/stm32f4_gpio.c
function GPIOx_Init (line 27) | void GPIOx_Init(GPIO_Driver* GPIOx)
FILE: Drivers/src/stm32f4_spi.c
function SPIx_Init (line 28) | void SPIx_Init(SPI_Driver* SPIx)
function SPIx_DeInit (line 130) | void SPIx_DeInit(SPI_Driver* SPIx)
function SPIx_Read_Reg (line 153) | uint8_t SPIx_Read_Reg(SPI_Driver* SPIx, uint8_t reg)
function SPIx_Write_Reg (line 172) | void SPIx_Write_Reg(SPI_Driver* SPIx, uint8_t regAddr, uint8_t data) {
function SPIx_Read_Regs (line 186) | void SPIx_Read_Regs(SPI_Driver* SPIx, uint8_t regAddr, uint8_t length, u...
function SPIx_DMA_Read_Regs (line 207) | void SPIx_DMA_Read_Regs(SPI_Driver* SPIx, uint8_t regAddr, uint8_t lengt...
function SPIx_SendByte (line 234) | uint8_t SPIx_SendByte(SPI_Driver* SPIx, uint8_t byte)
function SPIx_SendWord (line 246) | uint16_t SPIx_SendWord(SPI_Driver* SPIx, uint16_t word)
function SPIx_ReadBytes (line 258) | void SPIx_ReadBytes(SPI_Driver* SPIx,uint8_t length, uint8_t* buffer)
function SPIx_SetDivisor (line 274) | void SPIx_SetDivisor(SPI_Driver* SPIx, uint16_t Prescaler)
FILE: Drivers/src/stm32f4_usart.c
function USARTx_Init (line 27) | void USARTx_Init(USART_Driver* USARTx)
function USARTx_DeInit (line 126) | void USARTx_DeInit(USART_Driver* USARTx)
function USARTx_SendByte (line 149) | void USARTx_SendByte(USART_Driver* USARTx, uint8_t byte)
function USARTx_SendBytes (line 156) | void USARTx_SendBytes(USART_Driver* USARTx, uint8_t* buffer, uint8_t len...
function USARTx_DMA_SendBytes (line 167) | void USARTx_DMA_SendBytes(USART_Driver* USARTx, uint8_t* buffer, uint8_t...
FILE: Gps/inc/Map.h
type DoublePoint (line 31) | typedef struct DOUBLEPOINT_T
type Map (line 37) | typedef struct MAP_T
FILE: Gps/inc/Nema.h
type NEMA_MessageType (line 34) | typedef enum{
type nmeaGPGGA (line 43) | typedef struct _nmeaGPGGA
type nmeaGPGSA (line 62) | typedef struct _nmeaGPGSA
type nmeaGPRMC (line 74) | typedef struct _nmeaGPRMC
type nmeaGPVTG (line 92) | typedef struct _nmeaGPVTG
type nmeaINFO (line 105) | typedef struct _nmeaINFO
type u64 (line 114) | typedef uint64_t u64;
function NEMA_Fast_UintToFloat (line 118) | __inline float NEMA_Fast_UintToFloat(u32 x)
function NEMA_Fast_FloatInverse (line 125) | __inline float NEMA_Fast_FloatInverse(float x)
function NEMA_Fast_Uint64ToDouble (line 134) | __inline double NEMA_Fast_Uint64ToDouble(u64 x)
function NEMA_Fast_DoubleInverse (line 141) | __inline double NEMA_Fast_DoubleInverse(double x)
function u8 (line 149) | __inline u8 NEMA_FastCRCtoI(s8 *p, u8 *table){
FILE: Gps/src/Map.c
function DoublePoint (line 29) | DoublePoint Map_BLToGauss(Double latitude, Double longitude)
function Map_Init (line 98) | void Map_Init(Map* pMap, double _lat, double _lon, double lat, double lon,
function Map_GetXY (line 120) | void Map_GetXY(Map* pMap, double lat, double lon, double *x, double *y)
FILE: Gps/src/Nema.c
function NEMA_FastAtoF (line 53) | float NEMA_FastAtoF(s8 *p, s32 len)
function NEMA_FastAtoD (line 100) | double NEMA_FastAtoD(s8 *p, s32 len)
function s32 (line 140) | s32 NEMA_FastAtoI(s8 *p, s32 len)
function s16 (line 169) | s16 NEMA_GetMessage(void *data, s32* iType)
function u16 (line 317) | u16 NEMA_Parser(s8 *p, u16 len)
function NMEA_Convert2Degrees (line 378) | double NMEA_Convert2Degrees(double val)
function NMEA_Degree2Radian (line 395) | double NMEA_Degree2Radian(double val)
function Double (line 403) | Double NMEA_Degree2RadianD(Double val)
function NMEA_Radian2Degree (line 408) | double NMEA_Radian2Degree(double val)
FILE: Libraries/CMSIS/Device/ST/STM32F4xx/Include/stm32f4xx.h
type IRQn_Type (line 171) | typedef enum IRQn
type s32 (line 471) | typedef int32_t s32;
type s16 (line 472) | typedef int16_t s16;
type s8 (line 473) | typedef int8_t s8;
type sc32 (line 475) | typedef const int32_t sc32;
type sc16 (line 476) | typedef const int16_t sc16;
type sc8 (line 477) | typedef const int8_t sc8;
type __IO (line 479) | typedef __IO int32_t vs32;
type __IO (line 480) | typedef __IO int16_t vs16;
type __IO (line 481) | typedef __IO int8_t vs8;
type __I (line 483) | typedef __I int32_t vsc32;
type __I (line 484) | typedef __I int16_t vsc16;
type __I (line 485) | typedef __I int8_t vsc8;
type u32 (line 487) | typedef uint32_t u32;
type u16 (line 488) | typedef uint16_t u16;
type u8 (line 489) | typedef uint8_t u8;
type uc32 (line 491) | typedef const uint32_t uc32;
type uc16 (line 492) | typedef const uint16_t uc16;
type uc8 (line 493) | typedef const uint8_t uc8;
type __IO (line 495) | typedef __IO uint32_t vu32;
type __IO (line 496) | typedef __IO uint16_t vu16;
type __IO (line 497) | typedef __IO uint8_t vu8;
type __I (line 499) | typedef __I uint32_t vuc32;
type __I (line 500) | typedef __I uint16_t vuc16;
type __I (line 501) | typedef __I uint8_t vuc8;
type FlagStatus (line 503) | typedef enum {RESET = 0, SET = !RESET} FlagStatus, ITStatus;
type FunctionalState (line 505) | typedef enum {DISABLE = 0, ENABLE = !DISABLE} FunctionalState;
type ErrorStatus (line 508) | typedef enum {ERROR = 0, SUCCESS = !ERROR} ErrorStatus;
type ADC_TypeDef (line 522) | typedef struct
type ADC_Common_TypeDef (line 546) | typedef struct
type CAN_TxMailBox_TypeDef (line 559) | typedef struct
type CAN_FIFOMailBox_TypeDef (line 571) | typedef struct
type CAN_FilterRegister_TypeDef (line 583) | typedef struct
type CAN_TypeDef (line 593) | typedef struct
type CRC_TypeDef (line 623) | typedef struct
type DAC_TypeDef (line 636) | typedef struct
type DBGMCU_TypeDef (line 658) | typedef struct
type DCMI_TypeDef (line 670) | typedef struct
type DMA_Stream_TypeDef (line 689) | typedef struct
type DMA_TypeDef (line 699) | typedef struct
type DMA2D_TypeDef (line 711) | typedef struct
type ETH_TypeDef (line 742) | typedef struct
type EXTI_TypeDef (line 816) | typedef struct
type FLASH_TypeDef (line 830) | typedef struct
type FSMC_Bank1_TypeDef (line 846) | typedef struct
type FSMC_Bank1E_TypeDef (line 855) | typedef struct
type FSMC_Bank2_TypeDef (line 864) | typedef struct
type FSMC_Bank3_TypeDef (line 878) | typedef struct
type FSMC_Bank4_TypeDef (line 892) | typedef struct
type FMC_Bank1_TypeDef (line 907) | typedef struct
type FMC_Bank1E_TypeDef (line 916) | typedef struct
type FMC_Bank2_TypeDef (line 925) | typedef struct
type FMC_Bank3_TypeDef (line 939) | typedef struct
type FMC_Bank4_TypeDef (line 953) | typedef struct
type FMC_Bank5_6_TypeDef (line 966) | typedef struct
type GPIO_TypeDef (line 980) | typedef struct
type SYSCFG_TypeDef (line 998) | typedef struct
type I2C_TypeDef (line 1011) | typedef struct
type IWDG_TypeDef (line 1039) | typedef struct
type LTDC_TypeDef (line 1051) | typedef struct
type LTDC_Layer_TypeDef (line 1076) | typedef struct
type PWR_TypeDef (line 1099) | typedef struct
type RCC_TypeDef (line 1109) | typedef struct
type RTC_TypeDef (line 1150) | typedef struct
type SAI_TypeDef (line 1199) | typedef struct
type SAI_Block_TypeDef (line 1204) | typedef struct
type SDIO_TypeDef (line 1220) | typedef struct
type SPI_TypeDef (line 1248) | typedef struct
type TIM_TypeDef (line 1274) | typedef struct
type USART_TypeDef (line 1318) | typedef struct
type WWDG_TypeDef (line 1340) | typedef struct
type CRYP_TypeDef (line 1351) | typedef struct
type HASH_TypeDef (line 1395) | typedef struct
type HASH_DIGEST_TypeDef (line 1411) | typedef struct
type RNG_TypeDef (line 1420) | typedef struct
FILE: Libraries/CMSIS/Device/ST/STM32F4xx/Source/Templates/system_stm32f4xx.c
function SystemInit (line 337) | void SystemInit(void)
function SystemCoreClockUpdate (line 414) | void SystemCoreClockUpdate(void)
function SetSysClock (line 470) | static void SetSysClock(void)
function SystemInit_ExtMemCtl (line 588) | void SystemInit_ExtMemCtl(void)
function SystemInit_ExtMemCtl (line 745) | void SystemInit_ExtMemCtl(void)
FILE: Libraries/CMSIS/Include/arm_math.h
type arm_status (line 336) | typedef enum
type q7_t (line 350) | typedef int8_t q7_t;
type q15_t (line 355) | typedef int16_t q15_t;
type q31_t (line 360) | typedef int32_t q31_t;
type q63_t (line 365) | typedef int64_t q63_t;
type float32_t (line 370) | typedef float float32_t;
type float64_t (line 375) | typedef double float64_t;
function __INLINE (line 434) | static __INLINE q31_t clip_q63_to_q31(
function __INLINE (line 444) | static __INLINE q15_t clip_q63_to_q15(
function __INLINE (line 454) | static __INLINE q7_t clip_q31_to_q7(
function __INLINE (line 464) | static __INLINE q15_t clip_q31_to_q15(
function __INLINE (line 475) | static __INLINE q63_t mult32x64(
function __INLINE (line 494) | static __INLINE uint32_t __CLZ(
function __INLINE (line 516) | static __INLINE uint32_t arm_recip_q31(
function __INLINE (line 567) | static __INLINE uint32_t arm_recip_q15(
function __INLINE (line 620) | static __INLINE q31_t __SSAT(
function __INLINE (line 668) | static __INLINE q31_t __QADD8(
function __INLINE (line 695) | static __INLINE q31_t __QSUB8(
function __INLINE (line 725) | static __INLINE q31_t __QADD16(
function __INLINE (line 748) | static __INLINE q31_t __SHADD16(
function __INLINE (line 771) | static __INLINE q31_t __QSUB16(
function __INLINE (line 793) | static __INLINE q31_t __SHSUB16(
function __INLINE (line 815) | static __INLINE q31_t __QASX(
function __INLINE (line 833) | static __INLINE q31_t __SHASX(
function __INLINE (line 856) | static __INLINE q31_t __QSAX(
function __INLINE (line 874) | static __INLINE q31_t __SHSAX(
function __INLINE (line 896) | static __INLINE q31_t __SMUSDX(
function __INLINE (line 908) | static __INLINE q31_t __SMUADX(
function __INLINE (line 920) | static __INLINE q31_t __QADD(
function __INLINE (line 930) | static __INLINE q31_t __QSUB(
function __INLINE (line 940) | static __INLINE q31_t __SMLAD(
function __INLINE (line 953) | static __INLINE q31_t __SMLADX(
function __INLINE (line 966) | static __INLINE q31_t __SMLSDX(
function __INLINE (line 979) | static __INLINE q63_t __SMLALD(
function __INLINE (line 992) | static __INLINE q63_t __SMLALDX(
function __INLINE (line 1005) | static __INLINE q31_t __SMUAD(
function __INLINE (line 1017) | static __INLINE q31_t __SMUSD(
function __INLINE (line 1030) | static __INLINE q31_t __SXTB16(
type arm_fir_instance_q7 (line 1045) | typedef struct
type arm_fir_instance_q15 (line 1055) | typedef struct
type arm_fir_instance_q31 (line 1065) | typedef struct
type arm_fir_instance_f32 (line 1075) | typedef struct
type arm_biquad_casd_df1_inst_q15 (line 1239) | typedef struct
type arm_biquad_casd_df1_inst_q31 (line 1252) | typedef struct
type arm_biquad_casd_df1_inst_f32 (line 1264) | typedef struct
type arm_matrix_instance_f32 (line 1406) | typedef struct
type arm_matrix_instance_q15 (line 1417) | typedef struct
type arm_matrix_instance_q31 (line 1429) | typedef struct
type arm_pid_instance_q15 (line 1734) | typedef struct
type arm_pid_instance_q31 (line 1752) | typedef struct
type arm_pid_instance_f32 (line 1767) | typedef struct
type arm_linear_interp_instance_f32 (line 1841) | typedef struct
type arm_bilinear_interp_instance_f32 (line 1853) | typedef struct
type arm_bilinear_interp_instance_q31 (line 1864) | typedef struct
type arm_bilinear_interp_instance_q15 (line 1875) | typedef struct
type arm_bilinear_interp_instance_q7 (line 1886) | typedef struct
type arm_cfft_radix2_instance_q15 (line 1963) | typedef struct
type arm_cfft_radix4_instance_q15 (line 1990) | typedef struct
type arm_cfft_radix2_instance_q31 (line 2015) | typedef struct
type arm_cfft_radix4_instance_q31 (line 2040) | typedef struct
type arm_cfft_radix2_instance_f32 (line 2066) | typedef struct
type arm_cfft_radix4_instance_f32 (line 2094) | typedef struct
type arm_cfft_instance_f32 (line 2122) | typedef struct
type arm_rfft_instance_q15 (line 2140) | typedef struct
type arm_rfft_instance_q31 (line 2168) | typedef struct
type arm_rfft_instance_f32 (line 2196) | typedef struct
type arm_rfft_fast_instance_f32 (line 2224) | typedef struct
type arm_dct4_instance_f32 (line 2244) | typedef struct
type arm_dct4_instance_q31 (line 2291) | typedef struct
type arm_dct4_instance_q15 (line 2338) | typedef struct
type arm_fir_decimate_instance_q15 (line 3317) | typedef struct
type arm_fir_decimate_instance_q31 (line 3329) | typedef struct
type arm_fir_decimate_instance_f32 (line 3342) | typedef struct
type arm_fir_interpolate_instance_q15 (line 3498) | typedef struct
type arm_fir_interpolate_instance_q31 (line 3510) | typedef struct
type arm_fir_interpolate_instance_f32 (line 3522) | typedef struct
type arm_biquad_cas_df1_32x64_ins_q31 (line 3642) | typedef struct
type arm_biquad_cascade_df2T_instance_f32 (line 3689) | typedef struct
type arm_fir_lattice_instance_q15 (line 3734) | typedef struct
type arm_fir_lattice_instance_q31 (line 3745) | typedef struct
type arm_fir_lattice_instance_f32 (line 3756) | typedef struct
type arm_iir_lattice_instance_q15 (line 3857) | typedef struct
type arm_iir_lattice_instance_q31 (line 3868) | typedef struct
type arm_iir_lattice_instance_f32 (line 3879) | typedef struct
type arm_lms_instance_f32 (line 3997) | typedef struct
type arm_lms_instance_q15 (line 4047) | typedef struct
type arm_lms_instance_q31 (line 4102) | typedef struct
type arm_lms_norm_instance_f32 (line 4156) | typedef struct
type arm_lms_norm_instance_q31 (line 4208) | typedef struct
type arm_lms_norm_instance_q15 (line 4264) | typedef struct
type arm_fir_sparse_instance_f32 (line 4488) | typedef struct
type arm_fir_sparse_instance_q31 (line 4502) | typedef struct
type arm_fir_sparse_instance_q15 (line 4516) | typedef struct
type arm_fir_sparse_instance_q7 (line 4530) | typedef struct
function __INLINE (line 4880) | static __INLINE float32_t arm_pid_f32(
function __INLINE (line 4915) | static __INLINE q31_t arm_pid_q31(
function __INLINE (line 4963) | static __INLINE q15_t arm_pid_q15(
function __INLINE (line 5069) | static __INLINE void arm_clarke_f32(
function __INLINE (line 5099) | static __INLINE void arm_clarke_q31(
function __INLINE (line 5174) | static __INLINE void arm_inv_clarke_f32(
function __INLINE (line 5203) | static __INLINE void arm_inv_clarke_q31(
function __INLINE (line 5290) | static __INLINE void arm_park_f32(
function __INLINE (line 5324) | static __INLINE void arm_park_q31(
function __INLINE (line 5409) | static __INLINE void arm_inv_park_f32(
function __INLINE (line 5444) | static __INLINE void arm_inv_park_q31(
function __INLINE (line 5542) | static __INLINE float32_t arm_linear_interp_f32(
function __INLINE (line 5601) | static __INLINE q31_t arm_linear_interp_q31(
function __INLINE (line 5663) | static __INLINE q15_t arm_linear_interp_q15(
function __INLINE (line 5723) | static __INLINE q7_t arm_linear_interp_q7(
function __INLINE (line 5868) | static __INLINE arm_status arm_sqrt_f32(
function __INLINE (line 5928) | static __INLINE void arm_circularWrite_f32(
function __INLINE (line 5973) | static __INLINE void arm_circularRead_f32(
function __INLINE (line 6028) | static __INLINE void arm_circularWrite_q15(
function __INLINE (line 6073) | static __INLINE void arm_circularRead_q15(
function __INLINE (line 6130) | static __INLINE void arm_circularWrite_q7(
function __INLINE (line 6175) | static __INLINE void arm_circularRead_q7(
function __INLINE (line 6916) | static __INLINE float32_t arm_bilinear_interp_f32(
function __INLINE (line 6984) | static __INLINE q31_t arm_bilinear_interp_q31(
function __INLINE (line 7060) | static __INLINE q15_t arm_bilinear_interp_q15(
function __INLINE (line 7140) | static __INLINE q7_t arm_bilinear_interp_q7(
FILE: Libraries/CMSIS/Include/core_cm0.h
type APSR_Type (line 195) | typedef union
type IPSR_Type (line 218) | typedef union
type xPSR_Type (line 231) | typedef union
type CONTROL_Type (line 257) | typedef union
type NVIC_Type (line 280) | typedef struct
type SCB_Type (line 305) | typedef struct
type SysTick_Type (line 410) | typedef struct
function __STATIC_INLINE (line 515) | __STATIC_INLINE void NVIC_EnableIRQ(IRQn_Type IRQn)
function __STATIC_INLINE (line 527) | __STATIC_INLINE void NVIC_DisableIRQ(IRQn_Type IRQn)
function __STATIC_INLINE (line 543) | __STATIC_INLINE uint32_t NVIC_GetPendingIRQ(IRQn_Type IRQn)
function __STATIC_INLINE (line 555) | __STATIC_INLINE void NVIC_SetPendingIRQ(IRQn_Type IRQn)
function __STATIC_INLINE (line 567) | __STATIC_INLINE void NVIC_ClearPendingIRQ(IRQn_Type IRQn)
function __STATIC_INLINE (line 582) | __STATIC_INLINE void NVIC_SetPriority(IRQn_Type IRQn, uint32_t priority)
function __STATIC_INLINE (line 604) | __STATIC_INLINE uint32_t NVIC_GetPriority(IRQn_Type IRQn)
function __STATIC_INLINE (line 618) | __STATIC_INLINE void NVIC_SystemReset(void)
function __STATIC_INLINE (line 656) | __STATIC_INLINE uint32_t SysTick_Config(uint32_t ticks)
FILE: Libraries/CMSIS/Include/core_cm0plus.h
type APSR_Type (line 206) | typedef union
type IPSR_Type (line 229) | typedef union
type xPSR_Type (line 242) | typedef union
type CONTROL_Type (line 268) | typedef union
type NVIC_Type (line 291) | typedef struct
type SCB_Type (line 316) | typedef struct
type SysTick_Type (line 431) | typedef struct
type MPU_Type (line 481) | typedef struct
function __STATIC_INLINE (line 626) | __STATIC_INLINE void NVIC_EnableIRQ(IRQn_Type IRQn)
function __STATIC_INLINE (line 638) | __STATIC_INLINE void NVIC_DisableIRQ(IRQn_Type IRQn)
function __STATIC_INLINE (line 654) | __STATIC_INLINE uint32_t NVIC_GetPendingIRQ(IRQn_Type IRQn)
function __STATIC_INLINE (line 666) | __STATIC_INLINE void NVIC_SetPendingIRQ(IRQn_Type IRQn)
function __STATIC_INLINE (line 678) | __STATIC_INLINE void NVIC_ClearPendingIRQ(IRQn_Type IRQn)
function __STATIC_INLINE (line 693) | __STATIC_INLINE void NVIC_SetPriority(IRQn_Type IRQn, uint32_t priority)
function __STATIC_INLINE (line 715) | __STATIC_INLINE uint32_t NVIC_GetPriority(IRQn_Type IRQn)
function __STATIC_INLINE (line 729) | __STATIC_INLINE void NVIC_SystemReset(void)
function __STATIC_INLINE (line 767) | __STATIC_INLINE uint32_t SysTick_Config(uint32_t ticks)
FILE: Libraries/CMSIS/Include/core_cm3.h
type APSR_Type (line 211) | typedef union
type IPSR_Type (line 234) | typedef union
type xPSR_Type (line 247) | typedef union
type CONTROL_Type (line 273) | typedef union
type NVIC_Type (line 296) | typedef struct
type SCB_Type (line 328) | typedef struct
type SCnSCB_Type (line 553) | typedef struct
type SysTick_Type (line 590) | typedef struct
type DWT_Type (line 741) | typedef struct
type TPI_Type (line 886) | typedef struct
type MPU_Type (line 1040) | typedef struct
type CoreDebug_Type (line 1132) | typedef struct
function __STATIC_INLINE (line 1291) | __STATIC_INLINE void NVIC_SetPriorityGrouping(uint32_t PriorityGroup)
function __STATIC_INLINE (line 1311) | __STATIC_INLINE uint32_t NVIC_GetPriorityGrouping(void)
function __STATIC_INLINE (line 1323) | __STATIC_INLINE void NVIC_EnableIRQ(IRQn_Type IRQn)
function __STATIC_INLINE (line 1335) | __STATIC_INLINE void NVIC_DisableIRQ(IRQn_Type IRQn)
function __STATIC_INLINE (line 1351) | __STATIC_INLINE uint32_t NVIC_GetPendingIRQ(IRQn_Type IRQn)
function __STATIC_INLINE (line 1363) | __STATIC_INLINE void NVIC_SetPendingIRQ(IRQn_Type IRQn)
function __STATIC_INLINE (line 1375) | __STATIC_INLINE void NVIC_ClearPendingIRQ(IRQn_Type IRQn)
function __STATIC_INLINE (line 1390) | __STATIC_INLINE uint32_t NVIC_GetActive(IRQn_Type IRQn)
function __STATIC_INLINE (line 1405) | __STATIC_INLINE void NVIC_SetPriority(IRQn_Type IRQn, uint32_t priority)
function __STATIC_INLINE (line 1425) | __STATIC_INLINE uint32_t NVIC_GetPriority(IRQn_Type IRQn)
function __STATIC_INLINE (line 1447) | __STATIC_INLINE uint32_t NVIC_EncodePriority (uint32_t PriorityGroup, ui...
function __STATIC_INLINE (line 1475) | __STATIC_INLINE void NVIC_DecodePriority (uint32_t Priority, uint32_t Pr...
function __STATIC_INLINE (line 1493) | __STATIC_INLINE void NVIC_SystemReset(void)
function __STATIC_INLINE (line 1532) | __STATIC_INLINE uint32_t SysTick_Config(uint32_t ticks)
function __STATIC_INLINE (line 1572) | __STATIC_INLINE uint32_t ITM_SendChar (uint32_t ch)
function __STATIC_INLINE (line 1591) | __STATIC_INLINE int32_t ITM_ReceiveChar (void) {
function __STATIC_INLINE (line 1610) | __STATIC_INLINE int32_t ITM_CheckChar (void) {
FILE: Libraries/CMSIS/Include/core_cm4.h
type APSR_Type (line 251) | typedef union
type IPSR_Type (line 274) | typedef union
type xPSR_Type (line 287) | typedef union
type CONTROL_Type (line 313) | typedef union
type NVIC_Type (line 336) | typedef struct
type SCB_Type (line 368) | typedef struct
type SCnSCB_Type (line 585) | typedef struct
type SysTick_Type (line 623) | typedef struct
type DWT_Type (line 774) | typedef struct
type TPI_Type (line 919) | typedef struct
type MPU_Type (line 1073) | typedef struct
type FPU_Type (line 1166) | typedef struct
type CoreDebug_Type (line 1271) | typedef struct
function __STATIC_INLINE (line 1435) | __STATIC_INLINE void NVIC_SetPriorityGrouping(uint32_t PriorityGroup)
function __STATIC_INLINE (line 1455) | __STATIC_INLINE uint32_t NVIC_GetPriorityGrouping(void)
function __STATIC_INLINE (line 1467) | __STATIC_INLINE void NVIC_EnableIRQ(IRQn_Type IRQn)
function __STATIC_INLINE (line 1480) | __STATIC_INLINE void NVIC_DisableIRQ(IRQn_Type IRQn)
function __STATIC_INLINE (line 1496) | __STATIC_INLINE uint32_t NVIC_GetPendingIRQ(IRQn_Type IRQn)
function __STATIC_INLINE (line 1508) | __STATIC_INLINE void NVIC_SetPendingIRQ(IRQn_Type IRQn)
function __STATIC_INLINE (line 1520) | __STATIC_INLINE void NVIC_ClearPendingIRQ(IRQn_Type IRQn)
function __STATIC_INLINE (line 1535) | __STATIC_INLINE uint32_t NVIC_GetActive(IRQn_Type IRQn)
function __STATIC_INLINE (line 1550) | __STATIC_INLINE void NVIC_SetPriority(IRQn_Type IRQn, uint32_t priority)
function __STATIC_INLINE (line 1570) | __STATIC_INLINE uint32_t NVIC_GetPriority(IRQn_Type IRQn)
function __STATIC_INLINE (line 1592) | __STATIC_INLINE uint32_t NVIC_EncodePriority (uint32_t PriorityGroup, ui...
function __STATIC_INLINE (line 1620) | __STATIC_INLINE void NVIC_DecodePriority (uint32_t Priority, uint32_t Pr...
function __STATIC_INLINE (line 1638) | __STATIC_INLINE void NVIC_SystemReset(void)
function __STATIC_INLINE (line 1677) | __STATIC_INLINE uint32_t SysTick_Config(uint32_t ticks)
function __STATIC_INLINE (line 1717) | __STATIC_INLINE uint32_t ITM_SendChar (uint32_t ch)
function __STATIC_INLINE (line 1736) | __STATIC_INLINE int32_t ITM_ReceiveChar (void) {
function __STATIC_INLINE (line 1755) | __STATIC_INLINE int32_t ITM_CheckChar (void) {
FILE: Libraries/CMSIS/Include/core_cm4_simd.h
function __STATIC_INLINE (line 158) | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SADD8(uint...
function __STATIC_INLINE (line 166) | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __QADD8(uint...
function __STATIC_INLINE (line 174) | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SHADD8(uin...
function __STATIC_INLINE (line 182) | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UADD8(uint...
function __STATIC_INLINE (line 190) | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UQADD8(uin...
function __STATIC_INLINE (line 198) | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UHADD8(uin...
function __STATIC_INLINE (line 207) | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SSUB8(uint...
function __STATIC_INLINE (line 215) | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __QSUB8(uint...
function __STATIC_INLINE (line 223) | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SHSUB8(uin...
function __STATIC_INLINE (line 231) | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __USUB8(uint...
function __STATIC_INLINE (line 239) | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UQSUB8(uin...
function __STATIC_INLINE (line 247) | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UHSUB8(uin...
function __STATIC_INLINE (line 256) | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SADD16(uin...
function __STATIC_INLINE (line 264) | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __QADD16(uin...
function __STATIC_INLINE (line 272) | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SHADD16(ui...
function __STATIC_INLINE (line 280) | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UADD16(uin...
function __STATIC_INLINE (line 288) | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UQADD16(ui...
function __STATIC_INLINE (line 296) | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UHADD16(ui...
function __STATIC_INLINE (line 304) | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SSUB16(uin...
function __STATIC_INLINE (line 312) | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __QSUB16(uin...
function __STATIC_INLINE (line 320) | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SHSUB16(ui...
function __STATIC_INLINE (line 328) | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __USUB16(uin...
function __STATIC_INLINE (line 336) | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UQSUB16(ui...
function __STATIC_INLINE (line 344) | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UHSUB16(ui...
function __STATIC_INLINE (line 352) | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SASX(uint3...
function __STATIC_INLINE (line 360) | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __QASX(uint3...
function __STATIC_INLINE (line 368) | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SHASX(uint...
function __STATIC_INLINE (line 376) | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UASX(uint3...
function __STATIC_INLINE (line 384) | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UQASX(uint...
function __STATIC_INLINE (line 392) | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UHASX(uint...
function __STATIC_INLINE (line 400) | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SSAX(uint3...
function __STATIC_INLINE (line 408) | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __QSAX(uint3...
function __STATIC_INLINE (line 416) | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SHSAX(uint...
function __STATIC_INLINE (line 424) | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __USAX(uint3...
function __STATIC_INLINE (line 432) | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UQSAX(uint...
function __STATIC_INLINE (line 440) | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UHSAX(uint...
function __STATIC_INLINE (line 448) | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __USAD8(uint...
function __STATIC_INLINE (line 456) | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __USADA8(uin...
function __STATIC_INLINE (line 478) | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UXTB16(uin...
function __STATIC_INLINE (line 486) | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UXTAB16(ui...
function __STATIC_INLINE (line 494) | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SXTB16(uin...
function __STATIC_INLINE (line 502) | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SXTAB16(ui...
function __STATIC_INLINE (line 510) | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMUAD (ui...
function __STATIC_INLINE (line 518) | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMUADX (ui...
function __STATIC_INLINE (line 526) | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMLAD (uin...
function __STATIC_INLINE (line 534) | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMLADX (ui...
function __STATIC_INLINE (line 556) | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMUSD (ui...
function __STATIC_INLINE (line 564) | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMUSDX (ui...
function __STATIC_INLINE (line 572) | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMLSD (uin...
function __STATIC_INLINE (line 580) | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMLSDX (ui...
function __STATIC_INLINE (line 602) | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SEL (uint...
function __STATIC_INLINE (line 610) | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __QADD(uint3...
function __STATIC_INLINE (line 618) | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __QSUB(uint3...
function __STATIC_INLINE (line 643) | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMMLA (int...
FILE: Libraries/CMSIS/Include/core_cmFunc.h
function __STATIC_INLINE (line 64) | __STATIC_INLINE uint32_t __get_CONTROL(void)
function __STATIC_INLINE (line 77) | __STATIC_INLINE void __set_CONTROL(uint32_t control)
function __STATIC_INLINE (line 90) | __STATIC_INLINE uint32_t __get_IPSR(void)
function __STATIC_INLINE (line 103) | __STATIC_INLINE uint32_t __get_APSR(void)
function __STATIC_INLINE (line 116) | __STATIC_INLINE uint32_t __get_xPSR(void)
function __STATIC_INLINE (line 129) | __STATIC_INLINE uint32_t __get_PSP(void)
function __STATIC_INLINE (line 142) | __STATIC_INLINE void __set_PSP(uint32_t topOfProcStack)
function __STATIC_INLINE (line 155) | __STATIC_INLINE uint32_t __get_MSP(void)
function __STATIC_INLINE (line 168) | __STATIC_INLINE void __set_MSP(uint32_t topOfMainStack)
function __STATIC_INLINE (line 181) | __STATIC_INLINE uint32_t __get_PRIMASK(void)
function __STATIC_INLINE (line 194) | __STATIC_INLINE void __set_PRIMASK(uint32_t priMask)
function __STATIC_INLINE (line 225) | __STATIC_INLINE uint32_t __get_BASEPRI(void)
function __STATIC_INLINE (line 238) | __STATIC_INLINE void __set_BASEPRI(uint32_t basePri)
function __STATIC_INLINE (line 251) | __STATIC_INLINE uint32_t __get_FAULTMASK(void)
function __STATIC_INLINE (line 264) | __STATIC_INLINE void __set_FAULTMASK(uint32_t faultMask)
function __STATIC_INLINE (line 281) | __STATIC_INLINE uint32_t __get_FPSCR(void)
function __STATIC_INLINE (line 298) | __STATIC_INLINE void __set_FPSCR(uint32_t fpscr)
function __STATIC_INLINE (line 352) | __STATIC_INLINE uint32_t __get_CONTROL(void)
function __STATIC_INLINE (line 367) | __attribute__( ( always_inline ) ) __STATIC_INLINE void __set_CONTROL(ui...
function __STATIC_INLINE (line 379) | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_IPSR(v...
function __STATIC_INLINE (line 394) | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_APSR(v...
function __STATIC_INLINE (line 409) | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_xPSR(v...
function __STATIC_INLINE (line 424) | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_PSP(void)
function __STATIC_INLINE (line 439) | __attribute__( ( always_inline ) ) __STATIC_INLINE void __set_PSP(uint32...
function __STATIC_INLINE (line 451) | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_MSP(void)
function __STATIC_INLINE (line 466) | __attribute__( ( always_inline ) ) __STATIC_INLINE void __set_MSP(uint32...
function __STATIC_INLINE (line 478) | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_PRIMAS...
function __STATIC_INLINE (line 493) | __attribute__( ( always_inline ) ) __STATIC_INLINE void __set_PRIMASK(ui...
function __STATIC_INLINE (line 529) | __STATIC_INLINE uint32_t __get_BASEPRI(void)
function __STATIC_INLINE (line 544) | __attribute__( ( always_inline ) ) __STATIC_INLINE void __set_BASEPRI(ui...
function __STATIC_INLINE (line 556) | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_FAULTM...
function __STATIC_INLINE (line 571) | __attribute__( ( always_inline ) ) __STATIC_INLINE void __set_FAULTMASK(...
function __STATIC_INLINE (line 587) | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_FPSCR(...
function __STATIC_INLINE (line 609) | __attribute__( ( always_inline ) ) __STATIC_INLINE void __set_FPSCR(uint...
FILE: Libraries/CMSIS/Include/core_cmInstr.h
function __REV16 (line 129) | uint32_t __REV16(uint32_t value)
function __REVSH (line 144) | int32_t __REVSH(int32_t value)
function __STATIC_INLINE (line 325) | __attribute__( ( always_inline ) ) __STATIC_INLINE void __NOP(void)
function __STATIC_INLINE (line 336) | __attribute__( ( always_inline ) ) __STATIC_INLINE void __WFI(void)
function __STATIC_INLINE (line 347) | __attribute__( ( always_inline ) ) __STATIC_INLINE void __WFE(void)
function __STATIC_INLINE (line 357) | __attribute__( ( always_inline ) ) __STATIC_INLINE void __SEV(void)
function __STATIC_INLINE (line 369) | __attribute__( ( always_inline ) ) __STATIC_INLINE void __ISB(void)
function __STATIC_INLINE (line 380) | __attribute__( ( always_inline ) ) __STATIC_INLINE void __DSB(void)
function __STATIC_INLINE (line 391) | __attribute__( ( always_inline ) ) __STATIC_INLINE void __DMB(void)
function __STATIC_INLINE (line 404) | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __REV(uint32...
function __STATIC_INLINE (line 424) | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __REV16(uint...
function __STATIC_INLINE (line 440) | __attribute__( ( always_inline ) ) __STATIC_INLINE int32_t __REVSH(int32...
function __STATIC_INLINE (line 461) | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __ROR(uint32...
function __STATIC_INLINE (line 487) | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __RBIT(uint3...
function __STATIC_INLINE (line 503) | __attribute__( ( always_inline ) ) __STATIC_INLINE uint8_t __LDREXB(vola...
function __STATIC_INLINE (line 526) | __attribute__( ( always_inline ) ) __STATIC_INLINE uint16_t __LDREXH(vol...
function __STATIC_INLINE (line 549) | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __LDREXW(vol...
function __STATIC_INLINE (line 567) | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __STREXB(uin...
function __STATIC_INLINE (line 585) | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __STREXH(uin...
function __STATIC_INLINE (line 603) | __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __STREXW(uin...
function __STATIC_INLINE (line 617) | __attribute__( ( always_inline ) ) __STATIC_INLINE void __CLREX(void)
function __STATIC_INLINE (line 662) | __attribute__( ( always_inline ) ) __STATIC_INLINE uint8_t __CLZ(uint32_...
FILE: Libraries/CMSIS/Include/core_sc000.h
type APSR_Type (line 201) | typedef union
type IPSR_Type (line 224) | typedef union
type xPSR_Type (line 237) | typedef union
type CONTROL_Type (line 263) | typedef union
type NVIC_Type (line 286) | typedef struct
type SCB_Type (line 311) | typedef struct
type SCnSCB_Type (line 429) | typedef struct
type SysTick_Type (line 450) | typedef struct
type MPU_Type (line 500) | typedef struct
function __STATIC_INLINE (line 646) | __STATIC_INLINE void NVIC_EnableIRQ(IRQn_Type IRQn)
function __STATIC_INLINE (line 658) | __STATIC_INLINE void NVIC_DisableIRQ(IRQn_Type IRQn)
function __STATIC_INLINE (line 674) | __STATIC_INLINE uint32_t NVIC_GetPendingIRQ(IRQn_Type IRQn)
function __STATIC_INLINE (line 686) | __STATIC_INLINE void NVIC_SetPendingIRQ(IRQn_Type IRQn)
function __STATIC_INLINE (line 698) | __STATIC_INLINE void NVIC_ClearPendingIRQ(IRQn_Type IRQn)
function __STATIC_INLINE (line 713) | __STATIC_INLINE void NVIC_SetPriority(IRQn_Type IRQn, uint32_t priority)
function __STATIC_INLINE (line 735) | __STATIC_INLINE uint32_t NVIC_GetPriority(IRQn_Type IRQn)
function __STATIC_INLINE (line 749) | __STATIC_INLINE void NVIC_SystemReset(void)
function __STATIC_INLINE (line 787) | __STATIC_INLINE uint32_t SysTick_Config(uint32_t ticks)
FILE: Libraries/CMSIS/Include/core_sc300.h
type APSR_Type (line 202) | typedef union
type IPSR_Type (line 225) | typedef union
type xPSR_Type (line 238) | typedef union
type CONTROL_Type (line 264) | typedef union
type NVIC_Type (line 287) | typedef struct
type SCB_Type (line 319) | typedef struct
type SCnSCB_Type (line 539) | typedef struct
type SysTick_Type (line 561) | typedef struct
type DWT_Type (line 712) | typedef struct
type TPI_Type (line 857) | typedef struct
type MPU_Type (line 1011) | typedef struct
type CoreDebug_Type (line 1103) | typedef struct
function __STATIC_INLINE (line 1262) | __STATIC_INLINE void NVIC_SetPriorityGrouping(uint32_t PriorityGroup)
function __STATIC_INLINE (line 1282) | __STATIC_INLINE uint32_t NVIC_GetPriorityGrouping(void)
function __STATIC_INLINE (line 1294) | __STATIC_INLINE void NVIC_EnableIRQ(IRQn_Type IRQn)
function __STATIC_INLINE (line 1306) | __STATIC_INLINE void NVIC_DisableIRQ(IRQn_Type IRQn)
function __STATIC_INLINE (line 1322) | __STATIC_INLINE uint32_t NVIC_GetPendingIRQ(IRQn_Type IRQn)
function __STATIC_INLINE (line 1334) | __STATIC_INLINE void NVIC_SetPendingIRQ(IRQn_Type IRQn)
function __STATIC_INLINE (line 1346) | __STATIC_INLINE void NVIC_ClearPendingIRQ(IRQn_Type IRQn)
function __STATIC_INLINE (line 1361) | __STATIC_INLINE uint32_t NVIC_GetActive(IRQn_Type IRQn)
function __STATIC_INLINE (line 1376) | __STATIC_INLINE void NVIC_SetPriority(IRQn_Type IRQn, uint32_t priority)
function __STATIC_INLINE (line 1396) | __STATIC_INLINE uint32_t NVIC_GetPriority(IRQn_Type IRQn)
function __STATIC_INLINE (line 1418) | __STATIC_INLINE uint32_t NVIC_EncodePriority (uint32_t PriorityGroup, ui...
function __STATIC_INLINE (line 1446) | __STATIC_INLINE void NVIC_DecodePriority (uint32_t Priority, uint32_t Pr...
function __STATIC_INLINE (line 1464) | __STATIC_INLINE void NVIC_SystemReset(void)
function __STATIC_INLINE (line 1503) | __STATIC_INLINE uint32_t SysTick_Config(uint32_t ticks)
function __STATIC_INLINE (line 1543) | __STATIC_INLINE uint32_t ITM_SendChar (uint32_t ch)
function __STATIC_INLINE (line 1562) | __STATIC_INLINE int32_t ITM_ReceiveChar (void) {
function __STATIC_INLINE (line 1581) | __STATIC_INLINE int32_t ITM_CheckChar (void) {
FILE: Libraries/STM32F4xx_StdPeriph_Driver/inc/misc.h
type NVIC_InitTypeDef (line 54) | typedef struct
FILE: Libraries/STM32F4xx_StdPeriph_Driver/inc/stm32f4xx_adc.h
type ADC_InitTypeDef (line 53) | typedef struct
type ADC_CommonInitTypeDef (line 84) | typedef struct
FILE: Libraries/STM32F4xx_StdPeriph_Driver/inc/stm32f4xx_can.h
type CAN_InitTypeDef (line 56) | typedef struct
type CAN_FilterInitTypeDef (line 98) | typedef struct
type CanTxMsg (line 136) | typedef struct
type CanRxMsg (line 163) | typedef struct
FILE: Libraries/STM32F4xx_StdPeriph_Driver/inc/stm32f4xx_cryp.h
type CRYP_InitTypeDef (line 53) | typedef struct
type CRYP_KeyInitTypeDef (line 70) | typedef struct
type CRYP_IVInitTypeDef (line 84) | typedef struct
type CRYP_Context (line 95) | typedef struct
FILE: Libraries/STM32F4xx_StdPeriph_Driver/inc/stm32f4xx_dac.h
type DAC_InitTypeDef (line 54) | typedef struct
FILE: Libraries/STM32F4xx_StdPeriph_Driver/inc/stm32f4xx_dcmi.h
type DCMI_InitTypeDef (line 51) | typedef struct
type DCMI_CROPInitTypeDef (line 78) | typedef struct
type DCMI_CodesInitTypeDef (line 97) | typedef struct
FILE: Libraries/STM32F4xx_StdPeriph_Driver/inc/stm32f4xx_dma.h
type DMA_InitTypeDef (line 54) | typedef struct
FILE: Libraries/STM32F4xx_StdPeriph_Driver/inc/stm32f4xx_dma2d.h
type DMA2D_InitTypeDef (line 48) | typedef struct
type DMA2D_FG_InitTypeDef (line 101) | typedef struct
type DMA2D_BG_InitTypeDef (line 138) | typedef struct
FILE: Libraries/STM32F4xx_StdPeriph_Driver/inc/stm32f4xx_exti.h
type EXTIMode_TypeDef (line 54) | typedef enum
type EXTITrigger_TypeDef (line 66) | typedef enum
type EXTI_InitTypeDef (line 80) | typedef struct
FILE: Libraries/STM32F4xx_StdPeriph_Driver/inc/stm32f4xx_flash.h
type FLASH_Status (line 52) | typedef enum
FILE: Libraries/STM32F4xx_StdPeriph_Driver/inc/stm32f4xx_fmc.h
type FMC_NORSRAMTimingInitTypeDef (line 47) | typedef struct
type FMC_NORSRAMInitTypeDef (line 88) | typedef struct
type FMC_NAND_PCCARDTimingInitTypeDef (line 152) | typedef struct
type FMC_NANDInitTypeDef (line 183) | typedef struct
type FMC_PCCARDInitTypeDef (line 217) | typedef struct
type FMC_SDRAMTimingInitTypeDef (line 242) | typedef struct
type FMC_SDRAMCommandTypeDef (line 279) | typedef struct
type FMC_SDRAMInitTypeDef (line 299) | typedef struct
FILE: Libraries/STM32F4xx_StdPeriph_Driver/inc/stm32f4xx_fsmc.h
type FSMC_NORSRAMTimingInitTypeDef (line 53) | typedef struct
type FSMC_NORSRAMInitTypeDef (line 94) | typedef struct
type FSMC_NAND_PCCARDTimingInitTypeDef (line 152) | typedef struct
type FSMC_NANDInitTypeDef (line 183) | typedef struct
type FSMC_PCCARDInitTypeDef (line 217) | typedef struct
FILE: Libraries/STM32F4xx_StdPeriph_Driver/inc/stm32f4xx_gpio.h
type GPIOMode_TypeDef (line 65) | typedef enum
type GPIOOType_TypeDef (line 78) | typedef enum
type GPIOSpeed_TypeDef (line 89) | typedef enum
type GPIOPuPd_TypeDef (line 109) | typedef enum
type BitAction (line 121) | typedef enum
type GPIO_InitTypeDef (line 132) | typedef struct
FILE: Libraries/STM32F4xx_StdPeriph_Driver/inc/stm32f4xx_hash.h
type HASH_InitTypeDef (line 53) | typedef struct
type HASH_MsgDigest (line 69) | typedef struct
type HASH_Context (line 80) | typedef struct
FILE: Libraries/STM32F4xx_StdPeriph_Driver/inc/stm32f4xx_i2c.h
type I2C_InitTypeDef (line 54) | typedef struct
FILE: Libraries/STM32F4xx_StdPeriph_Driver/inc/stm32f4xx_ltdc.h
type LTDC_InitTypeDef (line 48) | typedef struct
type LTDC_Layer_InitTypeDef (line 100) | typedef struct
type LTDC_PosTypeDef (line 154) | typedef struct
type LTDC_RGBTypeDef (line 160) | typedef struct
type LTDC_ColorKeying_InitTypeDef (line 167) | typedef struct
type LTDC_CLUT_InitTypeDef (line 179) | typedef struct
FILE: Libraries/STM32F4xx_StdPeriph_Driver/inc/stm32f4xx_rcc.h
type RCC_ClocksTypeDef (line 48) | typedef struct
FILE: Libraries/STM32F4xx_StdPeriph_Driver/inc/stm32f4xx_rtc.h
type RTC_InitTypeDef (line 53) | typedef struct
type RTC_TimeTypeDef (line 68) | typedef struct
type RTC_DateTypeDef (line 88) | typedef struct
type RTC_AlarmTypeDef (line 106) | typedef struct
FILE: Libraries/STM32F4xx_StdPeriph_Driver/inc/stm32f4xx_sai.h
type SAI_InitTypeDef (line 54) | typedef struct
type SAI_FrameInitTypeDef (line 96) | typedef struct
type SAI_SlotInitTypeDef (line 131) | typedef struct
FILE: Libraries/STM32F4xx_StdPeriph_Driver/inc/stm32f4xx_sdio.h
type SDIO_InitTypeDef (line 50) | typedef struct
type SDIO_CmdInitTypeDef (line 74) | typedef struct
type SDIO_DataInitTypeDef (line 94) | typedef struct
FILE: Libraries/STM32F4xx_StdPeriph_Driver/inc/stm32f4xx_spi.h
type SPI_InitTypeDef (line 54) | typedef struct
type I2S_InitTypeDef (line 91) | typedef struct
FILE: Libraries/STM32F4xx_StdPeriph_Driver/inc/stm32f4xx_tim.h
type TIM_TimeBaseInitTypeDef (line 55) | typedef struct
type TIM_OCInitTypeDef (line 84) | typedef struct
type TIM_ICInitTypeDef (line 119) | typedef struct
type TIM_BDTRInitTypeDef (line 143) | typedef struct
FILE: Libraries/STM32F4xx_StdPeriph_Driver/inc/stm32f4xx_usart.h
type USART_InitTypeDef (line 54) | typedef struct
type USART_ClockInitTypeDef (line 87) | typedef struct
FILE: Libraries/STM32F4xx_StdPeriph_Driver/src/misc.c
function NVIC_PriorityGroupConfig (line 118) | void NVIC_PriorityGroupConfig(uint32_t NVIC_PriorityGroup)
function NVIC_Init (line 136) | void NVIC_Init(NVIC_InitTypeDef* NVIC_InitStruct)
function NVIC_SetVectorTable (line 180) | void NVIC_SetVectorTable(uint32_t NVIC_VectTab, uint32_t Offset)
function NVIC_SystemLPConfig (line 199) | void NVIC_SystemLPConfig(uint8_t LowPowerMode, FunctionalState NewState)
function SysTick_CLKSourceConfig (line 223) | void SysTick_CLKSourceConfig(uint32_t SysTick_CLKSource)
FILE: Libraries/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_adc.c
function ADC_DeInit (line 213) | void ADC_DeInit(void)
function ADC_Init (line 235) | void ADC_Init(ADC_TypeDef* ADCx, ADC_InitTypeDef* ADC_InitStruct)
function ADC_StructInit (line 310) | void ADC_StructInit(ADC_InitTypeDef* ADC_InitStruct)
function ADC_CommonInit (line 341) | void ADC_CommonInit(ADC_CommonInitTypeDef* ADC_CommonInitStruct)
function ADC_CommonStructInit (line 377) | void ADC_CommonStructInit(ADC_CommonInitTypeDef* ADC_CommonInitStruct)
function ADC_Cmd (line 399) | void ADC_Cmd(ADC_TypeDef* ADCx, FunctionalState NewState)
function ADC_AnalogWatchdogCmd (line 455) | void ADC_AnalogWatchdogCmd(ADC_TypeDef* ADCx, uint32_t ADC_AnalogWatchdog)
function ADC_AnalogWatchdogThresholdsConfig (line 484) | void ADC_AnalogWatchdogThresholdsConfig(ADC_TypeDef* ADCx, uint16_t High...
function ADC_AnalogWatchdogSingleChannelConfig (line 525) | void ADC_AnalogWatchdogSingleChannelConfig(ADC_TypeDef* ADCx, uint8_t AD...
function ADC_TempSensorVrefintCmd (line 589) | void ADC_TempSensorVrefintCmd(FunctionalState NewState)
function ADC_VBATCmd (line 615) | void ADC_VBATCmd(FunctionalState NewState)
function ADC_RegularChannelConfig (line 715) | void ADC_RegularChannelConfig(ADC_TypeDef* ADCx, uint8_t ADC_Channel, ui...
function ADC_SoftwareStartConv (line 835) | void ADC_SoftwareStartConv(ADC_TypeDef* ADCx)
function FlagStatus (line 849) | FlagStatus ADC_GetSoftwareStartConvStatus(ADC_TypeDef* ADCx)
function ADC_EOCOnEachRegularChannelCmd (line 879) | void ADC_EOCOnEachRegularChannelCmd(ADC_TypeDef* ADCx, FunctionalState N...
function ADC_ContinuousModeCmd (line 904) | void ADC_ContinuousModeCmd(ADC_TypeDef* ADCx, FunctionalState NewState)
function ADC_DiscModeChannelCountConfig (line 930) | void ADC_DiscModeChannelCountConfig(ADC_TypeDef* ADCx, uint8_t Number)
function ADC_DiscModeCmd (line 962) | void ADC_DiscModeCmd(ADC_TypeDef* ADCx, FunctionalState NewState)
function ADC_GetConversionValue (line 985) | uint16_t ADC_GetConversionValue(ADC_TypeDef* ADCx)
function ADC_GetMultiModeConversionValue (line 1006) | uint32_t ADC_GetMultiModeConversionValue(void)
function ADC_DMACmd (line 1052) | void ADC_DMACmd(ADC_TypeDef* ADCx, FunctionalState NewState)
function ADC_DMARequestAfterLastTransferCmd (line 1076) | void ADC_DMARequestAfterLastTransferCmd(ADC_TypeDef* ADCx, FunctionalSta...
function ADC_MultiModeDMARequestAfterLastTransferCmd (line 1103) | void ADC_MultiModeDMARequestAfterLastTransferCmd(FunctionalState NewState)
function ADC_InjectedChannelConfig (line 1190) | void ADC_InjectedChannelConfig(ADC_TypeDef* ADCx, uint8_t ADC_Channel, u...
function ADC_InjectedSequencerLengthConfig (line 1253) | void ADC_InjectedSequencerLengthConfig(ADC_TypeDef* ADCx, uint8_t Length)
function ADC_SetInjectedOffset (line 1288) | void ADC_SetInjectedOffset(ADC_TypeDef* ADCx, uint8_t ADC_InjectedChanne...
function ADC_ExternalTrigInjectedConvConfig (line 1326) | void ADC_ExternalTrigInjectedConvConfig(ADC_TypeDef* ADCx, uint32_t ADC_...
function ADC_ExternalTrigInjectedConvEdgeConfig (line 1360) | void ADC_ExternalTrigInjectedConvEdgeConfig(ADC_TypeDef* ADCx, uint32_t ...
function ADC_SoftwareStartInjectedConv (line 1381) | void ADC_SoftwareStartInjectedConv(ADC_TypeDef* ADCx)
function FlagStatus (line 1394) | FlagStatus ADC_GetSoftwareStartInjectedConvCmdStatus(ADC_TypeDef* ADCx)
function ADC_AutoInjectedConvCmd (line 1423) | void ADC_AutoInjectedConvCmd(ADC_TypeDef* ADCx, FunctionalState NewState)
function ADC_InjectedDiscModeCmd (line 1449) | void ADC_InjectedDiscModeCmd(ADC_TypeDef* ADCx, FunctionalState NewState)
function ADC_GetInjectedConversionValue (line 1477) | uint16_t ADC_GetInjectedConversionValue(ADC_TypeDef* ADCx, uint8_t ADC_I...
function ADC_ITConfig (line 1584) | void ADC_ITConfig(ADC_TypeDef* ADCx, uint16_t ADC_IT, FunctionalState Ne...
function FlagStatus (line 1621) | FlagStatus ADC_GetFlagStatus(ADC_TypeDef* ADCx, uint8_t ADC_FLAG)
function ADC_ClearFlag (line 1656) | void ADC_ClearFlag(ADC_TypeDef* ADCx, uint8_t ADC_FLAG)
function ITStatus (line 1677) | ITStatus ADC_GetITStatus(ADC_TypeDef* ADCx, uint16_t ADC_IT)
function ADC_ClearITPendingBit (line 1718) | void ADC_ClearITPendingBit(ADC_TypeDef* ADCx, uint16_t ADC_IT)
FILE: Libraries/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_can.c
function CAN_DeInit (line 167) | void CAN_DeInit(CAN_TypeDef* CANx)
function CAN_Init (line 197) | uint8_t CAN_Init(CAN_TypeDef* CANx, CAN_InitTypeDef* CAN_InitStruct)
function CAN_FilterInit (line 334) | void CAN_FilterInit(CAN_FilterInitTypeDef* CAN_FilterInitStruct)
function CAN_StructInit (line 425) | void CAN_StructInit(CAN_InitTypeDef* CAN_InitStruct)
function CAN_SlaveStartBank (line 468) | void CAN_SlaveStartBank(uint8_t CAN_BankNumber)
function CAN_DBGFreeze (line 493) | void CAN_DBGFreeze(CAN_TypeDef* CANx, FunctionalState NewState)
function CAN_TTComModeCmd (line 523) | void CAN_TTComModeCmd(CAN_TypeDef* CANx, FunctionalState NewState)
function CAN_Transmit (line 577) | uint8_t CAN_Transmit(CAN_TypeDef* CANx, CanTxMsg* TxMessage)
function CAN_TransmitStatus (line 649) | uint8_t CAN_TransmitStatus(CAN_TypeDef* CANx, uint8_t TransmitMailbox)
function CAN_CancelTransmit (line 703) | void CAN_CancelTransmit(CAN_TypeDef* CANx, uint8_t Mailbox)
function CAN_Receive (line 750) | void CAN_Receive(CAN_TypeDef* CANx, uint8_t FIFONumber, CanRxMsg* RxMess...
function CAN_FIFORelease (line 799) | void CAN_FIFORelease(CAN_TypeDef* CANx, uint8_t FIFONumber)
function CAN_MessagePending (line 822) | uint8_t CAN_MessagePending(CAN_TypeDef* CANx, uint8_t FIFONumber)
function CAN_OperatingModeRequest (line 872) | uint8_t CAN_OperatingModeRequest(CAN_TypeDef* CANx, uint8_t CAN_Operatin...
function CAN_Sleep (line 953) | uint8_t CAN_Sleep(CAN_TypeDef* CANx)
function CAN_WakeUp (line 978) | uint8_t CAN_WakeUp(CAN_TypeDef* CANx)
function CAN_GetLastErrorCode (line 1040) | uint8_t CAN_GetLastErrorCode(CAN_TypeDef* CANx)
function CAN_GetReceiveErrorCounter (line 1065) | uint8_t CAN_GetReceiveErrorCounter(CAN_TypeDef* CANx)
function CAN_GetLSBTransmitErrorCounter (line 1085) | uint8_t CAN_GetLSBTransmitErrorCounter(CAN_TypeDef* CANx)
function CAN_ITConfig (line 1290) | void CAN_ITConfig(CAN_TypeDef* CANx, uint32_t CAN_IT, FunctionalState Ne...
function FlagStatus (line 1330) | FlagStatus CAN_GetFlagStatus(CAN_TypeDef* CANx, uint32_t CAN_FLAG)
function CAN_ClearFlag (line 1430) | void CAN_ClearFlag(CAN_TypeDef* CANx, uint32_t CAN_FLAG)
function ITStatus (line 1490) | ITStatus CAN_GetITStatus(CAN_TypeDef* CANx, uint32_t CAN_IT)
function CAN_ClearITPendingBit (line 1594) | void CAN_ClearITPendingBit(CAN_TypeDef* CANx, uint32_t CAN_IT)
function ITStatus (line 1672) | static ITStatus CheckITStatus(uint32_t CAN_Reg, uint32_t It_Bit)
FILE: Libraries/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_crc.c
function CRC_ResetDR (line 56) | void CRC_ResetDR(void)
function CRC_CalcCRC (line 67) | uint32_t CRC_CalcCRC(uint32_t Data)
function CRC_CalcBlockCRC (line 80) | uint32_t CRC_CalcBlockCRC(uint32_t pBuffer[], uint32_t BufferLength)
function CRC_GetCRC (line 96) | uint32_t CRC_GetCRC(void)
function CRC_SetIDRegister (line 106) | void CRC_SetIDRegister(uint8_t IDValue)
function CRC_GetIDRegister (line 116) | uint8_t CRC_GetIDRegister(void)
FILE: Libraries/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_cryp.c
function CRYP_DeInit (line 219) | void CRYP_DeInit(void)
function CRYP_Init (line 235) | void CRYP_Init(CRYP_InitTypeDef* CRYP_InitStruct)
function CRYP_StructInit (line 274) | void CRYP_StructInit(CRYP_InitTypeDef* CRYP_InitStruct)
function CRYP_KeyInit (line 296) | void CRYP_KeyInit(CRYP_KeyInitTypeDef* CRYP_KeyInitStruct)
function CRYP_KeyStructInit (line 315) | void CRYP_KeyStructInit(CRYP_KeyInitTypeDef* CRYP_KeyInitStruct)
function CRYP_IVInit (line 333) | void CRYP_IVInit(CRYP_IVInitTypeDef* CRYP_IVInitStruct)
function CRYP_IVStructInit (line 347) | void CRYP_IVStructInit(CRYP_IVInitTypeDef* CRYP_IVInitStruct)
function CRYP_PhaseConfig (line 366) | void CRYP_PhaseConfig(uint32_t CRYP_Phase)
function CRYP_FIFOFlush (line 391) | void CRYP_FIFOFlush(void)
function CRYP_Cmd (line 403) | void CRYP_Cmd(FunctionalState NewState)
function CRYP_DataIn (line 446) | void CRYP_DataIn(uint32_t Data)
function CRYP_DataOut (line 456) | uint32_t CRYP_DataOut(void)
function ErrorStatus (line 497) | ErrorStatus CRYP_SaveContext(CRYP_Context* CRYP_ContextSave,
function CRYP_RestoreContext (line 602) | void CRYP_RestoreContext(CRYP_Context* CRYP_ContextRestore)
function CRYP_DMACmd (line 681) | void CRYP_DMACmd(uint8_t CRYP_DMAReq, FunctionalState NewState)
function CRYP_ITConfig (line 799) | void CRYP_ITConfig(uint8_t CRYP_IT, FunctionalState NewState)
function ITStatus (line 827) | ITStatus CRYP_GetITStatus(uint8_t CRYP_IT)
function FunctionalState (line 853) | FunctionalState CRYP_GetCmdStatus(void)
function FlagStatus (line 883) | FlagStatus CRYP_GetFlagStatus(uint8_t CRYP_FLAG)
FILE: Libraries/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_cryp_aes.c
function ErrorStatus (line 106) | ErrorStatus CRYP_AES_ECB(uint8_t Mode, uint8_t* Key, uint16_t Keysize,
function ErrorStatus (line 294) | ErrorStatus CRYP_AES_CBC(uint8_t Mode, uint8_t InitVectors[16], uint8_t ...
function ErrorStatus (line 496) | ErrorStatus CRYP_AES_CTR(uint8_t Mode, uint8_t InitVectors[16], uint8_t ...
function ErrorStatus (line 670) | ErrorStatus CRYP_AES_GCM(uint8_t Mode, uint8_t InitVectors[16],
function ErrorStatus (line 1135) | ErrorStatus CRYP_AES_CCM(uint8_t Mode,
FILE: Libraries/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_cryp_des.c
function ErrorStatus (line 99) | ErrorStatus CRYP_DES_ECB(uint8_t Mode, uint8_t Key[8], uint8_t *Input,
function ErrorStatus (line 200) | ErrorStatus CRYP_DES_CBC(uint8_t Mode, uint8_t Key[8], uint8_t InitVecto...
FILE: Libraries/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_cryp_tdes.c
function ErrorStatus (line 100) | ErrorStatus CRYP_TDES_ECB(uint8_t Mode, uint8_t Key[24], uint8_t *Input,
function ErrorStatus (line 208) | ErrorStatus CRYP_TDES_CBC(uint8_t Mode, uint8_t Key[24], uint8_t InitVec...
FILE: Libraries/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_dac.c
function DAC_DeInit (line 187) | void DAC_DeInit(void)
function DAC_Init (line 206) | void DAC_Init(uint32_t DAC_Channel, DAC_InitTypeDef* DAC_InitStruct)
function DAC_StructInit (line 242) | void DAC_StructInit(DAC_InitTypeDef* DAC_InitStruct)
function DAC_Cmd (line 266) | void DAC_Cmd(uint32_t DAC_Channel, FunctionalState NewState)
function DAC_SoftwareTriggerCmd (line 294) | void DAC_SoftwareTriggerCmd(uint32_t DAC_Channel, FunctionalState NewState)
function DAC_DualSoftwareTriggerCmd (line 318) | void DAC_DualSoftwareTriggerCmd(FunctionalState NewState)
function DAC_WaveGenerationCmd (line 349) | void DAC_WaveGenerationCmd(uint32_t DAC_Channel, uint32_t DAC_Wave, Func...
function DAC_SetChannel1Data (line 378) | void DAC_SetChannel1Data(uint32_t DAC_Align, uint16_t Data)
function DAC_SetChannel2Data (line 403) | void DAC_SetChannel2Data(uint32_t DAC_Align, uint16_t Data)
function DAC_SetDualChannelData (line 431) | void DAC_SetDualChannelData(uint32_t DAC_Align, uint16_t Data2, uint16_t...
function DAC_GetDataOutputValue (line 465) | uint16_t DAC_GetDataOutputValue(uint32_t DAC_Channel)
function DAC_DMACmd (line 510) | void DAC_DMACmd(uint32_t DAC_Channel, FunctionalState NewState)
function DAC_ITConfig (line 558) | void DAC_ITConfig(uint32_t DAC_Channel, uint32_t DAC_IT, FunctionalState...
function FlagStatus (line 590) | FlagStatus DAC_GetFlagStatus(uint32_t DAC_Channel, uint32_t DAC_FLAG)
function DAC_ClearFlag (line 625) | void DAC_ClearFlag(uint32_t DAC_Channel, uint32_t DAC_FLAG)
function ITStatus (line 648) | ITStatus DAC_GetITStatus(uint32_t DAC_Channel, uint32_t DAC_IT)
function DAC_ClearITPendingBit (line 688) | void DAC_ClearITPendingBit(uint32_t DAC_Channel, uint32_t DAC_IT)
FILE: Libraries/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_dbgmcu.c
function DBGMCU_GetREVID (line 58) | uint32_t DBGMCU_GetREVID(void)
function DBGMCU_GetDEVID (line 68) | uint32_t DBGMCU_GetDEVID(void)
function DBGMCU_Config (line 84) | void DBGMCU_Config(uint32_t DBGMCU_Periph, FunctionalState NewState)
function DBGMCU_APB1PeriphConfig (line 123) | void DBGMCU_APB1PeriphConfig(uint32_t DBGMCU_Periph, FunctionalState New...
function DBGMCU_APB2PeriphConfig (line 152) | void DBGMCU_APB2PeriphConfig(uint32_t DBGMCU_Periph, FunctionalState New...
FILE: Libraries/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_dcmi.c
function DCMI_DeInit (line 126) | void DCMI_DeInit(void)
function DCMI_Init (line 143) | void DCMI_Init(DCMI_InitTypeDef* DCMI_InitStruct)
function DCMI_StructInit (line 185) | void DCMI_StructInit(DCMI_InitTypeDef* DCMI_InitStruct)
function DCMI_CROPConfig (line 205) | void DCMI_CROPConfig(DCMI_CROPInitTypeDef* DCMI_CROPInitStruct)
function DCMI_CROPCmd (line 223) | void DCMI_CROPCmd(FunctionalState NewState)
function DCMI_SetEmbeddedSynchroCodes (line 246) | void DCMI_SetEmbeddedSynchroCodes(DCMI_CodesInitTypeDef* DCMI_CodesInitS...
function DCMI_JPEGCmd (line 261) | void DCMI_JPEGCmd(FunctionalState NewState)
function DCMI_Cmd (line 299) | void DCMI_Cmd(FunctionalState NewState)
function DCMI_CaptureCmd (line 322) | void DCMI_CaptureCmd(FunctionalState NewState)
function DCMI_ReadData (line 344) | uint32_t DCMI_ReadData(void)
function DCMI_ITConfig (line 377) | void DCMI_ITConfig(uint16_t DCMI_IT, FunctionalState NewState)
function FlagStatus (line 414) | FlagStatus DCMI_GetFlagStatus(uint16_t DCMI_FLAG)
function DCMI_ClearFlag (line 461) | void DCMI_ClearFlag(uint16_t DCMI_FLAG)
function ITStatus (line 483) | ITStatus DCMI_GetITStatus(uint16_t DCMI_IT)
function DCMI_ClearITPendingBit (line 515) | void DCMI_ClearITPendingBit(uint16_t DCMI_IT)
FILE: Libraries/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_dma.c
function DMA_DeInit (line 196) | void DMA_DeInit(DMA_Stream_TypeDef* DMAy_Streamx)
function DMA_Init (line 319) | void DMA_Init(DMA_Stream_TypeDef* DMAy_Streamx, DMA_InitTypeDef* DMA_Ini...
function DMA_StructInit (line 403) | void DMA_StructInit(DMA_InitTypeDef* DMA_InitStruct)
function DMA_Cmd (line 478) | void DMA_Cmd(DMA_Stream_TypeDef* DMAy_Streamx, FunctionalState NewState)
function DMA_PeriphIncOffsetSizeConfig (line 514) | void DMA_PeriphIncOffsetSizeConfig(DMA_Stream_TypeDef* DMAy_Streamx, uin...
function DMA_FlowControllerConfig (line 550) | void DMA_FlowControllerConfig(DMA_Stream_TypeDef* DMAy_Streamx, uint32_t...
function DMA_SetCurrDataCounter (line 632) | void DMA_SetCurrDataCounter(DMA_Stream_TypeDef* DMAy_Streamx, uint16_t C...
function DMA_GetCurrDataCounter (line 647) | uint16_t DMA_GetCurrDataCounter(DMA_Stream_TypeDef* DMAy_Streamx)
function DMA_DoubleBufferModeConfig (line 730) | void DMA_DoubleBufferModeConfig(DMA_Stream_TypeDef* DMAy_Streamx, uint32...
function DMA_DoubleBufferModeCmd (line 761) | void DMA_DoubleBufferModeCmd(DMA_Stream_TypeDef* DMAy_Streamx, Functiona...
function DMA_MemoryTargetConfig (line 802) | void DMA_MemoryTargetConfig(DMA_Stream_TypeDef* DMAy_Streamx, uint32_t M...
function DMA_GetCurrentMemoryTarget (line 828) | uint32_t DMA_GetCurrentMemoryTarget(DMA_Stream_TypeDef* DMAy_Streamx)
function FunctionalState (line 943) | FunctionalState DMA_GetCmdStatus(DMA_Stream_TypeDef* DMAy_Streamx)
function DMA_GetFIFOStatus (line 977) | uint32_t DMA_GetFIFOStatus(DMA_Stream_TypeDef* DMAy_Streamx)
function FlagStatus (line 1004) | FlagStatus DMA_GetFlagStatus(DMA_Stream_TypeDef* DMAy_Streamx, uint32_t ...
function DMA_ClearFlag (line 1071) | void DMA_ClearFlag(DMA_Stream_TypeDef* DMAy_Streamx, uint32_t DMA_FLAG)
function DMA_ITConfig (line 1118) | void DMA_ITConfig(DMA_Stream_TypeDef* DMAy_Streamx, uint32_t DMA_IT, Fun...
function ITStatus (line 1170) | ITStatus DMA_GetITStatus(DMA_Stream_TypeDef* DMAy_Streamx, uint32_t DMA_IT)
function DMA_ClearITPendingBit (line 1252) | void DMA_ClearITPendingBit(DMA_Stream_TypeDef* DMAy_Streamx, uint32_t DM...
FILE: Libraries/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_dma2d.c
function DMA2D_DeInit (line 105) | void DMA2D_DeInit(void)
function DMA2D_Init (line 122) | void DMA2D_Init(DMA2D_InitTypeDef* DMA2D_InitStruct)
function DMA2D_StructInit (line 211) | void DMA2D_StructInit(DMA2D_InitTypeDef* DMA2D_InitStruct)
function DMA2D_StartTransfer (line 242) | void DMA2D_StartTransfer(void)
function DMA2D_AbortTransfer (line 254) | void DMA2D_AbortTransfer(void)
function DMA2D_Suspend (line 267) | void DMA2D_Suspend(FunctionalState NewState)
function DMA2D_FGConfig (line 292) | void DMA2D_FGConfig(DMA2D_FG_InitTypeDef* DMA2D_FG_InitStruct)
function DMA2D_FG_StructInit (line 344) | void DMA2D_FG_StructInit(DMA2D_FG_InitTypeDef* DMA2D_FG_InitStruct)
function DMA2D_BGConfig (line 389) | void DMA2D_BGConfig(DMA2D_BG_InitTypeDef* DMA2D_BG_InitStruct)
function DMA2D_BG_StructInit (line 442) | void DMA2D_BG_StructInit(DMA2D_BG_InitTypeDef* DMA2D_BG_InitStruct)
function DMA2D_FGStart (line 485) | void DMA2D_FGStart(FunctionalState NewState)
function DMA2D_BGStart (line 509) | void DMA2D_BGStart(FunctionalState NewState)
function DMA2D_DeadTimeConfig (line 532) | void DMA2D_DeadTimeConfig(uint32_t DMA2D_DeadTime, FunctionalState NewSt...
function DMA2D_LineWatermarkConfig (line 559) | void DMA2D_LineWatermarkConfig(uint32_t DMA2D_LWatermarkConfig)
function DMA2D_ITConfig (line 626) | void DMA2D_ITConfig(uint32_t DMA2D_IT, FunctionalState NewState)
function FlagStatus (line 657) | FlagStatus DMA2D_GetFlagStatus(uint32_t DMA2D_FLAG)
function DMA2D_ClearFlag (line 691) | void DMA2D_ClearFlag(uint32_t DMA2D_FLAG)
function ITStatus (line 712) | ITStatus DMA2D_GetITStatus(uint32_t DMA2D_IT)
function DMA2D_ClearITPendingBit (line 752) | void DMA2D_ClearITPendingBit(uint32_t DMA2D_IT)
FILE: Libraries/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_exti.c
function EXTI_DeInit (line 110) | void EXTI_DeInit(void)
function EXTI_Init (line 126) | void EXTI_Init(EXTI_InitTypeDef* EXTI_InitStruct)
function EXTI_StructInit (line 182) | void EXTI_StructInit(EXTI_InitTypeDef* EXTI_InitStruct)
function EXTI_GenerateSWInterrupt (line 197) | void EXTI_GenerateSWInterrupt(uint32_t EXTI_Line)
function FlagStatus (line 227) | FlagStatus EXTI_GetFlagStatus(uint32_t EXTI_Line)
function EXTI_ClearFlag (line 250) | void EXTI_ClearFlag(uint32_t EXTI_Line)
function ITStatus (line 264) | ITStatus EXTI_GetITStatus(uint32_t EXTI_Line)
function EXTI_ClearITPendingBit (line 288) | void EXTI_ClearITPendingBit(uint32_t EXTI_Line)
FILE: Libraries/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_flash.c
function FLASH_SetLatency (line 248) | void FLASH_SetLatency(uint32_t FLASH_Latency)
function FLASH_PrefetchBufferCmd (line 263) | void FLASH_PrefetchBufferCmd(FunctionalState NewState)
function FLASH_InstructionCacheCmd (line 285) | void FLASH_InstructionCacheCmd(FunctionalState NewState)
function FLASH_DataCacheCmd (line 306) | void FLASH_DataCacheCmd(FunctionalState NewState)
function FLASH_InstructionCacheReset (line 327) | void FLASH_InstructionCacheReset(void)
function FLASH_DataCacheReset (line 338) | void FLASH_DataCacheReset(void)
function FLASH_Unlock (line 385) | void FLASH_Unlock(void)
function FLASH_Lock (line 400) | void FLASH_Lock(void)
function FLASH_Status (line 437) | FLASH_Status FLASH_EraseSector(uint32_t FLASH_Sector, uint8_t VoltageRange)
function FLASH_Status (line 505) | FLASH_Status FLASH_EraseAllSectors(uint8_t VoltageRange)
function FLASH_Status (line 586) | FLASH_Status FLASH_EraseAllBank1Sectors(uint8_t VoltageRange)
function FLASH_Status (line 653) | FLASH_Status FLASH_EraseAllBank2Sectors(uint8_t VoltageRange)
function FLASH_Status (line 710) | FLASH_Status FLASH_ProgramDoubleWord(uint32_t Address, uint64_t Data)
function FLASH_Status (line 753) | FLASH_Status FLASH_ProgramWord(uint32_t Address, uint32_t Data)
function FLASH_Status (line 795) | FLASH_Status FLASH_ProgramHalfWord(uint32_t Address, uint16_t Data)
function FLASH_Status (line 837) | FLASH_Status FLASH_ProgramByte(uint32_t Address, uint8_t Data)
function FLASH_OB_Unlock (line 936) | void FLASH_OB_Unlock(void)
function FLASH_OB_Lock (line 951) | void FLASH_OB_Lock(void)
function FLASH_OB_WRPConfig (line 974) | void FLASH_OB_WRPConfig(uint32_t OB_WRP, FunctionalState NewState)
function FLASH_OB_WRP1Config (line 1016) | void FLASH_OB_WRP1Config(uint32_t OB_WRP, FunctionalState NewState)
function FLASH_OB_PCROPSelectionConfig (line 1066) | void FLASH_OB_PCROPSelectionConfig(uint8_t OB_PcROP)
function FLASH_OB_PCROPConfig (line 1096) | void FLASH_OB_PCROPConfig(uint32_t OB_PCROP, FunctionalState NewState)
function FLASH_OB_PCROP1Config (line 1133) | void FLASH_OB_PCROP1Config(uint32_t OB_PCROP, FunctionalState NewState)
function FLASH_OB_RDPConfig (line 1169) | void FLASH_OB_RDPConfig(uint8_t OB_RDP)
function FLASH_OB_UserConfig (line 1201) | void FLASH_OB_UserConfig(uint8_t OB_IWDG, uint8_t OB_STOP, uint8_t OB_ST...
function FLASH_OB_BootConfig (line 1242) | void FLASH_OB_BootConfig(uint8_t OB_BOOT)
function FLASH_OB_BORConfig (line 1263) | void FLASH_OB_BORConfig(uint8_t OB_BOR)
function FLASH_Status (line 1280) | FLASH_Status FLASH_OB_Launch(void)
function FLASH_OB_GetUser (line 1299) | uint8_t FLASH_OB_GetUser(void)
function FLASH_OB_GetWRP (line 1310) | uint16_t FLASH_OB_GetWRP(void)
function FLASH_OB_GetWRP1 (line 1324) | uint16_t FLASH_OB_GetWRP1(void)
function FLASH_OB_GetPCROP (line 1338) | uint16_t FLASH_OB_GetPCROP(void)
function FLASH_OB_GetPCROP1 (line 1352) | uint16_t FLASH_OB_GetPCROP1(void)
function FlagStatus (line 1365) | FlagStatus FLASH_OB_GetRDP(void)
function FLASH_OB_GetBOR (line 1389) | uint8_t FLASH_OB_GetBOR(void)
function FLASH_ITConfig (line 1418) | void FLASH_ITConfig(uint32_t FLASH_IT, FunctionalState NewState)
function FlagStatus (line 1450) | FlagStatus FLASH_GetFlagStatus(uint32_t FLASH_FLAG)
function FLASH_ClearFlag (line 1481) | void FLASH_ClearFlag(uint32_t FLASH_FLAG)
function FLASH_Status (line 1496) | FLASH_Status FLASH_GetStatus(void)
function FLASH_Status (line 1546) | FLASH_Status FLASH_WaitForLastOperation(void)
FILE: Libraries/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_fmc.c
function FMC_NORSRAMDeInit (line 130) | void FMC_NORSRAMDeInit(uint32_t FMC_Bank)
function FMC_NORSRAMInit (line 157) | void FMC_NORSRAMInit(FMC_NORSRAMInitTypeDef* FMC_NORSRAMInitStruct)
function FMC_NORSRAMStructInit (line 257) | void FMC_NORSRAMStructInit(FMC_NORSRAMInitTypeDef* FMC_NORSRAMInitStruct)
function FMC_NORSRAMCmd (line 302) | void FMC_NORSRAMCmd(uint32_t FMC_Bank, FunctionalState NewState)
function FMC_NANDDeInit (line 375) | void FMC_NANDDeInit(uint32_t FMC_Bank)
function FMC_NANDInit (line 406) | void FMC_NANDInit(FMC_NANDInitTypeDef* FMC_NANDInitStruct)
function FMC_NANDStructInit (line 471) | void FMC_NANDStructInit(FMC_NANDInitTypeDef* FMC_NANDInitStruct)
function FMC_NANDCmd (line 500) | void FMC_NANDCmd(uint32_t FMC_Bank, FunctionalState NewState)
function FMC_NANDECCCmd (line 540) | void FMC_NANDECCCmd(uint32_t FMC_Bank, FunctionalState NewState)
function FMC_GetECC (line 579) | uint32_t FMC_GetECC(uint32_t FMC_Bank)
function FMC_PCCARDDeInit (line 643) | void FMC_PCCARDDeInit(void)
function FMC_PCCARDInit (line 660) | void FMC_PCCARDInit(FMC_PCCARDInitTypeDef* FMC_PCCARDInitStruct)
function FMC_PCCARDStructInit (line 712) | void FMC_PCCARDStructInit(FMC_PCCARDInitTypeDef* FMC_PCCARDInitStruct)
function FMC_PCCARDCmd (line 738) | void FMC_PCCARDCmd(FunctionalState NewState)
function FMC_SDRAMDeInit (line 810) | void FMC_SDRAMDeInit(uint32_t FMC_Bank)
function FMC_SDRAMInit (line 830) | void FMC_SDRAMInit(FMC_SDRAMInitTypeDef* FMC_SDRAMInitStruct)
function FMC_SDRAMStructInit (line 920) | void FMC_SDRAMStructInit(FMC_SDRAMInitTypeDef* FMC_SDRAMInitStruct)
function FMC_SDRAMCmdConfig (line 950) | void FMC_SDRAMCmdConfig(FMC_SDRAMCommandTypeDef* FMC_SDRAMCommandStruct)
function FMC_GetModeStatus (line 976) | uint32_t FMC_GetModeStatus(uint32_t SDRAM_Bank)
function FMC_SetRefreshCount (line 1002) | void FMC_SetRefreshCount(uint32_t FMC_Count)
function FMC_SetAutoRefresh_Number (line 1016) | void FMC_SetAutoRefresh_Number(uint32_t FMC_Number)
function FMC_SDRAMWriteProtectionConfig (line 1032) | void FMC_SDRAMWriteProtectionConfig(uint32_t SDRAM_Bank, FunctionalState...
function FMC_ITConfig (line 1084) | void FMC_ITConfig(uint32_t FMC_Bank, uint32_t FMC_IT, FunctionalState Ne...
function FlagStatus (line 1161) | FlagStatus FMC_GetFlagStatus(uint32_t FMC_Bank, uint32_t FMC_FLAG)
function FMC_ClearFlag (line 1217) | void FMC_ClearFlag(uint32_t FMC_Bank, uint32_t FMC_FLAG)
function ITStatus (line 1260) | ITStatus FMC_GetITStatus(uint32_t FMC_Bank, uint32_t FMC_IT)
function FMC_ClearITPendingBit (line 1332) | void FMC_ClearITPendingBit(uint32_t FMC_Bank, uint32_t FMC_IT)
FILE: Libraries/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_fsmc.c
function FSMC_NORSRAMDeInit (line 121) | void FSMC_NORSRAMDeInit(uint32_t FSMC_Bank)
function FSMC_NORSRAMInit (line 148) | void FSMC_NORSRAMInit(FSMC_NORSRAMInitTypeDef* FSMC_NORSRAMInitStruct)
function FSMC_NORSRAMStructInit (line 230) | void FSMC_NORSRAMStructInit(FSMC_NORSRAMInitTypeDef* FSMC_NORSRAMInitStr...
function FSMC_NORSRAMCmd (line 273) | void FSMC_NORSRAMCmd(uint32_t FSMC_Bank, FunctionalState NewState)
function FSMC_NANDDeInit (line 346) | void FSMC_NANDDeInit(uint32_t FSMC_Bank)
function FSMC_NANDInit (line 377) | void FSMC_NANDInit(FSMC_NANDInitTypeDef* FSMC_NANDInitStruct)
function FSMC_NANDStructInit (line 442) | void FSMC_NANDStructInit(FSMC_NANDInitTypeDef* FSMC_NANDInitStruct)
function FSMC_NANDCmd (line 471) | void FSMC_NANDCmd(uint32_t FSMC_Bank, FunctionalState NewState)
function FSMC_NANDECCCmd (line 511) | void FSMC_NANDECCCmd(uint32_t FSMC_Bank, FunctionalState NewState)
function FSMC_GetECC (line 550) | uint32_t FSMC_GetECC(uint32_t FSMC_Bank)
function FSMC_PCCARDDeInit (line 614) | void FSMC_PCCARDDeInit(void)
function FSMC_PCCARDInit (line 631) | void FSMC_PCCARDInit(FSMC_PCCARDInitTypeDef* FSMC_PCCARDInitStruct)
function FSMC_PCCARDStructInit (line 683) | void FSMC_PCCARDStructInit(FSMC_PCCARDInitTypeDef* FSMC_PCCARDInitStruct)
function FSMC_PCCARDCmd (line 709) | void FSMC_PCCARDCmd(FunctionalState NewState)
function FSMC_ITConfig (line 756) | void FSMC_ITConfig(uint32_t FSMC_Bank, uint32_t FSMC_IT, FunctionalState...
function FlagStatus (line 816) | FlagStatus FSMC_GetFlagStatus(uint32_t FSMC_Bank, uint32_t FSMC_FLAG)
function FSMC_ClearFlag (line 866) | void FSMC_ClearFlag(uint32_t FSMC_Bank, uint32_t FSMC_FLAG)
function ITStatus (line 901) | ITStatus FSMC_GetITStatus(uint32_t FSMC_Bank, uint32_t FSMC_IT)
function FSMC_ClearITPendingBit (line 952) | void FSMC_ClearITPendingBit(uint32_t FSMC_Bank, uint32_t FSMC_IT)
FILE: Libraries/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_gpio.c
function GPIO_DeInit (line 127) | void GPIO_DeInit(GPIO_TypeDef* GPIOx)
function GPIO_Init (line 202) | void GPIO_Init(GPIO_TypeDef* GPIOx, GPIO_InitTypeDef* GPIO_InitStruct)
function GPIO_StructInit (line 254) | void GPIO_StructInit(GPIO_InitTypeDef* GPIO_InitStruct)
function GPIO_PinLockConfig (line 277) | void GPIO_PinLockConfig(GPIO_TypeDef* GPIOx, uint16_t GPIO_Pin)
function GPIO_ReadInputDataBit (line 323) | uint8_t GPIO_ReadInputDataBit(GPIO_TypeDef* GPIOx, uint16_t GPIO_Pin)
function GPIO_ReadInputData (line 349) | uint16_t GPIO_ReadInputData(GPIO_TypeDef* GPIOx)
function GPIO_ReadOutputDataBit (line 366) | uint8_t GPIO_ReadOutputDataBit(GPIO_TypeDef* GPIOx, uint16_t GPIO_Pin)
function GPIO_ReadOutputData (line 392) | uint16_t GPIO_ReadOutputData(GPIO_TypeDef* GPIOx)
function GPIO_SetBits (line 412) | void GPIO_SetBits(GPIO_TypeDef* GPIOx, uint16_t GPIO_Pin)
function GPIO_ResetBits (line 433) | void GPIO_ResetBits(GPIO_TypeDef* GPIOx, uint16_t GPIO_Pin)
function GPIO_WriteBit (line 455) | void GPIO_WriteBit(GPIO_TypeDef* GPIOx, uint16_t GPIO_Pin, BitAction Bit...
function GPIO_Write (line 480) | void GPIO_Write(GPIO_TypeDef* GPIOx, uint16_t PortVal)
function GPIO_ToggleBits (line 496) | void GPIO_ToggleBits(GPIO_TypeDef* GPIOx, uint16_t GPIO_Pin)
function GPIO_PinAFConfig (line 579) | void GPIO_PinAFConfig(GPIO_TypeDef* GPIOx, uint16_t GPIO_PinSource, uint...
FILE: Libraries/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_hash.c
function HASH_DeInit (line 171) | void HASH_DeInit(void)
function HASH_Init (line 191) | void HASH_Init(HASH_InitTypeDef* HASH_InitStruct)
function HASH_StructInit (line 225) | void HASH_StructInit(HASH_InitTypeDef* HASH_InitStruct)
function HASH_Reset (line 249) | void HASH_Reset(void)
function HASH_SetLastWordValidBitsNbr (line 291) | void HASH_SetLastWordValidBitsNbr(uint16_t ValidNumber)
function HASH_DataIn (line 306) | void HASH_DataIn(uint32_t Data)
function HASH_GetInFIFOWordsNbr (line 317) | uint8_t HASH_GetInFIFOWordsNbr(void)
function HASH_GetDigest (line 335) | void HASH_GetDigest(HASH_MsgDigest* HASH_MessageDigest)
function HASH_StartDigest (line 353) | void HASH_StartDigest(void)
function HASH_SaveContext (line 396) | void HASH_SaveContext(HASH_Context* HASH_ContextSave)
function HASH_RestoreContext (line 418) | void HASH_RestoreContext(HASH_Context* HASH_ContextRestore)
function HASH_AutoStartDigest (line 465) | void HASH_AutoStartDigest(FunctionalState NewState)
function HASH_DMACmd (line 489) | void HASH_DMACmd(FunctionalState NewState)
function HASH_ITConfig (line 581) | void HASH_ITConfig(uint32_t HASH_IT, FunctionalState NewState)
function FlagStatus (line 610) | FlagStatus HASH_GetFlagStatus(uint32_t HASH_FLAG)
function HASH_ClearFlag (line 651) | void HASH_ClearFlag(uint32_t HASH_FLAG)
function ITStatus (line 667) | ITStatus HASH_GetITStatus(uint32_t HASH_IT)
function HASH_ClearITPendingBit (line 701) | void HASH_ClearITPendingBit(uint32_t HASH_IT)
FILE: Libraries/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_hash_md5.c
function ErrorStatus (line 93) | ErrorStatus HASH_MD5(uint8_t *Input, uint32_t Ilen, uint8_t Output[16])
function ErrorStatus (line 168) | ErrorStatus HMAC_MD5(uint8_t *Key, uint32_t Keylen, uint8_t *Input,
FILE: Libraries/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_hash_sha1.c
function ErrorStatus (line 93) | ErrorStatus HASH_SHA1(uint8_t *Input, uint32_t Ilen, uint8_t Output[20])
function ErrorStatus (line 169) | ErrorStatus HMAC_SHA1(uint8_t *Key, uint32_t Keylen, uint8_t *Input,
FILE: Libraries/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_i2c.c
function I2C_DeInit (line 137) | void I2C_DeInit(I2C_TypeDef* I2Cx)
function I2C_Init (line 180) | void I2C_Init(I2C_TypeDef* I2Cx, I2C_InitTypeDef* I2C_InitStruct)
function I2C_StructInit (line 289) | void I2C_StructInit(I2C_InitTypeDef* I2C_InitStruct)
function I2C_Cmd (line 313) | void I2C_Cmd(I2C_TypeDef* I2Cx, FunctionalState NewState)
function I2C_AnalogFilterCmd (line 342) | void I2C_AnalogFilterCmd(I2C_TypeDef* I2Cx, FunctionalState NewState)
function I2C_DigitalFilterConfig (line 371) | void I2C_DigitalFilterConfig(I2C_TypeDef* I2Cx, uint16_t I2C_DigitalFilter)
function I2C_GenerateSTART (line 399) | void I2C_GenerateSTART(I2C_TypeDef* I2Cx, FunctionalState NewState)
function I2C_GenerateSTOP (line 423) | void I2C_GenerateSTOP(I2C_TypeDef* I2Cx, FunctionalState NewState)
function I2C_Send7bitAddress (line 451) | void I2C_Send7bitAddress(I2C_TypeDef* I2Cx, uint8_t Address, uint8_t I2C...
function I2C_AcknowledgeConfig (line 478) | void I2C_AcknowledgeConfig(I2C_TypeDef* I2Cx, FunctionalState NewState)
function I2C_OwnAddress2Config (line 501) | void I2C_OwnAddress2Config(I2C_TypeDef* I2Cx, uint8_t Address)
function I2C_DualAddressCmd (line 528) | void I2C_DualAddressCmd(I2C_TypeDef* I2Cx, FunctionalState NewState)
function I2C_GeneralCallCmd (line 552) | void I2C_GeneralCallCmd(I2C_TypeDef* I2Cx, FunctionalState NewState)
function I2C_SoftwareResetCmd (line 578) | void I2C_SoftwareResetCmd(I2C_TypeDef* I2Cx, FunctionalState NewState)
function I2C_StretchClockCmd (line 602) | void I2C_StretchClockCmd(I2C_TypeDef* I2Cx, FunctionalState NewState)
function I2C_FastModeDutyCycleConfig (line 628) | void I2C_FastModeDutyCycleConfig(I2C_TypeDef* I2Cx, uint16_t I2C_DutyCycle)
function I2C_NACKPositionConfig (line 666) | void I2C_NACKPositionConfig(I2C_TypeDef* I2Cx, uint16_t I2C_NACKPosition)
function I2C_SMBusAlertConfig (line 694) | void I2C_SMBusAlertConfig(I2C_TypeDef* I2Cx, uint16_t I2C_SMBusAlert)
function I2C_ARPCmd (line 718) | void I2C_ARPCmd(I2C_TypeDef* I2Cx, FunctionalState NewState)
function I2C_SendData (line 756) | void I2C_SendData(I2C_TypeDef* I2Cx, uint8_t Data)
function I2C_ReceiveData (line 769) | uint8_t I2C_ReceiveData(I2C_TypeDef* I2Cx)
function I2C_TransmitPEC (line 800) | void I2C_TransmitPEC(I2C_TypeDef* I2Cx, FunctionalState NewState)
function I2C_PECPositionConfig (line 831) | void I2C_PECPositionConfig(I2C_TypeDef* I2Cx, uint16_t I2C_PECPosition)
function I2C_CalculatePEC (line 855) | void I2C_CalculatePEC(I2C_TypeDef* I2Cx, FunctionalState NewState)
function I2C_GetPEC (line 877) | uint8_t I2C_GetPEC(I2C_TypeDef* I2Cx)
function I2C_DMACmd (line 910) | void I2C_DMACmd(I2C_TypeDef* I2Cx, FunctionalState NewState)
function I2C_DMALastTransferCmd (line 934) | void I2C_DMALastTransferCmd(I2C_TypeDef* I2Cx, FunctionalState NewState)
function I2C_ReadRegister (line 1072) | uint16_t I2C_ReadRegister(I2C_TypeDef* I2Cx, uint8_t I2C_Register)
function I2C_ITConfig (line 1099) | void I2C_ITConfig(I2C_TypeDef* I2Cx, uint16_t I2C_IT, FunctionalState Ne...
function ErrorStatus (line 1158) | ErrorStatus I2C_CheckEvent(I2C_TypeDef* I2Cx, uint32_t I2C_EVENT)
function I2C_GetLastEvent (line 1206) | uint32_t I2C_GetLastEvent(I2C_TypeDef* I2Cx)
function FlagStatus (line 1261) | FlagStatus I2C_GetFlagStatus(I2C_TypeDef* I2Cx, uint32_t I2C_FLAG)
function I2C_ClearFlag (line 1338) | void I2C_ClearFlag(I2C_TypeDef* I2Cx, uint32_t I2C_FLAG)
function ITStatus (line 1372) | ITStatus I2C_GetITStatus(I2C_TypeDef* I2Cx, uint32_t I2C_IT)
function I2C_ClearITPendingBit (line 1432) | void I2C_ClearITPendingBit(I2C_TypeDef* I2Cx, uint32_t I2C_IT)
FILE: Libraries/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_iwdg.c
function IWDG_WriteAccessCmd (line 132) | void IWDG_WriteAccessCmd(uint16_t IWDG_WriteAccess)
function IWDG_SetPrescaler (line 152) | void IWDG_SetPrescaler(uint8_t IWDG_Prescaler)
function IWDG_SetReload (line 165) | void IWDG_SetReload(uint16_t Reload)
function IWDG_ReloadCounter (line 178) | void IWDG_ReloadCounter(void)
function IWDG_Enable (line 204) | void IWDG_Enable(void)
function FlagStatus (line 233) | FlagStatus IWDG_GetFlagStatus(uint16_t IWDG_FLAG)
FILE: Libraries/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_ltdc.c
function LTDC_DeInit (line 123) | void LTDC_DeInit(void)
function LTDC_Init (line 140) | void LTDC_Init(LTDC_InitTypeDef* LTDC_InitStruct)
function LTDC_StructInit (line 205) | void LTDC_StructInit(LTDC_InitTypeDef* LTDC_InitStruct)
function LTDC_Cmd (line 232) | void LTDC_Cmd(FunctionalState NewState)
function LTDC_DitherCmd (line 256) | void LTDC_DitherCmd(FunctionalState NewState)
function LTDC_RGBTypeDef (line 280) | LTDC_RGBTypeDef LTDC_GetRGBWidth(void)
function LTDC_RGBStructInit (line 300) | void LTDC_RGBStructInit(LTDC_RGBTypeDef* LTDC_RGB_InitStruct)
function LTDC_LIPConfig (line 314) | void LTDC_LIPConfig(uint32_t LTDC_LIPositionConfig)
function LTDC_ReloadConfig (line 332) | void LTDC_ReloadConfig(uint32_t LTDC_Reload)
function LTDC_LayerInit (line 353) | void LTDC_LayerInit(LTDC_Layer_TypeDef* LTDC_Layerx, LTDC_Layer_InitType...
function LTDC_LayerStructInit (line 431) | void LTDC_LayerStructInit(LTDC_Layer_InitTypeDef * LTDC_Layer_InitStruct)
function LTDC_LayerCmd (line 480) | void LTDC_LayerCmd(LTDC_Layer_TypeDef* LTDC_Layerx, FunctionalState NewS...
function LTDC_PosTypeDef (line 505) | LTDC_PosTypeDef LTDC_GetPosStatus(void)
function LTDC_PosStructInit (line 524) | void LTDC_PosStructInit(LTDC_PosTypeDef* LTDC_Pos_InitStruct)
function FlagStatus (line 541) | FlagStatus LTDC_GetCDStatus(uint32_t LTDC_CD)
function LTDC_ColorKeyingConfig (line 568) | void LTDC_ColorKeyingConfig(LTDC_Layer_TypeDef* LTDC_Layerx, LTDC_ColorK...
function LTDC_ColorKeyingStructInit (line 607) | void LTDC_ColorKeyingStructInit(LTDC_ColorKeying_InitTypeDef* LTDC_color...
function LTDC_CLUTCmd (line 625) | void LTDC_CLUTCmd(LTDC_Layer_TypeDef* LTDC_Layerx, FunctionalState NewSt...
function LTDC_CLUTInit (line 654) | void LTDC_CLUTInit(LTDC_Layer_TypeDef* LTDC_Layerx, LTDC_CLUT_InitTypeDe...
function LTDC_CLUTStructInit (line 681) | void LTDC_CLUTStructInit(LTDC_CLUT_InitTypeDef* LTDC_CLUT_InitStruct)
function LTDC_LayerPosition (line 701) | void LTDC_LayerPosition(LTDC_Layer_TypeDef* LTDC_Layerx, uint16_t Offset...
function LTDC_LayerAlpha (line 762) | void LTDC_LayerAlpha(LTDC_Layer_TypeDef* LTDC_Layerx, uint8_t ConstantAl...
function LTDC_LayerAddress (line 777) | void LTDC_LayerAddress(LTDC_Layer_TypeDef* LTDC_Layerx, uint32_t Address)
function LTDC_LayerSize (line 793) | void LTDC_LayerSize(LTDC_Layer_TypeDef* LTDC_Layerx, uint32_t Width, uin...
function LTDC_LayerPixelFormat (line 855) | void LTDC_LayerPixelFormat(LTDC_Layer_TypeDef* LTDC_Layerx, uint32_t Pix...
function LTDC_ITConfig (line 969) | void LTDC_ITConfig(uint32_t LTDC_IT, FunctionalState NewState)
function FlagStatus (line 995) | FlagStatus LTDC_GetFlagStatus(uint32_t LTDC_FLAG)
function LTDC_ClearFlag (line 1023) | void LTDC_ClearFlag(uint32_t LTDC_FLAG)
function ITStatus (line 1042) | ITStatus LTDC_GetITStatus(uint32_t LTDC_IT)
function LTDC_ClearITPendingBit (line 1080) | void LTDC_ClearITPendingBit(uint32_t LTDC_IT)
FILE: Libraries/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_pwr.c
function PWR_DeInit (line 134) | void PWR_DeInit(void)
function PWR_BackupAccessCmd (line 149) | void PWR_BackupAccessCmd(FunctionalState NewState)
function PWR_PVDLevelConfig (line 197) | void PWR_PVDLevelConfig(uint32_t PWR_PVDLevel)
function PWR_PVDCmd (line 222) | void PWR_PVDCmd(FunctionalState NewState)
function PWR_WakeUpPinCmd (line 256) | void PWR_WakeUpPinCmd(FunctionalState NewState)
function PWR_BackupRegulatorCmd (line 353) | void PWR_BackupRegulatorCmd(FunctionalState NewState)
function PWR_MainRegulatorModeConfig (line 375) | void PWR_MainRegulatorModeConfig(uint32_t PWR_Regulator_Voltage)
function PWR_OverDriveCmd (line 410) | void PWR_OverDriveCmd(FunctionalState NewState)
function PWR_OverDriveSWCmd (line 428) | void PWR_OverDriveSWCmd(FunctionalState NewState)
function PWR_UnderDriveCmd (line 455) | void PWR_UnderDriveCmd(FunctionalState NewState)
function PWR_FlashPowerDownCmd (line 499) | void PWR_FlashPowerDownCmd(FunctionalState NewState)
function PWR_EnterSTOPMode (line 645) | void PWR_EnterSTOPMode(uint32_t PWR_Regulator, uint8_t PWR_STOPEntry)
function PWR_EnterUnderDriveSTOPMode (line 709) | void PWR_EnterUnderDriveSTOPMode(uint32_t PWR_Regulator, uint8_t PWR_STO...
function PWR_EnterSTANDBYMode (line 757) | void PWR_EnterSTANDBYMode(void)
function FlagStatus (line 820) | FlagStatus PWR_GetFlagStatus(uint32_t PWR_FLAG)
function PWR_ClearFlag (line 848) | void PWR_ClearFlag(uint32_t PWR_FLAG)
FILE: Libraries/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_rcc.c
function RCC_DeInit (line 213) | void RCC_DeInit(void)
function RCC_HSEConfig (line 264) | void RCC_HSEConfig(uint8_t RCC_HSE)
function ErrorStatus (line 288) | ErrorStatus RCC_WaitForHSEStartUp(void)
function RCC_AdjustHSICalibrationValue (line 319) | void RCC_AdjustHSICalibrationValue(uint8_t HSICalibrationValue)
function RCC_HSICmd (line 355) | void RCC_HSICmd(FunctionalState NewState)
function RCC_LSEConfig (line 380) | void RCC_LSEConfig(uint8_t RCC_LSE)
function RCC_LSICmd (line 420) | void RCC_LSICmd(FunctionalState NewState)
function RCC_PLLConfig (line 463) | void RCC_PLLConfig(uint32_t RCC_PLLSource, uint32_t PLLM, uint32_t PLLN,...
function RCC_PLLCmd (line 486) | void RCC_PLLCmd(FunctionalState NewState)
function RCC_PLLI2SConfig (line 516) | void RCC_PLLI2SConfig(uint32_t PLLI2SN, uint32_t PLLI2SR)
function RCC_PLLI2SConfig (line 552) | void RCC_PLLI2SConfig(uint32_t PLLI2SN, uint32_t PLLI2SQ, uint32_t PLLI2SR)
function RCC_PLLI2SCmd (line 569) | void RCC_PLLI2SCmd(FunctionalState NewState)
function RCC_PLLSAIConfig (line 598) | void RCC_PLLSAIConfig(uint32_t PLLSAIN, uint32_t PLLSAIQ, uint32_t PLLSAIR)
function RCC_PLLSAICmd (line 616) | void RCC_PLLSAICmd(FunctionalState NewState)
function RCC_ClockSecuritySystemCmd (line 634) | void RCC_ClockSecuritySystemCmd(FunctionalState NewState)
function RCC_MCO1Config (line 659) | void RCC_MCO1Config(uint32_t RCC_MCO1Source, uint32_t RCC_MCO1Div)
function RCC_MCO2Config (line 697) | void RCC_MCO2Config(uint32_t RCC_MCO2Source, uint32_t RCC_MCO2Div)
function RCC_SYSCLKConfig (line 863) | void RCC_SYSCLKConfig(uint32_t RCC_SYSCLKSource)
function RCC_GetSYSCLKSource (line 891) | uint8_t RCC_GetSYSCLKSource(void)
function RCC_HCLKConfig (line 916) | void RCC_HCLKConfig(uint32_t RCC_SYSCLK)
function RCC_PCLK1Config (line 948) | void RCC_PCLK1Config(uint32_t RCC_HCLK)
function RCC_PCLK2Config (line 979) | void RCC_PCLK2Config(uint32_t RCC_HCLK)
function RCC_GetClocksFreq (line 1031) | void RCC_GetClocksFreq(RCC_ClocksTypeDef* RCC_Clocks)
function RCC_RTCCLKConfig (line 1156) | void RCC_RTCCLKConfig(uint32_t RCC_RTCCLKSource)
function RCC_RTCCLKCmd (line 1188) | void RCC_RTCCLKCmd(FunctionalState NewState)
function RCC_BackupResetCmd (line 1205) | void RCC_BackupResetCmd(FunctionalState NewState)
function RCC_I2SCLKConfig (line 1222) | void RCC_I2SCLKConfig(uint32_t RCC_I2SCLKSource)
function RCC_SAIPLLI2SClkDivConfig (line 1243) | void RCC_SAIPLLI2SClkDivConfig(uint32_t RCC_PLLI2SDivQ)
function RCC_SAIPLLSAIClkDivConfig (line 1275) | void RCC_SAIPLLSAIClkDivConfig(uint32_t RCC_PLLSAIDivQ)
function RCC_SAIBlockACLKConfig (line 1311) | void RCC_SAIBlockACLKConfig(uint32_t RCC_SAIBlockACLKSource)
function RCC_SAIBlockBCLKConfig (line 1347) | void RCC_SAIBlockBCLKConfig(uint32_t RCC_SAIBlockBCLKSource)
function RCC_LTDCCLKDivConfig (line 1380) | void RCC_LTDCCLKDivConfig(uint32_t RCC_PLLSAIDivR)
function RCC_TIMCLKPresConfig (line 1417) | void RCC_TIMCLKPresConfig(uint32_t RCC_TIMCLKPrescaler)
function RCC_AHB1PeriphClockCmd (line 1460) | void RCC_AHB1PeriphClockCmd(uint32_t RCC_AHB1Periph, FunctionalState New...
function RCC_AHB2PeriphClockCmd (line 1492) | void RCC_AHB2PeriphClockCmd(uint32_t RCC_AHB2Periph, FunctionalState New...
function RCC_AHB3PeriphClockCmd (line 1520) | void RCC_AHB3PeriphClockCmd(uint32_t RCC_AHB3Periph, FunctionalState New...
function RCC_APB1PeriphClockCmd (line 1572) | void RCC_APB1PeriphClockCmd(uint32_t RCC_APB1Periph, FunctionalState New...
function RCC_APB2PeriphClockCmd (line 1617) | void RCC_APB2PeriphClockCmd(uint32_t RCC_APB2Periph, FunctionalState New...
function RCC_AHB1PeriphResetCmd (line 1659) | void RCC_AHB1PeriphResetCmd(uint32_t RCC_AHB1Periph, FunctionalState New...
function RCC_AHB2PeriphResetCmd (line 1688) | void RCC_AHB2PeriphResetCmd(uint32_t RCC_AHB2Periph, FunctionalState New...
function RCC_AHB3PeriphResetCmd (line 1713) | void RCC_AHB3PeriphResetCmd(uint32_t RCC_AHB3Periph, FunctionalState New...
function RCC_APB1PeriphResetCmd (line 1762) | void RCC_APB1PeriphResetCmd(uint32_t RCC_APB1Periph, FunctionalState New...
function RCC_APB2PeriphResetCmd (line 1803) | void RCC_APB2PeriphResetCmd(uint32_t RCC_APB2Periph, FunctionalState New...
function RCC_AHB1PeriphClockLPModeCmd (line 1852) | void RCC_AHB1PeriphClockLPModeCmd(uint32_t RCC_AHB1Periph, FunctionalSta...
function RCC_AHB2PeriphClockLPModeCmd (line 1884) | void RCC_AHB2PeriphClockLPModeCmd(uint32_t RCC_AHB2Periph, FunctionalSta...
function RCC_AHB3PeriphClockLPModeCmd (line 1912) | void RCC_AHB3PeriphClockLPModeCmd(uint32_t RCC_AHB3Periph, FunctionalSta...
function RCC_APB1PeriphClockLPModeCmd (line 1964) | void RCC_APB1PeriphClockLPModeCmd(uint32_t RCC_APB1Periph, FunctionalSta...
function RCC_APB2PeriphClockLPModeCmd (line 2009) | void RCC_APB2PeriphClockLPModeCmd(uint32_t RCC_APB2Periph, FunctionalSta...
function RCC_ITConfig (line 2055) | void RCC_ITConfig(uint8_t RCC_IT, FunctionalState NewState)
function FlagStatus (line 2092) | FlagStatus RCC_GetFlagStatus(uint8_t RCC_FLAG)
function RCC_ClearFlag (line 2137) | void RCC_ClearFlag(void)
function ITStatus (line 2157) | ITStatus RCC_GetITStatus(uint8_t RCC_IT)
function RCC_ClearITPendingBit (line 2191) | void RCC_ClearITPendingBit(uint8_t RCC_IT)
FILE: Libraries/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_rng.c
function RNG_DeInit (line 99) | void RNG_DeInit(void)
function RNG_Cmd (line 114) | void RNG_Cmd(FunctionalState NewState)
function RNG_GetRandomNumber (line 176) | uint32_t RNG_GetRandomNumber(void)
function RNG_ITConfig (line 267) | void RNG_ITConfig(FunctionalState NewState)
function FlagStatus (line 293) | FlagStatus RNG_GetFlagStatus(uint8_t RNG_FLAG)
function RNG_ClearFlag (line 326) | void RNG_ClearFlag(uint8_t RNG_FLAG)
function ITStatus (line 342) | ITStatus RNG_GetITStatus(uint8_t RNG_IT)
function RNG_ClearITPendingBit (line 372) | void RNG_ClearITPendingBit(uint8_t RNG_IT)
FILE: Libraries/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_rtc.c
function ErrorStatus (line 375) | ErrorStatus RTC_DeInit(void)
function ErrorStatus (line 457) | ErrorStatus RTC_Init(RTC_InitTypeDef* RTC_InitStruct)
function RTC_StructInit (line 503) | void RTC_StructInit(RTC_InitTypeDef* RTC_InitStruct)
function RTC_WriteProtectionCmd (line 525) | void RTC_WriteProtectionCmd(FunctionalState NewState)
function ErrorStatus (line 552) | ErrorStatus RTC_EnterInitMode(void)
function RTC_ExitInitMode (line 597) | void RTC_ExitInitMode(void)
function ErrorStatus (line 619) | ErrorStatus RTC_WaitForSynchro(void)
function ErrorStatus (line 662) | ErrorStatus RTC_RefClockCmd(FunctionalState NewState)
function RTC_BypassShadowCmd (line 710) | void RTC_BypassShadowCmd(FunctionalState NewState)
function ErrorStatus (line 765) | ErrorStatus RTC_SetTime(uint32_t RTC_Format, RTC_TimeTypeDef* RTC_TimeSt...
function RTC_TimeStructInit (line 868) | void RTC_TimeStructInit(RTC_TimeTypeDef* RTC_TimeStruct)
function RTC_GetTime (line 887) | void RTC_GetTime(uint32_t RTC_Format, RTC_TimeTypeDef* RTC_TimeStruct)
function RTC_GetSubSecond (line 920) | uint32_t RTC_GetSubSecond(void)
function ErrorStatus (line 945) | ErrorStatus RTC_SetDate(uint32_t RTC_Format, RTC_DateTypeDef* RTC_DateSt...
function RTC_DateStructInit (line 1036) | void RTC_DateStructInit(RTC_DateTypeDef* RTC_DateStruct)
function RTC_GetDate (line 1055) | void RTC_GetDate(uint32_t RTC_Format, RTC_DateTypeDef* RTC_DateStruct)
function RTC_SetAlarm (line 1115) | void RTC_SetAlarm(uint32_t RTC_Format, uint32_t RTC_Alarm, RTC_AlarmType...
function RTC_AlarmStructInit (line 1226) | void RTC_AlarmStructInit(RTC_AlarmTypeDef* RTC_AlarmStruct)
function RTC_GetAlarm (line 1256) | void RTC_GetAlarm(uint32_t RTC_Format, uint32_t RTC_Alarm, RTC_AlarmType...
function ErrorStatus (line 1310) | ErrorStatus RTC_AlarmCmd(uint32_t RTC_Alarm, FunctionalState NewState)
function RTC_AlarmSubSecondConfig (line 1404) | void RTC_AlarmSubSecondConfig(uint32_t RTC_Alarm, uint32_t RTC_AlarmSubS...
function RTC_GetAlarmSubSecond (line 1445) | uint32_t RTC_GetAlarmSubSecond(uint32_t RTC_Alarm)
function RTC_WakeUpClockConfig (line 1494) | void RTC_WakeUpClockConfig(uint32_t RTC_WakeUpClock)
function RTC_SetWakeUpCounter (line 1521) | void RTC_SetWakeUpCounter(uint32_t RTC_WakeUpCounter)
function RTC_GetWakeUpCounter (line 1542) | uint32_t RTC_GetWakeUpCounter(void)
function ErrorStatus (line 1554) | ErrorStatus RTC_WakeUpCmd(FunctionalState NewState)
function RTC_DayLightSavingConfig (line 1631) | void RTC_DayLightSavingConfig(uint32_t RTC_DayLightSaving, uint32_t RTC_...
function RTC_GetStoreOperation (line 1658) | uint32_t RTC_GetStoreOperation(void)
function RTC_OutputConfig (line 1697) | void RTC_OutputConfig(uint32_t RTC_Output, uint32_t RTC_OutputPolarity)
function ErrorStatus (line 1751) | ErrorStatus RTC_CoarseCalibConfig(uint32_t RTC_CalibSign, uint32_t Value)
function ErrorStatus (line 1792) | ErrorStatus RTC_CoarseCalibCmd(FunctionalState NewState)
function RTC_CalibOutputCmd (line 1838) | void RTC_CalibOutputCmd(FunctionalState NewState)
function RTC_CalibOutputConfig (line 1870) | void RTC_CalibOutputConfig(uint32_t RTC_CalibOutput)
function ErrorStatus (line 1906) | ErrorStatus RTC_SmoothCalibConfig(uint32_t RTC_SmoothCalibPeriod,
function RTC_TimeStampCmd (line 1982) | void RTC_TimeStampCmd(uint32_t RTC_TimeStampEdge, FunctionalState NewState)
function RTC_GetTimeStamp (line 2026) | void RTC_GetTimeStamp(uint32_t RTC_Format, RTC_TimeTypeDef* RTC_StampTim...
function RTC_GetTimeStampSubSecond (line 2070) | uint32_t RTC_GetTimeStampSubSecond(void)
function RTC_TamperTriggerConfig (line 2105) | void RTC_TamperTriggerConfig(uint32_t RTC_Tamper, uint32_t RTC_TamperTri...
function RTC_TamperCmd (line 2131) | void RTC_TamperCmd(uint32_t RTC_Tamper, FunctionalState NewState)
function RTC_TamperFilterConfig (line 2162) | void RTC_TamperFilterConfig(uint32_t RTC_TamperFilter)
function RTC_TamperSamplingFreqConfig (line 2196) | void RTC_TamperSamplingFreqConfig(uint32_t RTC_TamperSamplingFreq)
function RTC_TamperPinsPrechargeDuration (line 2219) | void RTC_TamperPinsPrechargeDuration(uint32_t RTC_TamperPrechargeDuration)
function RTC_TimeStampOnTamperDetectionCmd (line 2239) | void RTC_TimeStampOnTamperDetectionCmd(FunctionalState NewState)
function RTC_TamperPullUpCmd (line 2262) | void RTC_TamperPullUpCmd(FunctionalState NewState)
function RTC_WriteBackupRegister (line 2303) | void RTC_WriteBackupRegister(uint32_t RTC_BKP_DR, uint32_t Data)
function RTC_ReadBackupRegister (line 2324) | uint32_t RTC_ReadBackupRegister(uint32_t RTC_BKP_DR)
function RTC_TamperPinSelection (line 2363) | void RTC_TamperPinSelection(uint32_t RTC_TamperPin)
function RTC_TimeStampPinSelection (line 2380) | void RTC_TimeStampPinSelection(uint32_t RTC_TimeStampPin)
function RTC_OutputTypeConfig (line 2399) | void RTC_OutputTypeConfig(uint32_t RTC_OutputType)
function ErrorStatus (line 2437) | ErrorStatus RTC_SynchroShiftConfig(uint32_t RTC_ShiftAdd1S, uint32_t RTC...
function RTC_ITConfig (line 2557) | void RTC_ITConfig(uint32_t RTC_IT, FunctionalState NewState)
function FlagStatus (line 2605) | FlagStatus RTC_GetFlagStatus(uint32_t RTC_FLAG)
function RTC_ClearFlag (line 2641) | void RTC_ClearFlag(uint32_t RTC_FLAG)
function ITStatus (line 2661) | ITStatus RTC_GetITStatus(uint32_t RTC_IT)
function RTC_ClearITPendingBit (line 2701) | void RTC_ClearITPendingBit(uint32_t RTC_IT)
function RTC_ByteToBcd2 (line 2724) | static uint8_t RTC_ByteToBcd2(uint8_t Value)
function RTC_Bcd2ToByte (line 2742) | static uint8_t RTC_Bcd2ToByte(uint8_t Value)
FILE: Libraries/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_sai.c
function SAI_DeInit (line 182) | void SAI_DeInit(SAI_TypeDef* SAIx)
function SAI_Init (line 205) | void SAI_Init(SAI_Block_TypeDef* SAI_Block_x, SAI_InitTypeDef* SAI_InitS...
function SAI_FrameInit (line 272) | void SAI_FrameInit(SAI_Block_TypeDef* SAI_Block_x, SAI_FrameInitTypeDef*...
function SAI_SlotInit (line 320) | void SAI_SlotInit(SAI_Block_TypeDef* SAI_Block_x, SAI_SlotInitTypeDef* S...
function SAI_StructInit (line 359) | void SAI_StructInit(SAI_InitTypeDef* SAI_InitStruct)
function SAI_FrameStructInit (line 390) | void SAI_FrameStructInit(SAI_FrameInitTypeDef* SAI_FrameInitStruct)
function SAI_SlotStructInit (line 411) | void SAI_SlotStructInit(SAI_SlotInitTypeDef* SAI_SlotInitStruct)
function SAI_Cmd (line 432) | void SAI_Cmd(SAI_Block_TypeDef* SAI_Block_x, FunctionalState NewState)
function SAI_MonoModeConfig (line 461) | void SAI_MonoModeConfig(SAI_Block_TypeDef* SAI_Block_x, uint32_t SAI_Mon...
function SAI_TRIStateConfig (line 484) | void SAI_TRIStateConfig(SAI_Block_TypeDef* SAI_Block_x, uint32_t SAI_TRI...
function SAI_CompandingModeConfig (line 512) | void SAI_CompandingModeConfig(SAI_Block_TypeDef* SAI_Block_x, uint32_t S...
function SAI_MuteModeCmd (line 536) | void SAI_MuteModeCmd(SAI_Block_TypeDef* SAI_Block_x, FunctionalState New...
function SAI_MuteValueConfig (line 567) | void SAI_MuteValueConfig(SAI_Block_TypeDef* SAI_Block_x, uint32_t SAI_Mu...
function SAI_MuteFrameCounterConfig (line 589) | void SAI_MuteFrameCounterConfig(SAI_Block_TypeDef* SAI_Block_x, uint32_t...
function SAI_FlushFIFO (line 612) | void SAI_FlushFIFO(SAI_Block_TypeDef* SAI_Block_x)
function SAI_ReceiveData (line 654) | uint32_t SAI_ReceiveData(SAI_Block_TypeDef* SAI_Block_x)
function SAI_SendData (line 670) | void SAI_SendData(SAI_Block_TypeDef* SAI_Block_x, uint32_t Data)
function SAI_DMACmd (line 702) | void SAI_DMACmd(SAI_Block_TypeDef* SAI_Block_x, FunctionalState NewState)
function SAI_ITConfig (line 846) | void SAI_ITConfig(SAI_Block_TypeDef* SAI_Block_x, uint32_t SAI_IT, Funct...
function FlagStatus (line 879) | FlagStatus SAI_GetFlagStatus(SAI_Block_TypeDef* SAI_Block_x, uint32_t SA...
function SAI_ClearFlag (line 922) | void SAI_ClearFlag(SAI_Block_TypeDef* SAI_Block_x, uint32_t SAI_FLAG)
function ITStatus (line 947) | ITStatus SAI_GetITStatus(SAI_Block_TypeDef* SAI_Block_x, uint32_t SAI_IT)
function SAI_ClearITPendingBit (line 994) | void SAI_ClearITPendingBit(SAI_Block_TypeDef* SAI_Block_x, uint32_t SAI_IT)
function FunctionalState (line 1016) | FunctionalState SAI_GetCmdStatus(SAI_Block_TypeDef* SAI_Block_x)
function SAI_GetFIFOStatus (line 1049) | uint32_t SAI_GetFIFOStatus(SAI_Block_TypeDef* SAI_Block_x)
FILE: Libraries/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_sdio.c
function SDIO_DeInit (line 266) | void SDIO_DeInit(void)
function SDIO_Init (line 279) | void SDIO_Init(SDIO_InitTypeDef* SDIO_InitStruct)
function SDIO_StructInit (line 317) | void SDIO_StructInit(SDIO_InitTypeDef* SDIO_InitStruct)
function SDIO_ClockCmd (line 334) | void SDIO_ClockCmd(FunctionalState NewState)
function SDIO_SetPowerState (line 350) | void SDIO_SetPowerState(uint32_t SDIO_PowerState)
function SDIO_GetPowerState (line 367) | uint32_t SDIO_GetPowerState(void)
function SDIO_SendCommand (line 399) | void SDIO_SendCommand(SDIO_CmdInitTypeDef *SDIO_CmdInitStruct)
function SDIO_CmdStructInit (line 435) | void SDIO_CmdStructInit(SDIO_CmdInitTypeDef* SDIO_CmdInitStruct)
function SDIO_GetCommandResponse (line 450) | uint8_t SDIO_GetCommandResponse(void)
function SDIO_GetResponse (line 465) | uint32_t SDIO_GetResponse(uint32_t SDIO_RESP)
function SDIO_DataConfig (line 503) | void SDIO_DataConfig(SDIO_DataInitTypeDef* SDIO_DataInitStruct)
function SDIO_DataStructInit (line 544) | void SDIO_DataStructInit(SDIO_DataInitTypeDef* SDIO_DataInitStruct)
function SDIO_GetDataCounter (line 560) | uint32_t SDIO_GetDataCounter(void)
function SDIO_ReadData (line 570) | uint32_t SDIO_ReadData(void)
function SDIO_WriteData (line 580) | void SDIO_WriteData(uint32_t Data)
function SDIO_GetFIFOCount (line 590) | uint32_t SDIO_GetFIFOCount(void)
function SDIO_StartSDIOReadWait (line 619) | void SDIO_StartSDIOReadWait(FunctionalState NewState)
function SDIO_StopSDIOReadWait (line 633) | void SDIO_StopSDIOReadWait(FunctionalState NewState)
function SDIO_SetSDIOReadWaitMode (line 649) | void SDIO_SetSDIOReadWaitMode(uint32_t SDIO_ReadWaitMode)
function SDIO_SetSDIOOperation (line 663) | void SDIO_SetSDIOOperation(FunctionalState NewState)
function SDIO_SendSDIOSuspendCmd (line 677) | void SDIO_SendSDIOSuspendCmd(FunctionalState NewState)
function SDIO_CommandCompletionCmd (line 709) | void SDIO_CommandCompletionCmd(FunctionalState NewState)
function SDIO_CEATAITCmd (line 723) | void SDIO_CEATAITCmd(FunctionalState NewState)
function SDIO_SendCEATACmd (line 737) | void SDIO_SendCEATACmd(FunctionalState NewState)
function SDIO_DMACmd (line 769) | void SDIO_DMACmd(FunctionalState NewState)
function SDIO_ITConfig (line 827) | void SDIO_ITConfig(uint32_t SDIO_IT, FunctionalState NewState)
function FlagStatus (line 875) | FlagStatus SDIO_GetFlagStatus(uint32_t SDIO_FLAG)
function SDIO_ClearFlag (line 912) | void SDIO_ClearFlag(uint32_t SDIO_FLAG)
function ITStatus (line 951) | ITStatus SDIO_GetITStatus(uint32_t SDIO_IT)
function SDIO_ClearITPendingBit (line 987) | void SDIO_ClearITPendingBit(uint32_t SDIO_IT)
FILE: Libraries/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_spi.c
function SPI_I2S_DeInit (line 224) | void SPI_I2S_DeInit(SPI_TypeDef* SPIx)
function SPI_Init (line 284) | void SPI_Init(SPI_TypeDef* SPIx, SPI_InitTypeDef* SPI_InitStruct)
function I2S_Init (line 348) | void I2S_Init(SPI_TypeDef* SPIx, I2S_InitTypeDef* I2S_InitStruct)
function SPI_StructInit (line 480) | void SPI_StructInit(SPI_InitTypeDef* SPI_InitStruct)
function I2S_StructInit (line 508) | void I2S_StructInit(I2S_InitTypeDef* I2S_InitStruct)
function SPI_Cmd (line 537) | void SPI_Cmd(SPI_TypeDef* SPIx, FunctionalState NewState)
function I2S_Cmd (line 562) | void I2S_Cmd(SPI_TypeDef* SPIx, FunctionalState NewState)
function SPI_DataSizeConfig (line 589) | void SPI_DataSizeConfig(SPI_TypeDef* SPIx, uint16_t SPI_DataSize)
function SPI_BiDirectionalLineConfig (line 609) | void SPI_BiDirectionalLineConfig(SPI_TypeDef* SPIx, uint16_t SPI_Direction)
function SPI_NSSInternalSoftwareConfig (line 635) | void SPI_NSSInternalSoftwareConfig(SPI_TypeDef* SPIx, uint16_t SPI_NSSIn...
function SPI_SSOutputCmd (line 659) | void SPI_SSOutputCmd(SPI_TypeDef* SPIx, FunctionalState NewState)
function SPI_TIModeCmd (line 690) | void SPI_TIModeCmd(SPI_TypeDef* SPIx, FunctionalState NewState)
function I2S_FullDuplexConfig (line 727) | void I2S_FullDuplexConfig(SPI_TypeDef* I2Sxext, I2S_InitTypeDef* I2S_Ini...
function SPI_I2S_ReceiveData (line 801) | uint16_t SPI_I2S_ReceiveData(SPI_TypeDef* SPIx)
function SPI_I2S_SendData (line 817) | void SPI_I2S_SendData(SPI_TypeDef* SPIx, uint16_t Data)
function SPI_CalculateCRC (line 907) | void SPI_CalculateCRC(SPI_TypeDef* SPIx, FunctionalState NewState)
function SPI_TransmitCRC (line 929) | void SPI_TransmitCRC(SPI_TypeDef* SPIx)
function SPI_GetCRC (line 947) | uint16_t SPI_GetCRC(SPI_TypeDef* SPIx, uint8_t SPI_CRC)
function SPI_GetCRCPolynomial (line 972) | uint16_t SPI_GetCRCPolynomial(SPI_TypeDef* SPIx)
function SPI_I2S_DMACmd (line 1009) | void SPI_I2S_DMACmd(SPI_TypeDef* SPIx, uint16_t SPI_I2S_DMAReq, Function...
function SPI_I2S_ITConfig (line 1117) | void SPI_I2S_ITConfig(SPI_TypeDef* SPIx, uint8_t SPI_I2S_IT, FunctionalS...
function FlagStatus (line 1161) | FlagStatus SPI_I2S_GetFlagStatus(SPI_TypeDef* SPIx, uint16_t SPI_I2S_FLAG)
function SPI_I2S_ClearFlag (line 1202) | void SPI_I2S_ClearFlag(SPI_TypeDef* SPIx, uint16_t SPI_I2S_FLAG)
function ITStatus (line 1227) | ITStatus SPI_I2S_GetITStatus(SPI_TypeDef* SPIx, uint8_t SPI_I2S_IT)
function SPI_I2S_ClearITPendingBit (line 1282) | void SPI_I2S_ClearITPendingBit(SPI_TypeDef* SPIx, uint8_t SPI_I2S_IT)
FILE: Libraries/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_syscfg.c
function SYSCFG_DeInit (line 100) | void SYSCFG_DeInit(void)
function SYSCFG_MemoryRemapConfig (line 118) | void SYSCFG_MemoryRemapConfig(uint8_t SYSCFG_MemoryRemap)
function SYSCFG_MemorySwappingBank (line 139) | void SYSCFG_MemorySwappingBank(FunctionalState NewState)
function SYSCFG_EXTILineConfig (line 162) | void SYSCFG_EXTILineConfig(uint8_t EXTI_PortSourceGPIOx, uint8_t EXTI_Pi...
function SYSCFG_ETH_MediaInterfaceConfig (line 183) | void SYSCFG_ETH_MediaInterfaceConfig(uint32_t SYSCFG_ETH_MediaInterface)
function SYSCFG_CompensationCellCmd (line 200) | void SYSCFG_CompensationCellCmd(FunctionalState NewState)
function FlagStatus (line 213) | FlagStatus SYSCFG_GetCompensationCellStatus(void)
FILE: Libraries/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_tim.c
function TIM_DeInit (line 200) | void TIM_DeInit(TIM_TypeDef* TIMx)
function TIM_TimeBaseInit (line 288) | void TIM_TimeBaseInit(TIM_TypeDef* TIMx, TIM_TimeBaseInitTypeDef* TIM_Ti...
function TIM_TimeBaseStructInit (line 340) | void TIM_TimeBaseStructInit(TIM_TimeBaseInitTypeDef* TIM_TimeBaseInitStr...
function TIM_PrescalerConfig (line 360) | void TIM_PrescalerConfig(TIM_TypeDef* TIMx, uint16_t Prescaler, uint16_t...
function TIM_CounterModeConfig (line 383) | void TIM_CounterModeConfig(TIM_TypeDef* TIMx, uint16_t TIM_CounterMode)
function TIM_SetCounter (line 409) | void TIM_SetCounter(TIM_TypeDef* TIMx, uint32_t Counter)
function TIM_SetAutoreload (line 424) | void TIM_SetAutoreload(TIM_TypeDef* TIMx, uint32_t Autoreload)
function TIM_GetCounter (line 438) | uint32_t TIM_GetCounter(TIM_TypeDef* TIMx)
function TIM_GetPrescaler (line 452) | uint16_t TIM_GetPrescaler(TIM_TypeDef* TIMx)
function TIM_UpdateDisableConfig (line 468) | void TIM_UpdateDisableConfig(TIM_TypeDef* TIMx, FunctionalState NewState)
function TIM_UpdateRequestConfig (line 497) | void TIM_UpdateRequestConfig(TIM_TypeDef* TIMx, uint16_t TIM_UpdateSource)
function TIM_ARRPreloadConfig (line 522) | void TIM_ARRPreloadConfig(TIM_TypeDef* TIMx, FunctionalState NewState)
function TIM_SelectOnePulseMode (line 549) | void TIM_SelectOnePulseMode(TIM_TypeDef* TIMx, uint16_t TIM_OPMode)
function TIM_SetClockDivision (line 572) | void TIM_SetClockDivision(TIM_TypeDef* TIMx, uint16_t TIM_CKD)
function TIM_Cmd (line 592) | void TIM_Cmd(TIM_TypeDef* TIMx, FunctionalState NewState)
function TIM_OC1Init (line 673) | void TIM_OC1Init(TIM_TypeDef* TIMx, TIM_OCInitTypeDef* TIM_OCInitStruct)
function TIM_OC2Init (line 754) | void TIM_OC2Init(TIM_TypeDef* TIMx, TIM_OCInitTypeDef* TIM_OCInitStruct)
function TIM_OC3Init (line 835) | void TIM_OC3Init(TIM_TypeDef* TIMx, TIM_OCInitTypeDef* TIM_OCInitStruct)
function TIM_OC4Init (line 915) | void TIM_OC4Init(TIM_TypeDef* TIMx, TIM_OCInitTypeDef* TIM_OCInitStruct)
function TIM_OCStructInit (line 978) | void TIM_OCStructInit(TIM_OCInitTypeDef* TIM_OCInitStruct)
function TIM_SelectOCxM (line 1014) | void TIM_SelectOCxM(TIM_TypeDef* TIMx, uint16_t TIM_Channel, uint16_t TI...
function TIM_SetCompare1 (line 1060) | void TIM_SetCompare1(TIM_TypeDef* TIMx, uint32_t Compare1)
function TIM_SetCompare2 (line 1076) | void TIM_SetCompare2(TIM_TypeDef* TIMx, uint32_t Compare2)
function TIM_SetCompare3 (line 1091) | void TIM_SetCompare3(TIM_TypeDef* TIMx, uint32_t Compare3)
function TIM_SetCompare4 (line 1106) | void TIM_SetCompare4(TIM_TypeDef* TIMx, uint32_t Compare4)
function TIM_ForcedOC1Config (line 1124) | void TIM_ForcedOC1Config(TIM_TypeDef* TIMx, uint16_t TIM_ForcedAction)
function TIM_ForcedOC2Config (line 1153) | void TIM_ForcedOC2Config(TIM_TypeDef* TIMx, uint16_t TIM_ForcedAction)
function TIM_ForcedOC3Config (line 1181) | void TIM_ForcedOC3Config(TIM_TypeDef* TIMx, uint16_t TIM_ForcedAction)
function TIM_ForcedOC4Config (line 1210) | void TIM_ForcedOC4Config(TIM_TypeDef* TIMx, uint16_t TIM_ForcedAction)
function TIM_OC1PreloadConfig (line 1238) | void TIM_OC1PreloadConfig(TIM_TypeDef* TIMx, uint16_t TIM_OCPreload)
function TIM_OC2PreloadConfig (line 1268) | void TIM_OC2PreloadConfig(TIM_TypeDef* TIMx, uint16_t TIM_OCPreload)
function TIM_OC3PreloadConfig (line 1297) | void TIM_OC3PreloadConfig(TIM_TypeDef* TIMx, uint16_t TIM_OCPreload)
function TIM_OC4PreloadConfig (line 1326) | void TIM_OC4PreloadConfig(TIM_TypeDef* TIMx, uint16_t TIM_OCPreload)
function TIM_OC1FastConfig (line 1355) | void TIM_OC1FastConfig(TIM_TypeDef* TIMx, uint16_t TIM_OCFast)
function TIM_OC2FastConfig (line 1386) | void TIM_OC2FastConfig(TIM_TypeDef* TIMx, uint16_t TIM_OCFast)
function TIM_OC3FastConfig (line 1416) | void TIM_OC3FastConfig(TIM_TypeDef* TIMx, uint16_t TIM_OCFast)
function TIM_OC4FastConfig (line 1446) | void TIM_OC4FastConfig(TIM_TypeDef* TIMx, uint16_t TIM_OCFast)
function TIM_ClearOC1Ref (line 1476) | void TIM_ClearOC1Ref(TIM_TypeDef* TIMx, uint16_t TIM_OCClear)
function TIM_ClearOC2Ref (line 1506) | void TIM_ClearOC2Ref(TIM_TypeDef* TIMx, uint16_t TIM_OCClear)
function TIM_ClearOC3Ref (line 1535) | void TIM_ClearOC3Ref(TIM_TypeDef* TIMx, uint16_t TIM_OCClear)
function TIM_ClearOC4Ref (line 1564) | void TIM_ClearOC4Ref(TIM_TypeDef* TIMx, uint16_t TIM_OCClear)
function TIM_OC1PolarityConfig (line 1593) | void TIM_OC1PolarityConfig(TIM_TypeDef* TIMx, uint16_t TIM_OCPolarity)
function TIM_OC1NPolarityConfig (line 1620) | void TIM_OC1NPolarityConfig(TIM_TypeDef* TIMx, uint16_t TIM_OCNPolarity)
function TIM_OC2PolarityConfig (line 1647) | void TIM_OC2PolarityConfig(TIM_TypeDef* TIMx, uint16_t TIM_OCPolarity)
function TIM_OC2NPolarityConfig (line 1674) | void TIM_OC2NPolarityConfig(TIM_TypeDef* TIMx, uint16_t TIM_OCNPolarity)
function TIM_OC3PolarityConfig (line 1701) | void TIM_OC3PolarityConfig(TIM_TypeDef* TIMx, uint16_t TIM_OCPolarity)
function TIM_OC3NPolarityConfig (line 1728) | void TIM_OC3NPolarityConfig(TIM_TypeDef* TIMx, uint16_t TIM_OCNPolarity)
function TIM_OC4PolarityConfig (line 1755) | void TIM_OC4PolarityConfig(TIM_TypeDef* TIMx, uint16_t TIM_OCPolarity)
function TIM_CCxCmd (line 1786) | void TIM_CCxCmd(TIM_TypeDef* TIMx, uint16_t TIM_Channel, uint16_t TIM_CCx)
function TIM_CCxNCmd (line 1816) | void TIM_CCxNCmd(TIM_TypeDef* TIMx, uint16_t TIM_Channel, uint16_t TIM_C...
function TIM_ICInit (line 1900) | void TIM_ICInit(TIM_TypeDef* TIMx, TIM_ICInitTypeDef* TIM_ICInitStruct)
function TIM_ICStructInit (line 1956) | void TIM_ICStructInit(TIM_ICInitTypeDef* TIM_ICInitStruct)
function TIM_PWMIConfig (line 1975) | void TIM_PWMIConfig(TIM_TypeDef* TIMx, TIM_ICInitTypeDef* TIM_ICInitStruct)
function TIM_GetCapture1 (line 2032) | uint32_t TIM_GetCapture1(TIM_TypeDef* TIMx)
function TIM_GetCapture2 (line 2047) | uint32_t TIM_GetCapture2(TIM_TypeDef* TIMx)
function TIM_GetCapture3 (line 2061) | uint32_t TIM_GetCapture3(TIM_TypeDef* TIMx)
function TIM_GetCapture4 (line 2075) | uint32_t TIM_GetCapture4(TIM_TypeDef* TIMx)
function TIM_SetIC1Prescaler (line 2095) | void TIM_SetIC1Prescaler(TIM_TypeDef* TIMx, uint16_t TIM_ICPSC)
function TIM_SetIC2Prescaler (line 2120) | void TIM_SetIC2Prescaler(TIM_TypeDef* TIMx, uint16_t TIM_ICPSC)
function TIM_SetIC3Prescaler (line 2144) | void TIM_SetIC3Prescaler(TIM_TypeDef* TIMx, uint16_t TIM_ICPSC)
function TIM_SetIC4Prescaler (line 2168) | void TIM_SetIC4Prescaler(TIM_TypeDef* TIMx, uint16_t TIM_ICPSC)
function TIM_BDTRConfig (line 2221) | void TIM_BDTRConfig(TIM_TypeDef* TIMx, TIM_BDTRInitTypeDef *TIM_BDTRInit...
function TIM_BDTRStructInit (line 2246) | void TIM_BDTRStructInit(TIM_BDTRInitTypeDef* TIM_BDTRInitStruct)
function TIM_CtrlPWMOutputs (line 2265) | void TIM_CtrlPWMOutputs(TIM_TypeDef* TIMx, FunctionalState NewState)
function TIM_SelectCOM (line 2290) | void TIM_SelectCOM(TIM_TypeDef* TIMx, FunctionalState NewState)
function TIM_CCPreloadControl (line 2315) | void TIM_CCPreloadControl(TIM_TypeDef* TIMx, FunctionalState NewState)
function TIM_ITConfig (line 2372) | void TIM_ITConfig(TIM_TypeDef* TIMx, uint16_t TIM_IT, FunctionalState Ne...
function TIM_GenerateEvent (line 2410) | void TIM_GenerateEvent(TIM_TypeDef* TIMx, uint16_t TIM_EventSource)
function FlagStatus (line 2443) | FlagStatus TIM_GetFlagStatus(TIM_TypeDef* TIMx, uint16_t TIM_FLAG)
function TIM_ClearFlag (line 2485) | void TIM_ClearFlag(TIM_TypeDef* TIMx, uint16_t TIM_FLAG)
function ITStatus (line 2513) | ITStatus TIM_GetITStatus(TIM_TypeDef* TIMx, uint16_t TIM_IT)
function TIM_ClearITPendingBit (line 2554) | void TIM_ClearITPendingBit(TIM_TypeDef* TIMx, uint16_t TIM_IT)
function TIM_DMAConfig (line 2591) | void TIM_DMAConfig(TIM_TypeDef* TIMx, uint16_t TIM_DMABase, uint16_t TIM...
function TIM_DMACmd (line 2618) | void TIM_DMACmd(TIM_TypeDef* TIMx, uint16_t TIM_DMASource, FunctionalSta...
function TIM_SelectCCDMA (line 2644) | void TIM_SelectCCDMA(TIM_TypeDef* TIMx, FunctionalState NewState)
function TIM_InternalClockConfig (line 2683) | void TIM_InternalClockConfig(TIM_TypeDef* TIMx)
function TIM_ITRxExternalClockConfig (line 2704) | void TIM_ITRxExternalClockConfig(TIM_TypeDef* TIMx, uint16_t TIM_InputTr...
function TIM_TIxExternalClockConfig (line 2734) | void TIM_TIxExternalClockConfig(TIM_TypeDef* TIMx, uint16_t TIM_TIxExter...
function TIM_ETRClockMode1Config (line 2774) | void TIM_ETRClockMode1Config(TIM_TypeDef* TIMx, uint16_t TIM_ExtTRGPresc...
function TIM_ETRClockMode2Config (line 2821) | void TIM_ETRClockMode2Config(TIM_TypeDef* TIMx, uint16_t TIM_ExtTRGPresc...
function TIM_SelectInputTrigger (line 2892) | void TIM_SelectInputTrigger(TIM_TypeDef* TIMx, uint16_t TIM_InputTrigger...
function TIM_SelectOutputTrigger (line 2935) | void TIM_SelectOutputTrigger(TIM_TypeDef* TIMx, uint16_t TIM_TRGOSource)
function TIM_SelectSlaveMode (line 2959) | void TIM_SelectSlaveMode(TIM_TypeDef* TIMx, uint16_t TIM_SlaveMode)
function TIM_SelectMasterSlaveMode (line 2982) | void TIM_SelectMasterSlaveMode(TIM_TypeDef* TIMx, uint16_t TIM_MasterSla...
function TIM_ETRConfig (line 3012) | void TIM_ETRConfig(TIM_TypeDef* TIMx, uint16_t TIM_ExtTRGPrescaler,
function TIM_EncoderInterfaceConfig (line 3070) | void TIM_EncoderInterfaceConfig(TIM_TypeDef* TIMx, uint16_t TIM_EncoderM...
function TIM_SelectHallSensor (line 3122) | void TIM_SelectHallSensor(TIM_TypeDef* TIMx, FunctionalState NewState)
function TIM_RemapConfig (line 3173) | void TIM_RemapConfig(TIM_TypeDef* TIMx, uint16_t TIM_Remap)
function TI1_Config (line 3204) | static void TI1_Config(TIM_TypeDef* TIMx, uint16_t TIM_ICPolarity, uint1...
function TI2_Config (line 3245) | static void TI2_Config(TIM_TypeDef* TIMx, uint16_t TIM_ICPolarity, uint1...
function TI3_Config (line 3287) | static void TI3_Config(TIM_TypeDef* TIMx, uint16_t TIM_ICPolarity, uint1...
function TI4_Config (line 3328) | static void TI4_Config(TIM_TypeDef* TIMx, uint16_t TIM_ICPolarity, uint1...
FILE: Libraries/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_usart.c
function USART_DeInit (line 187) | void USART_DeInit(USART_TypeDef* USARTx)
function USART_Init (line 246) | void USART_Init(USART_TypeDef* USARTx, USART_InitTypeDef* USART_InitStruct)
function USART_StructInit (line 359) | void USART_StructInit(USART_InitTypeDef* USART_InitStruct)
function USART_ClockInit (line 379) | void USART_ClockInit(USART_TypeDef* USARTx, USART_ClockInitTypeDef* USAR...
function USART_ClockStructInit (line 410) | void USART_ClockStructInit(USART_ClockInitTypeDef* USART_ClockInitStruct)
function USART_Cmd (line 427) | void USART_Cmd(USART_TypeDef* USARTx, FunctionalState NewState)
function USART_SetPrescaler (line 453) | void USART_SetPrescaler(USART_TypeDef* USARTx, uint8_t USART_Prescaler)
function USART_OverSampling8Cmd (line 474) | void USART_OverSampling8Cmd(USART_TypeDef* USARTx, FunctionalState NewSt...
function USART_OneBitMethodCmd (line 500) | void USART_OneBitMethodCmd(USART_TypeDef* USARTx, FunctionalState NewState)
function USART_SendData (line 557) | void USART_SendData(USART_TypeDef* USARTx, uint16_t Data)
function USART_ReceiveData (line 573) | uint16_t USART_ReceiveData(USART_TypeDef* USARTx)
function USART_SetAddress (line 625) | void USART_SetAddress(USART_TypeDef* USARTx, uint8_t USART_Address)
function USART_ReceiverWakeUpCmd (line 645) | void USART_ReceiverWakeUpCmd(USART_TypeDef* USARTx, FunctionalState NewS...
function USART_WakeUpConfig (line 672) | void USART_WakeUpConfig(USART_TypeDef* USARTx, uint16_t USART_WakeUp)
function USART_LINBreakDetectLengthConfig (line 741) | void USART_LINBreakDetectLengthConfig(USART_TypeDef* USARTx, uint16_t US...
function USART_LINCmd (line 759) | void USART_LINCmd(USART_TypeDef* USARTx, FunctionalState NewState)
function USART_SendBreak (line 783) | void USART_SendBreak(USART_TypeDef* USARTx)
function USART_HalfDuplexCmd (line 836) | void USART_HalfDuplexCmd(USART_TypeDef* USARTx, FunctionalState NewState)
function USART_SetGuardTime (line 920) | void USART_SetGuardTime(USART_TypeDef* USARTx, uint8_t USART_GuardTime)
function USART_SmartCardCmd (line 939) | void USART_SmartCardCmd(USART_TypeDef* USARTx, FunctionalState NewState)
function USART_SmartCardNACKCmd (line 964) | void USART_SmartCardNACKCmd(USART_TypeDef* USARTx, FunctionalState NewSt...
function USART_IrDAConfig (line 1035) | void USART_IrDAConfig(USART_TypeDef* USARTx, uint16_t USART_IrDAMode)
function USART_IrDACmd (line 1053) | void USART_IrDACmd(USART_TypeDef* USARTx, FunctionalState NewState)
function USART_DMACmd (line 1099) | void USART_DMACmd(USART_TypeDef* USARTx, uint16_t USART_DMAReq, Function...
function USART_ITConfig (line 1231) | void USART_ITConfig(USART_TypeDef* USARTx, uint16_t USART_IT, Functional...
function FlagStatus (line 1295) | FlagStatus USART_GetFlagStatus(USART_TypeDef* USARTx, uint16_t USART_FLAG)
function USART_ClearFlag (line 1344) | void USART_ClearFlag(USART_TypeDef* USARTx, uint16_t USART_FLAG)
function ITStatus (line 1378) | ITStatus USART_GetITStatus(USART_TypeDef* USARTx, uint16_t USART_IT)
function USART_ClearITPendingBit (line 1452) | void USART_ClearITPendingBit(USART_TypeDef* USARTx, uint16_t USART_IT)
FILE: Libraries/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_wwdg.c
function WWDG_DeInit (line 138) | void WWDG_DeInit(void)
function WWDG_SetPrescaler (line 154) | void WWDG_SetPrescaler(uint32_t WWDG_Prescaler)
function WWDG_SetWindowValue (line 173) | void WWDG_SetWindowValue(uint8_t WindowValue)
function WWDG_EnableIT (line 196) | void WWDG_EnableIT(void)
function WWDG_SetCounter (line 208) | void WWDG_SetCounter(uint8_t Counter)
function WWDG_Enable (line 239) | void WWDG_Enable(uint8_t Counter)
function FlagStatus (line 266) | FlagStatus WWDG_GetFlagStatus(void)
function WWDG_ClearFlag (line 286) | void WWDG_ClearFlag(void)
FILE: Libraries/eMPL/dmpKey.h
type tKeyLabel (line 287) | typedef struct {
FILE: Libraries/eMPL/inv_mpu.c
function reg_int_cb (line 48) | static inline int reg_int_cb(struct int_param_s *int_param)
function reg_int_cb (line 69) | static inline int reg_int_cb(struct int_param_s *int_param)
function reg_int_cb (line 94) | static inline int reg_int_cb(struct int_param_s *int_param)
function reg_int_cb (line 115) | static __inline int reg_int_cb(struct int_param_s *int_param)
type gyro_reg_s (line 168) | struct gyro_reg_s {
type hw_s (line 217) | struct hw_s {
type motion_int_cache_s (line 233) | struct motion_int_cache_s {
type chip_cfg_s (line 246) | struct chip_cfg_s {
type test_s (line 293) | struct test_s {
type gyro_state_s (line 315) | struct gyro_state_s {
type lpf_e (line 323) | enum lpf_e {
type gyro_fsr_e (line 336) | enum gyro_fsr_e {
type accel_fsr_e (line 345) | enum accel_fsr_e {
type clock_sel_e (line 354) | enum clock_sel_e {
type lp_accel_rate_e (line 361) | enum lp_accel_rate_e {
type gyro_reg_s (line 462) | struct gyro_reg_s
type hw_s (line 505) | struct hw_s
type test_s (line 517) | struct test_s
type gyro_state_s (line 534) | struct gyro_state_s
type gyro_reg_s (line 540) | struct gyro_reg_s
type hw_s (line 585) | struct hw_s
type test_s (line 597) | struct test_s
type gyro_state_s (line 616) | struct gyro_state_s
function set_int_enable (line 641) | static int set_int_enable(unsigned char enable)
function mpu_reg_dump (line 673) | int mpu_reg_dump(void)
function mpu_read_reg (line 695) | int mpu_read_reg(unsigned char reg, unsigned char *data)
function mpu_init (line 717) | int mpu_init(struct int_param_s *int_param)
function mpu_lp_accel_mode (line 822) | int mpu_lp_accel_mode(unsigned char rate)
function mpu_get_gyro_reg (line 906) | int mpu_get_gyro_reg(short *data, unsigned long *timestamp)
function mpu_get_accel_reg (line 929) | int mpu_get_accel_reg(short *data, unsigned long *timestamp)
function mpu_get_temperature (line 952) | int mpu_get_temperature(long *data, unsigned long *timestamp)
function mpu_read_6500_accel_bias (line 978) | int mpu_read_6500_accel_bias(long *accel_bias) {
function mpu_read_6050_accel_bias (line 1000) | int mpu_read_6050_accel_bias(long *accel_bias) {
function mpu_read_6500_gyro_bias (line 1014) | int mpu_read_6500_gyro_bias(long *gyro_bias) {
function mpu_set_gyro_bias_reg (line 1036) | int mpu_set_gyro_bias_reg(long *gyro_bias)
function mpu_set_accel_bias_6050_reg (line 1066) | int mpu_set_accel_bias_6050_reg(const long *accel_bias)
function mpu_set_accel_bias_6500_reg (line 1116) | int mpu_set_accel_bias_6500_reg(const long *accel_bias)
function mpu_reset_fifo (line 1162) | int mpu_reset_fifo(void)
function mpu_get_gyro_fsr (line 1224) | int mpu_get_gyro_fsr(unsigned short *fsr)
function mpu_set_gyro_fsr (line 1251) | int mpu_set_gyro_fsr(unsigned short fsr)
function mpu_get_accel_fsr (line 1288) | int mpu_get_accel_fsr(unsigned char *fsr)
function mpu_set_accel_fsr (line 1316) | int mpu_set_accel_fsr(unsigned char fsr)
function mpu_get_lpf (line 1353) | int mpu_get_lpf(unsigned short *lpf)
function mpu_set_lpf (line 1389) | int mpu_set_lpf(unsigned short lpf)
function mpu_get_sample_rate (line 1422) | int mpu_get_sample_rate(unsigned short *rate)
function mpu_set_sample_rate (line 1437) | int mpu_set_sample_rate(unsigned short rate)
function mpu_get_compass_sample_rate (line 1484) | int mpu_get_compass_sample_rate(unsigned short *rate)
function mpu_set_compass_sample_rate (line 1506) | int mpu_set_compass_sample_rate(unsigned short rate)
function mpu_get_gyro_sens (line 1528) | int mpu_get_gyro_sens(float *sens)
function mpu_get_accel_sens (line 1554) | int mpu_get_accel_sens(unsigned short *sens)
function mpu_get_fifo_config (line 1586) | int mpu_get_fifo_config(unsigned char *sensors)
function mpu_configure_fifo (line 1601) | int mpu_configure_fifo(unsigned char sensors)
function mpu_get_power_state (line 1643) | int mpu_get_power_state(unsigned char *power_on)
function mpu_set_sensors (line 1662) | int mpu_set_sensors(unsigned char sensors)
function mpu_get_int_status (line 1739) | int mpu_get_int_status(short *status)
function mpu_read_fifo (line 1768) | int mpu_read_fifo(short *gyro, short *accel, unsigned long *timestamp,
function mpu_read_fifo_stream (line 1849) | int mpu_read_fifo_stream(unsigned short length, unsigned char *data,
function mpu_set_bypass (line 1891) | int mpu_set_bypass(unsigned char bypass_on)
function mpu_set_int_level (line 1941) | int mpu_set_int_level(unsigned char active_low)
function mpu_set_int_latched (line 1953) | int mpu_set_int_latched(unsigned char enable)
function get_accel_prod_shift (line 1974) | static int get_accel_prod_shift(float *st_shift)
function accel_self_test (line 1999) | static int accel_self_test(long *bias_regular, long *bias_st)
function gyro_self_test (line 2019) | static int gyro_self_test(long *bias_regular, long *bias_st)
function compass_self_test (line 2050) | static int compass_self_test(void)
function get_st_biases (line 2113) | static int get_st_biases(long *gyro, long *accel, unsigned char hw_test)
function accel_6500_self_test (line 2270) | static int accel_6500_self_test(long *bias_regular, long *bias_st, int d...
function gyro_6500_self_test (line 2360) | static int gyro_6500_self_test(long *bias_regular, long *bias_st, int de...
function get_st_6500_biases (line 2453) | static int get_st_6500_biases(long *gyro, long *accel, unsigned char hw_...
function mpu_run_6500_self_test (line 2595) | int mpu_run_6500_self_test(long *gyro, long *accel, unsigned char debug)
function mpu_run_self_test (line 2716) | int mpu_run_self_test(long *gyro, long *accel)
function mpu_write_mem (line 2820) | int mpu_write_mem(unsigned short mem_addr, unsigned short length,
function mpu_read_mem (line 2853) | int mpu_read_mem(unsigned short mem_addr, unsigned short length,
function mpu_load_firmware (line 2885) | int mpu_load_firmware(unsigned short length, const unsigned char *firmware,
function mpu_set_dmp_state (line 2926) | int mpu_set_dmp_state(unsigned char enable)
function mpu_get_dmp_state (line 2965) | int mpu_get_dmp_state(unsigned char *enabled)
function setup_compass (line 2973) | static int setup_compass(void)
function mpu_get_compass_reg (line 3081) | int mpu_get_compass_reg(short *data, unsigned long *timestamp)
function mpu_get_compass_fsr (line 3134) | int mpu_get_compass_fsr(unsigned short *fsr)
function mpu_lp_motion_interrupt (line 3182) | int mpu_lp_motion_interrupt(unsigned short thresh, unsigned char time,
FILE: Libraries/eMPL/inv_mpu.h
type int_param_s (line 31) | struct int_param_s {
type int_param_s (line 60) | struct int_param_s
FILE: Libraries/eMPL/inv_mpu_dmp_motion_driver.c
type dmp_s (line 482) | struct dmp_s {
type dmp_s (line 491) | struct dmp_s
function dmp_load_motion_driver_firmware (line 504) | int dmp_load_motion_driver_firmware(void)
function dmp_set_orientation (line 517) | int dmp_set_orientation(unsigned short orient)
function dmp_set_gyro_bias (line 572) | int dmp_set_gyro_bias(long *bias)
function dmp_set_accel_bias (line 624) | int dmp_set_accel_bias(long *bias)
function dmp_set_fifo_rate (line 677) | int dmp_set_fifo_rate(unsigned short rate)
function dmp_get_fifo_rate (line 703) | int dmp_get_fifo_rate(unsigned short *rate)
function dmp_set_tap_thresh (line 715) | int dmp_set_tap_thresh(unsigned char axis, unsigned short thresh)
function dmp_set_tap_axes (line 781) | int dmp_set_tap_axes(unsigned char axis)
function dmp_set_tap_count (line 799) | int dmp_set_tap_count(unsigned char min_taps)
function dmp_set_tap_time (line 817) | int dmp_set_tap_time(unsigned short time)
function dmp_set_tap_time_multi (line 833) | int dmp_set_tap_time_multi(unsigned short time)
function dmp_set_shake_reject_thresh (line 851) | int dmp_set_shake_reject_thresh(long sf, unsigned short thresh)
function dmp_set_shake_reject_time (line 870) | int dmp_set_shake_reject_time(unsigned short time)
function dmp_set_shake_reject_timeout (line 888) | int dmp_set_shake_reject_timeout(unsigned short time)
function dmp_get_pedometer_step_count (line 903) | int dmp_get_pedometer_step_count(unsigned long *count)
function dmp_set_pedometer_step_count (line 924) | int dmp_set_pedometer_step_count(unsigned long count)
function dmp_get_pedometer_walk_time (line 940) | int dmp_get_pedometer_walk_time(unsigned long *time)
function dmp_set_pedometer_walk_time (line 960) | int dmp_set_pedometer_walk_time(unsigned long time)
function dmp_enable_feature (line 990) | int dmp_enable_feature(unsigned short mask)
function dmp_get_enabled_features (line 1112) | int dmp_get_enabled_features(unsigned short *mask)
function dmp_enable_gyro_cal (line 1127) | int dmp_enable_gyro_cal(unsigned char enable)
function dmp_enable_lp_quat (line 1145) | int dmp_enable_lp_quat(unsigned char enable)
function dmp_enable_6x_lp_quat (line 1169) | int dmp_enable_6x_lp_quat(unsigned char enable)
function decode_gesture (line 1190) | static int decode_gesture(unsigned char *gesture)
function dmp_set_interrupt_mode (line 1222) | int dmp_set_interrupt_mode(unsigned char mode)
function dmp_read_fifo (line 1261) | int dmp_read_fifo(short *gyro, short *accel, long *quat,
function dmp_register_tap_cb (line 1354) | int dmp_register_tap_cb(void (*func)(unsigned char, unsigned char))
function dmp_register_android_orient_cb (line 1365) | int dmp_register_android_orient_cb(void (*func)(unsigned char))
FILE: Math/inc/FastMath.h
function FastAbs (line 119) | __inline float FastAbs(float x){
function FastAbsD (line 126) | __inline double FastAbsD(double x){
FILE: Math/src/FastMath.c
function Modf (line 29) | float Modf(float x, float *i)
function FastPow (line 61) | float FastPow(float x,float y)
function FastTan (line 262) | float FastTan(float x)
function FastLn (line 327) | float FastLn(float x)
function FastAsin (line 365) | float FastAsin(float x)
function FastAtan2 (line 411) | float FastAtan2(float y, float x)
function FastSqrtI (line 490) | float FastSqrtI(float x)
function FastSqrt (line 510) | float FastSqrt(float x)
function FastSinCos (line 614) | void FastSinCos(float x, float *sinVal, float *cosVal)
function FastSin (line 674) | float FastSin(float x)
function FastCos (line 715) | float FastCos(float x)
function Double (line 756) | Double FastSqrtID(Double dx)
function Double (line 771) | Double FastSqrtD(Double dx)
FILE: Matrix/src/Matrix.c
function arm_status (line 27) | arm_status mat_identity(float32_t *pData, uint16_t numRows, uint16_t num...
function arm_mat_zero_f32 (line 41) | void arm_mat_zero_f32(arm_matrix_instance_f32* s)
function arm_status (line 53) | arm_status arm_mat_identity_f32(arm_matrix_instance_f32* s, float32_t va...
function arm_status (line 68) | arm_status arm_mat_remainlower_f32(arm_matrix_instance_f32* s)
function arm_status (line 89) | arm_status arm_mat_fill_f32(arm_matrix_instance_f32* s, float32_t *pData...
function arm_status (line 107) | arm_status arm_mat_chol_f32(arm_matrix_instance_f32* s)
function arm_mat_getsubmatrix_f32 (line 145) | void arm_mat_getsubmatrix_f32(arm_matrix_instance_f32* s, arm_matrix_ins...
function arm_mat_setsubmatrix_f32 (line 162) | void arm_mat_setsubmatrix_f32(arm_matrix_instance_f32* a, arm_matrix_ins...
function arm_mat_getcolumn_f32 (line 179) | void arm_mat_getcolumn_f32(arm_matrix_instance_f32* s, float32_t *x, uin...
function arm_mat_setcolumn_f32 (line 187) | void arm_mat_setcolumn_f32(arm_matrix_instance_f32* s, float32_t *x, uin...
function arm_mat_cumsum_f32 (line 195) | void arm_mat_cumsum_f32(arm_matrix_instance_f32* s, float32_t *tmp, floa...
function arm_mat_qr_decompositionT_f32 (line 211) | int arm_mat_qr_decompositionT_f32(arm_matrix_instance_f32 *A, arm_matrix...
FILE: miniAHRS/miniAHRS.c
function Calcultate_RotationMatrix (line 117) | static void Calcultate_RotationMatrix(float *accel, float *mag, float *R)
function EKF_AHRSInit (line 146) | void EKF_AHRSInit(float *accel, float *mag)
function EKF_AHRSUpdate (line 155) | void EKF_AHRSUpdate(float *gyro, float *accel, float *mag, float dt)
function EKF_AHRSGetQ (line 304) | void EKF_AHRSGetQ(float* Q)
function EKF_AHRSGetAngle (line 312) | void EKF_AHRSGetAngle(float* rpy)
FILE: miniIMU/FP/FP_Math.c
type L2F (line 26) | typedef union {
function Q16 (line 33) | Q16 FT_Q16(float f)
function Q16 (line 57) | Q16 FP_SDIV(Q16 num, Q16 den)
function UDIV64 (line 143) | uint32_t UDIV64(uint32_t a, uint32_t b, uint32_t c)
function Q16 (line 164) | Q16 FP_SDIV(Q16 a, Q16 b)
function FP_SqrtC (line 205) | uint32_t FP_SqrtC(uint32_t mant, int expn, uint32_t iter)
function CLZ (line 233) | uint8_t CLZ(uint32_t value)
function Q16 (line 240) | Q16 FP_Sqrt(Q16 xval, uint32_t frac)
function Q16 (line 276) | Q16 FP_SqrtI(Q16 xval, int32_t frac)
function Q16 (line 313) | Q16 FP_ScaleBack(Q16 a, Q16 s)
function Q16 (line 320) | Q16 FP_FastAsin(Q16 a)
function Q16 (line 364) | Q16 FP_FastAtan2(Q16 y, Q16 x)
FILE: miniIMU/FP/FP_Math.h
type Q16 (line 32) | typedef int Q16;
function Q16_FT (line 72) | __inline float Q16_FT(Q16 x)
FILE: miniIMU/FP/FP_Matrix.c
function FP_Matrix_Copy (line 29) | void FP_Matrix_Copy(Q16 *pSrc, int numRows, int numCols, Q16 *pDst)
function FP_Maxtrix_Add (line 52) | int FP_Maxtrix_Add(Q16 *pSrcA, unsigned short numRows, unsigned short nu...
function FP_Maxtrix_Sub (line 106) | int FP_Maxtrix_Sub(Q16 *pSrcA, unsigned short numRows, unsigned short nu...
function FP_Matrix_Multiply (line 160) | int FP_Matrix_Multiply(Q16 *A, int nrows, int ncols, Q16 *B, int mcols, ...
function FP_Matrix_Multiply_With_Transpose (line 179) | void FP_Matrix_Multiply_With_Transpose(Q16 *A, int nrows, int ncols, Q16...
function FP_Matrix_Inverse (line 196) | int FP_Matrix_Inverse(Q16* pSrc, unsigned short n, Q16* pDst)
FILE: miniIMU/FP/FP_miniIMU.c
function FP_EKF_IMUInit (line 144) | void FP_EKF_IMUInit(float *accel, float *gyro)
function FP_EKF_IMUUpdate (line 185) | void FP_EKF_IMUUpdate(float *gyro, float *accel, float dt)
function FP_EKF_IMUGetAngle (line 372) | void FP_EKF_IMUGetAngle(float* rpy)
FILE: miniIMU/miniIMU.c
function EKF_IMUInit (line 144) | void EKF_IMUInit(float *accel, float *gyro)
function EKF_IMUUpdate (line 185) | void EKF_IMUUpdate(float *gyro, float *accel, float dt)
function EKF_IMUGetAngle (line 331) | void EKF_IMUGetAngle(float* rpy)
function EKF_IMUGetQ (line 371) | void EKF_IMUGetQ(float* Q)
FILE: miniIMU/miniMatrix.c
function Matrix_Zero (line 31) | void Matrix_Zero(float *A, unsigned short numRows, unsigned short numCols)
function Matrix_Copy (line 53) | void Matrix_Copy(float *pSrc, unsigned short numRows, unsigned short num...
function Maxtrix_Add (line 98) | int Maxtrix_Add(float *pSrcA, unsigned short numRows, unsigned short num...
function Maxtrix_Sub (line 196) | int Maxtrix_Sub(float *pSrcA, unsigned short numRows, unsigned short num...
function Matrix_Multiply (line 296) | int Matrix_Multiply(float* pSrcA, unsigned short numRowsA, unsigned shor...
function Matrix_Multiply_With_Transpose (line 403) | void Matrix_Multiply_With_Transpose(float *A, unsigned short nrows, unsi...
function Maxtrix_Transpose (line 418) | void Maxtrix_Transpose(float *pSrc, unsigned short nRows, unsigned short...
function Matrix_Inverse (line 457) | int Matrix_Inverse(float * pSrc, unsigned short n, float* pDst)
Condensed preview — 201 files, each showing path, character count, and a content snippet. Download the .json file or copy for the full structured content (4,906K chars).
[
{
"path": ".gitignore",
"chars": 223,
"preview": "# Object files\n*.o\n*.ko\n*.obj\n*.elf\n\n# Precompiled Headers\n*.gch\n*.pch\n\n# Libraries\n*.lib\n*.a\n*.la\n*.lo\n\n# Shared object"
},
{
"path": "Algorithm/inc/CKF.h",
"chars": 4162,
"preview": "/*\nThe MIT License (MIT)\n\nCopyright (c) 2015-? suhetao\n\nPermission is hereby granted, free of charge, to any person obta"
},
{
"path": "Algorithm/inc/Control.h",
"chars": 2578,
"preview": "/*\nThe MIT License (MIT)\n\nCopyright (c) 2015-? suhetao\n\nPermission is hereby granted, free of charge, to any person obta"
},
{
"path": "Algorithm/inc/Double.h",
"chars": 4998,
"preview": "/*\nThe MIT License (MIT)\n\nCopyright (c) 2015-? suhetao\n\nPermission is hereby granted, free of charge, to any person obta"
},
{
"path": "Algorithm/inc/EKF.h",
"chars": 3729,
"preview": "/*\nThe MIT License (MIT)\n\nCopyright (c) 2015-? suhetao\n\nPermission is hereby granted, free of charge, to any person obta"
},
{
"path": "Algorithm/inc/INS_EKF.h",
"chars": 3805,
"preview": "/*\nThe MIT License (MIT)\n\nCopyright (c) 2015-? suhetao\n\nPermission is hereby granted, free of charge, to any person obta"
},
{
"path": "Algorithm/inc/PID.h",
"chars": 2220,
"preview": "/*\nThe MIT License (MIT)\n\nCopyright (c) 2015-? suhetao\n\nPermission is hereby granted, free of charge, to any person obta"
},
{
"path": "Algorithm/inc/Quaternion.h",
"chars": 2390,
"preview": "/*\nThe MIT License (MIT)\n\nCopyright (c) 2015-? suhetao\n\nPermission is hereby granted, free of charge, to any person obta"
},
{
"path": "Algorithm/inc/SRCKF.h",
"chars": 4908,
"preview": "/*\nThe MIT License (MIT)\n\nCopyright (c) 2015-? suhetao\n\nPermission is hereby granted, free of charge, to any person obta"
},
{
"path": "Algorithm/inc/UKF.h",
"chars": 4118,
"preview": "/*\nThe MIT License (MIT)\n\nCopyright (c) 2015-? suhetao\n\nPermission is hereby granted, free of charge, to any person obta"
},
{
"path": "Algorithm/src/CKF.C",
"chars": 18341,
"preview": "/*\nThe MIT License (MIT)\n\nCopyright (c) 2015-? suhetao\n\nPermission is hereby granted, free of charge, to any person obta"
},
{
"path": "Algorithm/src/Control.c",
"chars": 7501,
"preview": "/*\nThe MIT License (MIT)\n\nCopyright (c) 2015-? suhetao\n\nPermission is hereby granted, free of charge, to any person obta"
},
{
"path": "Algorithm/src/EKF.c",
"chars": 13341,
"preview": "/*\nThe MIT License (MIT)\n\nCopyright (c) 2015-? suhetao\n\nPermission is hereby granted, free of charge, to any person obta"
},
{
"path": "Algorithm/src/INS_EKF.c",
"chars": 22460,
"preview": "/*\nThe MIT License (MIT)\n\nCopyright (c) 2015-? suhetao\n\nPermission is hereby granted, free of charge, to any person obta"
},
{
"path": "Algorithm/src/PID.c",
"chars": 1101,
"preview": "/*\nThe MIT License (MIT)\n\nCopyright (c) 2015-? suhetao\n\nPermission is hereby granted, free of charge, to any person obta"
},
{
"path": "Algorithm/src/Quaternion.c",
"chars": 7901,
"preview": "/*\nThe MIT License (MIT)\n\nCopyright (c) 2015-? suhetao\n\nPermission is hereby granted, free of charge, to any person obta"
},
{
"path": "Algorithm/src/SRCKF.c",
"chars": 14204,
"preview": "/*\nThe MIT License (MIT)\n\nCopyright (c) 2015-? suhetao\n\nPermission is hereby granted, free of charge, to any person obta"
},
{
"path": "Algorithm/src/UKF.c",
"chars": 16329,
"preview": "/*\nThe MIT License (MIT)\n\nCopyright (c) 2015-? suhetao\n\nPermission is hereby granted, free of charge, to any person obta"
},
{
"path": "Application/inc/stm32f4_common.h",
"chars": 1167,
"preview": "/*\nThe MIT License (MIT)\n\nCopyright (c) 2015-? suhetao\n\nPermission is hereby granted, free of charge, to any person obta"
},
{
"path": "Application/inc/stm32f4_crc.h",
"chars": 1219,
"preview": "/*\nThe MIT License (MIT)\n\nCopyright (c) 2015-? suhetao\n\nPermission is hereby granted, free of charge, to any person obta"
},
{
"path": "Application/inc/stm32f4_delay.h",
"chars": 1308,
"preview": "/*\nThe MIT License (MIT)\n\nCopyright (c) 2015-? suhetao\n\nPermission is hereby granted, free of charge, to any person obta"
},
{
"path": "Application/inc/stm32f4_dmp.h",
"chars": 1928,
"preview": "/*\nThe MIT License (MIT)\n\nCopyright (c) 2015-? suhetao\n\nPermission is hereby granted, free of charge, to any person obta"
},
{
"path": "Application/inc/stm32f4_exti.h",
"chars": 1859,
"preview": "/*\nThe MIT License (MIT)\n\nCopyright (c) 2015-? suhetao\n\nPermission is hereby granted, free of charge, to any person obta"
},
{
"path": "Application/inc/stm32f4_gps.h",
"chars": 1137,
"preview": "/*\nThe MIT License (MIT)\n\nCopyright (c) 2015-? suhetao\n\nPermission is hereby granted, free of charge, to any person obta"
},
{
"path": "Application/inc/stm32f4_mpu9250.h",
"chars": 10610,
"preview": "/*\nThe MIT License (MIT)\n\nCopyright (c) 2015-? suhetao\n\nPermission is hereby granted, free of charge, to any person obta"
},
{
"path": "Application/inc/stm32f4_ms5611.h",
"chars": 2137,
"preview": "/*\nThe MIT License (MIT)\n\nCopyright (c) 2015-? suhetao\n\nPermission is hereby granted, free of charge, to any person obta"
},
{
"path": "Application/inc/stm32f4_rcc.h",
"chars": 1412,
"preview": "/*\nThe MIT License (MIT)\n\nCopyright (c) 2015-? suhetao\n\nPermission is hereby granted, free of charge, to any person obta"
},
{
"path": "Application/inc/stm32f4_serial.h",
"chars": 1447,
"preview": "/*\nThe MIT License (MIT)\n\nCopyright (c) 2015-? suhetao\n\nPermission is hereby granted, free of charge, to any person obta"
},
{
"path": "Application/inc/stm32f4_string.h",
"chars": 148,
"preview": "#ifndef _STM32F4_STRING_H_\n#define _STM32F4_STRING_H_\n\n#include \"stm32f4xx.h\"\n\nvoid FastMemCpy(uint8_t* dest, uint8_t* s"
},
{
"path": "Application/inc/stm32f4_ublox.h",
"chars": 1555,
"preview": "/*\nThe MIT License (MIT)\n\nCopyright (c) 2015-? suhetao\n\nPermission is hereby granted, free of charge, to any person obta"
},
{
"path": "Application/inc/stm32f4xx_conf.h",
"chars": 3754,
"preview": "/**\n ******************************************************************************\n * @file stm32f4xx_conf.h \n *"
},
{
"path": "Application/inc/stm32f4xx_it.h",
"chars": 2097,
"preview": "/**\n ******************************************************************************\n * @file USART/USART_TwoBoards/"
},
{
"path": "Application/src/main.c",
"chars": 9968,
"preview": "/*\nThe MIT License (MIT)\n\nCopyright (c) 2015-? suhetao\n\nPermission is hereby granted, free of charge, to any person obta"
},
{
"path": "Application/src/stm32f4_delay.c",
"chars": 2683,
"preview": "/*\nThe MIT License (MIT)\n\nCopyright (c) 2015-? suhetao\n\nPermission is hereby granted, free of charge, to any person obta"
},
{
"path": "Application/src/stm32f4_exti.c",
"chars": 2730,
"preview": "/*\nThe MIT License (MIT)\n\nCopyright (c) 2015-? suhetao\n\nPermission is hereby granted, free of charge, to any person obta"
},
{
"path": "Application/src/stm32f4_gps.c",
"chars": 1109,
"preview": "/*\nThe MIT License (MIT)\n\nCopyright (c) 2015-? suhetao\n\nPermission is hereby granted, free of charge, to any person obta"
},
{
"path": "Application/src/stm32f4_mpu9250.c",
"chars": 13742,
"preview": "/*\nThe MIT License (MIT)\n\nCopyright (c) 2015-? suhetao\n\nPermission is hereby granted, free of charge, to any person obta"
},
{
"path": "Application/src/stm32f4_ms5611.c",
"chars": 7065,
"preview": "/*\nThe MIT License (MIT)\n\nCopyright (c) 2015-? suhetao\n\nPermission is hereby granted, free of charge, to any person obta"
},
{
"path": "Application/src/stm32f4_rcc.c",
"chars": 2118,
"preview": "/*\nThe MIT License (MIT)\n\nCopyright (c) 2015-? suhetao\n\nPermission is hereby granted, free of charge, to any person obta"
},
{
"path": "Application/src/stm32f4_serial.c",
"chars": 5598,
"preview": "/*\nThe MIT License (MIT)\n\nCopyright (c) 2015-? suhetao\n\nPermission is hereby granted, free of charge, to any person obta"
},
{
"path": "Application/src/stm32f4_string.c",
"chars": 995,
"preview": "#include \"stm32f4_string.h\"\n\n//make sure 4-bytes aligned\n__asm void FastMemCpy(uint8_t* dest, uint8_t* src, uint16_t siz"
},
{
"path": "Application/src/stm32f4_ublox.c",
"chars": 6019,
"preview": "/*\nThe MIT License (MIT)\n\nCopyright (c) 2015-? suhetao\n\nPermission is hereby granted, free of charge, to any person obta"
},
{
"path": "Application/src/system_stm32f4xx.c",
"chars": 21447,
"preview": "/**\n ******************************************************************************\n * @file system_stm32f4xx.c\n *"
},
{
"path": "Common/inc/Memory.h",
"chars": 1325,
"preview": "/*\nThe MIT License (MIT)\n\nCopyright (c) 2015-? suhetao\n\nPermission is hereby granted, free of charge, to any person obta"
},
{
"path": "Common/src/Memory.c",
"chars": 9897,
"preview": "/*\nThe MIT License (MIT)\n\nCopyright (c) 2015-? suhetao\n\nPermission is hereby granted, free of charge, to any person obta"
},
{
"path": "Data/inc/Fifo.h",
"chars": 1396,
"preview": "/*\nThe MIT License (MIT)\n\nCopyright (c) 2015-? suhetao\n\nPermission is hereby granted, free of charge, to any person obta"
},
{
"path": "Data/inc/Queue.h",
"chars": 1730,
"preview": "/*\nThe MIT License (MIT)\n\nCopyright (c) 2015-? suhetao\n\nPermission is hereby granted, free of charge, to any person obta"
},
{
"path": "Data/src/Fifo.c",
"chars": 1937,
"preview": "/*\nThe MIT License (MIT)\n\nCopyright (c) 2015-? suhetao\n\nPermission is hereby granted, free of charge, to any person obta"
},
{
"path": "Data/src/Queue.c",
"chars": 1906,
"preview": "/*\nThe MIT License (MIT)\n\nCopyright (c) 2015-? suhetao\n\nPermission is hereby granted, free of charge, to any person obta"
},
{
"path": "Drivers/inc/stm32f4_exti.h",
"chars": 1483,
"preview": "/*\nThe MIT License (MIT)\n\nCopyright (c) 2015-? suhetao\n\nPermission is hereby granted, free of charge, to any person obta"
},
{
"path": "Drivers/inc/stm32f4_gpio.h",
"chars": 1607,
"preview": "/*\nThe MIT License (MIT)\n\nCopyright (c) 2015-? suhetao\n\nPermission is hereby granted, free of charge, to any person obta"
},
{
"path": "Drivers/inc/stm32f4_spi.h",
"chars": 2927,
"preview": "/*\nThe MIT License (MIT)\n\nCopyright (c) 2015-? suhetao\n\nPermission is hereby granted, free of charge, to any person obta"
},
{
"path": "Drivers/inc/stm32f4_usart.h",
"chars": 2430,
"preview": "/*\nThe MIT License (MIT)\n\nCopyright (c) 2015-? suhetao\n\nPermission is hereby granted, free of charge, to any person obta"
},
{
"path": "Drivers/src/stm32f4_exti.c",
"chars": 1821,
"preview": "/*\nThe MIT License (MIT)\n\nCopyright (c) 2015-? suhetao\n\nPermission is hereby granted, free of charge, to any person obta"
},
{
"path": "Drivers/src/stm32f4_gpio.c",
"chars": 1250,
"preview": "/*\nThe MIT License (MIT)\n\nCopyright (c) 2015-? suhetao\n\nPermission is hereby granted, free of charge, to any person obta"
},
{
"path": "Drivers/src/stm32f4_spi.c",
"chars": 9160,
"preview": "/*\nThe MIT License (MIT)\n\nCopyright (c) 2015-? suhetao\n\nPermission is hereby granted, free of charge, to any person obta"
},
{
"path": "Drivers/src/stm32f4_usart.c",
"chars": 6825,
"preview": "/*\nThe MIT License (MIT)\n\nCopyright (c) 2015-? suhetao\n\nPermission is hereby granted, free of charge, to any person obta"
},
{
"path": "Gps/inc/Map.h",
"chars": 1829,
"preview": "/*\nThe MIT License (MIT)\n\nCopyright (c) 2015-? suhetao\n\nPermission is hereby granted, free of charge, to any person obta"
},
{
"path": "Gps/inc/Nema.h",
"chars": 3671,
"preview": "/*\nThe MIT License (MIT)\n\nCopyright (c) 2015-? suhetao\n\nPermission is hereby granted, free of charge, to any person obta"
},
{
"path": "Gps/src/Map.c",
"chars": 6002,
"preview": "/*\nThe MIT License (MIT)\n\nCopyright (c) 2015-? suhetao\n\nPermission is hereby granted, free of charge, to any person obta"
},
{
"path": "Gps/src/Nema.c",
"chars": 13793,
"preview": "/*\nThe MIT License (MIT)\n\nCopyright (c) 2015-? suhetao\n\nPermission is hereby granted, free of charge, to any person obta"
},
{
"path": "LICENSE",
"chars": 1075,
"preview": "The MIT License (MIT)\n\nCopyright (c) 2015 suhetao\n\nPermission is hereby granted, free of charge, to any person obtaining"
},
{
"path": "Libraries/CMSIS/Device/ST/STM32F4xx/Include/stm32f4xx.h",
"chars": 693350,
"preview": "/**\n ******************************************************************************\n * @file stm32f4xx.h\n * @autho"
},
{
"path": "Libraries/CMSIS/Device/ST/STM32F4xx/Include/system_stm32f4xx.h",
"chars": 2130,
"preview": "/**\n ******************************************************************************\n * @file system_stm32f4xx.h\n *"
},
{
"path": "Libraries/CMSIS/Device/ST/STM32F4xx/Release_Notes.html",
"chars": 33610,
"preview": "<!DOCTYPE HTML PUBLIC \"-//W3C//DTD HTML 4.01 Transitional//EN\">\n<html xmlns:v=\"urn:schemas-microsoft-com:vml\" xmlns:o=\"u"
},
{
"path": "Libraries/CMSIS/Device/ST/STM32F4xx/Source/Templates/TASKING/cstart_thumb2.asm",
"chars": 3530,
"preview": "\n\n;; NOTE: To allow the use of this file for both ARMv6M and ARMv7M,\n;; we will only use 16-bit Thumb intruction"
},
{
"path": "Libraries/CMSIS/Device/ST/STM32F4xx/Source/Templates/TrueSTUDIO/startup_stm32f401xx.s",
"chars": 20252,
"preview": "/**\n ******************************************************************************\n * @file startup_stm32f401xx."
},
{
"path": "Libraries/CMSIS/Device/ST/STM32F4xx/Source/Templates/TrueSTUDIO/startup_stm32f40_41xxx.s",
"chars": 23633,
"preview": "/**\n ******************************************************************************\n * @file startup_stm32f40_41x"
},
{
"path": "Libraries/CMSIS/Device/ST/STM32F4xx/Source/Templates/TrueSTUDIO/startup_stm32f40xx.s",
"chars": 23717,
"preview": "/**\n ******************************************************************************\n * @file startup_stm32f40_41x"
},
{
"path": "Libraries/CMSIS/Device/ST/STM32F4xx/Source/Templates/TrueSTUDIO/startup_stm32f427_437xx.s",
"chars": 25335,
"preview": "/**\n ******************************************************************************\n * @file startup_stm32f427_43"
},
{
"path": "Libraries/CMSIS/Device/ST/STM32F4xx/Source/Templates/TrueSTUDIO/startup_stm32f427xx.s",
"chars": 25428,
"preview": "/**\n ******************************************************************************\n * @file startup_stm32f427x.s"
},
{
"path": "Libraries/CMSIS/Device/ST/STM32F4xx/Source/Templates/TrueSTUDIO/startup_stm32f429_439xx.s",
"chars": 25335,
"preview": "/**\n ******************************************************************************\n * @file startup_stm32f429_43"
},
{
"path": "Libraries/CMSIS/Device/ST/STM32F4xx/Source/Templates/arm/startup_stm32f401xx.s",
"chars": 25435,
"preview": ";******************** (C) COPYRIGHT 2013 STMicroelectronics ********************\n;* File Name : startup_stm32f4"
},
{
"path": "Libraries/CMSIS/Device/ST/STM32F4xx/Source/Templates/arm/startup_stm32f40_41xxx.s",
"chars": 29231,
"preview": ";******************** (C) COPYRIGHT 2013 STMicroelectronics ********************\n;* File Name : startup_stm32f4"
},
{
"path": "Libraries/CMSIS/Device/ST/STM32F4xx/Source/Templates/arm/startup_stm32f40xx.s",
"chars": 29261,
"preview": ";******************** (C) COPYRIGHT 2013 STMicroelectronics ********************\n;* File Name : startup_stm32f4"
},
{
"path": "Libraries/CMSIS/Device/ST/STM32F4xx/Source/Templates/arm/startup_stm32f427_437xx.s",
"chars": 30525,
"preview": ";******************** (C) COPYRIGHT 2013 STMicroelectronics ********************\n;* File Name : startup_stm32f4"
},
{
"path": "Libraries/CMSIS/Device/ST/STM32F4xx/Source/Templates/arm/startup_stm32f427x.s",
"chars": 30599,
"preview": ";******************** (C) COPYRIGHT 2013 STMicroelectronics ********************\n;* File Name : startup_stm32f4"
},
{
"path": "Libraries/CMSIS/Device/ST/STM32F4xx/Source/Templates/arm/startup_stm32f429_439xx.s",
"chars": 30676,
"preview": ";******************** (C) COPYRIGHT 2013 STMicroelectronics ********************\n;* File Name : startup_stm32f4"
},
{
"path": "Libraries/CMSIS/Device/ST/STM32F4xx/Source/Templates/gcc_ride7/startup_stm32f401xx.s",
"chars": 20208,
"preview": "/**\n ******************************************************************************\n * @file startup_stm32f40xx.s"
},
{
"path": "Libraries/CMSIS/Device/ST/STM32F4xx/Source/Templates/gcc_ride7/startup_stm32f40_41xxx.s",
"chars": 23596,
"preview": "/**\n ******************************************************************************\n * @file startup_stm32f40_41x"
},
{
"path": "Libraries/CMSIS/Device/ST/STM32F4xx/Source/Templates/gcc_ride7/startup_stm32f40xx.s",
"chars": 23673,
"preview": "/**\n ******************************************************************************\n * @file startup_stm32f40xx.s"
},
{
"path": "Libraries/CMSIS/Device/ST/STM32F4xx/Source/Templates/gcc_ride7/startup_stm32f427_437xx.s",
"chars": 25183,
"preview": "/**\n ******************************************************************************\n * @file startup_stm32f427_43"
},
{
"path": "Libraries/CMSIS/Device/ST/STM32F4xx/Source/Templates/gcc_ride7/startup_stm32f427x.s",
"chars": 25265,
"preview": "/**\n ******************************************************************************\n * @file startup_stm32f427x.s"
},
{
"path": "Libraries/CMSIS/Device/ST/STM32F4xx/Source/Templates/gcc_ride7/startup_stm32f429_439xx.s",
"chars": 25183,
"preview": "/**\n ******************************************************************************\n * @file startup_stm32f429_43"
},
{
"path": "Libraries/CMSIS/Device/ST/STM32F4xx/Source/Templates/iar/startup_stm32f401xx.s",
"chars": 20557,
"preview": ";/******************** (C) COPYRIGHT 2013 STMicroelectronics ********************\n;* File Name : startup_stm32f"
},
{
"path": "Libraries/CMSIS/Device/ST/STM32F4xx/Source/Templates/iar/startup_stm32f40_41xxx.s",
"chars": 24063,
"preview": ";/******************** (C) COPYRIGHT 2013 STMicroelectronics ********************\n;* File Name : startup_stm32f"
},
{
"path": "Libraries/CMSIS/Device/ST/STM32F4xx/Source/Templates/iar/startup_stm32f40xx.s",
"chars": 24150,
"preview": ";/******************** (C) COPYRIGHT 2013 STMicroelectronics ********************\n;* File Name : startup_stm32f"
},
{
"path": "Libraries/CMSIS/Device/ST/STM32F4xx/Source/Templates/iar/startup_stm32f427_437xx.s",
"chars": 25526,
"preview": ";/******************** (C) COPYRIGHT 2013 STMicroelectronics ********************\n;* File Name : startup_stm32f"
},
{
"path": "Libraries/CMSIS/Device/ST/STM32F4xx/Source/Templates/iar/startup_stm32f427x.s",
"chars": 25893,
"preview": ";/******************** (C) COPYRIGHT 2013 STMicroelectronics ********************\n;* File Name : startup_stm32f"
},
{
"path": "Libraries/CMSIS/Device/ST/STM32F4xx/Source/Templates/iar/startup_stm32f429_439xx.s",
"chars": 25801,
"preview": ";/******************** (C) COPYRIGHT 2013 STMicroelectronics ********************\n;* File Name : startup_stm32f"
},
{
"path": "Libraries/CMSIS/Device/ST/STM32F4xx/Source/Templates/system_stm32f4xx.c",
"chars": 38281,
"preview": "/**\n ******************************************************************************\n * @file system_stm32f4xx.c\n *"
},
{
"path": "Libraries/CMSIS/Include/arm_common_tables.h",
"chars": 4583,
"preview": "/* ----------------------------------------------------------------------\n* Copyright (C) 2010-2013 ARM Limited. All rig"
},
{
"path": "Libraries/CMSIS/Include/arm_const_structs.h",
"chars": 3581,
"preview": "/* ----------------------------------------------------------------------\n* Copyright (C) 2010-2013 ARM Limited. All rig"
},
{
"path": "Libraries/CMSIS/Include/arm_math.h",
"chars": 235138,
"preview": "/* ----------------------------------------------------------------------\n* Copyright (C) 2010-2013 ARM Limited. All rig"
},
{
"path": "Libraries/CMSIS/Include/core_cm0.h",
"chars": 32360,
"preview": "/**************************************************************************//**\n * @file core_cm0.h\n * @brief CMS"
},
{
"path": "Libraries/CMSIS/Include/core_cm0plus.h",
"chars": 39636,
"preview": "/**************************************************************************//**\n * @file core_cm0plus.h\n * @brief "
},
{
"path": "Libraries/CMSIS/Include/core_cm3.h",
"chars": 98205,
"preview": "/**************************************************************************//**\n * @file core_cm3.h\n * @brief CMS"
},
{
"path": "Libraries/CMSIS/Include/core_cm4.h",
"chars": 107370,
"preview": "/**************************************************************************//**\n * @file core_cm4.h\n * @brief CMS"
},
{
"path": "Libraries/CMSIS/Include/core_cm4_simd.h",
"chars": 22062,
"preview": "/**************************************************************************//**\n * @file core_cm4_simd.h\n * @brief "
},
{
"path": "Libraries/CMSIS/Include/core_cmFunc.h",
"chars": 16510,
"preview": "/**************************************************************************//**\n * @file core_cmFunc.h\n * @brief "
},
{
"path": "Libraries/CMSIS/Include/core_cmInstr.h",
"chars": 19825,
"preview": "/**************************************************************************//**\n * @file core_cmInstr.h\n * @brief "
},
{
"path": "Libraries/CMSIS/Include/core_sc000.h",
"chars": 40931,
"preview": "/**************************************************************************//**\n * @file core_sc000.h\n * @brief C"
},
{
"path": "Libraries/CMSIS/Include/core_sc300.h",
"chars": 96613,
"preview": "/**************************************************************************//**\n * @file core_sc300.h\n * @brief C"
},
{
"path": "Libraries/CMSIS/README.txt",
"chars": 1479,
"preview": "* -------------------------------------------------------------------\n* Copyright (C) 2011-2013 ARM Limited. All rights "
},
{
"path": "Libraries/CMSIS/index.html",
"chars": 534,
"preview": "<!DOCTYPE html PUBLIC \"-//W3C//DTD XHTML 1.0 Transitional//EN\" \"http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd"
},
{
"path": "Libraries/STM32F4xx_StdPeriph_Driver/Release_Notes.html",
"chars": 77431,
"preview": "<!DOCTYPE HTML PUBLIC \"-//W3C//DTD HTML 4.01 Transitional//EN\">\n<html xmlns:v=\"urn:schemas-microsoft-com:vml\" xmlns:o=\"u"
},
{
"path": "Libraries/STM32F4xx_StdPeriph_Driver/inc/misc.h",
"chars": 6748,
"preview": "/**\n ******************************************************************************\n * @file misc.h\n * @author MC"
},
{
"path": "Libraries/STM32F4xx_StdPeriph_Driver/inc/stm32f4xx_adc.h",
"chars": 32186,
"preview": "/**\n ******************************************************************************\n * @file stm32f4xx_adc.h\n * @a"
},
{
"path": "Libraries/STM32F4xx_StdPeriph_Driver/inc/stm32f4xx_can.h",
"chars": 26676,
"preview": "/**\n ******************************************************************************\n * @file stm32f4xx_can.h\n * @a"
},
{
"path": "Libraries/STM32F4xx_StdPeriph_Driver/inc/stm32f4xx_crc.h",
"chars": 2335,
"preview": "/**\n ******************************************************************************\n * @file stm32f4xx_crc.h\n * @a"
},
{
"path": "Libraries/STM32F4xx_StdPeriph_Driver/inc/stm32f4xx_cryp.h",
"chars": 14099,
"preview": "/**\n ******************************************************************************\n * @file stm32f4xx_cryp.h\n * @"
},
{
"path": "Libraries/STM32F4xx_StdPeriph_Driver/inc/stm32f4xx_dac.h",
"chars": 14644,
"preview": "/**\n ******************************************************************************\n * @file stm32f4xx_dac.h\n * @a"
},
{
"path": "Libraries/STM32F4xx_StdPeriph_Driver/inc/stm32f4xx_dbgmcu.h",
"chars": 4189,
"preview": "/**\n ******************************************************************************\n * @file stm32f4xx_dbgmcu.h\n *"
},
{
"path": "Libraries/STM32F4xx_StdPeriph_Driver/inc/stm32f4xx_dcmi.h",
"chars": 12667,
"preview": "/**\n ******************************************************************************\n * @file stm32f4xx_dcmi.h\n * @"
},
{
"path": "Libraries/STM32F4xx_StdPeriph_Driver/inc/stm32f4xx_dma.h",
"chars": 28275,
"preview": "/**\n ******************************************************************************\n * @file stm32f4xx_dma.h\n * @a"
},
{
"path": "Libraries/STM32F4xx_StdPeriph_Driver/inc/stm32f4xx_dma2d.h",
"chars": 19091,
"preview": "/**\n ******************************************************************************\n * @file stm32f4xx_dma2d.h\n * "
},
{
"path": "Libraries/STM32F4xx_StdPeriph_Driver/inc/stm32f4xx_exti.h",
"chars": 7831,
"preview": "/**\n ******************************************************************************\n * @file stm32f4xx_exti.h\n * @"
},
{
"path": "Libraries/STM32F4xx_StdPeriph_Driver/inc/stm32f4xx_flash.h",
"chars": 23950,
"preview": "/**\n ******************************************************************************\n * @file stm32f4xx_flash.h\n * "
},
{
"path": "Libraries/STM32F4xx_StdPeriph_Driver/inc/stm32f4xx_fmc.h",
"chars": 43654,
"preview": "/**\n ******************************************************************************\n * @file stm32f4xx_fmc.h\n * @a"
},
{
"path": "Libraries/STM32F4xx_StdPeriph_Driver/inc/stm32f4xx_fsmc.h",
"chars": 26508,
"preview": "/**\n ******************************************************************************\n * @file stm32f4xx_fsmc.h\n * @"
},
{
"path": "Libraries/STM32F4xx_StdPeriph_Driver/inc/stm32f4xx_gpio.h",
"chars": 22338,
"preview": "/**\n ******************************************************************************\n * @file stm32f4xx_gpio.h\n * @"
},
{
"path": "Libraries/STM32F4xx_StdPeriph_Driver/inc/stm32f4xx_hash.h",
"chars": 9829,
"preview": "/**\n ******************************************************************************\n * @file stm32f4xx_hash.h\n * @"
},
{
"path": "Libraries/STM32F4xx_StdPeriph_Driver/inc/stm32f4xx_i2c.h",
"chars": 31230,
"preview": "/**\n ******************************************************************************\n * @file stm32f4xx_i2c.h\n * @a"
},
{
"path": "Libraries/STM32F4xx_StdPeriph_Driver/inc/stm32f4xx_iwdg.h",
"chars": 4194,
"preview": "/**\n ******************************************************************************\n * @file stm32f4xx_iwdg.h\n * @"
},
{
"path": "Libraries/STM32F4xx_StdPeriph_Driver/inc/stm32f4xx_ltdc.h",
"chars": 20534,
"preview": "/**\n ******************************************************************************\n * @file stm32f4xx_ltdc.h\n * @"
},
{
"path": "Libraries/STM32F4xx_StdPeriph_Driver/inc/stm32f4xx_pwr.h",
"chars": 7393,
"preview": "/**\n ******************************************************************************\n * @file stm32f4xx_pwr.h\n * @a"
},
{
"path": "Libraries/STM32F4xx_StdPeriph_Driver/inc/stm32f4xx_rcc.h",
"chars": 28975,
"preview": "/**\n ******************************************************************************\n * @file stm32f4xx_rcc.h\n * @a"
},
{
"path": "Libraries/STM32F4xx_StdPeriph_Driver/inc/stm32f4xx_rng.h",
"chars": 3840,
"preview": "/**\n ******************************************************************************\n * @file stm32f4xx_rng.h\n * @a"
},
{
"path": "Libraries/STM32F4xx_StdPeriph_Driver/inc/stm32f4xx_rtc.h",
"chars": 39667,
"preview": "/**\n ******************************************************************************\n * @file stm32f4xx_rtc.h\n * @a"
},
{
"path": "Libraries/STM32F4xx_StdPeriph_Driver/inc/stm32f4xx_sai.h",
"chars": 24861,
"preview": "/**\n ******************************************************************************\n * @file stm32f4xx_sai.h\n * @a"
},
{
"path": "Libraries/STM32F4xx_StdPeriph_Driver/inc/stm32f4xx_sdio.h",
"chars": 22243,
"preview": "/**\n ******************************************************************************\n * @file stm32f4xx_sdio.h\n * @"
},
{
"path": "Libraries/STM32F4xx_StdPeriph_Driver/inc/stm32f4xx_spi.h",
"chars": 20631,
"preview": "/**\n ******************************************************************************\n * @file stm32f4xx_spi.h\n * @a"
},
{
"path": "Libraries/STM32F4xx_StdPeriph_Driver/inc/stm32f4xx_syscfg.h",
"chars": 8727,
"preview": "/**\n ******************************************************************************\n * @file stm32f4xx_syscfg.h\n *"
},
{
"path": "Libraries/STM32F4xx_StdPeriph_Driver/inc/stm32f4xx_tim.h",
"chars": 50541,
"preview": "/**\n ******************************************************************************\n * @file stm32f4xx_tim.h\n * @a"
},
{
"path": "Libraries/STM32F4xx_StdPeriph_Driver/inc/stm32f4xx_usart.h",
"chars": 17506,
"preview": "/**\n ******************************************************************************\n * @file stm32f4xx_usart.h\n * "
},
{
"path": "Libraries/STM32F4xx_StdPeriph_Driver/inc/stm32f4xx_wwdg.h",
"chars": 3433,
"preview": "/**\n ******************************************************************************\n * @file stm32f4xx_wwdg.h\n * @"
},
{
"path": "Libraries/STM32F4xx_StdPeriph_Driver/src/misc.c",
"chars": 11196,
"preview": "/**\n ******************************************************************************\n * @file misc.c\n * @author MC"
},
{
"path": "Libraries/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_adc.c",
"chars": 67327,
"preview": "/**\n ******************************************************************************\n * @file stm32f4xx_adc.c\n * @a"
},
{
"path": "Libraries/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_can.c",
"chars": 58582,
"preview": "/**\n ******************************************************************************\n * @file stm32f4xx_can.c\n * @a"
},
{
"path": "Libraries/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_crc.c",
"chars": 3465,
"preview": "/**\n ******************************************************************************\n * @file stm32f4xx_crc.c\n * @a"
},
{
"path": "Libraries/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_cryp.c",
"chars": 34572,
"preview": "/**\n ******************************************************************************\n * @file stm32f4xx_cryp.c\n * @"
},
{
"path": "Libraries/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_cryp_aes.c",
"chars": 56321,
"preview": "/**\n ******************************************************************************\n * @file stm32f4xx_cryp_aes.c\n "
},
{
"path": "Libraries/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_cryp_des.c",
"chars": 9516,
"preview": "/**\n ******************************************************************************\n * @file stm32f4xx_cryp_des.c\n "
},
{
"path": "Libraries/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_cryp_tdes.c",
"chars": 10221,
"preview": "/**\n ******************************************************************************\n * @file stm32f4xx_cryp_tdes.c\n"
},
{
"path": "Libraries/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_dac.c",
"chars": 26092,
"preview": "/**\n ******************************************************************************\n * @file stm32f4xx_dac.c\n * @a"
},
{
"path": "Libraries/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_dbgmcu.c",
"chars": 6631,
"preview": "/**\n ******************************************************************************\n * @file stm32f4xx_dbgmcu.c\n *"
},
{
"path": "Libraries/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_dcmi.c",
"chars": 18256,
"preview": "/**\n ******************************************************************************\n * @file stm32f4xx_dcmi.c\n * @"
},
{
"path": "Libraries/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_dma.c",
"chars": 51651,
"preview": "/**\n ******************************************************************************\n * @file stm32f4xx_dma.c\n * @a"
},
{
"path": "Libraries/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_dma2d.c",
"chars": 26254,
"preview": "/**\n ******************************************************************************\n * @file stm32f4xx_dma2d.c\n * "
},
{
"path": "Libraries/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_exti.c",
"chars": 9609,
"preview": "/**\n ******************************************************************************\n * @file stm32f4xx_exti.c\n * @"
},
{
"path": "Libraries/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_flash.c",
"chars": 59351,
"preview": "/**\n ******************************************************************************\n * @file stm32f4xx_flash.c\n * "
},
{
"path": "Libraries/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_fmc.c",
"chars": 55422,
"preview": "/**\n ******************************************************************************\n * @file stm32f4xx_fmc.c\n * @a"
},
{
"path": "Libraries/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_fsmc.c",
"chars": 41010,
"preview": "/**\n ******************************************************************************\n * @file stm32f4xx_fsmc.c\n * @"
},
{
"path": "Libraries/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_gpio.c",
"chars": 24593,
"preview": "/**\n ******************************************************************************\n * @file stm32f4xx_gpio.c\n * @"
},
{
"path": "Libraries/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_hash.c",
"chars": 25938,
"preview": "/**\n ******************************************************************************\n * @file stm32f4xx_hash.c\n * @"
},
{
"path": "Libraries/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_hash_md5.c",
"chars": 9276,
"preview": "/**\n ******************************************************************************\n * @file stm32f4xx_hash_md5.c\n "
},
{
"path": "Libraries/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_hash_sha1.c",
"chars": 9472,
"preview": "/**\n ******************************************************************************\n * @file stm32f4xx_hash_sha1.c\n"
},
{
"path": "Libraries/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_i2c.c",
"chars": 52907,
"preview": "/**\n ******************************************************************************\n * @file stm32f4xx_i2c.c\n * @a"
},
{
"path": "Libraries/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_iwdg.c",
"chars": 9084,
"preview": "/**\n ******************************************************************************\n * @file stm32f4xx_iwdg.c\n * @"
},
{
"path": "Libraries/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_ltdc.c",
"chars": 38568,
"preview": "/**\n ******************************************************************************\n * @file stm32f4xx_ltdc.c\n * @"
},
{
"path": "Libraries/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_pwr.c",
"chars": 35123,
"preview": "/**\n ******************************************************************************\n * @file stm32f4xx_pwr.c\n * @a"
},
{
"path": "Libraries/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_rcc.c",
"chars": 91946,
"preview": "/**\n ******************************************************************************\n * @file stm32f4xx_rcc.c\n * @a"
},
{
"path": "Libraries/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_rng.c",
"chars": 13665,
"preview": "/**\n ******************************************************************************\n * @file stm32f4xx_rng.c\n * @a"
},
{
"path": "Libraries/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_rtc.c",
"chars": 100132,
"preview": "/**\n ******************************************************************************\n * @file stm32f4xx_rtc.c\n * @a"
},
{
"path": "Libraries/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_sai.c",
"chars": 45370,
"preview": "/**\n ******************************************************************************\n * @file stm32f4xx_sai.c\n * @a"
},
{
"path": "Libraries/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_sdio.c",
"chars": 37948,
"preview": "/**\n ******************************************************************************\n * @file stm32f4xx_sdio.c\n * @"
},
{
"path": "Libraries/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_spi.c",
"chars": 51004,
"preview": "/**\n ******************************************************************************\n * @file stm32f4xx_spi.c\n * @a"
},
{
"path": "Libraries/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_syscfg.c",
"chars": 9337,
"preview": "/**\n ******************************************************************************\n * @file stm32f4xx_syscfg.c\n *"
},
{
"path": "Libraries/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_tim.c",
"chars": 121369,
"preview": "/**\n ******************************************************************************\n * @file stm32f4xx_tim.c\n * @a"
},
{
"path": "Libraries/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_usart.c",
"chars": 56465,
"preview": "/**\n ******************************************************************************\n * @file stm32f4xx_usart.c\n * "
},
{
"path": "Libraries/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_wwdg.c",
"chars": 10244,
"preview": "/**\n ******************************************************************************\n * @file stm32f4xx_wwdg.c\n * @"
},
{
"path": "Libraries/eMPL/dmpKey.h",
"chars": 18848,
"preview": "/*\n $License:\n Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.\n $\n */\n#ifndef DMPKEY_H__\n#define DMPK"
},
{
"path": "Libraries/eMPL/dmpmap.h",
"chars": 6501,
"preview": "/*\n $License:\n Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.\n $\n */\n#ifndef DMPMAP_H\n#define DMPMAP"
},
{
"path": "Libraries/eMPL/inv_mpu.c",
"chars": 88715,
"preview": "/*\n$License:\nCopyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.\nSee included License.txt for License i"
},
{
"path": "Libraries/eMPL/inv_mpu.h",
"chars": 4641,
"preview": "/*\n $License:\n Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.\n See included License.txt for "
},
{
"path": "Libraries/eMPL/inv_mpu_dmp_motion_driver.c",
"chars": 56966,
"preview": "/*\n $License:\n Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.\n See included License.txt for "
},
{
"path": "Libraries/eMPL/inv_mpu_dmp_motion_driver.h",
"chars": 3441,
"preview": "/*\n $License:\n Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.\n See included License.txt for "
},
{
"path": "Math/inc/FastMath.h",
"chars": 4891,
"preview": "/*\nThe MIT License (MIT)\n\nCopyright (c) 2015-? suhetao\n\nPermission is hereby granted, free of charge, to any person obta"
},
{
"path": "Math/src/FastMath.c",
"chars": 20923,
"preview": "/*\nThe MIT License (MIT)\n\nCopyright (c) 2015-? suhetao\n\nPermission is hereby granted, free of charge, to any person obta"
},
{
"path": "Matrix/inc/DoubleMatrix.h",
"chars": 1162,
"preview": "/*\nThe MIT License (MIT)\n\nCopyright (c) 2015-? suhetao\n\nPermission is hereby granted, free of charge, to any person obta"
},
{
"path": "Matrix/inc/Matrix.h",
"chars": 2148,
"preview": "/*\nThe MIT License (MIT)\n\nCopyright (c) 2015-? suhetao\n\nPermission is hereby granted, free of charge, to any person obta"
},
{
"path": "Matrix/src/DoubleMatrix.c",
"chars": 1132,
"preview": "/*\nThe MIT License (MIT)\n\nCopyright (c) 2015-? suhetao\n\nPermission is hereby granted, free of charge, to any person obta"
},
{
"path": "Matrix/src/Matrix.c",
"chars": 6663,
"preview": "/*\nThe MIT License (MIT)\n\nCopyright (c) 2015-? suhetao\n\nPermission is hereby granted, free of charge, to any person obta"
},
{
"path": "Project/JLinkLog.txt",
"chars": 32935,
"preview": "\nT169C 000:523 SEGGER J-Link V4.96f Log File (0000ms, 0309ms total)\nT169C 000:523 DLL Compiled: Feb 4 2015 14:13:39 (00"
},
{
"path": "Project/JLinkSettings.ini",
"chars": 636,
"preview": "[BREAKPOINTS]\nForceImpTypeAny = 0\nShowInfoWin = 1\nEnableFlashBP = 2\nBPDuringExecution = 0\n[CFI]\nCFISize = 0x00\nCFIAddr ="
},
{
"path": "Project/stm32f4_dmp.uvopt",
"chars": 35787,
"preview": "<?xml version=\"1.0\" encoding=\"UTF-8\" standalone=\"no\" ?>\n<ProjectOpt xmlns:xsi=\"http://www.w3.org/2001/XMLSchema-instance"
},
{
"path": "Project/stm32f4_dmp.uvproj",
"chars": 26725,
"preview": "<?xml version=\"1.0\" encoding=\"UTF-8\" standalone=\"no\" ?>\n<Project xmlns:xsi=\"http://www.w3.org/2001/XMLSchema-instance\" x"
},
{
"path": "README.md",
"chars": 1809,
"preview": "# stm32f4_mpu9250\nAccess the data of 3-axis magnetometer and DMP from MPU9250 with SPI interface \n\nAll data fusion (incl"
},
{
"path": "miniAHRS/miniAHRS.c",
"chars": 12219,
"preview": "/*\nThe MIT License (MIT)\n\nCopyright (c) 2015-? suhetao\n\nPermission is hereby granted, free of charge, to any person obta"
},
{
"path": "miniAHRS/miniAHRS.h",
"chars": 1692,
"preview": "/*\nThe MIT License (MIT)\n\nCopyright (c) 2015-? suhetao\n\nPermission is hereby granted, free of charge, to any person obta"
},
{
"path": "miniIMU/FP/FP_Math.c",
"chars": 9366,
"preview": "/*\nThe MIT License (MIT)\n\nCopyright (c) 2015-? suhetao\n\nPermission is hereby granted, free of charge, to any person obta"
},
{
"path": "miniIMU/FP/FP_Math.h",
"chars": 2650,
"preview": "/*\nThe MIT License (MIT)\n\nCopyright (c) 2015-? suhetao\n\nPermission is hereby granted, free of charge, to any person obta"
},
{
"path": "miniIMU/FP/FP_Matrix.c",
"chars": 10934,
"preview": "/*\nThe MIT License (MIT)\n\nCopyright (c) 2015-? suhetao\n\nPermission is hereby granted, free of charge, to any person obta"
},
{
"path": "miniIMU/FP/FP_Matrix.h",
"chars": 1752,
"preview": "/*\nThe MIT License (MIT)\n\nCopyright (c) 2015-? suhetao\n\nPermission is hereby granted, free of charge, to any person obta"
},
{
"path": "miniIMU/FP/FP_miniIMU.c",
"chars": 13426,
"preview": "/*\nThe MIT License (MIT)\n\nCopyright (c) 2015-? suhetao\n\nPermission is hereby granted, free of charge, to any person obta"
},
{
"path": "miniIMU/FP/FP_miniIMU.h",
"chars": 1508,
"preview": "/*\nThe MIT License (MIT)\n\nCopyright (c) 2015-? suhetao\n\nPermission is hereby granted, free of charge, to any person obta"
},
{
"path": "miniIMU/Usage.txt",
"chars": 1668,
"preview": "/*\nThe MIT License (MIT)\n\nCopyright (c) 2015-? suhetao\n\nPermission is hereby granted, free of charge, to any person obta"
},
{
"path": "miniIMU/miniIMU.c",
"chars": 11823,
"preview": "/*\nThe MIT License (MIT)\n\nCopyright (c) 2015-? suhetao\n\nPermission is hereby granted, free of charge, to any person obta"
},
{
"path": "miniIMU/miniIMU.h",
"chars": 1702,
"preview": "/*\nThe MIT License (MIT)\n\nCopyright (c) 2015-? suhetao\n\nPermission is hereby granted, free of charge, to any person obta"
},
{
"path": "miniIMU/miniMatrix.c",
"chars": 17713,
"preview": "/*\nThe MIT License (MIT)\n\nCopyright (c) 2015-? suhetao\n\nPermission is hereby granted, free of charge, to any person obta"
}
]
// ... and 1 more files (download for full content)
About this extraction
This page contains the full source code of the suhetao/stm32f4_mpu9250 GitHub repository, extracted and formatted as plain text for AI agents and large language models (LLMs). The extraction includes 201 files (4.5 MB), approximately 1.2M tokens, and a symbol index with 1619 extracted functions, classes, methods, constants, and types. Use this with OpenClaw, Claude, ChatGPT, Cursor, Windsurf, or any other AI tool that accepts text input. You can copy the full output to your clipboard or download it as a .txt file.
Extracted by GitExtract — free GitHub repo to text converter for AI. Built by Nikandr Surkov.