├── .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 ├── Calibration App └── ahrs v1.003.zip ├── 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 ├── Hardware ├── IMU_PCB.pdf └── imu_gerber.zip ├── LICENSE ├── Libraries ├── CMSIS │ ├── CMSIS END USER LICENCE AGREEMENT.pdf │ ├── 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 ├── arm_cortexM4lf_math.zip └── 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 /.gitignore: -------------------------------------------------------------------------------- 1 | # Object files 2 | *.o 3 | *.ko 4 | *.obj 5 | *.elf 6 | 7 | # Precompiled Headers 8 | *.gch 9 | *.pch 10 | 11 | # Libraries 12 | *.lib 13 | *.a 14 | *.la 15 | *.lo 16 | 17 | # Shared objects (inc. Windows DLLs) 18 | *.dll 19 | *.so 20 | *.so.* 21 | *.dylib 22 | 23 | # Executables 24 | *.exe 25 | *.out 26 | *.app 27 | *.i*86 28 | *.x86_64 29 | *.hex 30 | -------------------------------------------------------------------------------- /Algorithm/inc/CKF.h: -------------------------------------------------------------------------------- 1 | /* 2 | The MIT License (MIT) 3 | 4 | Copyright (c) 2015-? suhetao 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | this software and associated documentation files (the "Software"), to deal in 8 | the Software without restriction, including without limitation the rights to 9 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | the Software, and to permit persons to whom the Software is furnished to do so, 11 | subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | #ifndef _CKF_H 25 | #define _CKF_H 26 | 27 | #include "Matrix.h" 28 | 29 | //7-state q0 q1 q2 q3 wx wy wz 30 | #define CKF_STATE_DIM 7 31 | //13-measurement q0 q1 q2 q3 ax ay az wx wy wz mx my mz 32 | #define CKF_MEASUREMENT_DIM 13 33 | 34 | #define CKF_CP_POINTS 14//(2 * CKF_STATE_DIM) 35 | 36 | #define CKF_HALFPI 1.5707963267948966192313216916398f 37 | #define CKF_PI 3.1415926535897932384626433832795f 38 | #define CKF_TWOPI 6.283185307179586476925286766559f 39 | #define CKF_TODEG(x) ((x) * 57.2957796f) 40 | 41 | typedef struct CKF_FILTER_T{ 42 | //weights 43 | float32_t W; 44 | //Kesi 45 | float32_t Kesi_f32[CKF_STATE_DIM * CKF_CP_POINTS]; 46 | float32_t iKesi_f32[CKF_STATE_DIM]; 47 | //state covariance 48 | float32_t P_f32[CKF_STATE_DIM * CKF_STATE_DIM]; 49 | float32_t PX_f32[CKF_STATE_DIM * CKF_STATE_DIM]; 50 | float32_t PY_f32[CKF_MEASUREMENT_DIM * CKF_MEASUREMENT_DIM]; 51 | float32_t tmpPY_f32[CKF_MEASUREMENT_DIM * CKF_MEASUREMENT_DIM]; 52 | float32_t PXY_f32[CKF_STATE_DIM * CKF_MEASUREMENT_DIM]; 53 | float32_t tmpPXY_f32[CKF_STATE_DIM * CKF_MEASUREMENT_DIM]; 54 | float32_t Q_f32[CKF_STATE_DIM * CKF_STATE_DIM]; 55 | float32_t R_f32[CKF_MEASUREMENT_DIM * CKF_MEASUREMENT_DIM]; 56 | //cubature points 57 | float32_t XCP_f32[CKF_STATE_DIM * CKF_CP_POINTS]; 58 | float32_t XminusCP_f32[CKF_STATE_DIM * CKF_CP_POINTS]; 59 | float32_t YSP_f32[CKF_MEASUREMENT_DIM * CKF_CP_POINTS]; 60 | 61 | //Kalman gain 62 | float32_t K_f32[CKF_STATE_DIM * CKF_MEASUREMENT_DIM]; 63 | float32_t KT_f32[CKF_MEASUREMENT_DIM * CKF_STATE_DIM]; 64 | //state vector 65 | float32_t X_f32[CKF_STATE_DIM]; 66 | float32_t XT_f32[CKF_STATE_DIM]; 67 | float32_t Xminus_f32[CKF_STATE_DIM]; 68 | float32_t XminusT_f32[CKF_STATE_DIM]; 69 | float32_t tmpX_f32[CKF_STATE_DIM]; 70 | float32_t tmpS_f32[CKF_STATE_DIM]; 71 | //measurement vector 72 | float32_t Y_f32[CKF_MEASUREMENT_DIM]; 73 | float32_t YT_f32[CKF_MEASUREMENT_DIM]; 74 | float32_t Yminus_f32[CKF_MEASUREMENT_DIM]; 75 | float32_t YminusT_f32[CKF_MEASUREMENT_DIM]; 76 | float32_t tmpY_f32[CKF_MEASUREMENT_DIM]; 77 | // 78 | arm_matrix_instance_f32 Kesi; 79 | arm_matrix_instance_f32 iKesi; 80 | arm_matrix_instance_f32 P; 81 | arm_matrix_instance_f32 PX; 82 | arm_matrix_instance_f32 PY; 83 | arm_matrix_instance_f32 tmpPY; 84 | arm_matrix_instance_f32 PXY; 85 | arm_matrix_instance_f32 tmpPXY; 86 | arm_matrix_instance_f32 Q; 87 | arm_matrix_instance_f32 R; 88 | // 89 | arm_matrix_instance_f32 XCP; 90 | arm_matrix_instance_f32 XminusCP; 91 | arm_matrix_instance_f32 YCP; 92 | // 93 | arm_matrix_instance_f32 K; 94 | arm_matrix_instance_f32 KT; 95 | // 96 | arm_matrix_instance_f32 X; 97 | arm_matrix_instance_f32 XT; 98 | arm_matrix_instance_f32 Xminus; 99 | arm_matrix_instance_f32 XminusT; 100 | arm_matrix_instance_f32 tmpX; 101 | arm_matrix_instance_f32 tmpS; 102 | arm_matrix_instance_f32 Y; 103 | arm_matrix_instance_f32 YT; 104 | arm_matrix_instance_f32 Yminus; 105 | arm_matrix_instance_f32 YminusT; 106 | arm_matrix_instance_f32 tmpY; 107 | }CKF_Filter; 108 | 109 | void CKF_New(CKF_Filter* ckf); 110 | void CKF_Init(CKF_Filter* ckf, float32_t *q, float32_t *gyro); 111 | void CKF_Update(CKF_Filter* ckf, float32_t *q, float32_t *gyro, float32_t *accel, float32_t *mag, float32_t dt); 112 | void CKF_GetAngle(CKF_Filter* ckf, float32_t* rpy); 113 | 114 | __inline void CKF_GetQ(CKF_Filter* ckf, float32_t* Q) 115 | { 116 | Q[0] = ckf->X_f32[0]; 117 | Q[1] = ckf->X_f32[1]; 118 | Q[2] = ckf->X_f32[2]; 119 | Q[3] = ckf->X_f32[3]; 120 | } 121 | 122 | #endif 123 | -------------------------------------------------------------------------------- /Algorithm/inc/Control.h: -------------------------------------------------------------------------------- 1 | /* 2 | The MIT License (MIT) 3 | 4 | Copyright (c) 2015-? suhetao 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | this software and associated documentation files (the "Software"), to deal in 8 | the Software without restriction, including without limitation the rights to 9 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | the Software, and to permit persons to whom the Software is furnished to do so, 11 | subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | #ifndef _CONTROL_H_ 25 | #define _CONTROL_H_ 26 | 27 | #include "PID.h" 28 | 29 | //Modelling, Identification and Control of a Quadrotor Helicopter 30 | 31 | #define PI (3.1415926535897932384626433832795f) 32 | #define PI_6 (0.52359877559829887307710723054658f) 33 | #define MIN_ANG (-PI_6) 34 | #define MAX_ANG PI_6 35 | 36 | typedef struct CONTROLLERPARAMETER_T 37 | { 38 | float m; //mass of the quadrotor 39 | float Ixx; //body moment of inertia around the x-axis 40 | float Iyy; //body moment of inertia around the y-axis 41 | float Izz; //body moment of inertia around the z-axis 42 | float I[9]; //body inertia matrix 43 | float d; //drag factor 44 | float b; //thrust factor 45 | float l; //center of quadrotor to center of propeller distance 46 | 47 | float X; 48 | float Y; 49 | float Z; 50 | PIDController PostionX; 51 | PIDController PostionY; 52 | PIDController PostionZ; 53 | 54 | // not use yet 55 | float g; //acceleration due to gravity 56 | float n; //number of data acquired 57 | float N; //gear box reduction ratio 58 | float h; //PWM code vector 59 | float R; //motor resistance 60 | float Jtp; //total rotational moment of inertia around the propeller axis 61 | float Ke; //electric motor constant 62 | float Km; //mechanic motor constant 63 | float L; //motor inductance 64 | 65 | }QuadrotorParameter; 66 | 67 | typedef enum TRAJECTORY_T{ 68 | X = 0, 69 | Y = 1, 70 | Z = 2, 71 | PSI = 3 72 | }Trajectory; 73 | 74 | 75 | typedef enum EULER_T{ 76 | ROLL = 0, 77 | PITCH = 1, 78 | YAW = 2 79 | }Euler; 80 | 81 | void EulerConv(float* dt, float *deta); 82 | void TorqueConv(float *eta, float *deta, float *dt); 83 | void ForceConv(float *eta, float dz, float *df); 84 | void TorqueInv(float *dt, float df, float *domega); 85 | 86 | #endif 87 | -------------------------------------------------------------------------------- /Algorithm/inc/Double.h: -------------------------------------------------------------------------------- 1 | /* 2 | The MIT License (MIT) 3 | 4 | Copyright (c) 2015-? suhetao 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | this software and associated documentation files (the "Software"), to deal in 8 | the Software without restriction, including without limitation the rights to 9 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | the Software, and to permit persons to whom the Software is furnished to do so, 11 | subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | #ifndef _DOUBLE_H 25 | #define _DOUBLE_H 26 | 27 | ////////////////////////////////////////////////////////////////////////// 28 | //algorithm from the book 29 | //<> 30 | //References: 31 | //Emulated double precision Double single routine from nvidia developer zone 32 | ////////////////////////////////////////////////////////////////////////// 33 | //define my own double struct 34 | typedef struct 35 | { 36 | float hi; 37 | float lo; 38 | } Double; 39 | ////////////////////////////////////////////////////////////////////////// 40 | //transmit double precision float to my own Double 41 | __inline Double intToDouble(int A) 42 | { 43 | Double B; 44 | 45 | B.hi = (float)A; 46 | B.lo = 0.0f; 47 | 48 | return B; 49 | } 50 | // 51 | __inline Double floatToDouble(float A) 52 | { 53 | Double B; 54 | 55 | B.hi = A; 56 | B.lo = 0.0f; 57 | 58 | return B; 59 | } 60 | // 61 | __inline Double doubleToDouble(double A) 62 | { 63 | Double B; 64 | 65 | B.hi = (float)A; 66 | B.lo = (float)(A - (double)B.hi); 67 | 68 | return B; 69 | } 70 | //transmit my own Double to double precision float 71 | __inline double DoubleTodouble(Double B) 72 | { 73 | double A; 74 | 75 | A = B.hi; 76 | A += B.lo; 77 | 78 | return A; 79 | } 80 | 81 | //addition: Double + Double 82 | __inline Double DoubleAdd(Double A, Double B) 83 | { 84 | Double C; 85 | float t1, t2, e; 86 | 87 | //Compute high order sum and error 88 | t1 = A.hi + B.hi; 89 | e = t1 - A.hi; 90 | //Compute low order term, including error and overflows 91 | t2 = ((B.hi - e) + ( A.hi - (t1 - e))) + A.lo + B.lo; 92 | //Normalise to get final result 93 | C.hi = t1 + t2; 94 | C.lo = t2 -(C.hi - t1); 95 | 96 | return C; 97 | } 98 | 99 | //Subtraction: Double - Double 100 | __inline Double DoubleSub(Double A, Double B) 101 | { 102 | Double C; 103 | float t1, t2, e; 104 | 105 | //Compute high order sub and error 106 | t1 = A.hi - B.hi; 107 | e = t1 - A.hi; 108 | //Compute low order term, including error and overflows 109 | t2 = ((-B.hi - e) + ( A.hi - (t1 - e))) + A.lo - B.lo; 110 | //Normalise to get final result 111 | C.hi = t1 + t2; 112 | C.lo = t2 -(C.hi - t1); 113 | 114 | return C; 115 | } 116 | 117 | //multiplication: Double * Double 118 | __inline Double DoubleMul(Double A, Double B) 119 | { 120 | Double C; 121 | float cona, conb, a1, a2, b1, b2; 122 | float c11, c21, c2, e, t1, t2; 123 | 124 | //Compute initial high order approximation and error 125 | //If a fused multiply-add is available 126 | //c11 = A.hi * B.hi; 127 | //c21 = A.hi * B.hi - c11; 128 | 129 | //If no fused multiply-add is available 130 | cona = A.hi * 8193.0f; 131 | conb = B.hi * 8193.0f; 132 | a1 = cona - (cona - A.hi); 133 | b1 = conb - (conb - B.hi); 134 | a2 = A.hi - a1; 135 | b2 = B.hi - b1; 136 | c11 = A.hi * B.hi; 137 | c21 = (((a1 * b1 - c11) + a1 * b2) + a2 * b1) + a2 * b2; 138 | 139 | //Compute high order word of mixed term: 140 | c2 = A.hi * B.lo + A.lo * B.hi; 141 | //Compute (c11, c21) + c2 using Knuth's trick, including low order product 142 | t1 = c11 + c2; 143 | e = t1 - c11; 144 | t2 = ((c2-e) + (c11 - (t1 -e))) + c21 + A.lo * B.lo; 145 | //Normalise to get final result 146 | C.hi = t1 + t2; 147 | C.lo = t2 - ( C.hi - t1); 148 | return C; 149 | } 150 | 151 | //divides: Double / Double 152 | __inline Double DoubleDiv(Double A, Double B) 153 | { 154 | Double C; 155 | float a1, a2, b1, b2, cona, conb, c11, c2, c21, e, s1, s2; 156 | float t1, t2, t11, t12, t21, t22; 157 | 158 | // Compute a DP approximation to the quotient. 159 | s2 = 1.0f / B.hi; 160 | s1 = A.hi * s2; 161 | 162 | //This splits s1 and b.x into high-order and low-order words. 163 | cona = s1 * 8193.0f; 164 | conb = B.hi * 8193.0f; 165 | a1 = cona - (cona - s1); 166 | b1 = conb - (conb - B.hi); 167 | a2 = s1 -a1; 168 | b2 = B.hi - b1; 169 | //Multiply s1 * dsb(1) using Dekker's method. 170 | c11 = s1 * B.hi; 171 | c21 = (((a1 * b1 - c11) + a1 * b2) + a2 * b1) + a2 * b2; 172 | //Compute s1 * b.lo (only high-order word is needed). 173 | c2 = s1 * B.lo; 174 | //Compute (c11, c21) + c2 using Knuth's trick. 175 | t1 = c11 + c2; 176 | e = t1 - c11; 177 | t2 = ((c2 - e) + (c11 - (t1 - e))) + c21; 178 | //The result is t1 + t2, after normalization. 179 | t12 = t1 + t2; 180 | t22 = t2 - (t12 - t1); 181 | //Compute dsa - (t12, t22) using Knuth's trick. 182 | t11 = A.hi - t12; 183 | e = t11 - A.hi; 184 | t21 = ((-t12 - e) + (A.hi - (t11 - e))) + A.lo - t22; 185 | //Compute high-order word of (t11, t21) and divide by b.hi. 186 | s2 *= (t11 + t21); 187 | //The result is s1 + s2, after normalization. 188 | C.hi = s1 + s2; 189 | C.lo = s2 - (C.hi - s1); 190 | 191 | return C; 192 | } 193 | 194 | #endif 195 | -------------------------------------------------------------------------------- /Algorithm/inc/EKF.h: -------------------------------------------------------------------------------- 1 | /* 2 | The MIT License (MIT) 3 | 4 | Copyright (c) 2015-? suhetao 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | this software and associated documentation files (the "Software"), to deal in 8 | the Software without restriction, including without limitation the rights to 9 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | the Software, and to permit persons to whom the Software is furnished to do so, 11 | subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | #ifndef _EKF_H 25 | #define _EKF_H 26 | 27 | #include "Matrix.h" 28 | 29 | //7-state q0 q1 q2 q3 wx wy wz 30 | #define EKF_STATE_DIM 7 31 | //13-measurement q0 q1 q2 q3 ax ay az wx wy wz mx my mz 32 | #define EKF_MEASUREMENT_DIM 13 33 | 34 | #define EKF_HALFPI 1.5707963267948966192313216916398f 35 | #define EKF_PI 3.1415926535897932384626433832795f 36 | #define EKF_TWOPI 6.283185307179586476925286766559f 37 | #define EKF_TODEG(x) ((x) * 57.2957796f) 38 | 39 | typedef struct EKF_FILTER_T{ 40 | //state covariance 41 | float32_t P_f32[EKF_STATE_DIM * EKF_STATE_DIM]; 42 | float32_t Q_f32[EKF_STATE_DIM * EKF_STATE_DIM]; 43 | float32_t R_f32[EKF_MEASUREMENT_DIM * EKF_MEASUREMENT_DIM]; 44 | //Kalman gain 45 | float32_t K_f32[EKF_STATE_DIM * EKF_MEASUREMENT_DIM]; 46 | float32_t KT_f32[EKF_MEASUREMENT_DIM * EKF_STATE_DIM]; 47 | //Measurement covariance 48 | float32_t S_f32[EKF_MEASUREMENT_DIM * EKF_MEASUREMENT_DIM]; 49 | //The H matrix maps the measurement to the states 50 | float32_t F_f32[EKF_STATE_DIM * EKF_STATE_DIM]; 51 | float32_t FT_f32[EKF_STATE_DIM * EKF_STATE_DIM]; 52 | float32_t H_f32[EKF_MEASUREMENT_DIM * EKF_STATE_DIM]; 53 | float32_t HT_f32[EKF_STATE_DIM * EKF_MEASUREMENT_DIM]; 54 | float32_t I_f32[EKF_STATE_DIM * EKF_STATE_DIM]; 55 | //state vector 56 | float32_t X_f32[EKF_STATE_DIM]; 57 | //measurement vector 58 | float32_t Y_f32[EKF_MEASUREMENT_DIM]; 59 | // 60 | float32_t tmpP_f32[EKF_STATE_DIM * EKF_STATE_DIM]; 61 | float32_t tmpS_f32[EKF_MEASUREMENT_DIM * EKF_MEASUREMENT_DIM]; 62 | float32_t tmpX_f32[EKF_STATE_DIM]; 63 | float32_t tmpXX_f32[EKF_STATE_DIM * EKF_STATE_DIM]; 64 | float32_t tmpXXT_f32[EKF_STATE_DIM * EKF_STATE_DIM]; 65 | float32_t tmpXY_f32[EKF_STATE_DIM * EKF_MEASUREMENT_DIM]; 66 | float32_t tmpYX_f32[EKF_MEASUREMENT_DIM * EKF_STATE_DIM]; 67 | 68 | arm_matrix_instance_f32 P; 69 | arm_matrix_instance_f32 Q; 70 | arm_matrix_instance_f32 R; 71 | arm_matrix_instance_f32 K; 72 | arm_matrix_instance_f32 KT; 73 | arm_matrix_instance_f32 S; 74 | arm_matrix_instance_f32 F; 75 | arm_matrix_instance_f32 FT; 76 | arm_matrix_instance_f32 H; 77 | arm_matrix_instance_f32 HT; 78 | arm_matrix_instance_f32 I; 79 | // 80 | arm_matrix_instance_f32 X; 81 | arm_matrix_instance_f32 Y; 82 | // 83 | arm_matrix_instance_f32 tmpP; 84 | arm_matrix_instance_f32 tmpX; 85 | arm_matrix_instance_f32 tmpYX; 86 | arm_matrix_instance_f32 tmpXY; 87 | arm_matrix_instance_f32 tmpXX; 88 | arm_matrix_instance_f32 tmpXXT; 89 | arm_matrix_instance_f32 tmpS; 90 | }EKF_Filter; 91 | 92 | void EKF_New(EKF_Filter* ekf); 93 | void EKF_Init(EKF_Filter* ekf, float32_t *q, float32_t *gyro); 94 | void EFK_Update(EKF_Filter* ekf, float32_t *q, float32_t *gyro, float32_t *accel, float32_t *mag, float32_t dt); 95 | void EKF_GetAngle(EKF_Filter* ekf, float32_t* rpy); 96 | 97 | __inline void EKF_GetQ(EKF_Filter* efk, float32_t* Q) 98 | { 99 | Q[0] = efk->X_f32[0]; 100 | Q[1] = efk->X_f32[1]; 101 | Q[2] = efk->X_f32[2]; 102 | Q[3] = efk->X_f32[3]; 103 | } 104 | 105 | #endif 106 | -------------------------------------------------------------------------------- /Algorithm/inc/INS_EKF.h: -------------------------------------------------------------------------------- 1 | /* 2 | The MIT License (MIT) 3 | 4 | Copyright (c) 2015-? suhetao 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | this software and associated documentation files (the "Software"), to deal in 8 | the Software without restriction, including without limitation the rights to 9 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | the Software, and to permit persons to whom the Software is furnished to do so, 11 | subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | #ifndef _INS_EKF_H_ 25 | #define _INS_EKF_H_ 26 | 27 | #include "Matrix.h" 28 | 29 | //16-state q0 q1 q2 q3 Pn Pe Alt Vn Ve Vd bwx bwy bwz bax bay baz 30 | #define INS_EKF_STATE_DIM 16 31 | 32 | //9-measurement mx my mz (3D magnetometer) Pn Pe Alt Vn Ve Vd 33 | //unit vector pointing to MagNorth in body coords 34 | //north pos, east pos, altitude 35 | //north vel, east vel, down velocity 36 | #define INS_EKF_MEASUREMENT_DIM 9 37 | 38 | #define INS_EKF_HALFPI 1.5707963267948966192313216916398f 39 | #define INS_EKF_PI 3.1415926535897932384626433832795f 40 | #define INS_EKF_TWOPI 6.283185307179586476925286766559f 41 | #define INS_EKF_TODEG(x) ((x) * 57.2957796f) 42 | 43 | typedef struct INS_EKF_FILTER_T{ 44 | //state covariance 45 | float32_t P_f32[INS_EKF_STATE_DIM * INS_EKF_STATE_DIM]; 46 | float32_t PX_f32[INS_EKF_STATE_DIM * INS_EKF_STATE_DIM]; 47 | float32_t PHT_f32[INS_EKF_STATE_DIM * INS_EKF_MEASUREMENT_DIM]; 48 | 49 | float32_t Q_f32[INS_EKF_STATE_DIM * INS_EKF_STATE_DIM]; 50 | float32_t R_f32[INS_EKF_MEASUREMENT_DIM * INS_EKF_MEASUREMENT_DIM]; 51 | 52 | float32_t K_f32[INS_EKF_STATE_DIM * INS_EKF_MEASUREMENT_DIM]; 53 | float32_t KH_f32[INS_EKF_STATE_DIM * INS_EKF_STATE_DIM]; 54 | float32_t KHP_f32[INS_EKF_STATE_DIM * INS_EKF_STATE_DIM]; 55 | 56 | float32_t KY_f32[INS_EKF_STATE_DIM]; 57 | 58 | float32_t F_f32[INS_EKF_STATE_DIM * INS_EKF_STATE_DIM]; 59 | float32_t FT_f32[INS_EKF_STATE_DIM * INS_EKF_STATE_DIM]; 60 | //The H matrix maps the measurement to the states 61 | float32_t H_f32[INS_EKF_MEASUREMENT_DIM * INS_EKF_STATE_DIM]; 62 | float32_t HT_f32[INS_EKF_STATE_DIM * INS_EKF_MEASUREMENT_DIM]; 63 | float32_t HP_f32[INS_EKF_MEASUREMENT_DIM * INS_EKF_STATE_DIM]; 64 | 65 | float32_t S_f32[INS_EKF_MEASUREMENT_DIM * INS_EKF_MEASUREMENT_DIM]; 66 | float32_t SI_f32[INS_EKF_MEASUREMENT_DIM * INS_EKF_MEASUREMENT_DIM]; 67 | //state vector 68 | float32_t X_f32[INS_EKF_STATE_DIM]; 69 | //measurement vector 70 | float32_t Y_f32[INS_EKF_MEASUREMENT_DIM]; 71 | 72 | arm_matrix_instance_f32 P; 73 | arm_matrix_instance_f32 PX; 74 | arm_matrix_instance_f32 PHT; 75 | arm_matrix_instance_f32 Q; 76 | arm_matrix_instance_f32 R; 77 | arm_matrix_instance_f32 K; 78 | arm_matrix_instance_f32 KH; 79 | arm_matrix_instance_f32 KHP; 80 | arm_matrix_instance_f32 KY; 81 | arm_matrix_instance_f32 F; 82 | arm_matrix_instance_f32 FT; 83 | arm_matrix_instance_f32 H; 84 | arm_matrix_instance_f32 HT; 85 | arm_matrix_instance_f32 HP; 86 | arm_matrix_instance_f32 S; 87 | arm_matrix_instance_f32 SI; 88 | 89 | arm_matrix_instance_f32 X; 90 | arm_matrix_instance_f32 Y; 91 | 92 | float32_t declination; 93 | float32_t gravity; //m/s^2 94 | }INS_EKF_Filter; 95 | 96 | void INS_EKF_New(INS_EKF_Filter* ins); 97 | void INS_EKF_Init(INS_EKF_Filter* ins, float32_t *p, float32_t *v, float32_t *accel, float32_t *mag); 98 | 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); 99 | void INS_EKF_GetAngle(INS_EKF_Filter* ins, float32_t* rpy); 100 | 101 | #endif 102 | -------------------------------------------------------------------------------- /Algorithm/inc/PID.h: -------------------------------------------------------------------------------- 1 | /* 2 | The MIT License (MIT) 3 | 4 | Copyright (c) 2015-? suhetao 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | this software and associated documentation files (the "Software"), to deal in 8 | the Software without restriction, including without limitation the rights to 9 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | the Software, and to permit persons to whom the Software is furnished to do so, 11 | subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | #ifndef _PID_H_ 25 | #define _PID_H_ 26 | 27 | typedef struct PIDCONTROLLER_T 28 | { 29 | float A0; // < The derived gain, A0 = Kp + Ki + Kd . 30 | float A1; // < The derived gain, A1 = -Kp - 2Kd. 31 | float A2; // < The derived gain, A2 = Kd . 32 | float state[3]; // < The state array of length 3. 33 | float Kp; // < The proportional gain. 34 | float Ki; // < The integral gain. 35 | float Kd; // < The derivative gain. 36 | } PIDController; 37 | 38 | __inline float PID_Calculate(PIDController *S, float in) 39 | { 40 | float out; 41 | // y[n] = y[n-1] + A0 * x[n] + A1 * x[n-1] + A2 * x[n-2] 42 | out = (S->A0 * in) + (S->A1 * S->state[0]) + (S->A2 * S->state[1]) + (S->state[2]); 43 | 44 | // Update state 45 | S->state[1] = S->state[0]; 46 | S->state[0] = in; 47 | S->state[2] = out; 48 | 49 | // return to application 50 | return (out); 51 | } 52 | 53 | __inline void PID_Init(PIDController *S) 54 | { 55 | //Derived coefficient A0 56 | S->A0 = S->Kp + S->Ki + S->Kd; 57 | 58 | //Derived coefficient A1 59 | S->A1 = (-S->Kp) - ((float) 2.0 * S->Kd); 60 | 61 | //Derived coefficient A2 62 | S->A2 = S->Kd; 63 | 64 | S->state[0] = 0.0f; 65 | S->state[1] = 0.0f; 66 | S->state[2] = 0.0f; 67 | } 68 | 69 | __inline void PID_Reset(PIDController *S) 70 | { 71 | S->state[0] = 0.0f; 72 | S->state[1] = 0.0f; 73 | S->state[2] = 0.0f; 74 | } 75 | 76 | #endif 77 | -------------------------------------------------------------------------------- /Algorithm/inc/Quaternion.h: -------------------------------------------------------------------------------- 1 | /* 2 | The MIT License (MIT) 3 | 4 | Copyright (c) 2015-? suhetao 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | this software and associated documentation files (the "Software"), to deal in 8 | the Software without restriction, including without limitation the rights to 9 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | the Software, and to permit persons to whom the Software is furnished to do so, 11 | subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | #ifndef _QUATERNION_H_ 25 | #define _QUATERNION_H_ 26 | 27 | #include "FastMath.h" 28 | 29 | __inline void Quaternion_Add(float *r, float *a, float *b) 30 | { 31 | r[0] = a[0] + b[0]; 32 | r[1] = a[1] + b[1]; 33 | r[2] = a[2] + b[2]; 34 | r[3] = a[3] + b[3]; 35 | } 36 | 37 | __inline void Quaternion_Sub(float *r, float *a, float *b) 38 | { 39 | r[0] = a[0] - b[0]; 40 | r[1] = a[1] - b[1]; 41 | r[2] = a[2] - b[2]; 42 | r[3] = a[3] - b[3]; 43 | } 44 | 45 | __inline void Quaternion_Multiply(float *r, float *a, float *b) 46 | { 47 | r[0] = a[0] * b[0] - a[1] * b[1] - a[2] * b[2] - a[3] * b[3]; 48 | r[1] = a[0] * b[1] + a[1] * b[0] + a[2] * b[3] - a[3] * b[2]; 49 | r[2] = a[0] * b[2] - a[1] * b[3] + a[2] * b[0] + a[3] * b[1]; 50 | r[3] = a[0] * b[3] + a[1] * b[2] - a[2] * b[1] + a[3] * b[0]; 51 | } 52 | 53 | __inline void Quaternion_Conjugate(float *r, float *a) 54 | { 55 | r[0] = a[0]; 56 | r[1] = -a[1]; 57 | r[2] = -a[2]; 58 | r[3] = -a[3]; 59 | } 60 | 61 | __inline void Quaternion_Scalar(float *r, float *q, float scalar) 62 | { 63 | r[0] = q[0] * scalar; 64 | r[1] = q[1] * scalar; 65 | r[2] = q[2] * scalar; 66 | r[3] = q[3] * scalar; 67 | } 68 | 69 | void Quaternion_Normalize(float *q); 70 | void Quaternion_FromEuler(float *q, float *rpy); 71 | void Quaternion_ToEuler(float *q, float* rpy); 72 | void Quaternion_FromRotationMatrix(float *R, float *Q); 73 | void Quaternion_RungeKutta4(float *q, float *w, float dt, int normalize); 74 | void Quaternion_From6AxisData(float* q, float *accel, float *mag); 75 | 76 | #endif 77 | -------------------------------------------------------------------------------- /Algorithm/inc/SRCKF.h: -------------------------------------------------------------------------------- 1 | /* 2 | The MIT License (MIT) 3 | 4 | Copyright (c) 2015-? suhetao 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | this software and associated documentation files (the "Software"), to deal in 8 | the Software without restriction, including without limitation the rights to 9 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | the Software, and to permit persons to whom the Software is furnished to do so, 11 | subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | #ifndef _SRCKF_H 25 | #define _SRCKF_H 26 | 27 | #include "Matrix.h" 28 | 29 | //7-state q0 q1 q2 q3 wbx wby wbz 30 | #define SRCKF_STATE_DIM 7 31 | //6-measurement ax ay az mx my mz 32 | #define SRCKF_MEASUREMENT_DIM 6 33 | 34 | #define SRCKF_CP_POINTS 14//(2 * SRCKF_STATE_DIM) 35 | 36 | #define SRCKF_HALFPI 1.5707963267948966192313216916398f 37 | #define SRCKF_PI 3.1415926535897932384626433832795f 38 | #define SRCKF_TWOPI 6.283185307179586476925286766559f 39 | #define SRCKF_TODEG(x) ((x) * 57.2957796f) 40 | 41 | typedef struct SRCKF_FILTER_T{ 42 | //weights 43 | float32_t W; 44 | float32_t SW; 45 | //Kesi 46 | float32_t Kesi_f32[SRCKF_STATE_DIM * SRCKF_CP_POINTS]; 47 | float32_t iKesi_f32[SRCKF_STATE_DIM]; 48 | //state covariance 49 | float32_t S_f32[SRCKF_STATE_DIM * SRCKF_STATE_DIM]; 50 | float32_t ST_f32[SRCKF_STATE_DIM * SRCKF_STATE_DIM]; 51 | //measurement covariance 52 | float32_t SY_f32[SRCKF_MEASUREMENT_DIM * SRCKF_MEASUREMENT_DIM]; 53 | float32_t SYI_f32[SRCKF_MEASUREMENT_DIM * SRCKF_MEASUREMENT_DIM]; 54 | float32_t SYT_f32[SRCKF_MEASUREMENT_DIM * SRCKF_MEASUREMENT_DIM]; 55 | float32_t SYTI_f32[SRCKF_MEASUREMENT_DIM * SRCKF_MEASUREMENT_DIM]; 56 | //cross covariance 57 | float32_t PXY_f32[SRCKF_STATE_DIM * SRCKF_MEASUREMENT_DIM]; 58 | float32_t tmpPXY_f32[SRCKF_STATE_DIM * SRCKF_MEASUREMENT_DIM]; 59 | // 60 | float32_t SQ_f32[SRCKF_STATE_DIM * SRCKF_STATE_DIM]; 61 | float32_t SR_f32[SRCKF_MEASUREMENT_DIM * SRCKF_MEASUREMENT_DIM]; 62 | //cubature points 63 | float32_t XCP_f32[SRCKF_STATE_DIM * SRCKF_CP_POINTS]; 64 | //propagated cubature points 65 | float32_t XPCP_f32[SRCKF_STATE_DIM * SRCKF_CP_POINTS]; 66 | float32_t YPCP_f32[SRCKF_MEASUREMENT_DIM * SRCKF_CP_POINTS]; 67 | //centered matrix 68 | float32_t XCPCM_f32[SRCKF_STATE_DIM * SRCKF_CP_POINTS]; 69 | float32_t tmpXCPCM_f32[SRCKF_STATE_DIM * SRCKF_CP_POINTS]; 70 | float32_t YCPCM_f32[SRCKF_MEASUREMENT_DIM * SRCKF_CP_POINTS]; 71 | float32_t YCPCMT_f32[SRCKF_CP_POINTS * SRCKF_MEASUREMENT_DIM]; 72 | float32_t XCM_f32[SRCKF_STATE_DIM * (SRCKF_CP_POINTS + SRCKF_STATE_DIM)]; 73 | float32_t YCM_f32[SRCKF_MEASUREMENT_DIM * (SRCKF_CP_POINTS + SRCKF_MEASUREMENT_DIM)]; 74 | float32_t XYCM_f32[SRCKF_STATE_DIM * (SRCKF_CP_POINTS + SRCKF_MEASUREMENT_DIM)]; 75 | 76 | //Kalman gain 77 | float32_t K_f32[SRCKF_STATE_DIM * SRCKF_MEASUREMENT_DIM]; 78 | //state vector 79 | float32_t X_f32[SRCKF_STATE_DIM]; 80 | float32_t tmpX_f32[SRCKF_STATE_DIM]; 81 | //measurement vector 82 | float32_t Y_f32[SRCKF_MEASUREMENT_DIM]; 83 | float32_t tmpY_f32[SRCKF_MEASUREMENT_DIM]; 84 | // 85 | //stuff 86 | //Kesi 87 | arm_matrix_instance_f32 Kesi; 88 | arm_matrix_instance_f32 iKesi; 89 | //state covariance 90 | arm_matrix_instance_f32 S; 91 | arm_matrix_instance_f32 ST; 92 | //measurement covariance 93 | arm_matrix_instance_f32 SY; 94 | arm_matrix_instance_f32 SYI; 95 | arm_matrix_instance_f32 SYT; 96 | arm_matrix_instance_f32 SYTI; 97 | //cross covariance 98 | arm_matrix_instance_f32 PXY; 99 | arm_matrix_instance_f32 tmpPXY; 100 | // 101 | arm_matrix_instance_f32 SQ; 102 | arm_matrix_instance_f32 SR; 103 | //cubature points 104 | arm_matrix_instance_f32 XCP; 105 | //propagated cubature points 106 | arm_matrix_instance_f32 XPCP; 107 | arm_matrix_instance_f32 YPCP; 108 | //centered matrix 109 | arm_matrix_instance_f32 XCPCM; 110 | arm_matrix_instance_f32 tmpXCPCM; 111 | arm_matrix_instance_f32 YCPCM; 112 | arm_matrix_instance_f32 YCPCMT; 113 | arm_matrix_instance_f32 XCM; 114 | arm_matrix_instance_f32 YCM; 115 | arm_matrix_instance_f32 XYCM; 116 | //Kalman gain 117 | arm_matrix_instance_f32 K; 118 | //state vector 119 | arm_matrix_instance_f32 X; 120 | arm_matrix_instance_f32 tmpX; 121 | //measurement vector 122 | arm_matrix_instance_f32 Y; 123 | arm_matrix_instance_f32 tmpY; 124 | 125 | float32_t declination; 126 | }SRCKF_Filter; 127 | 128 | void SRCKF_New(SRCKF_Filter* srckf); 129 | void SRCKF_Init(SRCKF_Filter* srckf, float32_t *accel, float32_t *mag); 130 | void SRCKF_Update(SRCKF_Filter* srckf, float32_t *gyro, float32_t *accel, float32_t *mag, float32_t dt); 131 | void SRCKF_GetAngle(SRCKF_Filter* srckf, float32_t* rpy); 132 | 133 | __inline void SRCKF_GetQ(SRCKF_Filter* srckf, float32_t* Q) 134 | { 135 | Q[0] = srckf->X_f32[0]; 136 | Q[1] = srckf->X_f32[1]; 137 | Q[2] = srckf->X_f32[2]; 138 | Q[3] = srckf->X_f32[3]; 139 | } 140 | 141 | #endif 142 | -------------------------------------------------------------------------------- /Algorithm/inc/UKF.h: -------------------------------------------------------------------------------- 1 | /* 2 | The MIT License (MIT) 3 | 4 | Copyright (c) 2015-? suhetao 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | this software and associated documentation files (the "Software"), to deal in 8 | the Software without restriction, including without limitation the rights to 9 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | the Software, and to permit persons to whom the Software is furnished to do so, 11 | subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | #ifndef _UKF_H 25 | #define _UKF_H 26 | 27 | #include "Matrix.h" 28 | 29 | //7-state q0 q1 q2 q3 wx wy wz 30 | #define UKF_STATE_DIM 7 31 | //13-measurement q0 q1 q2 q3 ax ay az wx wy wz mx my mz 32 | #define UKF_MEASUREMENT_DIM 13 33 | 34 | #define UKF_SP_POINTS 15//(2 * UKF_STATE_DIM + 1) 35 | 36 | #define UKF_HALFPI 1.5707963267948966192313216916398f 37 | #define UKF_PI 3.1415926535897932384626433832795f 38 | #define UKF_TWOPI 6.283185307179586476925286766559f 39 | #define UKF_TODEG(x) ((x) * 57.2957796f) 40 | 41 | typedef struct UKF_FILTER_T{ 42 | //scaling factor 43 | float32_t gamma; 44 | //weights for means 45 | float32_t Wm0, Wmi; 46 | //weights for covariance 47 | float32_t Wc_f32[UKF_SP_POINTS * UKF_SP_POINTS]; 48 | //state covariance 49 | float32_t P_f32[UKF_STATE_DIM * UKF_STATE_DIM]; 50 | float32_t PX_f32[UKF_STATE_DIM * UKF_STATE_DIM]; 51 | float32_t PY_f32[UKF_MEASUREMENT_DIM * UKF_MEASUREMENT_DIM]; 52 | float32_t tmpPY_f32[UKF_MEASUREMENT_DIM * UKF_MEASUREMENT_DIM]; 53 | float32_t PXY_f32[UKF_STATE_DIM * UKF_MEASUREMENT_DIM]; 54 | float32_t PXYT_f32[UKF_MEASUREMENT_DIM * UKF_STATE_DIM]; 55 | float32_t Q_f32[UKF_STATE_DIM * UKF_STATE_DIM]; 56 | float32_t R_f32[UKF_MEASUREMENT_DIM * UKF_MEASUREMENT_DIM]; 57 | //Sigma points 58 | float32_t XSP_f32[UKF_STATE_DIM * UKF_SP_POINTS]; 59 | float32_t tmpXSP_f32[UKF_STATE_DIM * UKF_SP_POINTS]; 60 | float32_t tmpXSPT_f32[UKF_SP_POINTS * UKF_STATE_DIM]; 61 | float32_t tmpWcXSP_f32[UKF_STATE_DIM * UKF_SP_POINTS]; 62 | float32_t tmpWcYSP_f32[UKF_MEASUREMENT_DIM * UKF_SP_POINTS]; 63 | float32_t YSP_f32[UKF_MEASUREMENT_DIM * UKF_SP_POINTS]; 64 | float32_t tmpYSP_f32[UKF_MEASUREMENT_DIM * UKF_SP_POINTS]; 65 | float32_t tmpYSPT_f32[UKF_SP_POINTS * UKF_MEASUREMENT_DIM]; 66 | //Kalman gain 67 | float32_t K_f32[UKF_STATE_DIM * UKF_MEASUREMENT_DIM]; 68 | float32_t KT_f32[UKF_MEASUREMENT_DIM * UKF_STATE_DIM]; 69 | //state vector 70 | float32_t X_f32[UKF_STATE_DIM]; 71 | float32_t tmpX_f32[UKF_STATE_DIM]; 72 | //measurement vector 73 | float32_t Y_f32[UKF_MEASUREMENT_DIM]; 74 | float32_t tmpY_f32[UKF_MEASUREMENT_DIM]; 75 | // 76 | arm_matrix_instance_f32 Wc; 77 | arm_matrix_instance_f32 P; 78 | arm_matrix_instance_f32 PX; 79 | arm_matrix_instance_f32 PY; 80 | arm_matrix_instance_f32 tmpPY; 81 | arm_matrix_instance_f32 PXY; 82 | arm_matrix_instance_f32 PXYT; 83 | arm_matrix_instance_f32 Q; 84 | arm_matrix_instance_f32 R; 85 | // 86 | arm_matrix_instance_f32 XSP; 87 | arm_matrix_instance_f32 tmpXSP; 88 | arm_matrix_instance_f32 tmpXSPT; 89 | arm_matrix_instance_f32 tmpWcXSP; 90 | arm_matrix_instance_f32 tmpWcYSP; 91 | arm_matrix_instance_f32 YSP; 92 | arm_matrix_instance_f32 tmpYSP; 93 | arm_matrix_instance_f32 tmpYSPT; 94 | // 95 | arm_matrix_instance_f32 K; 96 | arm_matrix_instance_f32 KT; 97 | // 98 | arm_matrix_instance_f32 X; 99 | arm_matrix_instance_f32 tmpX; 100 | arm_matrix_instance_f32 Y; 101 | arm_matrix_instance_f32 tmpY; 102 | }UKF_Filter; 103 | 104 | void UKF_New(UKF_Filter* UKF); 105 | void UKF_Init(UKF_Filter* UKF, float32_t *q, float32_t *gyro); 106 | void UKF_Update(UKF_Filter* UKF, float32_t *q, float32_t *gyro, float32_t *accel, float32_t *mag, float32_t dt); 107 | void UKF_GetAngle(UKF_Filter* UKF, float32_t* rpy); 108 | 109 | __inline void UKF_GetQ(UKF_Filter* ukf, float32_t* Q) 110 | { 111 | Q[0] = ukf->X_f32[0]; 112 | Q[1] = ukf->X_f32[1]; 113 | Q[2] = ukf->X_f32[2]; 114 | Q[3] = ukf->X_f32[3]; 115 | } 116 | 117 | #endif 118 | -------------------------------------------------------------------------------- /Algorithm/src/Control.c: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/suhetao/stm32f4_mpu9250/b86160c2b781f16973cd20293c67c5fab25f54f4/Algorithm/src/Control.c -------------------------------------------------------------------------------- /Algorithm/src/PID.c: -------------------------------------------------------------------------------- 1 | /* 2 | The MIT License (MIT) 3 | 4 | Copyright (c) 2015-? suhetao 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | this software and associated documentation files (the "Software"), to deal in 8 | the Software without restriction, including without limitation the rights to 9 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | the Software, and to permit persons to whom the Software is furnished to do so, 11 | subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | #include "PID.h" 25 | 26 | -------------------------------------------------------------------------------- /Application/inc/stm32f4_common.h: -------------------------------------------------------------------------------- 1 | /* 2 | The MIT License (MIT) 3 | 4 | Copyright (c) 2015-? suhetao 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | this software and associated documentation files (the "Software"), to deal in 8 | the Software without restriction, including without limitation the rights to 9 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | the Software, and to permit persons to whom the Software is furnished to do so, 11 | subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | #ifndef _STM32F4_COMMON_H 25 | #define _STM32F4_COMMON_H 26 | 27 | #include "stm32f4xx.h" 28 | 29 | #endif 30 | -------------------------------------------------------------------------------- /Application/inc/stm32f4_crc.h: -------------------------------------------------------------------------------- 1 | /* 2 | The MIT License (MIT) 3 | 4 | Copyright (c) 2015-? suhetao 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | this software and associated documentation files (the "Software"), to deal in 8 | the Software without restriction, including without limitation the rights to 9 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | the Software, and to permit persons to whom the Software is furnished to do so, 11 | subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | #ifndef __STM32F4_CRC_H 25 | #define __STM32F4_CRC_H 26 | 27 | // Includes 28 | #include "stm32f4xx.h" 29 | 30 | u16 CRC_Cal16(u8 *puchMsg, u16 usDataLen); 31 | 32 | #endif 33 | -------------------------------------------------------------------------------- /Application/inc/stm32f4_delay.h: -------------------------------------------------------------------------------- 1 | /* 2 | The MIT License (MIT) 3 | 4 | Copyright (c) 2015-? suhetao 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | this software and associated documentation files (the "Software"), to deal in 8 | the Software without restriction, including without limitation the rights to 9 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | the Software, and to permit persons to whom the Software is furnished to do so, 11 | subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | #ifndef STM32F4_DELAY_H 25 | #define STM32F4_DELAY_H 26 | 27 | #include "stm32f4xx.h" 28 | 29 | void Delay_Init(void); 30 | void Delay_Ms(u32 ms); 31 | void Delay_Us(u32 value); 32 | 33 | int Get_Ms(unsigned long *count); 34 | 35 | u32 Micros(void); 36 | u32 Millis(void); 37 | 38 | #endif 39 | -------------------------------------------------------------------------------- /Application/inc/stm32f4_dmp.h: -------------------------------------------------------------------------------- 1 | /* 2 | The MIT License (MIT) 3 | 4 | Copyright (c) 2015-? suhetao 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | this software and associated documentation files (the "Software"), to deal in 8 | the Software without restriction, including without limitation the rights to 9 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | the Software, and to permit persons to whom the Software is furnished to do so, 11 | subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | #define DEFAULT_MPU_HZ (200) 25 | 26 | __inline unsigned short inv_row_2_scale(const signed char *row) 27 | { 28 | unsigned short b; 29 | if (row[0] > 0) 30 | b = 0; 31 | else if (row[0] < 0) 32 | b = 4; 33 | else if (row[1] > 0) 34 | b = 1; 35 | else if (row[1] < 0) 36 | b = 5; 37 | else if (row[2] > 0) 38 | b = 2; 39 | else if (row[2] < 0) 40 | b = 6; 41 | else 42 | b = 7; // error 43 | return b; 44 | } 45 | 46 | static __inline unsigned short inv_orientation_matrix_to_scalar(const signed char *mtx){ 47 | unsigned short scalar; 48 | /* 49 | XYZ 010_001_000 Identity Matrix 50 | XZY 001_010_000 51 | YXZ 010_000_001 52 | YZX 000_010_001 53 | ZXY 001_000_010 54 | ZYX 000_001_010 55 | */ 56 | scalar = inv_row_2_scale(mtx); 57 | scalar |= inv_row_2_scale(mtx + 3) << 3; 58 | scalar |= inv_row_2_scale(mtx + 6) << 6; 59 | return scalar; 60 | } 61 | -------------------------------------------------------------------------------- /Application/inc/stm32f4_exti.h: -------------------------------------------------------------------------------- 1 | /* 2 | The MIT License (MIT) 3 | 4 | Copyright (c) 2015-? suhetao 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | this software and associated documentation files (the "Software"), to deal in 8 | the Software without restriction, including without limitation the rights to 9 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | the Software, and to permit persons to whom the Software is furnished to do so, 11 | subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | #ifndef _STM32F4_EXTI_H 25 | #define _STM32F4_EXTI_H 26 | 27 | #include "stm32f4xx.h" 28 | 29 | //Define the Interrupt pin 30 | #define INTERRUPT_PIN GPIO_Pin_8 31 | #define INTERRUPT_GPIO_PORT GPIOB 32 | #define INTERRUPT_GPIO_CLK RCC_AHB1Periph_GPIOB 33 | #define INTERRUPT_EXTI_LINE EXTI_Line8 34 | #define INTERRUPT_EXTI_PORT_SOURCE EXTI_PortSourceGPIOB 35 | #define INTERRUPT_EXTI_PIN_SOURCE GPIO_PinSource8 36 | #define INTERRUPT_EDGE EXTI_Trigger_Rising 37 | #define INTERRUPT_EXTI_IRQN EXTI9_5_IRQn 38 | 39 | #define INTERRUPT_EXTI_PREEMPTION_PRIORITY 14 40 | #define INTERRUPT_EXTI_SUB_PRIORITY 0 41 | 42 | void Interrupt_Init(void); 43 | u8 Interrupt_GetState(void); 44 | 45 | #endif 46 | -------------------------------------------------------------------------------- /Application/inc/stm32f4_gps.h: -------------------------------------------------------------------------------- 1 | /* 2 | The MIT License (MIT) 3 | 4 | Copyright (c) 2015-? suhetao 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | this software and associated documentation files (the "Software"), to deal in 8 | the Software without restriction, including without limitation the rights to 9 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | the Software, and to permit persons to whom the Software is furnished to do so, 11 | subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | #ifndef _STM32F4_GPS_H 25 | #define _STM32F4_GPS_H 26 | 27 | #endif 28 | -------------------------------------------------------------------------------- /Application/inc/stm32f4_ms5611.h: -------------------------------------------------------------------------------- 1 | /* 2 | The MIT License (MIT) 3 | 4 | Copyright (c) 2015-? suhetao 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | this software and associated documentation files (the "Software"), to deal in 8 | the Software without restriction, including without limitation the rights to 9 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | the Software, and to permit persons to whom the Software is furnished to do so, 11 | subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | #ifndef STM32F4_MS5611_H 25 | #define STM32F4_MS5611_H 26 | 27 | #include "stm32f4xx.h" 28 | 29 | // 30 | #define MS5611_RESET 0x1E 31 | #define MS5611_READ_ADC 0x00 32 | #define MS5611_READ 0x00 33 | //OSR 256 0.60 mSec conversion time (1666.67 Hz) 34 | //OSR 512 1.17 mSec conversion time ( 854.70 Hz) 35 | //OSR 1024 2.28 mSec conversion time ( 357.14 Hz) 36 | //OSR 2048 4.54 mSec conversion time ( 220.26 Hz) 37 | //OSR 4096 9.04 mSec conversion time ( 110.62 Hz) 38 | #define D1_OSR_256 0x40 39 | #define D1_OSR_512 0x42 40 | #define D1_OSR_1024 0x44 41 | #define D1_OSR_2048 0x46 42 | #define D1_OSR_4096 0x48 43 | #define D2_OSR_256 0x50 44 | #define D2_OSR_512 0x52 45 | #define D2_OSR_1024 0x54 46 | #define D2_OSR_2048 0x56 47 | #define D2_OSR_4096 0x58 48 | #define MS5611_READ_PROM_RSV 0xA0 //0xA0 to 0xAE 49 | #define MS5611_READ_PROM_C1 0xA2 50 | #define MS5611_READ_PROM_C2 0xA4 51 | #define MS5611_READ_PROM_C3 0xA6 52 | #define MS5611_READ_PROM_C4 0xA8 53 | #define MS5611_READ_PROM_C5 0xAA 54 | #define MS5611_READ_PROM_C6 0xAC 55 | #define MS5611_READ_PROM_CRC 0xAE 56 | 57 | u8 MS5611_SPIx_SendByte(u8); 58 | 59 | void MS5611_Init(void); 60 | void MS5611_GetTemperatureAndPressure(s32* TEMP, s32 *P); 61 | 62 | #endif 63 | -------------------------------------------------------------------------------- /Application/inc/stm32f4_rcc.h: -------------------------------------------------------------------------------- 1 | /* 2 | The MIT License (MIT) 3 | 4 | Copyright (c) 2015-? suhetao 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | this software and associated documentation files (the "Software"), to deal in 8 | the Software without restriction, including without limitation the rights to 9 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | the Software, and to permit persons to whom the Software is furnished to do so, 11 | subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | #ifndef _STM32F4_RCC_H 25 | #define _STM32F4_RCC_H 26 | 27 | #include "stm32f4xx.h" 28 | 29 | typedef struct PLL_PARAMS_T 30 | { 31 | uint32_t PLLM; 32 | uint32_t PLLN; 33 | uint32_t PLLP; 34 | uint32_t PLLQ; 35 | } 36 | PLL_PARAMS; 37 | 38 | typedef void (*RCC_AXXPeriphClockCmd)(uint32_t RCC_AXXPeriph, FunctionalState NewState); 39 | 40 | void RCC_SystemCoreClockUpdate(PLL_PARAMS params); 41 | 42 | #endif 43 | -------------------------------------------------------------------------------- /Application/inc/stm32f4_serial.h: -------------------------------------------------------------------------------- 1 | /* 2 | The MIT License (MIT) 3 | 4 | Copyright (c) 2015-? suhetao 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | this software and associated documentation files (the "Software"), to deal in 8 | the Software without restriction, including without limitation the rights to 9 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | the Software, and to permit persons to whom the Software is furnished to do so, 11 | subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | #ifndef _STM32F4_SERIAL_H 25 | #define _STM32F4_SERIAL_H 26 | 27 | #include "stm32f4xx.h" 28 | 29 | #define PACKET_LENGTH (46) 30 | 31 | //low function 32 | void Serial_Init(void); 33 | void Serial_SendByte(uint8_t byte); 34 | void Serial_SendBytes(uint8_t* buffer, uint8_t length); 35 | 36 | // 37 | void Serial_Upload(short accel[3], short gyro[3], short compass[3], long quat[4], long temperature, long pressure); 38 | 39 | #endif 40 | -------------------------------------------------------------------------------- /Application/inc/stm32f4_string.h: -------------------------------------------------------------------------------- 1 | #ifndef _STM32F4_STRING_H_ 2 | #define _STM32F4_STRING_H_ 3 | 4 | #include "stm32f4xx.h" 5 | 6 | void FastMemCpy(uint8_t* dest, uint8_t* src, uint16_t size); 7 | 8 | #endif 9 | -------------------------------------------------------------------------------- /Application/inc/stm32f4_ublox.h: -------------------------------------------------------------------------------- 1 | /* 2 | The MIT License (MIT) 3 | 4 | Copyright (c) 2015-? suhetao 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | this software and associated documentation files (the "Software"), to deal in 8 | the Software without restriction, including without limitation the rights to 9 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | the Software, and to permit persons to whom the Software is furnished to do so, 11 | subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | #ifndef _STM32F4_UBLOX_H 25 | #define _STM32F4_UBLOX_H 26 | 27 | #include "stm32f4xx.h" 28 | #include "Nema.h" 29 | 30 | #define UBLOX_DEFAULT_BAUDRATE (9600) 31 | #define UBLOX_DEFAULT_TX_BUFFERSIZE (64) 32 | #define UBLOX_DEFAULT_RX_BUFFERSIZE (512) 33 | #define UBLOX_DEFAULT_PARSER_MAXSIZE (128) 34 | 35 | typedef struct UBLOX_PARSERBUFF 36 | { 37 | s8 *Data; 38 | u16 Size; 39 | u16 Need; 40 | u16 Left; 41 | }Ublox_ParserBuff; 42 | 43 | void Ublox_Init(void); 44 | void Ublox_GetMessage(void); 45 | void Ublox_GetPostion(double *x, double *y, double *z); 46 | 47 | #endif 48 | -------------------------------------------------------------------------------- /Application/inc/stm32f4xx_conf.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file stm32f4xx_conf.h 4 | * @author MCD Application Team 5 | * @version V1.0.0 6 | * @date 19-September-2011 7 | * @brief Library configuration file. 8 | ****************************************************************************** 9 | * @attention 10 | * 11 | * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS 12 | * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE 13 | * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY 14 | * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING 15 | * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE 16 | * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. 17 | * 18 | *

© COPYRIGHT 2011 STMicroelectronics

19 | ****************************************************************************** 20 | */ 21 | 22 | /* Define to prevent recursive inclusion -------------------------------------*/ 23 | #ifndef __STM32F4xx_CONF_H 24 | #define __STM32F4xx_CONF_H 25 | 26 | #if defined (HSE_VALUE) 27 | /* Redefine the HSE value; it's equal to 12 MHz on the STM32F4-DISCOVERY Kit */ 28 | #undef HSE_VALUE 29 | #define HSE_VALUE ((uint32_t)12000000) 30 | #endif /* HSE_VALUE */ 31 | 32 | /* Includes ------------------------------------------------------------------*/ 33 | /* Uncomment the line below to enable peripheral header file inclusion */ 34 | // #include "stm32f4xx_adc.h" 35 | // #include "stm32f4xx_can.h" 36 | //#include "stm32f4xx_crc.h" 37 | // #include "stm32f4xx_cryp.h" 38 | // #include "stm32f4xx_dac.h" 39 | // #include "stm32f4xx_dbgmcu.h" 40 | // #include "stm32f4xx_dcmi.h" 41 | #include "stm32f4xx_dma.h" 42 | #include "stm32f4xx_exti.h" 43 | #include "stm32f4xx_flash.h" 44 | // #include "stm32f4xx_fsmc.h" 45 | // #include "stm32f4xx_hash.h" 46 | #include "stm32f4xx_gpio.h" 47 | //#include "stm32f4xx_i2c.h" 48 | // #include "stm32f4xx_iwdg.h" 49 | // #include "stm32f4xx_pwr.h" 50 | #include "stm32f4xx_rcc.h" 51 | // #include "stm32f4xx_rng.h" 52 | // #include "stm32f4xx_rtc.h" 53 | // #include "stm32f4xx_sdio.h" 54 | #include "stm32f4xx_spi.h" 55 | #include "stm32f4xx_syscfg.h" 56 | //#include "stm32f4xx_tim.h" 57 | #include "stm32f4xx_usart.h" 58 | // #include "stm32f4xx_wwdg.h" 59 | #include "misc.h" /* High level functions for NVIC and SysTick (add-on to CMSIS functions) */ 60 | 61 | /* Exported types ------------------------------------------------------------*/ 62 | /* Exported constants --------------------------------------------------------*/ 63 | 64 | /* If an external clock source is used, then the value of the following define 65 | should be set to the value of the external clock source, else, if no external 66 | clock is used, keep this define commented */ 67 | /*#define I2S_EXTERNAL_CLOCK_VAL 12288000 */ /* Value of the external clock in Hz */ 68 | 69 | 70 | /* Uncomment the line below to expanse the "assert_param" macro in the 71 | Standard Peripheral Library drivers code */ 72 | /* #define USE_FULL_ASSERT 1 */ 73 | 74 | /* Exported macro ------------------------------------------------------------*/ 75 | #ifdef USE_FULL_ASSERT 76 | 77 | /** 78 | * @brief The assert_param macro is used for function's parameters check. 79 | * @param expr: If expr is false, it calls assert_failed function 80 | * which reports the name of the source file and the source 81 | * line number of the call that failed. 82 | * If expr is true, it returns no value. 83 | * @retval None 84 | */ 85 | #define assert_param(expr) ((expr) ? (void)0 : assert_failed((uint8_t *)__FILE__, __LINE__)) 86 | /* Exported functions ------------------------------------------------------- */ 87 | void assert_failed(uint8_t* file, uint32_t line); 88 | #else 89 | #define assert_param(expr) ((void)0) 90 | #endif /* USE_FULL_ASSERT */ 91 | 92 | #endif /* __STM32F4xx_CONF_H */ 93 | 94 | /******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/ 95 | -------------------------------------------------------------------------------- /Application/inc/stm32f4xx_it.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file USART/USART_TwoBoards/USART_DataExchangeDMA/stm32f4xx_it.h 4 | * @author MCD Application Team 5 | * @version V1.4.0 6 | * @date 04-August-2014 7 | * @brief This file contains the headers of the interrupt handlers. 8 | ****************************************************************************** 9 | * @attention 10 | * 11 | *

© COPYRIGHT 2013 STMicroelectronics

12 | * 13 | * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); 14 | * You may not use this file except in compliance with the License. 15 | * You may obtain a copy of the License at: 16 | * 17 | * http://www.st.com/software_license_agreement_liberty_v2 18 | * 19 | * Unless required by applicable law or agreed to in writing, software 20 | * distributed under the License is distributed on an "AS IS" BASIS, 21 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 22 | * See the License for the specific language governing permissions and 23 | * limitations under the License. 24 | * 25 | ****************************************************************************** 26 | */ 27 | 28 | /* Define to prevent recursive inclusion -------------------------------------*/ 29 | #ifndef __STM32F4xx_IT_H 30 | #define __STM32F4xx_IT_H 31 | 32 | /* Includes ------------------------------------------------------------------*/ 33 | #include "stm32f4xx.h" 34 | 35 | /* Exported types ------------------------------------------------------------*/ 36 | /* Exported constants --------------------------------------------------------*/ 37 | /* Exported macro ------------------------------------------------------------*/ 38 | /* Exported functions ------------------------------------------------------- */ 39 | 40 | void NMI_Handler(void); 41 | void HardFault_Handler(void); 42 | void MemManage_Handler(void); 43 | void BusFault_Handler(void); 44 | void UsageFault_Handler(void); 45 | void SVC_Handler(void); 46 | void DebugMon_Handler(void); 47 | void PendSV_Handler(void); 48 | void SysTick_Handler(void); 49 | 50 | #endif /* __STM32F4xx_IT_H */ 51 | 52 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ 53 | -------------------------------------------------------------------------------- /Application/src/stm32f4_delay.c: -------------------------------------------------------------------------------- 1 | /* 2 | The MIT License (MIT) 3 | 4 | Copyright (c) 2015-? suhetao 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | this software and associated documentation files (the "Software"), to deal in 8 | the Software without restriction, including without limitation the rights to 9 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | the Software, and to permit persons to whom the Software is furnished to do so, 11 | subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | #include"stm32f4_delay.h" 25 | 26 | //Cycles per microsecond 27 | static __IO uint32_t us_ticks = 0; 28 | static __IO uint32_t uptime_ticks = 0; 29 | static __IO uint32_t cycle_ticks = 0; 30 | 31 | void Delay_Init() 32 | { 33 | RCC_ClocksTypeDef RCC_Clocks; 34 | RCC_GetClocksFreq(&RCC_Clocks); 35 | us_ticks = RCC_Clocks.SYSCLK_Frequency / 1000000u; 36 | 37 | //enable DWT access 38 | CoreDebug->DEMCR &= ~CoreDebug_DEMCR_TRCENA_Msk; 39 | CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk; 40 | //enable the CPU cycle counter 41 | DWT->CTRL &= ~DWT_CTRL_CYCCNTENA_Msk; 42 | DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk; 43 | //Reset counter 44 | DWT->CYCCNT = 0u; 45 | 46 | if(SysTick_Config(RCC_Clocks.HCLK_Frequency / 1000u)){ 47 | while (1); // Handle Error 48 | } 49 | } 50 | 51 | __inline uint32_t Millis(void) 52 | { 53 | return uptime_ticks; 54 | } 55 | 56 | u32 Micros(void) 57 | { 58 | register uint32_t old_cycle, cycle, timeMs; 59 | 60 | do{ 61 | timeMs = __LDREXW(&uptime_ticks); 62 | cycle = DWT->CYCCNT; 63 | old_cycle = cycle_ticks; 64 | } 65 | while ( __STREXW( timeMs , &uptime_ticks ) ); 66 | return (timeMs * 1000) + (cycle - old_cycle) / us_ticks; 67 | } 68 | 69 | void Delay_Ms(u32 ms) 70 | { 71 | while (ms--){ 72 | Delay_Us(1000); 73 | } 74 | } 75 | 76 | void Delay_Us(u32 us) 77 | { 78 | uint32_t elapsed = 0; 79 | uint32_t elapsed_us = 0; 80 | uint32_t lastCount = DWT->CYCCNT; 81 | register uint32_t current_count = DWT->CYCCNT; 82 | 83 | for (;;) { 84 | current_count = DWT->CYCCNT; 85 | elapsed += current_count - lastCount; 86 | lastCount = current_count; 87 | 88 | elapsed_us = elapsed / us_ticks; 89 | if (elapsed_us >= us){ 90 | break; 91 | } 92 | us -= elapsed_us; 93 | elapsed %= us_ticks; 94 | } 95 | } 96 | 97 | int Get_Ms(unsigned long *count) 98 | { 99 | count[0] = uptime_ticks; 100 | return 0; 101 | } 102 | 103 | void SysTick_Handler(void) 104 | { 105 | cycle_ticks = DWT->CYCCNT; 106 | uptime_ticks++; 107 | } 108 | -------------------------------------------------------------------------------- /Application/src/stm32f4_exti.c: -------------------------------------------------------------------------------- 1 | /* 2 | The MIT License (MIT) 3 | 4 | Copyright (c) 2015-? suhetao 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | this software and associated documentation files (the "Software"), to deal in 8 | the Software without restriction, including without limitation the rights to 9 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | the Software, and to permit persons to whom the Software is furnished to do so, 11 | subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | #include "stm32f4_exti.h" 25 | #include "stm32f4_mpu9250.h" 26 | 27 | static __IO u8 gu8InterruptState = 0; 28 | 29 | void Interrupt_Init() 30 | { 31 | GPIO_InitTypeDef GPIO_InitStructure; 32 | EXTI_InitTypeDef EXTI_InitStructure; 33 | NVIC_InitTypeDef NVIC_InitStructure; 34 | 35 | //Enable MPU9250int GPIO clocks 36 | RCC_AHB1PeriphClockCmd(INTERRUPT_GPIO_CLK, ENABLE); 37 | //Enable SYSCFG clock 38 | RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE); 39 | 40 | //Configure MPU9250int pin as input floating 41 | GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN; 42 | GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; 43 | GPIO_InitStructure.GPIO_Pin = INTERRUPT_PIN; 44 | GPIO_Init(INTERRUPT_GPIO_PORT, &GPIO_InitStructure); 45 | 46 | //Connect MPU9250int EXTI Line to MPU9250int GPIO Pin 47 | SYSCFG_EXTILineConfig(INTERRUPT_EXTI_PORT_SOURCE, INTERRUPT_EXTI_PIN_SOURCE); 48 | 49 | //Configure MPU9250int EXTI line 50 | EXTI_InitStructure.EXTI_Line = INTERRUPT_EXTI_LINE; 51 | EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt; 52 | EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Rising; 53 | EXTI_InitStructure.EXTI_LineCmd = ENABLE; 54 | EXTI_Init(&EXTI_InitStructure); 55 | 56 | //Enable and set MPU9250int EXTI Interrupt priority 57 | NVIC_InitStructure.NVIC_IRQChannel = INTERRUPT_EXTI_IRQN; 58 | NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = INTERRUPT_EXTI_PREEMPTION_PRIORITY; 59 | NVIC_InitStructure.NVIC_IRQChannelSubPriority = INTERRUPT_EXTI_SUB_PRIORITY; 60 | NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; 61 | NVIC_Init(&NVIC_InitStructure); 62 | } 63 | 64 | void EXTI9_5_IRQHandler(void) 65 | { 66 | gu8InterruptState = 1; 67 | EXTI_ClearITPendingBit(EXTI_Line8); 68 | } 69 | 70 | u8 Interrupt_GetState(void){ 71 | u8 u8State = gu8InterruptState; 72 | gu8InterruptState = 0; 73 | return u8State; 74 | } 75 | -------------------------------------------------------------------------------- /Application/src/stm32f4_gps.c: -------------------------------------------------------------------------------- 1 | /* 2 | The MIT License (MIT) 3 | 4 | Copyright (c) 2015-? suhetao 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | this software and associated documentation files (the "Software"), to deal in 8 | the Software without restriction, including without limitation the rights to 9 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | the Software, and to permit persons to whom the Software is furnished to do so, 11 | subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | #include "stm32f4_gps.h" 25 | 26 | -------------------------------------------------------------------------------- /Application/src/stm32f4_rcc.c: -------------------------------------------------------------------------------- 1 | /* 2 | The MIT License (MIT) 3 | 4 | Copyright (c) 2015-? suhetao 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | this software and associated documentation files (the "Software"), to deal in 8 | the Software without restriction, including without limitation the rights to 9 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | the Software, and to permit persons to whom the Software is furnished to do so, 11 | subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | #include "stm32f4_rcc.h" 25 | 26 | void RCC_SystemCoreClockUpdate(PLL_PARAMS params) 27 | { 28 | //reset RCC 29 | RCC_DeInit(); 30 | 31 | //start HSI, wait until up 32 | RCC_HSICmd(ENABLE); 33 | while(RCC_GetFlagStatus(RCC_FLAG_HSIRDY) == RESET); 34 | 35 | //switch the system clock to HSI 36 | RCC_SYSCLKConfig(RCC_SYSCLKSource_HSI); 37 | while(RCC_GetSYSCLKSource() != 0x00); 38 | 39 | //stop PLL, wait until down 40 | RCC_PLLCmd(DISABLE); 41 | while(RCC_GetFlagStatus(RCC_FLAG_PLLRDY) == SET); 42 | 43 | //start HSE, wait until up 44 | RCC_HSEConfig(RCC_HSE_ON); 45 | while(RCC_GetFlagStatus(RCC_FLAG_HSERDY) == RESET); 46 | 47 | //change PLL params; 48 | RCC_HCLKConfig(RCC_SYSCLK_Div1); 49 | RCC_PCLK1Config(RCC_HCLK_Div4); 50 | RCC_PCLK2Config(RCC_HCLK_Div2); 51 | RCC_PLLConfig(RCC_PLLSource_HSE, params.PLLM, 52 | params.PLLN, params.PLLP, params.PLLQ); 53 | 54 | //start PLL, wait until up 55 | RCC_PLLCmd(ENABLE); 56 | while(RCC_GetFlagStatus(RCC_FLAG_PLLRDY) == RESET); 57 | 58 | //switch system clock to PLL, wait until switched 59 | RCC_SYSCLKConfig(RCC_SYSCLKSource_PLLCLK); 60 | while(RCC_GetSYSCLKSource() != 0x08); 61 | 62 | //update 63 | SystemCoreClockUpdate(); 64 | } 65 | -------------------------------------------------------------------------------- /Application/src/stm32f4_serial.c: -------------------------------------------------------------------------------- 1 | /* 2 | The MIT License (MIT) 3 | 4 | Copyright (c) 2015-? suhetao 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | this software and associated documentation files (the "Software"), to deal in 8 | the Software without restriction, including without limitation the rights to 9 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | the Software, and to permit persons to whom the Software is furnished to do so, 11 | subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | #include "stm32f4_serial.h" 25 | #include "stm32f4_usart.h" 26 | #include "Memory.h" 27 | 28 | #ifdef USARTx_USE_DMA 29 | static uint8_t DMA_TxBuffer[DEFAULT_BUFFERSIZE]; 30 | static uint8_t DMA_RxBuffer[DEFAULT_BUFFERSIZE]; 31 | //static uint8_t USARTx_Rx_Buffer[DEFAULT_BUFFERSIZE]; 32 | #endif 33 | 34 | static USART_Driver Serial = { 35 | USART1, RCC_APB2PeriphClockCmd, RCC_APB2Periph_USART1, DEFAULT_BAUDRATE, 36 | GPIOA, RCC_AHB1PeriphClockCmd , RCC_AHB1Periph_GPIOA, GPIO_Pin_9, GPIO_PinSource9, 37 | GPIOA, RCC_AHB1PeriphClockCmd , RCC_AHB1Periph_GPIOA, GPIO_Pin_10, GPIO_PinSource10, 38 | #ifdef USARTx_USE_DMA 39 | { USART1_IRQn, 1, 2, ENABLE }, 40 | { DMA2_Stream7_IRQn, 1, 3, ENABLE }, 41 | 42 | RCC_AHB1PeriphClockCmd, RCC_AHB1Periph_DMA2, 43 | DEFAULT_BUFFERSIZE, DMA_TxBuffer, DMA2_Stream7, DMA_Channel_4, 44 | DEFAULT_BUFFERSIZE, DMA_RxBuffer, DMA2_Stream5, DMA_Channel_4, 45 | #endif 46 | GPIO_AF_USART1 47 | }; 48 | static USART_Driver* pSerial = &Serial; 49 | 50 | void Serial_Init(void) 51 | { 52 | USARTx_Init(pSerial); 53 | } 54 | 55 | void Serial_SendByte(uint8_t byte) 56 | { 57 | USARTx_SendByte(pSerial, byte); 58 | } 59 | 60 | void Serial_SendBytes(uint8_t* buffer, uint8_t length) 61 | { 62 | #ifdef USARTx_USE_DMA 63 | USARTx_DMA_SendBytes(pSerial, buffer, length); 64 | #else 65 | USARTx_SendBytes(pSerial, buffer, length); 66 | #endif 67 | } 68 | 69 | void Serial_Upload(short accel[3], short gyro[3], short compass[3], long quat[4], long temperature, long pressure) 70 | { 71 | //must 4-bytes alignment 72 | uint8_t out[PACKET_LENGTH]; 73 | short* sDest = (short*)out; 74 | long* lDest; 75 | //uint8_t count = 0; 76 | //memset(out, 0, PACKET_LENGTH); 77 | //out[0] = 0x55; 78 | //out[1] = 0xAA; 79 | 80 | *sDest++ = 0xAA55; 81 | //accel 82 | /* 83 | out[4] = (uint8_t)accel[0]; 84 | out[5] = (uint8_t)(accel[0] >> 8); 85 | out[6] = (uint8_t)accel[1]; 86 | out[7] = (uint8_t)(accel[1] >> 8); 87 | out[8] = (uint8_t)accel[2]; 88 | out[9] = (uint8_t)(accel[2] >> 8); 89 | */ 90 | *sDest++ = accel[0]; 91 | *sDest++ = accel[1]; 92 | *sDest++ = accel[2]; 93 | //gyro 94 | /* 95 | out[10] = (uint8_t)gyro[0]; 96 | out[11] = (uint8_t)(gyro[0] >> 8); 97 | out[12] = (uint8_t)gyro[1]; 98 | out[13] = (uint8_t)(gyro[1] >> 8); 99 | out[14] = (uint8_t)gyro[2]; 100 | out[15] = (uint8_t)(gyro[2] >> 8); 101 | */ 102 | *sDest++ = gyro[0]; 103 | *sDest++ = gyro[1]; 104 | *sDest++ = gyro[2]; 105 | //compass 106 | /* 107 | out[16] = (uint8_t)compass[0]; 108 | out[17] = (uint8_t)(compass[0] >> 8); 109 | out[18] = (uint8_t)compass[1]; 110 | out[19] = (uint8_t)(compass[1] >> 8); 111 | out[20] = (uint8_t)compass[2]; 112 | out[21] = (uint8_t)(compass[2] >> 8); 113 | */ 114 | *sDest++ = compass[0]; 115 | *sDest++ = compass[1]; 116 | *sDest++ = compass[2]; 117 | //quat 118 | /* 119 | out[22] = (uint8_t)quat[0]; 120 | out[23] = (uint8_t)(quat[0] >> 8); 121 | out[24] = (uint8_t)(quat[0] >> 16); 122 | out[25] = (uint8_t)(quat[0] >> 24); 123 | out[26] = (uint8_t)quat[1]; 124 | out[27] = (uint8_t)(quat[1] >> 8); 125 | out[28] = (uint8_t)(quat[1] >> 16); 126 | out[29] = (uint8_t)(quat[1] >> 24); 127 | out[30] = (uint8_t)quat[2]; 128 | out[31] = (uint8_t)(quat[2] >> 8); 129 | out[32] = (uint8_t)(quat[2] >> 16); 130 | out[33] = (uint8_t)(quat[2] >> 24); 131 | out[34] = (uint8_t)quat[3]; 132 | out[35] = (uint8_t)(quat[3] >> 8); 133 | out[36] = (uint8_t)(quat[3] >> 16); 134 | out[37] = (uint8_t)(quat[3] >> 24); 135 | */ 136 | lDest = (long*)sDest; 137 | *lDest++ = quat[0]; 138 | *lDest++ = quat[1]; 139 | *lDest++ = quat[2]; 140 | *lDest++ = quat[3]; 141 | //tempperature 142 | /* 143 | out[38] = (uint8_t)temperature; 144 | out[39] = (uint8_t)(temperature >> 8); 145 | out[40] = (uint8_t)(temperature >> 16); 146 | out[41] = (uint8_t)(temperature >> 24); 147 | */ 148 | *lDest++ = temperature; 149 | //pressure 150 | /* 151 | out[42] = (uint8_t)pressure; 152 | out[43] = (uint8_t)(pressure >> 8); 153 | out[44] = (uint8_t)(pressure >> 16); 154 | out[45] = (uint8_t)(pressure >> 24); 155 | */ 156 | *lDest++ = pressure; 157 | 158 | //out[44] = '\r'; 159 | //out[45] = '\n'; 160 | 161 | sDest = (short*)lDest; 162 | *sDest = 0x0A0D; 163 | 164 | Serial_SendBytes((uint8_t*)out, PACKET_LENGTH); 165 | } 166 | 167 | #ifdef USARTx_USE_DMA 168 | //according to the hardware 169 | //such as using usart1, DMA2_Stream7 for tx, DMA2_Stream5 for rx 170 | void USART1_IRQHandler(void) 171 | { 172 | //u16 DATA_LEN = 0; 173 | if(USART_GetITStatus(USART1, USART_IT_TC) != RESET){ 174 | USART_ITConfig(USART1, USART_IT_TC, DISABLE); 175 | } 176 | else if(USART_GetITStatus(USART1, USART_IT_IDLE) != RESET){ 177 | USART1->SR; 178 | USART1->DR; 179 | 180 | DMA_Cmd(DMA2_Stream5, DISABLE); 181 | DMA_ClearFlag(DMA2_Stream5, DMA_FLAG_TCIF5); 182 | //DATA_LEN = DEFAULT_BUFFERSIZE - DMA_GetCurrDataCounter(DMA2_Stream5); 183 | //FastMemCpy(USARTx_Rx_Buffer, DMA_RxBuffer, DATA_LEN); 184 | DMA_SetCurrDataCounter(DMA2_Stream5, DEFAULT_BUFFERSIZE); 185 | DMA_Cmd(DMA2_Stream5, ENABLE); 186 | } 187 | } 188 | 189 | //TX 190 | void DMA2_Stream7_IRQHandler(void) 191 | { 192 | if(DMA_GetITStatus(DMA2_Stream7, DMA_IT_TCIF7)){ 193 | DMA_ClearITPendingBit(DMA2_Stream7, DMA_IT_TCIF7); 194 | DMA_Cmd(DMA2_Stream7, DISABLE); 195 | USART_ITConfig(USART1, USART_IT_TC, ENABLE); 196 | } 197 | } 198 | 199 | #endif 200 | -------------------------------------------------------------------------------- /Application/src/stm32f4_string.c: -------------------------------------------------------------------------------- 1 | #include "stm32f4_string.h" 2 | 3 | //make sure 4-bytes aligned 4 | __asm void FastMemCpy(uint8_t* dest, uint8_t* src, uint16_t size) 5 | { 6 | push {r4-r11}; 7 | lsr r3, r2, #5; 32 bytes 8 | cmp r3, #0; 9 | beq notenough_32; 10 | and r2, r2, #31; left bytes 11 | loop_32 12 | ldmia r1!, {r4-r11}; 13 | stmia r0!, {r4-r11}; 14 | subs r3, r3, #1; 15 | bne loop_32; 16 | notenough_32 17 | lsr r3, r2, #4; 16 bytes 18 | cmp r3, #0; 19 | beq notenough_16; 20 | and r2, r2, #15; left bytes 21 | loop_16 22 | ldmia r1!, {r4-r7}; 23 | stmia r0!, {r4-r7}; 24 | subs r3, r3, #1; 25 | bne loop_16; 26 | notenough_16 27 | lsr r3, r2, #3; 8 bytes 28 | cmp r3, #0; 29 | beq notenough_8; 30 | and r2, r2, #7; left bytes 31 | loop_8 32 | ldmia r1!, {r4-r5}; 33 | stmia r0!, {r4-r5}; 34 | subs r3, r3, #1; 35 | bne loop_8; 36 | notenough_8 37 | lsr r3, r2, #2; 4 bytes 38 | cmp r3, #0; 39 | beq notenough_4; 40 | and r2, r2, #3; left bytes 41 | loop_4 42 | ldmia r1!, {r4}; 43 | stmia r0!, {r4}; 44 | subs r3, r3, #1; 45 | bne loop_4; 46 | notenough_4 47 | cmp r2, #0; 48 | beq exit 49 | 50 | loop_1 51 | subs r2, r2, #1; 52 | ldrb r3, [r1, r2] 53 | strb r3, [r0, r2] 54 | cmp r2, #0 55 | bne loop_1; 56 | exit 57 | pop {r4-r11} 58 | bx lr 59 | } 60 | -------------------------------------------------------------------------------- /Application/src/stm32f4_ublox.c: -------------------------------------------------------------------------------- 1 | /* 2 | The MIT License (MIT) 3 | 4 | Copyright (c) 2015-? suhetao 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | this software and associated documentation files (the "Software"), to deal in 8 | the Software without restriction, including without limitation the rights to 9 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | the Software, and to permit persons to whom the Software is furnished to do so, 11 | subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | #include "stm32f4_ublox.h" 25 | #include "stm32f4_usart.h" 26 | // 27 | #include "Map.h" 28 | #include "Memory.h" 29 | #include "Fifo.h" 30 | 31 | //ungly implement: the max size of the data 32 | //max(sizeof(nmeaGPGGA), sizeof(nmeaGPGSA), sizeof(nmeaGPRMC), sizeof(nmeaGPVTG)) 33 | #define UBLOX_MAX_MESSAGESIZE sizeof(nmeaGPGGA) 34 | 35 | ////////////////////////////////////////////////////////////////////////// 36 | //UBlox Driver 37 | #ifdef USARTx_USE_DMA 38 | static uint8_t DMA_TxBuffer[UBLOX_DEFAULT_TX_BUFFERSIZE]; 39 | static uint8_t DMA_RxBuffer[UBLOX_DEFAULT_RX_BUFFERSIZE]; 40 | static uint8_t USARTx_Rx_Buffer[UBLOX_DEFAULT_RX_BUFFERSIZE]; 41 | #endif 42 | 43 | static USART_Driver Ublox = { 44 | UART4, RCC_APB1PeriphClockCmd, RCC_APB1Periph_UART4, UBLOX_DEFAULT_BAUDRATE, 45 | GPIOC, RCC_AHB1PeriphClockCmd , RCC_AHB1Periph_GPIOC, GPIO_Pin_10, GPIO_PinSource10, 46 | GPIOC, RCC_AHB1PeriphClockCmd , RCC_AHB1Periph_GPIOC, GPIO_Pin_11, GPIO_PinSource11, 47 | #ifdef USARTx_USE_DMA 48 | { UART4_IRQn, 1, 2, ENABLE }, 49 | { DMA1_Stream4_IRQn, 1, 3, ENABLE }, 50 | 51 | RCC_AHB1PeriphClockCmd, RCC_AHB1Periph_DMA2, 52 | UBLOX_DEFAULT_TX_BUFFERSIZE, DMA_TxBuffer, DMA1_Stream4, DMA_Channel_4, 53 | UBLOX_DEFAULT_RX_BUFFERSIZE, DMA_RxBuffer, DMA1_Stream2, DMA_Channel_4, 54 | #endif 55 | GPIO_AF_UART4 56 | }; 57 | // 58 | static USART_Driver* pUblox = &Ublox; 59 | ////////////////////////////////////////////////////////////////////////// 60 | //Fifo 61 | static Fifo UbloxFifo; 62 | static Fifo* pUbloxFifo = &UbloxFifo; 63 | ////////////////////////////////////////////////////////////////////////// 64 | //Ublox GPS Parser 65 | static s8 _ParserBuff[UBLOX_DEFAULT_PARSER_MAXSIZE]; 66 | static Ublox_ParserBuff ParserBuffer = {_ParserBuff, UBLOX_DEFAULT_PARSER_MAXSIZE, UBLOX_DEFAULT_PARSER_MAXSIZE, 0}; 67 | static s8 Message[UBLOX_MAX_MESSAGESIZE]; 68 | static nmeaINFO uBloxInfo = {0}; 69 | 70 | static Map UbloxMap = {0}; 71 | static Map *pUbloxMap = &UbloxMap; 72 | ////////////////////////////////////////////////////////////////////////// 73 | void Ublox_Init(void) 74 | { 75 | Fifo_Init(pUbloxFifo, USARTx_Rx_Buffer, UBLOX_DEFAULT_RX_BUFFERSIZE); 76 | USARTx_Init(pUblox); 77 | //must init with local two reference points including lat, lon and the length 78 | //betweeb two reference points 79 | //Map_Init(pUbloxMap, first_lat, first_lon, second_lat, second_lon, length); 80 | } 81 | 82 | void Ublox_SendBytes(uint8_t* buffer, uint8_t length) 83 | { 84 | #ifdef USARTx_USE_DMA 85 | USARTx_DMA_SendBytes(pUblox, buffer, length); 86 | #else 87 | USARTx_SendBytes(pUblox, buffer, length); 88 | #endif 89 | } 90 | 91 | void Ublox_ParserMessage() 92 | { 93 | u16 useSize; 94 | ////////////////////////////////////////////////////////////////////////// 95 | //read fully UBLOX_DEFAULT_PARSER_MAXSIZE from fifo 96 | useSize = Fifo_Get(pUbloxFifo, (u8*)(ParserBuffer.Data + ParserBuffer.Left), ParserBuffer.Need); 97 | ParserBuffer.Need -= useSize; 98 | ParserBuffer.Left += useSize; 99 | if(0 == ParserBuffer.Need){ 100 | useSize = NEMA_Parser(ParserBuffer.Data, UBLOX_DEFAULT_PARSER_MAXSIZE); 101 | ParserBuffer.Left = UBLOX_DEFAULT_PARSER_MAXSIZE - useSize; 102 | MemMove((u8*)ParserBuffer.Data, (u8*)(ParserBuffer.Data + useSize), ParserBuffer.Left); 103 | ParserBuffer.Need = useSize; 104 | } 105 | } 106 | 107 | void Ublox_GetMessage() 108 | { 109 | s32 iMessageType; 110 | s16 ret = 0; 111 | Ublox_ParserMessage(); 112 | ret = NEMA_GetMessage(Message, &iMessageType); 113 | if(ret <= 0){ 114 | return; 115 | } 116 | switch(iMessageType){ 117 | case GPNON: 118 | break; 119 | case GPGGA: 120 | uBloxInfo.lat = NMEA_Convert2Degrees(((PnmeaGPGGA)Message)->lat); 121 | uBloxInfo.lon = NMEA_Convert2Degrees(((PnmeaGPGGA)Message)->lon); 122 | uBloxInfo.alt = ((PnmeaGPGGA)Message)->alt; 123 | break; 124 | case GPGSA: 125 | break; 126 | case GPRMC: 127 | uBloxInfo.lat = NMEA_Convert2Degrees(((PnmeaGPRMC)Message)->lat); 128 | uBloxInfo.lon = NMEA_Convert2Degrees(((PnmeaGPRMC)Message)->lon); 129 | uBloxInfo.spd = ((PnmeaGPRMC)Message)->spd; 130 | uBloxInfo.cog = ((PnmeaGPRMC)Message)->cog; 131 | break; 132 | case GPVTG: 133 | break; 134 | } 135 | } 136 | 137 | void Ublox_GetPostion(double *x, double *y, double *z) 138 | { 139 | Map_GetXY(pUbloxMap, uBloxInfo.lat, uBloxInfo.lon, x, y); 140 | *z = uBloxInfo.alt; 141 | } 142 | 143 | #ifdef USARTx_USE_DMA 144 | //according to the hardware 145 | //such as using uart4, DMA1_Stream4 for tx, DMA1_Stream2 for rx 146 | void UART4_IRQHandler(void) 147 | { 148 | u16 DATA_LEN = 0; 149 | if(USART_GetITStatus(UART4, USART_IT_TC) != RESET){ 150 | USART_ITConfig(UART4, USART_IT_TC, DISABLE); 151 | } 152 | else if(USART_GetITStatus(UART4, USART_IT_IDLE) != RESET){ 153 | UART4->SR; 154 | UART4->DR; 155 | 156 | DMA_Cmd(DMA1_Stream2, DISABLE); 157 | DMA_ClearFlag(DMA1_Stream2, DMA_FLAG_TCIF2); 158 | DATA_LEN = UBLOX_DEFAULT_RX_BUFFERSIZE - DMA_GetCurrDataCounter(DMA1_Stream2); 159 | ////////////////////////////////////////////////////////////////////////// 160 | //put DMA data to FIFO 161 | Fifo_Put(pUbloxFifo, DMA_RxBuffer, DATA_LEN); 162 | ////////////////////////////////////////////////////////////////////////// 163 | DMA_SetCurrDataCounter(DMA1_Stream2, DEFAULT_BUFFERSIZE); 164 | DMA_Cmd(DMA1_Stream2, ENABLE); 165 | } 166 | } 167 | 168 | //TX 169 | void DMA1_Stream4_IRQHandler(void) 170 | { 171 | if(DMA_GetITStatus(DMA1_Stream4, DMA_IT_TCIF4)){ 172 | DMA_ClearITPendingBit(DMA1_Stream4, DMA_IT_TCIF4); 173 | DMA_Cmd(DMA1_Stream4, DISABLE); 174 | USART_ITConfig(UART4, USART_IT_TC, ENABLE); 175 | } 176 | } 177 | 178 | #endif 179 | 180 | -------------------------------------------------------------------------------- /Calibration App/ahrs v1.003.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/suhetao/stm32f4_mpu9250/b86160c2b781f16973cd20293c67c5fab25f54f4/Calibration App/ahrs v1.003.zip -------------------------------------------------------------------------------- /Common/inc/Memory.h: -------------------------------------------------------------------------------- 1 | /* 2 | The MIT License (MIT) 3 | 4 | Copyright (c) 2015-? suhetao 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | this software and associated documentation files (the "Software"), to deal in 8 | the Software without restriction, including without limitation the rights to 9 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | the Software, and to permit persons to whom the Software is furnished to do so, 11 | subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | #ifndef _MEMORY_H_ 25 | #define _MEMORY_H_ 26 | 27 | #include "stm32f4xx.h" 28 | 29 | void FastMemCpy(u8* dest, u8* src, u16 size); 30 | void *MemCpy(u8* dest, u8* src, u16 size); 31 | void *MemMove(u8* dest, u8* src, u16 size); 32 | s32 MemCmp(u8 *dest, u8 *src, u16 n); 33 | 34 | #endif 35 | -------------------------------------------------------------------------------- /Data/inc/Fifo.h: -------------------------------------------------------------------------------- 1 | /* 2 | The MIT License (MIT) 3 | 4 | Copyright (c) 2015-? suhetao 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | this software and associated documentation files (the "Software"), to deal in 8 | the Software without restriction, including without limitation the rights to 9 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | the Software, and to permit persons to whom the Software is furnished to do so, 11 | subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | #ifndef _FIFO_H 25 | #define _FIFO_H 26 | 27 | #include "stm32f4xx.h" 28 | 29 | #define MIN(a,b) ((a) < (b) ? (a) : (b)) 30 | 31 | typedef struct FIFO_T 32 | { 33 | u8 *Data; 34 | u16 Size; 35 | u16 In, Out; 36 | }Fifo; 37 | 38 | void Fifo_Init(Fifo* fifo, u8 *buff, u16 len); 39 | u16 Fifo_Get(Fifo* fifo, u8 *buff, u16 len); 40 | void Fifo_Put(Fifo *fifo, u8 *buff, u16 len); 41 | 42 | #endif 43 | -------------------------------------------------------------------------------- /Data/inc/Queue.h: -------------------------------------------------------------------------------- 1 | /* 2 | The MIT License (MIT) 3 | 4 | Copyright (c) 2015-? suhetao 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | this software and associated documentation files (the "Software"), to deal in 8 | the Software without restriction, including without limitation the rights to 9 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | the Software, and to permit persons to whom the Software is furnished to do so, 11 | subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | #ifndef _QUEUE_H 25 | #define _QUEUE_H 26 | 27 | #include "stm32f4xx.h" 28 | 29 | #define MAX_MESSAGE_LEN (10) 30 | #define MAX_QUEUE_LEN (32) 31 | #define MAX_QUEUE_MASK (31) 32 | 33 | typedef struct Buff_T 34 | { 35 | s8 Buff[MAX_MESSAGE_LEN]; 36 | u16 Len; 37 | }Buff; 38 | 39 | typedef struct QUEUE_T 40 | { 41 | Buff Buffs[MAX_QUEUE_LEN]; 42 | u16 Head; 43 | u16 Tail; 44 | u16 Size; 45 | }Queue, *PQueue; 46 | 47 | __inline s32 Queue_IsFull(PQueue queue) 48 | { 49 | return (queue->Size == MAX_QUEUE_LEN); 50 | } 51 | 52 | __inline u16 Queue_Size(PQueue queue) 53 | { 54 | return queue->Size; 55 | } 56 | 57 | __inline s32 Queue_IsEmpty(PQueue queue) 58 | { 59 | return (queue->Size == 0); 60 | } 61 | 62 | s32 Queue_Enqueue(PQueue queue, s8* string, u16 len); 63 | s32 Queue_Dequeue(PQueue queue, Buff* buff); 64 | 65 | #endif 66 | -------------------------------------------------------------------------------- /Data/src/Fifo.c: -------------------------------------------------------------------------------- 1 | /* 2 | The MIT License (MIT) 3 | 4 | Copyright (c) 2015-? suhetao 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | this software and associated documentation files (the "Software"), to deal in 8 | the Software without restriction, including without limitation the rights to 9 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | the Software, and to permit persons to whom the Software is furnished to do so, 11 | subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | #include "Fifo.h" 25 | #include "Memory.h" 26 | 27 | void Fifo_Init(Fifo* fifo, u8 *buff, u16 len) 28 | { 29 | fifo->Data = buff; 30 | fifo->Size = len; 31 | fifo->In = fifo->Out = 0; 32 | } 33 | 34 | u16 Fifo_Get(Fifo* fifo, u8 *buff, u16 len) 35 | { 36 | u16 l; 37 | len = MIN(len, fifo->In - fifo->Out); 38 | l = MIN(len, fifo->Size - (fifo->Out & (fifo->Size - 1))); 39 | MemCpy(buff, fifo->Data + (fifo->Out & (fifo->Size - 1)), l); 40 | MemCpy(buff + l, fifo->Data, len - l); 41 | fifo->Out += len; 42 | return len; 43 | } 44 | 45 | void Fifo_Put(Fifo *fifo, u8 *buff, u16 len) 46 | { 47 | u16 fixLen; 48 | ////////////////////////////////////////////////////////////////////////// 49 | len = MIN(len, fifo->Size - fifo->In + fifo->Out); 50 | fixLen = MIN(len, fifo->Size - (fifo->In & (fifo->Size - 1))); 51 | MemCpy(fifo->Data + (fifo->In & (fifo->Size - 1)), buff, fixLen); 52 | MemCpy(fifo->Data, buff + fixLen, len - fixLen); 53 | fifo->In += len; 54 | } 55 | -------------------------------------------------------------------------------- /Data/src/Queue.c: -------------------------------------------------------------------------------- 1 | /* 2 | The MIT License (MIT) 3 | 4 | Copyright (c) 2015-? suhetao 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | this software and associated documentation files (the "Software"), to deal in 8 | the Software without restriction, including without limitation the rights to 9 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | the Software, and to permit persons to whom the Software is furnished to do so, 11 | subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | #include "Queue.h" 25 | #include "Memory.h" 26 | 27 | s32 Queue_Enqueue(PQueue queue, s8* string, u16 len) 28 | { 29 | if(Queue_IsFull(queue)){ 30 | return -1; 31 | } 32 | queue->Size++; 33 | ////////////////////////////////////////////////////////////////////////// 34 | queue->Buffs[queue->Tail].Len = len; 35 | MemCpy((u8*)queue->Buffs[queue->Tail].Buff, (u8*)string, len); 36 | ////////////////////////////////////////////////////////////////////////// 37 | queue->Tail++; 38 | queue->Tail &= MAX_QUEUE_MASK; 39 | return 0; 40 | } 41 | 42 | s32 Queue_Dequeue(PQueue queue, Buff* buff) 43 | { 44 | if(Queue_IsEmpty(queue)){ 45 | return -1; 46 | } 47 | queue->Size--; 48 | ////////////////////////////////////////////////////////////////////////// 49 | *buff = queue->Buffs[queue->Head]; 50 | ////////////////////////////////////////////////////////////////////////// 51 | queue->Head++; 52 | queue->Head &= MAX_QUEUE_MASK; 53 | return 0; 54 | } 55 | -------------------------------------------------------------------------------- /Drivers/inc/stm32f4_exti.h: -------------------------------------------------------------------------------- 1 | /* 2 | The MIT License (MIT) 3 | 4 | Copyright (c) 2015-? suhetao 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | this software and associated documentation files (the "Software"), to deal in 8 | the Software without restriction, including without limitation the rights to 9 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | the Software, and to permit persons to whom the Software is furnished to do so, 11 | subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | #ifndef _STM32F4_EXTI_H 25 | #define _STM32F4_EXTI_H 26 | 27 | #include "stm32f4xx.h" 28 | #include "stm32f4_rcc.h" 29 | 30 | typedef struct EXTI_DRIVER_T 31 | { 32 | GPIO_TypeDef* Gpio; 33 | RCC_AXXPeriphClockCmd GPIO_CLK; 34 | uint32_t GPIO_Func; 35 | uint16_t GPIO_Pin; 36 | uint8_t EXTI_PortSourceGPIO; 37 | uint8_t EXTI_PinSource; 38 | 39 | EXTI_InitTypeDef EXIT_Init; 40 | NVIC_InitTypeDef NVIC_Init; 41 | }EXTI_Driver; 42 | 43 | void EXTIx_Init(EXTI_Driver* EXTIx); 44 | 45 | #endif 46 | -------------------------------------------------------------------------------- /Drivers/inc/stm32f4_gpio.h: -------------------------------------------------------------------------------- 1 | /* 2 | The MIT License (MIT) 3 | 4 | Copyright (c) 2015-? suhetao 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | this software and associated documentation files (the "Software"), to deal in 8 | the Software without restriction, including without limitation the rights to 9 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | the Software, and to permit persons to whom the Software is furnished to do so, 11 | subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | #ifndef __STM32F4_GPIO_H 25 | #define __STM32F4_GPIO_H 26 | 27 | // Includes 28 | #include "stm32f4xx.h" 29 | #include "stm32f4_rcc.h" 30 | 31 | 32 | typedef struct GPIO_DRIVER_T 33 | { 34 | GPIO_TypeDef* Gpio; 35 | RCC_AXXPeriphClockCmd GPIO_CLK; 36 | uint32_t GPIO_Func; 37 | GPIO_InitTypeDef GPIO_Init; 38 | }GPIO_Driver; 39 | 40 | void GPIOx_Init(GPIO_Driver* GPIOx); 41 | 42 | __inline void GPIOx_SetLow(GPIO_Driver* GPIOx) 43 | { 44 | GPIO_ResetBits(GPIOx->Gpio, GPIOx->GPIO_Init.GPIO_Pin); 45 | } 46 | 47 | __inline void GPIOx_SetHigh(GPIO_Driver* GPIOx){ 48 | GPIO_SetBits(GPIOx->Gpio, GPIOx->GPIO_Init.GPIO_Pin); 49 | } 50 | 51 | #endif 52 | -------------------------------------------------------------------------------- /Drivers/inc/stm32f4_spi.h: -------------------------------------------------------------------------------- 1 | /* 2 | The MIT License (MIT) 3 | 4 | Copyright (c) 2015-? suhetao 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | this software and associated documentation files (the "Software"), to deal in 8 | the Software without restriction, including without limitation the rights to 9 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | the Software, and to permit persons to whom the Software is furnished to do so, 11 | subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | #ifndef __STM32F4_SPI_H 25 | #define __STM32F4_SPI_H 26 | 27 | // Includes 28 | #include "stm32f4xx.h" 29 | #include "stm32f4_rcc.h" 30 | 31 | //#define SPIx_USE_DMA 32 | #define SPIx_BR_CLEAR_MASK ((uint16_t)(0xFFC7)) 33 | 34 | typedef struct SPI_DRIVER_T 35 | { 36 | SPI_TypeDef* SPI; 37 | RCC_AXXPeriphClockCmd SPI_CLK; 38 | uint32_t SPI_Func; 39 | 40 | GPIO_TypeDef* Gpio; 41 | RCC_AXXPeriphClockCmd GPIO_CLK; 42 | uint32_t GPIO_Func; 43 | 44 | GPIO_TypeDef* Gpio_CS; 45 | RCC_AXXPeriphClockCmd GPIO_CS_CLK; 46 | uint32_t CS_Func; 47 | uint16_t CS_Pin; 48 | 49 | uint16_t SCK_Pin; 50 | uint16_t MISO_Pin; 51 | uint16_t MOSI_Pin; 52 | uint16_t SCK_Src; 53 | uint16_t MISO_Src; 54 | uint16_t MOSI_Src; 55 | 56 | SPI_InitTypeDef SPI_Init; 57 | 58 | #ifdef SPIx_USE_DMA 59 | RCC_AXXPeriphClockCmd DMA_CLK; 60 | uint32_t DMA_Func; 61 | DMA_TypeDef* DMA_TX; 62 | DMA_Stream_TypeDef* DMA_TX_Stream; 63 | NVIC_InitTypeDef NVIC_DMA_TX; 64 | uint32_t DMA_TX_CH; 65 | uint32_t DMA_TX_Flag; 66 | DMA_TypeDef* DMA_RX; 67 | DMA_Stream_TypeDef* DMA_RX_Stream; 68 | NVIC_InitTypeDef NVIC_DMA_RX; 69 | uint32_t DMA_RX_CH; 70 | uint32_t DMA_RX_Flag; 71 | #endif 72 | uint8_t GPIO_AF_SPI; 73 | 74 | }SPI_Driver; 75 | 76 | __inline void Chip_Select(SPI_Driver* SPIx) 77 | { 78 | GPIO_ResetBits((SPIx)->Gpio_CS, (SPIx)->CS_Pin); 79 | } 80 | 81 | __inline void Chip_DeSelect(SPI_Driver* SPIx){ 82 | GPIO_SetBits((SPIx)->Gpio_CS, (SPIx)->CS_Pin); 83 | } 84 | 85 | void SPIx_Init(SPI_Driver* SPIx); 86 | void SPIx_DeInit(SPI_Driver* SPIx); 87 | uint8_t SPIx_Read_Reg(SPI_Driver* SPIx, uint8_t reg); 88 | void SPIx_Write_Reg(SPI_Driver* SPIx, uint8_t regAddr, uint8_t data); 89 | void SPIx_Read_Regs(SPI_Driver* SPIx, uint8_t regAddr, uint8_t length, uint8_t* buffer); 90 | #ifdef SPIx_USE_DMA 91 | void SPIx_DMA_Read_Regs(SPI_Driver* SPIx, uint8_t regAddr, uint8_t length, uint8_t* buffer); 92 | #endif 93 | uint8_t SPIx_SendByte(SPI_Driver* SPIx, uint8_t byte); 94 | uint16_t SPIx_SendWord(SPI_Driver* SPIx, uint16_t word); 95 | void SPIx_ReadBytes(SPI_Driver* SPIx, uint8_t length, uint8_t* buffer); 96 | void SPIx_SetDivisor(SPI_Driver* SPIx, uint16_t Prescaler); 97 | 98 | #endif 99 | -------------------------------------------------------------------------------- /Drivers/inc/stm32f4_usart.h: -------------------------------------------------------------------------------- 1 | /* 2 | The MIT License (MIT) 3 | 4 | Copyright (c) 2015-? suhetao 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | this software and associated documentation files (the "Software"), to deal in 8 | the Software without restriction, including without limitation the rights to 9 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | the Software, and to permit persons to whom the Software is furnished to do so, 11 | subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | #ifndef __STM32F4_USART_H 25 | #define __STM32F4_USART_H 26 | 27 | // include 28 | #include "stm32f4xx.h" 29 | #include "stm32f4_rcc.h" 30 | 31 | #define DEFAULT_BAUDRATE 115200 32 | #define USARTx_USE_DMA 33 | 34 | #ifdef USARTx_USE_DMA 35 | #define DEFAULT_BUFFERSIZE 256 36 | #endif 37 | 38 | typedef struct USART_DRIVER_T 39 | { 40 | USART_TypeDef* USART; 41 | RCC_AXXPeriphClockCmd USART_CLK; 42 | uint32_t USART_Func; 43 | uint32_t USART_BaudRate; 44 | 45 | GPIO_TypeDef* TX_GPIO; 46 | RCC_AXXPeriphClockCmd TX_GPIOClk; 47 | uint32_t TX_GPIOFunc; 48 | uint16_t TX_Pin; 49 | uint16_t TX_Src; 50 | 51 | GPIO_TypeDef* RX_GPIO; 52 | RCC_AXXPeriphClockCmd RX_GPIOClk; 53 | uint32_t RX_GPIOFunc; 54 | uint16_t RX_Pin; 55 | uint16_t RX_Src; 56 | 57 | #ifdef USARTx_USE_DMA 58 | NVIC_InitTypeDef NVIC_USART; 59 | NVIC_InitTypeDef NVIC_DMA_TX; 60 | 61 | RCC_AXXPeriphClockCmd DMA_CLK; 62 | uint32_t DMA_Func; 63 | 64 | uint32_t DMA_TX_Size; 65 | uint8_t* DMA_TX_Buffer; 66 | DMA_Stream_TypeDef* DMA_TX_Stream; 67 | uint32_t DMA_TX_CH; 68 | 69 | uint32_t DMA_RX_Size; 70 | uint8_t* DMA_RX_Buffer; 71 | DMA_Stream_TypeDef* DMA_RX_Stream; 72 | uint32_t DMA_RX_CH; 73 | #endif 74 | uint8_t GPIO_AF_USART; 75 | 76 | }USART_Driver; 77 | 78 | void USARTx_Init(USART_Driver* USARTx); 79 | void USARTx_DeInit(USART_Driver* USARTx); 80 | 81 | void USARTx_SendByte(USART_Driver* USARTx, uint8_t byte); 82 | void USARTx_SendBytes(USART_Driver* USARTx, uint8_t* buffer, uint8_t length); 83 | #ifdef USARTx_USE_DMA 84 | void USARTx_DMA_SendBytes(USART_Driver* USARTx, uint8_t* buffer, uint8_t length); 85 | #endif 86 | 87 | #endif 88 | -------------------------------------------------------------------------------- /Drivers/src/stm32f4_exti.c: -------------------------------------------------------------------------------- 1 | /* 2 | The MIT License (MIT) 3 | 4 | Copyright (c) 2015-? suhetao 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | this software and associated documentation files (the "Software"), to deal in 8 | the Software without restriction, including without limitation the rights to 9 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | the Software, and to permit persons to whom the Software is furnished to do so, 11 | subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | #include "stm32f4_exti.h" 25 | 26 | void EXTIx_Init(EXTI_Driver* EXTIx) 27 | { 28 | GPIO_InitTypeDef GPIO_InitStructure; 29 | 30 | //Enable GPIO clocks 31 | EXTIx->GPIO_CLK(EXTIx->GPIO_Func, ENABLE); 32 | //Enable SYSCFG clock 33 | RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE); 34 | 35 | //Configure GPIO pin as input floating 36 | GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN; 37 | GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; 38 | GPIO_InitStructure.GPIO_Pin = EXTIx->GPIO_Pin; 39 | GPIO_Init(EXTIx->Gpio, &GPIO_InitStructure); 40 | 41 | //Connect EXTI Line to GPIO Pin 42 | SYSCFG_EXTILineConfig(EXTIx->EXTI_PortSourceGPIO, EXTIx->EXTI_PinSource); 43 | 44 | //Configure EXTI line 45 | EXTI_Init(&EXTIx->EXIT_Init); 46 | 47 | //Enable and set EXTI Interrupt priority 48 | NVIC_Init(&EXTIx->NVIC_Init); 49 | } 50 | -------------------------------------------------------------------------------- /Drivers/src/stm32f4_gpio.c: -------------------------------------------------------------------------------- 1 | /* 2 | The MIT License (MIT) 3 | 4 | Copyright (c) 2015-? suhetao 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | this software and associated documentation files (the "Software"), to deal in 8 | the Software without restriction, including without limitation the rights to 9 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | the Software, and to permit persons to whom the Software is furnished to do so, 11 | subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | // Includes 25 | #include "stm32f4_gpio.h" 26 | 27 | void GPIOx_Init(GPIO_Driver* GPIOx) 28 | { 29 | GPIOx->GPIO_CLK(GPIOx->GPIO_Func, ENABLE); 30 | GPIO_Init(GPIOx->Gpio, &GPIOx->GPIO_Init); 31 | } 32 | -------------------------------------------------------------------------------- /Gps/inc/Map.h: -------------------------------------------------------------------------------- 1 | /* 2 | The MIT License (MIT) 3 | 4 | Copyright (c) 2015-? suhetao 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | this software and associated documentation files (the "Software"), to deal in 8 | the Software without restriction, including without limitation the rights to 9 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | the Software, and to permit persons to whom the Software is furnished to do so, 11 | subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | #ifndef _MAP_H 25 | #define _MAP_H 26 | 27 | #include "stm32f4xx.h" 28 | 29 | #include "Double.h" 30 | 31 | typedef struct DOUBLEPOINT_T 32 | { 33 | Double lat; 34 | Double lon; 35 | }DoublePoint; 36 | 37 | typedef struct MAP_T 38 | { 39 | //reference point 40 | DoublePoint _dPoint; 41 | DoublePoint dPoint; 42 | 43 | DoublePoint _gsPoint; 44 | DoublePoint gsPoint; 45 | // 46 | //reference position 47 | Double X, Y; 48 | // 49 | //coefficient 50 | Double a, b, c, bl; 51 | // 52 | 53 | } Map; 54 | 55 | ////////////////////////////////////////////////////////////////////////// 56 | //must init with local two reference points including lat, lon and the length 57 | //betweeb two reference points 58 | void Map_Init(Map* pMap, double x1, double y1, double x2, double y2, 59 | double x, double y, double length); 60 | void Map_GetXY(Map* pMap, double lat, double lon, double *x, double *y); 61 | 62 | #endif 63 | -------------------------------------------------------------------------------- /Gps/inc/Nema.h: -------------------------------------------------------------------------------- 1 | /* 2 | The MIT License (MIT) 3 | 4 | Copyright (c) 2015-? suhetao 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | this software and associated documentation files (the "Software"), to deal in 8 | the Software without restriction, including without limitation the rights to 9 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | the Software, and to permit persons to whom the Software is furnished to do so, 11 | subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | #ifndef _STM32F4_NEMA_H 25 | #define _STM32F4_NEMA_H 26 | 27 | #include "stm32f4xx.h" 28 | 29 | #include "Double.h" 30 | 31 | #define NEMA_MESSAGEID_SIZE (5) 32 | #define NMEA_MAXSVS (12) 33 | 34 | typedef enum{ 35 | GPNON = 0x0000, 36 | GPGGA = 0x0001, 37 | GPGSA = 0x0002, 38 | GPRMC = 0x0004, 39 | GPVTG = 0x0008 40 | } NEMA_MessageType; 41 | 42 | //GGA Global positioning system fix data 43 | typedef struct _nmeaGPGGA 44 | { 45 | double time; 46 | double lat; 47 | char NS; 48 | double lon; 49 | char EW; 50 | int quality; 51 | int numSV; 52 | double HDOP; 53 | double alt; 54 | char uAlt; 55 | double sep; 56 | char uSep; 57 | double diffAge; 58 | int diffStation; 59 | }nmeaGPGGA, *PnmeaGPGGA; 60 | 61 | //GSA GNSS DOP and Active Satellites 62 | typedef struct _nmeaGPGSA 63 | { 64 | char opMode; 65 | int navMode; 66 | int sv[NMEA_MAXSVS];//Satellite number 67 | double PDOP; 68 | double HDOP; 69 | double VDOP; 70 | int systemId; 71 | } nmeaGPGSA, *PnmeaGPGSA; 72 | 73 | //RMC Recommended Minimum data 74 | typedef struct _nmeaGPRMC 75 | { 76 | double time; 77 | char status; 78 | double lat; 79 | char NS; 80 | double lon; 81 | char EW; 82 | double spd; 83 | double cog; 84 | int date; 85 | double mv; 86 | char mvEW; 87 | char posMode; 88 | char navStatus; 89 | } nmeaGPRMC, *PnmeaGPRMC; 90 | 91 | //VTG Course over ground and Ground speed 92 | typedef struct _nmeaGPVTG 93 | { 94 | double cogt; 95 | char T; 96 | double cogm; 97 | char M; 98 | double knots; 99 | char N; 100 | double kph; 101 | char K; 102 | char posMode; 103 | } nmeaGPVTG, *PnmeaGPVTG; 104 | 105 | typedef struct _nmeaINFO 106 | { 107 | double lat; 108 | double lon; 109 | double alt; 110 | double spd; 111 | double cog; 112 | } nmeaINFO; 113 | 114 | typedef uint64_t u64; 115 | 116 | ////////////////////////////////////////////////////////////////////////// 117 | //input -2^23 < x < 2^23 118 | __inline float NEMA_Fast_UintToFloat(u32 x) 119 | { 120 | union { float f; u32 i; } u = { 8388608.0f }; 121 | u.i |= x; 122 | return u.f - 8388608.0f; 123 | } 124 | // 125 | __inline float NEMA_Fast_FloatInverse(float x) 126 | { 127 | union { float f; u32 i; } u; 128 | u.f = x; 129 | u.i = 0x7F000000 - u.i; 130 | return u.f; 131 | } 132 | ////////////////////////////////////////////////////////////////////////// 133 | //input -2^52 < x < 2^52 134 | __inline double NEMA_Fast_Uint64ToDouble(u64 x) 135 | { 136 | union { double d; u64 i; } u = {4503599627370496.0 }; 137 | u.i |= x; 138 | return u.d - 4503599627370496.0; 139 | } 140 | 141 | __inline double NEMA_Fast_DoubleInverse(double x) 142 | { 143 | union { double d; u64 i; } u; 144 | u.d = x; 145 | u.i = (u64)0x7FE0000000000000 - u.i; 146 | return u.d; 147 | } 148 | 149 | __inline u8 NEMA_FastCRCtoI(s8 *p, u8 *table){ 150 | return table[*p] << 4 | table[*(p + 1)]; 151 | } 152 | 153 | float NEMA_FastAtoF(s8 *p, s32 len); 154 | double NEMA_FastAtoD(s8 *p, s32 len); 155 | s32 NEMA_FastAtoI(s8 *p, s32 len); 156 | u16 NEMA_Parser(s8 *p, u16 len); 157 | s16 NEMA_GetMessage(void *data, s32* iType); 158 | 159 | // 160 | double NMEA_Convert2Degrees(double val); 161 | double NMEA_Degree2Radian(double val); 162 | double NMEA_Radian2Degree(double val); 163 | 164 | Double NMEA_Degree2RadianD(Double val); 165 | 166 | #endif 167 | -------------------------------------------------------------------------------- /Gps/src/Map.c: -------------------------------------------------------------------------------- 1 | /* 2 | The MIT License (MIT) 3 | 4 | Copyright (c) 2015-? suhetao 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | this software and associated documentation files (the "Software"), to deal in 8 | the Software without restriction, including without limitation the rights to 9 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | the Software, and to permit persons to whom the Software is furnished to do so, 11 | subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | #include "Map.h" 25 | #include "Nema.h" 26 | 27 | #include "FastMath.h" 28 | 29 | DoublePoint Map_BLToGauss(Double latitude, Double longitude) 30 | { 31 | DoublePoint point; 32 | 33 | int ProjNo = 0, ZoneWide = 6; 34 | __int64 X0, Y0; 35 | 36 | Double lon, lat, _lon; 37 | Double e2, ee, NN, T, C, A, M; 38 | // 39 | float sin, cos; 40 | float tan, _2sin, _4sin, _6sin; 41 | Double dsin, dcos; 42 | Double dtan, _d2sin, _d4sin, _d6sin; 43 | //beijing 54 44 | Double a = doubleToDouble(6378245.0); 45 | Double f = doubleToDouble(0.00335232986925913509889373114314); 46 | 47 | //xi'an 80 48 | //Double a = doubleToDouble(6378140.0); 49 | //Double f = doubleToDouble(0.00335281317789691440603238146967); 50 | 51 | ProjNo = (int)DoubleTodouble(DoubleDiv(lon, intToDouble(ZoneWide))); 52 | _lon = intToDouble(ProjNo * ZoneWide + ZoneWide / 2); 53 | _lon = NMEA_Degree2RadianD(_lon); 54 | lon = NMEA_Degree2RadianD(longitude); 55 | lat = NMEA_Degree2RadianD(latitude); 56 | 57 | tan = (float)DoubleTodouble(lat); 58 | dtan = floatToDouble(FastTan(tan)); 59 | FastSinCos((float)DoubleTodouble(lat), &sin, &cos); 60 | dsin = floatToDouble(sin); 61 | dcos = floatToDouble(cos); 62 | 63 | _2sin = (float)DoubleTodouble(DoubleMul(intToDouble(2), lat)); 64 | _4sin = (float)DoubleTodouble(DoubleMul(intToDouble(4), lat)); 65 | _6sin = (float)DoubleTodouble(DoubleMul(intToDouble(6), lat)); 66 | _d2sin = floatToDouble(FastSin(_2sin)); 67 | _d4sin = floatToDouble(FastSin(_4sin)); 68 | _d6sin = floatToDouble(FastSin(_6sin)); 69 | 70 | e2 = DoubleSub(DoubleMul(intToDouble(2),f), DoubleMul(f,f)); 71 | ee = DoubleMul(e2, DoubleSub(intToDouble(1), e2)); 72 | NN = DoubleMul(a, FastSqrtID(DoubleSub(doubleToDouble(1.0), DoubleMul(e2, DoubleMul(dsin, dsin))))); 73 | T = DoubleMul(dtan, dtan); 74 | C = DoubleMul(ee, DoubleMul(dcos, dcos)); 75 | A = DoubleMul(DoubleSub(lon, _lon), dcos); 76 | 77 | //todo not finish yet! 78 | /* 79 | M = a * ((1 - e2 / 4 - 3 * e2 * e2 / 64 - 5 * e2 * e2 * e2 / 256) * lat 80 | - (3 * e2 / 8 + 3 * e2 * e2 / 32 + 45 * e2 * e2 * e2 / 1024) * _d2sin 81 | + (15 * e2 * e2 / 256 + 45 * e2 * e2 * e2 / 1024) * _d4sin 82 | - (35 * e2 * e2 * e2 / 3072) * _d6sin); 83 | point.lon = NN * (A + (1 - T + C) * A * A * A / 6 + (5 - 18 * T + T * T + 72 * C - 58 * ee) * A * A * A * A * A / 120); 84 | point.lat = M + NN * tan * (A * A / 2 + (5 - T + 9 * C + 4 * C * C) * A * A * A * A / 24 + (61 - 58 * T + T * T + 600 * C - 330 * ee) * A * A * A * A * A * A / 720); 85 | */ 86 | X0 = 1000000L * (ProjNo + 1) + 500000L; 87 | Y0 = 0; 88 | point.lon = DoubleAdd(point.lon, doubleToDouble(NEMA_Fast_Uint64ToDouble(X0))); 89 | point.lat = DoubleAdd(point.lat, doubleToDouble(NEMA_Fast_Uint64ToDouble(Y0))); 90 | 91 | return point; 92 | } 93 | 94 | ////////////////////////////////////////////////////////////////////////// 95 | //must init with local two reference points including lat, lon and the length 96 | //betweeb two reference points 97 | 98 | void Map_Init(Map* pMap, double _lat, double _lon, double lat, double lon, 99 | double x, double y, double length) 100 | { 101 | Double dic; 102 | 103 | pMap->X = doubleToDouble(x); 104 | pMap->Y = doubleToDouble(y); 105 | 106 | pMap->_dPoint.lat = doubleToDouble(_lat); pMap->_dPoint.lon = doubleToDouble(_lon); 107 | pMap->dPoint.lat = doubleToDouble(lat); pMap->_dPoint.lon = doubleToDouble(lon); 108 | 109 | pMap->_gsPoint = Map_BLToGauss(pMap->_dPoint.lat, pMap->_dPoint.lon); 110 | pMap->gsPoint = Map_BLToGauss(pMap->dPoint.lat, pMap->dPoint.lon); 111 | 112 | pMap->a = DoubleSub(pMap->_gsPoint.lat, pMap->gsPoint.lat); 113 | pMap->b = DoubleSub(pMap->_gsPoint.lon, pMap->gsPoint.lon); 114 | 115 | dic = FastSqrtID(DoubleAdd(DoubleMul(pMap->a, pMap->a), DoubleMul(pMap->b, pMap->b))); 116 | pMap->c = DoubleDiv(intToDouble(1), dic); 117 | pMap->bl = DoubleMul(doubleToDouble(length), dic); 118 | } 119 | 120 | void Map_GetXY(Map* pMap, double lat, double lon, double *x, double *y) 121 | { 122 | Double d, e, f, gf, g, h, i; 123 | DoublePoint gs = Map_BLToGauss(doubleToDouble(lat), doubleToDouble(lon)); 124 | 125 | d = DoubleSub(gs.lon, pMap->gsPoint.lon); 126 | d = doubleToDouble(FastAbsD(DoubleTodouble(d))); 127 | gf = DoubleSub(gs.lat, pMap->gsPoint.lat); 128 | gf = doubleToDouble(FastAbsD(DoubleTodouble(gf))); 129 | // 130 | if (lon > DoubleTodouble(pMap->_dPoint.lon)){ 131 | e = DoubleDiv(DoubleMul(d, pMap->c), pMap->b); //e = d * c / b 132 | f = DoubleDiv(DoubleMul(pMap->a, e), pMap->c); //f = a * e / c 133 | g = DoubleSub(gf, f); 134 | h = DoubleDiv(DoubleMul(pMap->a, g), pMap->c); //h = a * g / c 135 | i = DoubleDiv(DoubleMul(pMap->a, g), pMap->c); //i = b * g / c 136 | 137 | //lon mean X, lat mean Y 138 | //x = X - i * bl; 139 | //y = Y + (h + e) * bl; 140 | *x = DoubleTodouble(DoubleSub(pMap->X, DoubleMul(i, pMap->bl))); 141 | *y = DoubleTodouble(DoubleAdd(pMap->Y, DoubleMul(DoubleAdd(h, e), pMap->bl))); 142 | } 143 | else{ 144 | e = DoubleDiv(DoubleMul(d, pMap->c), pMap->a); //e = d * c / a; 145 | f = DoubleDiv(DoubleMul(e, pMap->b), pMap->c); //f = e * b / c; 146 | 147 | g = DoubleSub(gf, f); 148 | h = DoubleDiv(DoubleMul(pMap->a, g), pMap->c); //h = a * g / c; 149 | i = DoubleDiv(DoubleMul(pMap->a, g), pMap->c); //i = b * g / c; 150 | 151 | //lon mean X, lat mean Y; 152 | //x = X - (e + i) * bl; 153 | //y = Y + h * bl; 154 | *x = DoubleTodouble(DoubleSub(pMap->X, DoubleMul(DoubleAdd(e, i), pMap->bl))); 155 | *y = DoubleTodouble(DoubleAdd(pMap->Y, DoubleMul(h, pMap->bl))); 156 | } 157 | } 158 | -------------------------------------------------------------------------------- /Hardware/IMU_PCB.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/suhetao/stm32f4_mpu9250/b86160c2b781f16973cd20293c67c5fab25f54f4/Hardware/IMU_PCB.pdf -------------------------------------------------------------------------------- /Hardware/imu_gerber.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/suhetao/stm32f4_mpu9250/b86160c2b781f16973cd20293c67c5fab25f54f4/Hardware/imu_gerber.zip -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | The MIT License (MIT) 2 | 3 | Copyright (c) 2015 suhetao 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | 23 | -------------------------------------------------------------------------------- /Libraries/CMSIS/CMSIS END USER LICENCE AGREEMENT.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/suhetao/stm32f4_mpu9250/b86160c2b781f16973cd20293c67c5fab25f54f4/Libraries/CMSIS/CMSIS END USER LICENCE AGREEMENT.pdf -------------------------------------------------------------------------------- /Libraries/CMSIS/Device/ST/STM32F4xx/Include/stm32f4xx.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/suhetao/stm32f4_mpu9250/b86160c2b781f16973cd20293c67c5fab25f54f4/Libraries/CMSIS/Device/ST/STM32F4xx/Include/stm32f4xx.h -------------------------------------------------------------------------------- /Libraries/CMSIS/Device/ST/STM32F4xx/Include/system_stm32f4xx.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file system_stm32f4xx.h 4 | * @author MCD Application Team 5 | * @version V1.3.0 6 | * @date 08-November-2013 7 | * @brief CMSIS Cortex-M4 Device System Source File for STM32F4xx devices. 8 | ****************************************************************************** 9 | * @attention 10 | * 11 | *

© COPYRIGHT 2013 STMicroelectronics

12 | * 13 | * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); 14 | * You may not use this file except in compliance with the License. 15 | * You may obtain a copy of the License at: 16 | * 17 | * http://www.st.com/software_license_agreement_liberty_v2 18 | * 19 | * Unless required by applicable law or agreed to in writing, software 20 | * distributed under the License is distributed on an "AS IS" BASIS, 21 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 22 | * See the License for the specific language governing permissions and 23 | * limitations under the License. 24 | * 25 | ****************************************************************************** 26 | */ 27 | 28 | /** @addtogroup CMSIS 29 | * @{ 30 | */ 31 | 32 | /** @addtogroup stm32f4xx_system 33 | * @{ 34 | */ 35 | 36 | /** 37 | * @brief Define to prevent recursive inclusion 38 | */ 39 | #ifndef __SYSTEM_STM32F4XX_H 40 | #define __SYSTEM_STM32F4XX_H 41 | 42 | #ifdef __cplusplus 43 | extern "C" { 44 | #endif 45 | 46 | /** @addtogroup STM32F4xx_System_Includes 47 | * @{ 48 | */ 49 | 50 | /** 51 | * @} 52 | */ 53 | 54 | 55 | /** @addtogroup STM32F4xx_System_Exported_types 56 | * @{ 57 | */ 58 | 59 | extern uint32_t SystemCoreClock; /*!< System Clock Frequency (Core Clock) */ 60 | 61 | 62 | /** 63 | * @} 64 | */ 65 | 66 | /** @addtogroup STM32F4xx_System_Exported_Constants 67 | * @{ 68 | */ 69 | 70 | /** 71 | * @} 72 | */ 73 | 74 | /** @addtogroup STM32F4xx_System_Exported_Macros 75 | * @{ 76 | */ 77 | 78 | /** 79 | * @} 80 | */ 81 | 82 | /** @addtogroup STM32F4xx_System_Exported_Functions 83 | * @{ 84 | */ 85 | 86 | extern void SystemInit(void); 87 | extern void SystemCoreClockUpdate(void); 88 | /** 89 | * @} 90 | */ 91 | 92 | #ifdef __cplusplus 93 | } 94 | #endif 95 | 96 | #endif /*__SYSTEM_STM32F4XX_H */ 97 | 98 | /** 99 | * @} 100 | */ 101 | 102 | /** 103 | * @} 104 | */ 105 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ 106 | -------------------------------------------------------------------------------- /Libraries/CMSIS/Device/ST/STM32F4xx/Source/Templates/TASKING/cstart_thumb2.asm: -------------------------------------------------------------------------------- 1 | 2 | 3 | ;; NOTE: To allow the use of this file for both ARMv6M and ARMv7M, 4 | ;; we will only use 16-bit Thumb intructions. 5 | 6 | .extern _lc_ub_stack ; usr/sys mode stack pointer 7 | .extern _lc_ue_stack ; symbol required by debugger 8 | .extern _lc_ub_table ; ROM to RAM copy table 9 | .extern main 10 | .extern _Exit 11 | .extern exit 12 | .weak exit 13 | .global __get_argcv 14 | .weak __get_argcv 15 | .extern __argcvbuf 16 | .weak __argcvbuf 17 | ;;.extern __init_hardware 18 | 19 | .extern SystemInit 20 | 21 | .if @defined('__PROF_ENABLE__') 22 | .extern __prof_init 23 | .endif 24 | .if @defined('__POSIX__') 25 | .extern posix_main 26 | .extern _posix_boot_stack_top 27 | .endif 28 | 29 | .global _START 30 | 31 | .section .text.cstart 32 | 33 | .thumb 34 | _START: 35 | ;; anticipate possible ROM/RAM remapping 36 | ;; by loading the 'real' program address 37 | ldr r1,=_Next 38 | bx r1 39 | _Next: 40 | ;; initialize the stack pointer 41 | ldr r1,=_lc_ub_stack ; TODO: make this part of the vector table 42 | mov sp,r1 43 | 44 | ; Call the clock system intitialization function. 45 | bl SystemInit 46 | 47 | ;; copy initialized sections from ROM to RAM 48 | ;; and clear uninitialized data sections in RAM 49 | 50 | ldr r3,=_lc_ub_table 51 | movs r0,#0 52 | cploop: 53 | ldr r4,[r3,#0] ; load type 54 | ldr r5,[r3,#4] ; dst address 55 | ldr r6,[r3,#8] ; src address 56 | ldr r7,[r3,#12] ; size 57 | 58 | cmp r4,#1 59 | beq copy 60 | cmp r4,#2 61 | beq clear 62 | b done 63 | 64 | copy: 65 | subs r7,r7,#1 66 | ldrb r1,[r6,r7] 67 | strb r1,[r5,r7] 68 | bne copy 69 | 70 | adds r3,r3,#16 71 | b cploop 72 | 73 | clear: 74 | subs r7,r7,#1 75 | strb r0,[r5,r7] 76 | bne clear 77 | 78 | adds r3,r3,#16 79 | b cploop 80 | 81 | done: 82 | 83 | 84 | .if @defined('__POSIX__') 85 | 86 | ;; posix stack buffer for system upbringing 87 | ldr r0,=_posix_boot_stack_top 88 | ldr r0, [r0] 89 | mov sp,r0 90 | 91 | .else 92 | 93 | ;; load r10 with end of USR/SYS stack, which is 94 | ;; needed in case stack overflow checking is on 95 | ;; NOTE: use 16-bit instructions only, for ARMv6M 96 | ldr r0,=_lc_ue_stack 97 | mov r10,r0 98 | 99 | .endif 100 | 101 | .if @defined('__PROF_ENABLE__') 102 | bl __prof_init 103 | .endif 104 | 105 | .if @defined('__POSIX__') 106 | ;; call posix_main with no arguments 107 | bl posix_main 108 | .else 109 | ;; retrieve argc and argv (default argv[0]==NULL & argc==0) 110 | bl __get_argcv 111 | ldr r1,=__argcvbuf 112 | ;; call main 113 | bl main 114 | .endif 115 | 116 | ;; call exit using the return value from main() 117 | ;; Note. Calling exit will also run all functions 118 | ;; that were supplied through atexit(). 119 | bl exit 120 | 121 | __get_argcv: ; weak definition 122 | movs r0,#0 123 | bx lr 124 | 125 | .ltorg 126 | .endsec 127 | 128 | .calls '_START', ' ' 129 | .calls '_START','__init_vector_table' 130 | .if @defined('__PROF_ENABLE__') 131 | .calls '_START','__prof_init' 132 | .endif 133 | .if @defined('__POSIX__') 134 | .calls '_START','posix_main' 135 | .else 136 | .calls '_START','__get_argcv' 137 | .calls '_START','main' 138 | .endif 139 | .calls '_START','exit' 140 | .calls '_START','',0 141 | 142 | .end 143 | -------------------------------------------------------------------------------- /Libraries/CMSIS/Include/arm_common_tables.h: -------------------------------------------------------------------------------- 1 | /* ---------------------------------------------------------------------- 2 | * Copyright (C) 2010-2013 ARM Limited. All rights reserved. 3 | * 4 | * $Date: 17. January 2013 5 | * $Revision: V1.4.1 6 | * 7 | * Project: CMSIS DSP Library 8 | * Title: arm_common_tables.h 9 | * 10 | * Description: This file has extern declaration for common tables like Bitreverse, reciprocal etc which are used across different functions 11 | * 12 | * Target Processor: Cortex-M4/Cortex-M3 13 | * 14 | * Redistribution and use in source and binary forms, with or without 15 | * modification, are permitted provided that the following conditions 16 | * are met: 17 | * - Redistributions of source code must retain the above copyright 18 | * notice, this list of conditions and the following disclaimer. 19 | * - Redistributions in binary form must reproduce the above copyright 20 | * notice, this list of conditions and the following disclaimer in 21 | * the documentation and/or other materials provided with the 22 | * distribution. 23 | * - Neither the name of ARM LIMITED nor the names of its contributors 24 | * may be used to endorse or promote products derived from this 25 | * software without specific prior written permission. 26 | * 27 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 28 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 29 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 30 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 31 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 32 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 33 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 34 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 35 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 36 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 37 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 38 | * POSSIBILITY OF SUCH DAMAGE. 39 | * -------------------------------------------------------------------- */ 40 | 41 | #ifndef _ARM_COMMON_TABLES_H 42 | #define _ARM_COMMON_TABLES_H 43 | 44 | #include "arm_math.h" 45 | 46 | extern const uint16_t armBitRevTable[1024]; 47 | extern const q15_t armRecipTableQ15[64]; 48 | extern const q31_t armRecipTableQ31[64]; 49 | extern const q31_t realCoefAQ31[1024]; 50 | extern const q31_t realCoefBQ31[1024]; 51 | extern const float32_t twiddleCoef_16[32]; 52 | extern const float32_t twiddleCoef_32[64]; 53 | extern const float32_t twiddleCoef_64[128]; 54 | extern const float32_t twiddleCoef_128[256]; 55 | extern const float32_t twiddleCoef_256[512]; 56 | extern const float32_t twiddleCoef_512[1024]; 57 | extern const float32_t twiddleCoef_1024[2048]; 58 | extern const float32_t twiddleCoef_2048[4096]; 59 | extern const float32_t twiddleCoef_4096[8192]; 60 | #define twiddleCoef twiddleCoef_4096 61 | extern const q31_t twiddleCoefQ31[6144]; 62 | extern const q15_t twiddleCoefQ15[6144]; 63 | extern const float32_t twiddleCoef_rfft_32[32]; 64 | extern const float32_t twiddleCoef_rfft_64[64]; 65 | extern const float32_t twiddleCoef_rfft_128[128]; 66 | extern const float32_t twiddleCoef_rfft_256[256]; 67 | extern const float32_t twiddleCoef_rfft_512[512]; 68 | extern const float32_t twiddleCoef_rfft_1024[1024]; 69 | extern const float32_t twiddleCoef_rfft_2048[2048]; 70 | extern const float32_t twiddleCoef_rfft_4096[4096]; 71 | 72 | 73 | #define ARMBITREVINDEXTABLE__16_TABLE_LENGTH ((uint16_t)20 ) 74 | #define ARMBITREVINDEXTABLE__32_TABLE_LENGTH ((uint16_t)48 ) 75 | #define ARMBITREVINDEXTABLE__64_TABLE_LENGTH ((uint16_t)56 ) 76 | #define ARMBITREVINDEXTABLE_128_TABLE_LENGTH ((uint16_t)208 ) 77 | #define ARMBITREVINDEXTABLE_256_TABLE_LENGTH ((uint16_t)440 ) 78 | #define ARMBITREVINDEXTABLE_512_TABLE_LENGTH ((uint16_t)448 ) 79 | #define ARMBITREVINDEXTABLE1024_TABLE_LENGTH ((uint16_t)1800) 80 | #define ARMBITREVINDEXTABLE2048_TABLE_LENGTH ((uint16_t)3808) 81 | #define ARMBITREVINDEXTABLE4096_TABLE_LENGTH ((uint16_t)4032) 82 | 83 | extern const uint16_t armBitRevIndexTable16[ARMBITREVINDEXTABLE__16_TABLE_LENGTH]; 84 | extern const uint16_t armBitRevIndexTable32[ARMBITREVINDEXTABLE__32_TABLE_LENGTH]; 85 | extern const uint16_t armBitRevIndexTable64[ARMBITREVINDEXTABLE__64_TABLE_LENGTH]; 86 | extern const uint16_t armBitRevIndexTable128[ARMBITREVINDEXTABLE_128_TABLE_LENGTH]; 87 | extern const uint16_t armBitRevIndexTable256[ARMBITREVINDEXTABLE_256_TABLE_LENGTH]; 88 | extern const uint16_t armBitRevIndexTable512[ARMBITREVINDEXTABLE_512_TABLE_LENGTH]; 89 | extern const uint16_t armBitRevIndexTable1024[ARMBITREVINDEXTABLE1024_TABLE_LENGTH]; 90 | extern const uint16_t armBitRevIndexTable2048[ARMBITREVINDEXTABLE2048_TABLE_LENGTH]; 91 | extern const uint16_t armBitRevIndexTable4096[ARMBITREVINDEXTABLE4096_TABLE_LENGTH]; 92 | 93 | #endif /* ARM_COMMON_TABLES_H */ 94 | -------------------------------------------------------------------------------- /Libraries/CMSIS/Include/arm_const_structs.h: -------------------------------------------------------------------------------- 1 | /* ---------------------------------------------------------------------- 2 | * Copyright (C) 2010-2013 ARM Limited. All rights reserved. 3 | * 4 | * $Date: 17. January 2013 5 | * $Revision: V1.4.1 6 | * 7 | * Project: CMSIS DSP Library 8 | * Title: arm_const_structs.h 9 | * 10 | * Description: This file has constant structs that are initialized for 11 | * user convenience. For example, some can be given as 12 | * arguments to the arm_cfft_f32() function. 13 | * 14 | * Target Processor: Cortex-M4/Cortex-M3 15 | * 16 | * Redistribution and use in source and binary forms, with or without 17 | * modification, are permitted provided that the following conditions 18 | * are met: 19 | * - Redistributions of source code must retain the above copyright 20 | * notice, this list of conditions and the following disclaimer. 21 | * - Redistributions in binary form must reproduce the above copyright 22 | * notice, this list of conditions and the following disclaimer in 23 | * the documentation and/or other materials provided with the 24 | * distribution. 25 | * - Neither the name of ARM LIMITED nor the names of its contributors 26 | * may be used to endorse or promote products derived from this 27 | * software without specific prior written permission. 28 | * 29 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 30 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 31 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 32 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 33 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 34 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 35 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 36 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 37 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 38 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 39 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 40 | * POSSIBILITY OF SUCH DAMAGE. 41 | * -------------------------------------------------------------------- */ 42 | 43 | #ifndef _ARM_CONST_STRUCTS_H 44 | #define _ARM_CONST_STRUCTS_H 45 | 46 | #include "arm_math.h" 47 | #include "arm_common_tables.h" 48 | 49 | const arm_cfft_instance_f32 arm_cfft_sR_f32_len16 = { 50 | 16, twiddleCoef_16, armBitRevIndexTable16, ARMBITREVINDEXTABLE__16_TABLE_LENGTH 51 | }; 52 | 53 | const arm_cfft_instance_f32 arm_cfft_sR_f32_len32 = { 54 | 32, twiddleCoef_32, armBitRevIndexTable32, ARMBITREVINDEXTABLE__32_TABLE_LENGTH 55 | }; 56 | 57 | const arm_cfft_instance_f32 arm_cfft_sR_f32_len64 = { 58 | 64, twiddleCoef_64, armBitRevIndexTable64, ARMBITREVINDEXTABLE__64_TABLE_LENGTH 59 | }; 60 | 61 | const arm_cfft_instance_f32 arm_cfft_sR_f32_len128 = { 62 | 128, twiddleCoef_128, armBitRevIndexTable128, ARMBITREVINDEXTABLE_128_TABLE_LENGTH 63 | }; 64 | 65 | const arm_cfft_instance_f32 arm_cfft_sR_f32_len256 = { 66 | 256, twiddleCoef_256, armBitRevIndexTable256, ARMBITREVINDEXTABLE_256_TABLE_LENGTH 67 | }; 68 | 69 | const arm_cfft_instance_f32 arm_cfft_sR_f32_len512 = { 70 | 512, twiddleCoef_512, armBitRevIndexTable512, ARMBITREVINDEXTABLE_512_TABLE_LENGTH 71 | }; 72 | 73 | const arm_cfft_instance_f32 arm_cfft_sR_f32_len1024 = { 74 | 1024, twiddleCoef_1024, armBitRevIndexTable1024, ARMBITREVINDEXTABLE1024_TABLE_LENGTH 75 | }; 76 | 77 | const arm_cfft_instance_f32 arm_cfft_sR_f32_len2048 = { 78 | 2048, twiddleCoef_2048, armBitRevIndexTable2048, ARMBITREVINDEXTABLE2048_TABLE_LENGTH 79 | }; 80 | 81 | const arm_cfft_instance_f32 arm_cfft_sR_f32_len4096 = { 82 | 4096, twiddleCoef_4096, armBitRevIndexTable4096, ARMBITREVINDEXTABLE4096_TABLE_LENGTH 83 | }; 84 | 85 | #endif 86 | -------------------------------------------------------------------------------- /Libraries/CMSIS/README.txt: -------------------------------------------------------------------------------- 1 | * ------------------------------------------------------------------- 2 | * Copyright (C) 2011-2013 ARM Limited. All rights reserved. 3 | * 4 | * Date: 18 March 2013 5 | * Revision: V3.20 6 | * 7 | * Project: Cortex Microcontroller Software Interface Standard (CMSIS) 8 | * Title: Release Note for CMSIS 9 | * 10 | * ------------------------------------------------------------------- 11 | 12 | 13 | NOTE - Open the index.html file to access CMSIS documentation 14 | 15 | 16 | The Cortex Microcontroller Software Interface Standard (CMSIS) provides a single standard across all 17 | Cortex-Mx processor series vendors. It enables code re-use and code sharing across software projects 18 | and reduces time-to-market for new embedded applications. 19 | 20 | CMSIS is released under the terms of the end user license agreement ("CMSIS END USER LICENCE AGREEMENT.pdf"). 21 | Any user of the software package is bound to the terms and conditions of the end user license agreement. 22 | 23 | 24 | You will find the following sub-directories: 25 | 26 | Documentation - Contains CMSIS documentation. 27 | 28 | DSP_Lib - MDK project files, Examples and source files etc.. to build the 29 | CMSIS DSP Software Library for Cortex-M0, Cortex-M3, Cortex-M4 processors. 30 | 31 | Include - CMSIS Core Support and CMSIS DSP Include Files. 32 | 33 | Lib - CMSIS DSP Libraries. 34 | 35 | RTOS - CMSIS RTOS API template header file. 36 | 37 | SVD - CMSIS SVD Schema files and Conversion Utility. 38 | -------------------------------------------------------------------------------- /Libraries/CMSIS/index.html: -------------------------------------------------------------------------------- 1 |  2 | 3 | 4 | Redirect to the CMSIS main page after 0 seconds 5 | 6 | 7 | 8 | 9 | 10 | 11 | If the automatic redirection is failing, click open CMSIS Documentation. 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /Libraries/STM32F4xx_StdPeriph_Driver/Release_Notes.html: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/suhetao/stm32f4_mpu9250/b86160c2b781f16973cd20293c67c5fab25f54f4/Libraries/STM32F4xx_StdPeriph_Driver/Release_Notes.html -------------------------------------------------------------------------------- /Libraries/STM32F4xx_StdPeriph_Driver/inc/misc.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file misc.h 4 | * @author MCD Application Team 5 | * @version V1.3.0 6 | * @date 08-November-2013 7 | * @brief This file contains all the functions prototypes for the miscellaneous 8 | * firmware library functions (add-on to CMSIS functions). 9 | ****************************************************************************** 10 | * @attention 11 | * 12 | *

© COPYRIGHT 2013 STMicroelectronics

13 | * 14 | * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); 15 | * You may not use this file except in compliance with the License. 16 | * You may obtain a copy of the License at: 17 | * 18 | * http://www.st.com/software_license_agreement_liberty_v2 19 | * 20 | * Unless required by applicable law or agreed to in writing, software 21 | * distributed under the License is distributed on an "AS IS" BASIS, 22 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 23 | * See the License for the specific language governing permissions and 24 | * limitations under the License. 25 | * 26 | ****************************************************************************** 27 | */ 28 | 29 | /* Define to prevent recursive inclusion -------------------------------------*/ 30 | #ifndef __MISC_H 31 | #define __MISC_H 32 | 33 | #ifdef __cplusplus 34 | extern "C" { 35 | #endif 36 | 37 | /* Includes ------------------------------------------------------------------*/ 38 | #include "stm32f4xx.h" 39 | 40 | /** @addtogroup STM32F4xx_StdPeriph_Driver 41 | * @{ 42 | */ 43 | 44 | /** @addtogroup MISC 45 | * @{ 46 | */ 47 | 48 | /* Exported types ------------------------------------------------------------*/ 49 | 50 | /** 51 | * @brief NVIC Init Structure definition 52 | */ 53 | 54 | typedef struct 55 | { 56 | uint8_t NVIC_IRQChannel; /*!< Specifies the IRQ channel to be enabled or disabled. 57 | This parameter can be an enumerator of @ref IRQn_Type 58 | enumeration (For the complete STM32 Devices IRQ Channels 59 | list, please refer to stm32f4xx.h file) */ 60 | 61 | uint8_t NVIC_IRQChannelPreemptionPriority; /*!< Specifies the pre-emption priority for the IRQ channel 62 | specified in NVIC_IRQChannel. This parameter can be a value 63 | between 0 and 15 as described in the table @ref MISC_NVIC_Priority_Table 64 | A lower priority value indicates a higher priority */ 65 | 66 | uint8_t NVIC_IRQChannelSubPriority; /*!< Specifies the subpriority level for the IRQ channel specified 67 | in NVIC_IRQChannel. This parameter can be a value 68 | between 0 and 15 as described in the table @ref MISC_NVIC_Priority_Table 69 | A lower priority value indicates a higher priority */ 70 | 71 | FunctionalState NVIC_IRQChannelCmd; /*!< Specifies whether the IRQ channel defined in NVIC_IRQChannel 72 | will be enabled or disabled. 73 | This parameter can be set either to ENABLE or DISABLE */ 74 | } NVIC_InitTypeDef; 75 | 76 | /* Exported constants --------------------------------------------------------*/ 77 | 78 | /** @defgroup MISC_Exported_Constants 79 | * @{ 80 | */ 81 | 82 | /** @defgroup MISC_Vector_Table_Base 83 | * @{ 84 | */ 85 | 86 | #define NVIC_VectTab_RAM ((uint32_t)0x20000000) 87 | #define NVIC_VectTab_FLASH ((uint32_t)0x08000000) 88 | #define IS_NVIC_VECTTAB(VECTTAB) (((VECTTAB) == NVIC_VectTab_RAM) || \ 89 | ((VECTTAB) == NVIC_VectTab_FLASH)) 90 | /** 91 | * @} 92 | */ 93 | 94 | /** @defgroup MISC_System_Low_Power 95 | * @{ 96 | */ 97 | 98 | #define NVIC_LP_SEVONPEND ((uint8_t)0x10) 99 | #define NVIC_LP_SLEEPDEEP ((uint8_t)0x04) 100 | #define NVIC_LP_SLEEPONEXIT ((uint8_t)0x02) 101 | #define IS_NVIC_LP(LP) (((LP) == NVIC_LP_SEVONPEND) || \ 102 | ((LP) == NVIC_LP_SLEEPDEEP) || \ 103 | ((LP) == NVIC_LP_SLEEPONEXIT)) 104 | /** 105 | * @} 106 | */ 107 | 108 | /** @defgroup MISC_Preemption_Priority_Group 109 | * @{ 110 | */ 111 | 112 | #define NVIC_PriorityGroup_0 ((uint32_t)0x700) /*!< 0 bits for pre-emption priority 113 | 4 bits for subpriority */ 114 | #define NVIC_PriorityGroup_1 ((uint32_t)0x600) /*!< 1 bits for pre-emption priority 115 | 3 bits for subpriority */ 116 | #define NVIC_PriorityGroup_2 ((uint32_t)0x500) /*!< 2 bits for pre-emption priority 117 | 2 bits for subpriority */ 118 | #define NVIC_PriorityGroup_3 ((uint32_t)0x400) /*!< 3 bits for pre-emption priority 119 | 1 bits for subpriority */ 120 | #define NVIC_PriorityGroup_4 ((uint32_t)0x300) /*!< 4 bits for pre-emption priority 121 | 0 bits for subpriority */ 122 | 123 | #define IS_NVIC_PRIORITY_GROUP(GROUP) (((GROUP) == NVIC_PriorityGroup_0) || \ 124 | ((GROUP) == NVIC_PriorityGroup_1) || \ 125 | ((GROUP) == NVIC_PriorityGroup_2) || \ 126 | ((GROUP) == NVIC_PriorityGroup_3) || \ 127 | ((GROUP) == NVIC_PriorityGroup_4)) 128 | 129 | #define IS_NVIC_PREEMPTION_PRIORITY(PRIORITY) ((PRIORITY) < 0x10) 130 | 131 | #define IS_NVIC_SUB_PRIORITY(PRIORITY) ((PRIORITY) < 0x10) 132 | 133 | #define IS_NVIC_OFFSET(OFFSET) ((OFFSET) < 0x000FFFFF) 134 | 135 | /** 136 | * @} 137 | */ 138 | 139 | /** @defgroup MISC_SysTick_clock_source 140 | * @{ 141 | */ 142 | 143 | #define SysTick_CLKSource_HCLK_Div8 ((uint32_t)0xFFFFFFFB) 144 | #define SysTick_CLKSource_HCLK ((uint32_t)0x00000004) 145 | #define IS_SYSTICK_CLK_SOURCE(SOURCE) (((SOURCE) == SysTick_CLKSource_HCLK) || \ 146 | ((SOURCE) == SysTick_CLKSource_HCLK_Div8)) 147 | /** 148 | * @} 149 | */ 150 | 151 | /** 152 | * @} 153 | */ 154 | 155 | /* Exported macro ------------------------------------------------------------*/ 156 | /* Exported functions --------------------------------------------------------*/ 157 | 158 | void NVIC_PriorityGroupConfig(uint32_t NVIC_PriorityGroup); 159 | void NVIC_Init(NVIC_InitTypeDef* NVIC_InitStruct); 160 | void NVIC_SetVectorTable(uint32_t NVIC_VectTab, uint32_t Offset); 161 | void NVIC_SystemLPConfig(uint8_t LowPowerMode, FunctionalState NewState); 162 | void SysTick_CLKSourceConfig(uint32_t SysTick_CLKSource); 163 | 164 | #ifdef __cplusplus 165 | } 166 | #endif 167 | 168 | #endif /* __MISC_H */ 169 | 170 | /** 171 | * @} 172 | */ 173 | 174 | /** 175 | * @} 176 | */ 177 | 178 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ 179 | -------------------------------------------------------------------------------- /Libraries/STM32F4xx_StdPeriph_Driver/inc/stm32f4xx_crc.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file stm32f4xx_crc.h 4 | * @author MCD Application Team 5 | * @version V1.3.0 6 | * @date 08-November-2013 7 | * @brief This file contains all the functions prototypes for the CRC firmware 8 | * library. 9 | ****************************************************************************** 10 | * @attention 11 | * 12 | *

© COPYRIGHT 2013 STMicroelectronics

13 | * 14 | * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); 15 | * You may not use this file except in compliance with the License. 16 | * You may obtain a copy of the License at: 17 | * 18 | * http://www.st.com/software_license_agreement_liberty_v2 19 | * 20 | * Unless required by applicable law or agreed to in writing, software 21 | * distributed under the License is distributed on an "AS IS" BASIS, 22 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 23 | * See the License for the specific language governing permissions and 24 | * limitations under the License. 25 | * 26 | ****************************************************************************** 27 | */ 28 | 29 | /* Define to prevent recursive inclusion -------------------------------------*/ 30 | #ifndef __STM32F4xx_CRC_H 31 | #define __STM32F4xx_CRC_H 32 | 33 | #ifdef __cplusplus 34 | extern "C" { 35 | #endif 36 | 37 | /* Includes ------------------------------------------------------------------*/ 38 | #include "stm32f4xx.h" 39 | 40 | /** @addtogroup STM32F4xx_StdPeriph_Driver 41 | * @{ 42 | */ 43 | 44 | /** @addtogroup CRC 45 | * @{ 46 | */ 47 | 48 | /* Exported types ------------------------------------------------------------*/ 49 | /* Exported constants --------------------------------------------------------*/ 50 | 51 | /** @defgroup CRC_Exported_Constants 52 | * @{ 53 | */ 54 | 55 | /** 56 | * @} 57 | */ 58 | 59 | /* Exported macro ------------------------------------------------------------*/ 60 | /* Exported functions --------------------------------------------------------*/ 61 | 62 | void CRC_ResetDR(void); 63 | uint32_t CRC_CalcCRC(uint32_t Data); 64 | uint32_t CRC_CalcBlockCRC(uint32_t pBuffer[], uint32_t BufferLength); 65 | uint32_t CRC_GetCRC(void); 66 | void CRC_SetIDRegister(uint8_t IDValue); 67 | uint8_t CRC_GetIDRegister(void); 68 | 69 | #ifdef __cplusplus 70 | } 71 | #endif 72 | 73 | #endif /* __STM32F4xx_CRC_H */ 74 | 75 | /** 76 | * @} 77 | */ 78 | 79 | /** 80 | * @} 81 | */ 82 | 83 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ 84 | -------------------------------------------------------------------------------- /Libraries/STM32F4xx_StdPeriph_Driver/inc/stm32f4xx_dbgmcu.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file stm32f4xx_dbgmcu.h 4 | * @author MCD Application Team 5 | * @version V1.3.0 6 | * @date 08-November-2013 7 | * @brief This file contains all the functions prototypes for the DBGMCU firmware library. 8 | ****************************************************************************** 9 | * @attention 10 | * 11 | *

© COPYRIGHT 2013 STMicroelectronics

12 | * 13 | * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); 14 | * You may not use this file except in compliance with the License. 15 | * You may obtain a copy of the License at: 16 | * 17 | * http://www.st.com/software_license_agreement_liberty_v2 18 | * 19 | * Unless required by applicable law or agreed to in writing, software 20 | * distributed under the License is distributed on an "AS IS" BASIS, 21 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 22 | * See the License for the specific language governing permissions and 23 | * limitations under the License. 24 | * 25 | ****************************************************************************** 26 | */ 27 | 28 | /* Define to prevent recursive inclusion -------------------------------------*/ 29 | #ifndef __STM32F4xx_DBGMCU_H 30 | #define __STM32F4xx_DBGMCU_H 31 | 32 | #ifdef __cplusplus 33 | extern "C" { 34 | #endif 35 | 36 | /* Includes ------------------------------------------------------------------*/ 37 | #include "stm32f4xx.h" 38 | 39 | /** @addtogroup STM32F4xx_StdPeriph_Driver 40 | * @{ 41 | */ 42 | 43 | /** @addtogroup DBGMCU 44 | * @{ 45 | */ 46 | 47 | /* Exported types ------------------------------------------------------------*/ 48 | /* Exported constants --------------------------------------------------------*/ 49 | 50 | /** @defgroup DBGMCU_Exported_Constants 51 | * @{ 52 | */ 53 | #define DBGMCU_SLEEP ((uint32_t)0x00000001) 54 | #define DBGMCU_STOP ((uint32_t)0x00000002) 55 | #define DBGMCU_STANDBY ((uint32_t)0x00000004) 56 | #define IS_DBGMCU_PERIPH(PERIPH) ((((PERIPH) & 0xFFFFFFF8) == 0x00) && ((PERIPH) != 0x00)) 57 | 58 | #define DBGMCU_TIM2_STOP ((uint32_t)0x00000001) 59 | #define DBGMCU_TIM3_STOP ((uint32_t)0x00000002) 60 | #define DBGMCU_TIM4_STOP ((uint32_t)0x00000004) 61 | #define DBGMCU_TIM5_STOP ((uint32_t)0x00000008) 62 | #define DBGMCU_TIM6_STOP ((uint32_t)0x00000010) 63 | #define DBGMCU_TIM7_STOP ((uint32_t)0x00000020) 64 | #define DBGMCU_TIM12_STOP ((uint32_t)0x00000040) 65 | #define DBGMCU_TIM13_STOP ((uint32_t)0x00000080) 66 | #define DBGMCU_TIM14_STOP ((uint32_t)0x00000100) 67 | #define DBGMCU_RTC_STOP ((uint32_t)0x00000400) 68 | #define DBGMCU_WWDG_STOP ((uint32_t)0x00000800) 69 | #define DBGMCU_IWDG_STOP ((uint32_t)0x00001000) 70 | #define DBGMCU_I2C1_SMBUS_TIMEOUT ((uint32_t)0x00200000) 71 | #define DBGMCU_I2C2_SMBUS_TIMEOUT ((uint32_t)0x00400000) 72 | #define DBGMCU_I2C3_SMBUS_TIMEOUT ((uint32_t)0x00800000) 73 | #define DBGMCU_CAN1_STOP ((uint32_t)0x02000000) 74 | #define DBGMCU_CAN2_STOP ((uint32_t)0x04000000) 75 | #define IS_DBGMCU_APB1PERIPH(PERIPH) ((((PERIPH) & 0xF91FE200) == 0x00) && ((PERIPH) != 0x00)) 76 | 77 | #define DBGMCU_TIM1_STOP ((uint32_t)0x00000001) 78 | #define DBGMCU_TIM8_STOP ((uint32_t)0x00000002) 79 | #define DBGMCU_TIM9_STOP ((uint32_t)0x00010000) 80 | #define DBGMCU_TIM10_STOP ((uint32_t)0x00020000) 81 | #define DBGMCU_TIM11_STOP ((uint32_t)0x00040000) 82 | #define IS_DBGMCU_APB2PERIPH(PERIPH) ((((PERIPH) & 0xFFF8FFFC) == 0x00) && ((PERIPH) != 0x00)) 83 | /** 84 | * @} 85 | */ 86 | 87 | /* Exported macro ------------------------------------------------------------*/ 88 | /* Exported functions --------------------------------------------------------*/ 89 | uint32_t DBGMCU_GetREVID(void); 90 | uint32_t DBGMCU_GetDEVID(void); 91 | void DBGMCU_Config(uint32_t DBGMCU_Periph, FunctionalState NewState); 92 | void DBGMCU_APB1PeriphConfig(uint32_t DBGMCU_Periph, FunctionalState NewState); 93 | void DBGMCU_APB2PeriphConfig(uint32_t DBGMCU_Periph, FunctionalState NewState); 94 | 95 | #ifdef __cplusplus 96 | } 97 | #endif 98 | 99 | #endif /* __STM32F4xx_DBGMCU_H */ 100 | 101 | /** 102 | * @} 103 | */ 104 | 105 | /** 106 | * @} 107 | */ 108 | 109 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ 110 | -------------------------------------------------------------------------------- /Libraries/STM32F4xx_StdPeriph_Driver/inc/stm32f4xx_iwdg.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file stm32f4xx_iwdg.h 4 | * @author MCD Application Team 5 | * @version V1.3.0 6 | * @date 08-November-2013 7 | * @brief This file contains all the functions prototypes for the IWDG 8 | * firmware library. 9 | ****************************************************************************** 10 | * @attention 11 | * 12 | *

© COPYRIGHT 2013 STMicroelectronics

13 | * 14 | * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); 15 | * You may not use this file except in compliance with the License. 16 | * You may obtain a copy of the License at: 17 | * 18 | * http://www.st.com/software_license_agreement_liberty_v2 19 | * 20 | * Unless required by applicable law or agreed to in writing, software 21 | * distributed under the License is distributed on an "AS IS" BASIS, 22 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 23 | * See the License for the specific language governing permissions and 24 | * limitations under the License. 25 | * 26 | ****************************************************************************** 27 | */ 28 | 29 | /* Define to prevent recursive inclusion -------------------------------------*/ 30 | #ifndef __STM32F4xx_IWDG_H 31 | #define __STM32F4xx_IWDG_H 32 | 33 | #ifdef __cplusplus 34 | extern "C" { 35 | #endif 36 | 37 | /* Includes ------------------------------------------------------------------*/ 38 | #include "stm32f4xx.h" 39 | 40 | /** @addtogroup STM32F4xx_StdPeriph_Driver 41 | * @{ 42 | */ 43 | 44 | /** @addtogroup IWDG 45 | * @{ 46 | */ 47 | 48 | /* Exported types ------------------------------------------------------------*/ 49 | /* Exported constants --------------------------------------------------------*/ 50 | 51 | /** @defgroup IWDG_Exported_Constants 52 | * @{ 53 | */ 54 | 55 | /** @defgroup IWDG_WriteAccess 56 | * @{ 57 | */ 58 | #define IWDG_WriteAccess_Enable ((uint16_t)0x5555) 59 | #define IWDG_WriteAccess_Disable ((uint16_t)0x0000) 60 | #define IS_IWDG_WRITE_ACCESS(ACCESS) (((ACCESS) == IWDG_WriteAccess_Enable) || \ 61 | ((ACCESS) == IWDG_WriteAccess_Disable)) 62 | /** 63 | * @} 64 | */ 65 | 66 | /** @defgroup IWDG_prescaler 67 | * @{ 68 | */ 69 | #define IWDG_Prescaler_4 ((uint8_t)0x00) 70 | #define IWDG_Prescaler_8 ((uint8_t)0x01) 71 | #define IWDG_Prescaler_16 ((uint8_t)0x02) 72 | #define IWDG_Prescaler_32 ((uint8_t)0x03) 73 | #define IWDG_Prescaler_64 ((uint8_t)0x04) 74 | #define IWDG_Prescaler_128 ((uint8_t)0x05) 75 | #define IWDG_Prescaler_256 ((uint8_t)0x06) 76 | #define IS_IWDG_PRESCALER(PRESCALER) (((PRESCALER) == IWDG_Prescaler_4) || \ 77 | ((PRESCALER) == IWDG_Prescaler_8) || \ 78 | ((PRESCALER) == IWDG_Prescaler_16) || \ 79 | ((PRESCALER) == IWDG_Prescaler_32) || \ 80 | ((PRESCALER) == IWDG_Prescaler_64) || \ 81 | ((PRESCALER) == IWDG_Prescaler_128)|| \ 82 | ((PRESCALER) == IWDG_Prescaler_256)) 83 | /** 84 | * @} 85 | */ 86 | 87 | /** @defgroup IWDG_Flag 88 | * @{ 89 | */ 90 | #define IWDG_FLAG_PVU ((uint16_t)0x0001) 91 | #define IWDG_FLAG_RVU ((uint16_t)0x0002) 92 | #define IS_IWDG_FLAG(FLAG) (((FLAG) == IWDG_FLAG_PVU) || ((FLAG) == IWDG_FLAG_RVU)) 93 | #define IS_IWDG_RELOAD(RELOAD) ((RELOAD) <= 0xFFF) 94 | /** 95 | * @} 96 | */ 97 | 98 | /** 99 | * @} 100 | */ 101 | 102 | /* Exported macro ------------------------------------------------------------*/ 103 | /* Exported functions --------------------------------------------------------*/ 104 | 105 | /* Prescaler and Counter configuration functions ******************************/ 106 | void IWDG_WriteAccessCmd(uint16_t IWDG_WriteAccess); 107 | void IWDG_SetPrescaler(uint8_t IWDG_Prescaler); 108 | void IWDG_SetReload(uint16_t Reload); 109 | void IWDG_ReloadCounter(void); 110 | 111 | /* IWDG activation function ***************************************************/ 112 | void IWDG_Enable(void); 113 | 114 | /* Flag management function ***************************************************/ 115 | FlagStatus IWDG_GetFlagStatus(uint16_t IWDG_FLAG); 116 | 117 | #ifdef __cplusplus 118 | } 119 | #endif 120 | 121 | #endif /* __STM32F4xx_IWDG_H */ 122 | 123 | /** 124 | * @} 125 | */ 126 | 127 | /** 128 | * @} 129 | */ 130 | 131 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ 132 | -------------------------------------------------------------------------------- /Libraries/STM32F4xx_StdPeriph_Driver/inc/stm32f4xx_rng.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file stm32f4xx_rng.h 4 | * @author MCD Application Team 5 | * @version V1.3.0 6 | * @date 08-November-2013 7 | * @brief This file contains all the functions prototypes for the Random 8 | * Number Generator(RNG) firmware library. 9 | ****************************************************************************** 10 | * @attention 11 | * 12 | *

© COPYRIGHT 2013 STMicroelectronics

13 | * 14 | * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); 15 | * You may not use this file except in compliance with the License. 16 | * You may obtain a copy of the License at: 17 | * 18 | * http://www.st.com/software_license_agreement_liberty_v2 19 | * 20 | * Unless required by applicable law or agreed to in writing, software 21 | * distributed under the License is distributed on an "AS IS" BASIS, 22 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 23 | * See the License for the specific language governing permissions and 24 | * limitations under the License. 25 | * 26 | ****************************************************************************** 27 | */ 28 | 29 | /* Define to prevent recursive inclusion -------------------------------------*/ 30 | #ifndef __STM32F4xx_RNG_H 31 | #define __STM32F4xx_RNG_H 32 | 33 | #ifdef __cplusplus 34 | extern "C" { 35 | #endif 36 | 37 | /* Includes ------------------------------------------------------------------*/ 38 | #include "stm32f4xx.h" 39 | 40 | /** @addtogroup STM32F4xx_StdPeriph_Driver 41 | * @{ 42 | */ 43 | 44 | /** @addtogroup RNG 45 | * @{ 46 | */ 47 | 48 | /* Exported types ------------------------------------------------------------*/ 49 | /* Exported constants --------------------------------------------------------*/ 50 | 51 | /** @defgroup RNG_Exported_Constants 52 | * @{ 53 | */ 54 | 55 | /** @defgroup RNG_flags_definition 56 | * @{ 57 | */ 58 | #define RNG_FLAG_DRDY ((uint8_t)0x0001) /*!< Data ready */ 59 | #define RNG_FLAG_CECS ((uint8_t)0x0002) /*!< Clock error current status */ 60 | #define RNG_FLAG_SECS ((uint8_t)0x0004) /*!< Seed error current status */ 61 | 62 | #define IS_RNG_GET_FLAG(RNG_FLAG) (((RNG_FLAG) == RNG_FLAG_DRDY) || \ 63 | ((RNG_FLAG) == RNG_FLAG_CECS) || \ 64 | ((RNG_FLAG) == RNG_FLAG_SECS)) 65 | #define IS_RNG_CLEAR_FLAG(RNG_FLAG) (((RNG_FLAG) == RNG_FLAG_CECS) || \ 66 | ((RNG_FLAG) == RNG_FLAG_SECS)) 67 | /** 68 | * @} 69 | */ 70 | 71 | /** @defgroup RNG_interrupts_definition 72 | * @{ 73 | */ 74 | #define RNG_IT_CEI ((uint8_t)0x20) /*!< Clock error interrupt */ 75 | #define RNG_IT_SEI ((uint8_t)0x40) /*!< Seed error interrupt */ 76 | 77 | #define IS_RNG_IT(IT) ((((IT) & (uint8_t)0x9F) == 0x00) && ((IT) != 0x00)) 78 | #define IS_RNG_GET_IT(RNG_IT) (((RNG_IT) == RNG_IT_CEI) || ((RNG_IT) == RNG_IT_SEI)) 79 | /** 80 | * @} 81 | */ 82 | 83 | /** 84 | * @} 85 | */ 86 | 87 | /* Exported macro ------------------------------------------------------------*/ 88 | /* Exported functions --------------------------------------------------------*/ 89 | 90 | /* Function used to set the RNG configuration to the default reset state *****/ 91 | void RNG_DeInit(void); 92 | 93 | /* Configuration function *****************************************************/ 94 | void RNG_Cmd(FunctionalState NewState); 95 | 96 | /* Get 32 bit Random number function ******************************************/ 97 | uint32_t RNG_GetRandomNumber(void); 98 | 99 | /* Interrupts and flags management functions **********************************/ 100 | void RNG_ITConfig(FunctionalState NewState); 101 | FlagStatus RNG_GetFlagStatus(uint8_t RNG_FLAG); 102 | void RNG_ClearFlag(uint8_t RNG_FLAG); 103 | ITStatus RNG_GetITStatus(uint8_t RNG_IT); 104 | void RNG_ClearITPendingBit(uint8_t RNG_IT); 105 | 106 | #ifdef __cplusplus 107 | } 108 | #endif 109 | 110 | #endif /*__STM32F4xx_RNG_H */ 111 | 112 | /** 113 | * @} 114 | */ 115 | 116 | /** 117 | * @} 118 | */ 119 | 120 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ 121 | -------------------------------------------------------------------------------- /Libraries/STM32F4xx_StdPeriph_Driver/inc/stm32f4xx_wwdg.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file stm32f4xx_wwdg.h 4 | * @author MCD Application Team 5 | * @version V1.3.0 6 | * @date 08-November-2013 7 | * @brief This file contains all the functions prototypes for the WWDG firmware 8 | * library. 9 | ****************************************************************************** 10 | * @attention 11 | * 12 | *

© COPYRIGHT 2013 STMicroelectronics

13 | * 14 | * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); 15 | * You may not use this file except in compliance with the License. 16 | * You may obtain a copy of the License at: 17 | * 18 | * http://www.st.com/software_license_agreement_liberty_v2 19 | * 20 | * Unless required by applicable law or agreed to in writing, software 21 | * distributed under the License is distributed on an "AS IS" BASIS, 22 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 23 | * See the License for the specific language governing permissions and 24 | * limitations under the License. 25 | * 26 | ****************************************************************************** 27 | */ 28 | 29 | /* Define to prevent recursive inclusion -------------------------------------*/ 30 | #ifndef __STM32F4xx_WWDG_H 31 | #define __STM32F4xx_WWDG_H 32 | 33 | #ifdef __cplusplus 34 | extern "C" { 35 | #endif 36 | 37 | /* Includes ------------------------------------------------------------------*/ 38 | #include "stm32f4xx.h" 39 | 40 | /** @addtogroup STM32F4xx_StdPeriph_Driver 41 | * @{ 42 | */ 43 | 44 | /** @addtogroup WWDG 45 | * @{ 46 | */ 47 | 48 | /* Exported types ------------------------------------------------------------*/ 49 | /* Exported constants --------------------------------------------------------*/ 50 | 51 | /** @defgroup WWDG_Exported_Constants 52 | * @{ 53 | */ 54 | 55 | /** @defgroup WWDG_Prescaler 56 | * @{ 57 | */ 58 | 59 | #define WWDG_Prescaler_1 ((uint32_t)0x00000000) 60 | #define WWDG_Prescaler_2 ((uint32_t)0x00000080) 61 | #define WWDG_Prescaler_4 ((uint32_t)0x00000100) 62 | #define WWDG_Prescaler_8 ((uint32_t)0x00000180) 63 | #define IS_WWDG_PRESCALER(PRESCALER) (((PRESCALER) == WWDG_Prescaler_1) || \ 64 | ((PRESCALER) == WWDG_Prescaler_2) || \ 65 | ((PRESCALER) == WWDG_Prescaler_4) || \ 66 | ((PRESCALER) == WWDG_Prescaler_8)) 67 | #define IS_WWDG_WINDOW_VALUE(VALUE) ((VALUE) <= 0x7F) 68 | #define IS_WWDG_COUNTER(COUNTER) (((COUNTER) >= 0x40) && ((COUNTER) <= 0x7F)) 69 | 70 | /** 71 | * @} 72 | */ 73 | 74 | /** 75 | * @} 76 | */ 77 | 78 | /* Exported macro ------------------------------------------------------------*/ 79 | /* Exported functions --------------------------------------------------------*/ 80 | 81 | /* Function used to set the WWDG configuration to the default reset state ****/ 82 | void WWDG_DeInit(void); 83 | 84 | /* Prescaler, Refresh window and Counter configuration functions **************/ 85 | void WWDG_SetPrescaler(uint32_t WWDG_Prescaler); 86 | void WWDG_SetWindowValue(uint8_t WindowValue); 87 | void WWDG_EnableIT(void); 88 | void WWDG_SetCounter(uint8_t Counter); 89 | 90 | /* WWDG activation function ***************************************************/ 91 | void WWDG_Enable(uint8_t Counter); 92 | 93 | /* Interrupts and flags management functions **********************************/ 94 | FlagStatus WWDG_GetFlagStatus(void); 95 | void WWDG_ClearFlag(void); 96 | 97 | #ifdef __cplusplus 98 | } 99 | #endif 100 | 101 | #endif /* __STM32F4xx_WWDG_H */ 102 | 103 | /** 104 | * @} 105 | */ 106 | 107 | /** 108 | * @} 109 | */ 110 | 111 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ 112 | -------------------------------------------------------------------------------- /Libraries/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_crc.c: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file stm32f4xx_crc.c 4 | * @author MCD Application Team 5 | * @version V1.3.0 6 | * @date 08-November-2013 7 | * @brief This file provides all the CRC firmware functions. 8 | ****************************************************************************** 9 | * @attention 10 | * 11 | *

© COPYRIGHT 2013 STMicroelectronics

12 | * 13 | * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); 14 | * You may not use this file except in compliance with the License. 15 | * You may obtain a copy of the License at: 16 | * 17 | * http://www.st.com/software_license_agreement_liberty_v2 18 | * 19 | * Unless required by applicable law or agreed to in writing, software 20 | * distributed under the License is distributed on an "AS IS" BASIS, 21 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 22 | * See the License for the specific language governing permissions and 23 | * limitations under the License. 24 | * 25 | ****************************************************************************** 26 | */ 27 | 28 | /* Includes ------------------------------------------------------------------*/ 29 | #include "stm32f4xx_crc.h" 30 | 31 | /** @addtogroup STM32F4xx_StdPeriph_Driver 32 | * @{ 33 | */ 34 | 35 | /** @defgroup CRC 36 | * @brief CRC driver modules 37 | * @{ 38 | */ 39 | 40 | /* Private typedef -----------------------------------------------------------*/ 41 | /* Private define ------------------------------------------------------------*/ 42 | /* Private macro -------------------------------------------------------------*/ 43 | /* Private variables ---------------------------------------------------------*/ 44 | /* Private function prototypes -----------------------------------------------*/ 45 | /* Private functions ---------------------------------------------------------*/ 46 | 47 | /** @defgroup CRC_Private_Functions 48 | * @{ 49 | */ 50 | 51 | /** 52 | * @brief Resets the CRC Data register (DR). 53 | * @param None 54 | * @retval None 55 | */ 56 | void CRC_ResetDR(void) 57 | { 58 | /* Reset CRC generator */ 59 | CRC->CR = CRC_CR_RESET; 60 | } 61 | 62 | /** 63 | * @brief Computes the 32-bit CRC of a given data word(32-bit). 64 | * @param Data: data word(32-bit) to compute its CRC 65 | * @retval 32-bit CRC 66 | */ 67 | uint32_t CRC_CalcCRC(uint32_t Data) 68 | { 69 | CRC->DR = Data; 70 | 71 | return (CRC->DR); 72 | } 73 | 74 | /** 75 | * @brief Computes the 32-bit CRC of a given buffer of data word(32-bit). 76 | * @param pBuffer: pointer to the buffer containing the data to be computed 77 | * @param BufferLength: length of the buffer to be computed 78 | * @retval 32-bit CRC 79 | */ 80 | uint32_t CRC_CalcBlockCRC(uint32_t pBuffer[], uint32_t BufferLength) 81 | { 82 | uint32_t index = 0; 83 | 84 | for(index = 0; index < BufferLength; index++) 85 | { 86 | CRC->DR = pBuffer[index]; 87 | } 88 | return (CRC->DR); 89 | } 90 | 91 | /** 92 | * @brief Returns the current CRC value. 93 | * @param None 94 | * @retval 32-bit CRC 95 | */ 96 | uint32_t CRC_GetCRC(void) 97 | { 98 | return (CRC->DR); 99 | } 100 | 101 | /** 102 | * @brief Stores a 8-bit data in the Independent Data(ID) register. 103 | * @param IDValue: 8-bit value to be stored in the ID register 104 | * @retval None 105 | */ 106 | void CRC_SetIDRegister(uint8_t IDValue) 107 | { 108 | CRC->IDR = IDValue; 109 | } 110 | 111 | /** 112 | * @brief Returns the 8-bit data stored in the Independent Data(ID) register 113 | * @param None 114 | * @retval 8-bit value of the ID register 115 | */ 116 | uint8_t CRC_GetIDRegister(void) 117 | { 118 | return (CRC->IDR); 119 | } 120 | 121 | /** 122 | * @} 123 | */ 124 | 125 | /** 126 | * @} 127 | */ 128 | 129 | /** 130 | * @} 131 | */ 132 | 133 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ 134 | -------------------------------------------------------------------------------- /Libraries/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_dbgmcu.c: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file stm32f4xx_dbgmcu.c 4 | * @author MCD Application Team 5 | * @version V1.3.0 6 | * @date 08-November-2013 7 | * @brief This file provides all the DBGMCU firmware functions. 8 | ****************************************************************************** 9 | * @attention 10 | * 11 | *

© COPYRIGHT 2013 STMicroelectronics

12 | * 13 | * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); 14 | * You may not use this file except in compliance with the License. 15 | * You may obtain a copy of the License at: 16 | * 17 | * http://www.st.com/software_license_agreement_liberty_v2 18 | * 19 | * Unless required by applicable law or agreed to in writing, software 20 | * distributed under the License is distributed on an "AS IS" BASIS, 21 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 22 | * See the License for the specific language governing permissions and 23 | * limitations under the License. 24 | * 25 | ****************************************************************************** 26 | */ 27 | 28 | /* Includes ------------------------------------------------------------------*/ 29 | #include "stm32f4xx_dbgmcu.h" 30 | 31 | /** @addtogroup STM32F4xx_StdPeriph_Driver 32 | * @{ 33 | */ 34 | 35 | /** @defgroup DBGMCU 36 | * @brief DBGMCU driver modules 37 | * @{ 38 | */ 39 | 40 | /* Private typedef -----------------------------------------------------------*/ 41 | /* Private define ------------------------------------------------------------*/ 42 | #define IDCODE_DEVID_MASK ((uint32_t)0x00000FFF) 43 | 44 | /* Private macro -------------------------------------------------------------*/ 45 | /* Private variables ---------------------------------------------------------*/ 46 | /* Private function prototypes -----------------------------------------------*/ 47 | /* Private functions ---------------------------------------------------------*/ 48 | 49 | /** @defgroup DBGMCU_Private_Functions 50 | * @{ 51 | */ 52 | 53 | /** 54 | * @brief Returns the device revision identifier. 55 | * @param None 56 | * @retval Device revision identifier 57 | */ 58 | uint32_t DBGMCU_GetREVID(void) 59 | { 60 | return(DBGMCU->IDCODE >> 16); 61 | } 62 | 63 | /** 64 | * @brief Returns the device identifier. 65 | * @param None 66 | * @retval Device identifier 67 | */ 68 | uint32_t DBGMCU_GetDEVID(void) 69 | { 70 | return(DBGMCU->IDCODE & IDCODE_DEVID_MASK); 71 | } 72 | 73 | /** 74 | * @brief Configures low power mode behavior when the MCU is in Debug mode. 75 | * @param DBGMCU_Periph: specifies the low power mode. 76 | * This parameter can be any combination of the following values: 77 | * @arg DBGMCU_SLEEP: Keep debugger connection during SLEEP mode 78 | * @arg DBGMCU_STOP: Keep debugger connection during STOP mode 79 | * @arg DBGMCU_STANDBY: Keep debugger connection during STANDBY mode 80 | * @param NewState: new state of the specified low power mode in Debug mode. 81 | * This parameter can be: ENABLE or DISABLE. 82 | * @retval None 83 | */ 84 | void DBGMCU_Config(uint32_t DBGMCU_Periph, FunctionalState NewState) 85 | { 86 | /* Check the parameters */ 87 | assert_param(IS_DBGMCU_PERIPH(DBGMCU_Periph)); 88 | assert_param(IS_FUNCTIONAL_STATE(NewState)); 89 | if (NewState != DISABLE) 90 | { 91 | DBGMCU->CR |= DBGMCU_Periph; 92 | } 93 | else 94 | { 95 | DBGMCU->CR &= ~DBGMCU_Periph; 96 | } 97 | } 98 | 99 | /** 100 | * @brief Configures APB1 peripheral behavior when the MCU is in Debug mode. 101 | * @param DBGMCU_Periph: specifies the APB1 peripheral. 102 | * This parameter can be any combination of the following values: 103 | * @arg DBGMCU_TIM2_STOP: TIM2 counter stopped when Core is halted 104 | * @arg DBGMCU_TIM3_STOP: TIM3 counter stopped when Core is halted 105 | * @arg DBGMCU_TIM4_STOP: TIM4 counter stopped when Core is halted 106 | * @arg DBGMCU_TIM5_STOP: TIM5 counter stopped when Core is halted 107 | * @arg DBGMCU_TIM6_STOP: TIM6 counter stopped when Core is halted 108 | * @arg DBGMCU_TIM7_STOP: TIM7 counter stopped when Core is halted 109 | * @arg DBGMCU_TIM12_STOP: TIM12 counter stopped when Core is halted 110 | * @arg DBGMCU_TIM13_STOP: TIM13 counter stopped when Core is halted 111 | * @arg DBGMCU_TIM14_STOP: TIM14 counter stopped when Core is halted 112 | * @arg DBGMCU_RTC_STOP: RTC Calendar and Wakeup counter stopped when Core is halted. 113 | * @arg DBGMCU_WWDG_STOP: Debug WWDG stopped when Core is halted 114 | * @arg DBGMCU_IWDG_STOP: Debug IWDG stopped when Core is halted 115 | * @arg DBGMCU_I2C1_SMBUS_TIMEOUT: I2C1 SMBUS timeout mode stopped when Core is halted 116 | * @arg DBGMCU_I2C2_SMBUS_TIMEOUT: I2C2 SMBUS timeout mode stopped when Core is halted 117 | * @arg DBGMCU_I2C3_SMBUS_TIMEOUT: I2C3 SMBUS timeout mode stopped when Core is halted 118 | * @arg DBGMCU_CAN2_STOP: Debug CAN1 stopped when Core is halted 119 | * @arg DBGMCU_CAN1_STOP: Debug CAN2 stopped when Core is halted 120 | * This parameter can be: ENABLE or DISABLE. 121 | * @retval None 122 | */ 123 | void DBGMCU_APB1PeriphConfig(uint32_t DBGMCU_Periph, FunctionalState NewState) 124 | { 125 | /* Check the parameters */ 126 | assert_param(IS_DBGMCU_APB1PERIPH(DBGMCU_Periph)); 127 | assert_param(IS_FUNCTIONAL_STATE(NewState)); 128 | 129 | if (NewState != DISABLE) 130 | { 131 | DBGMCU->APB1FZ |= DBGMCU_Periph; 132 | } 133 | else 134 | { 135 | DBGMCU->APB1FZ &= ~DBGMCU_Periph; 136 | } 137 | } 138 | 139 | /** 140 | * @brief Configures APB2 peripheral behavior when the MCU is in Debug mode. 141 | * @param DBGMCU_Periph: specifies the APB2 peripheral. 142 | * This parameter can be any combination of the following values: 143 | * @arg DBGMCU_TIM1_STOP: TIM1 counter stopped when Core is halted 144 | * @arg DBGMCU_TIM8_STOP: TIM8 counter stopped when Core is halted 145 | * @arg DBGMCU_TIM9_STOP: TIM9 counter stopped when Core is halted 146 | * @arg DBGMCU_TIM10_STOP: TIM10 counter stopped when Core is halted 147 | * @arg DBGMCU_TIM11_STOP: TIM11 counter stopped when Core is halted 148 | * @param NewState: new state of the specified peripheral in Debug mode. 149 | * This parameter can be: ENABLE or DISABLE. 150 | * @retval None 151 | */ 152 | void DBGMCU_APB2PeriphConfig(uint32_t DBGMCU_Periph, FunctionalState NewState) 153 | { 154 | /* Check the parameters */ 155 | assert_param(IS_DBGMCU_APB2PERIPH(DBGMCU_Periph)); 156 | assert_param(IS_FUNCTIONAL_STATE(NewState)); 157 | 158 | if (NewState != DISABLE) 159 | { 160 | DBGMCU->APB2FZ |= DBGMCU_Periph; 161 | } 162 | else 163 | { 164 | DBGMCU->APB2FZ &= ~DBGMCU_Periph; 165 | } 166 | } 167 | 168 | /** 169 | * @} 170 | */ 171 | 172 | /** 173 | * @} 174 | */ 175 | 176 | /** 177 | * @} 178 | */ 179 | 180 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ 181 | -------------------------------------------------------------------------------- /Libraries/arm_cortexM4lf_math.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/suhetao/stm32f4_mpu9250/b86160c2b781f16973cd20293c67c5fab25f54f4/Libraries/arm_cortexM4lf_math.zip -------------------------------------------------------------------------------- /Libraries/eMPL/inv_mpu.h: -------------------------------------------------------------------------------- 1 | /* 2 | $License: 3 | Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. 4 | See included License.txt for License information. 5 | $ 6 | */ 7 | /** 8 | * @addtogroup DRIVERS Sensor Driver Layer 9 | * @brief Hardware drivers to communicate with sensors via I2C. 10 | * 11 | * @{ 12 | * @file inv_mpu.h 13 | * @brief An I2C-based driver for Invensense gyroscopes. 14 | * @details This driver currently works for the following devices: 15 | * MPU6050 16 | * MPU6500 17 | * MPU9150 (or MPU6050 w/ AK8975 on the auxiliary bus) 18 | * MPU9250 (or MPU6500 w/ AK8963 on the auxiliary bus) 19 | */ 20 | 21 | #ifndef _INV_MPU_H_ 22 | #define _INV_MPU_H_ 23 | 24 | #define INV_X_GYRO (0x40) 25 | #define INV_Y_GYRO (0x20) 26 | #define INV_Z_GYRO (0x10) 27 | #define INV_XYZ_GYRO (INV_X_GYRO | INV_Y_GYRO | INV_Z_GYRO) 28 | #define INV_XYZ_ACCEL (0x08) 29 | #define INV_XYZ_COMPASS (0x01) 30 | 31 | struct int_param_s { 32 | #if defined EMPL_TARGET_MSP430 || defined MOTION_DRIVER_TARGET_MSP430 || STM32F40_41xxx 33 | void (*cb)(void); 34 | unsigned short pin; 35 | unsigned char lp_exit; 36 | unsigned char active_low; 37 | #elif defined EMPL_TARGET_UC3L0 38 | unsigned long pin; 39 | void (*cb)(volatile void*); 40 | void *arg; 41 | #endif 42 | }; 43 | 44 | #define MPU_INT_STATUS_DATA_READY (0x0001) 45 | #define MPU_INT_STATUS_DMP (0x0002) 46 | #define MPU_INT_STATUS_PLL_READY (0x0004) 47 | #define MPU_INT_STATUS_I2C_MST (0x0008) 48 | #define MPU_INT_STATUS_FIFO_OVERFLOW (0x0010) 49 | #define MPU_INT_STATUS_ZMOT (0x0020) 50 | #define MPU_INT_STATUS_MOT (0x0040) 51 | #define MPU_INT_STATUS_FREE_FALL (0x0080) 52 | #define MPU_INT_STATUS_DMP_0 (0x0100) 53 | #define MPU_INT_STATUS_DMP_1 (0x0200) 54 | #define MPU_INT_STATUS_DMP_2 (0x0400) 55 | #define MPU_INT_STATUS_DMP_3 (0x0800) 56 | #define MPU_INT_STATUS_DMP_4 (0x1000) 57 | #define MPU_INT_STATUS_DMP_5 (0x2000) 58 | 59 | /* Set up APIs */ 60 | int mpu_init(struct int_param_s *int_param); 61 | int mpu_init_slave(void); 62 | int mpu_set_bypass(unsigned char bypass_on); 63 | 64 | /* Configuration APIs */ 65 | int mpu_lp_accel_mode(unsigned char rate); 66 | int mpu_lp_motion_interrupt(unsigned short thresh, unsigned char time, 67 | unsigned char lpa_freq); 68 | int mpu_set_int_level(unsigned char active_low); 69 | int mpu_set_int_latched(unsigned char enable); 70 | 71 | int mpu_set_dmp_state(unsigned char enable); 72 | int mpu_get_dmp_state(unsigned char *enabled); 73 | 74 | int mpu_get_lpf(unsigned short *lpf); 75 | int mpu_set_lpf(unsigned short lpf); 76 | 77 | int mpu_get_gyro_fsr(unsigned short *fsr); 78 | int mpu_set_gyro_fsr(unsigned short fsr); 79 | 80 | int mpu_get_accel_fsr(unsigned char *fsr); 81 | int mpu_set_accel_fsr(unsigned char fsr); 82 | 83 | int mpu_get_compass_fsr(unsigned short *fsr); 84 | 85 | int mpu_get_gyro_sens(float *sens); 86 | int mpu_get_accel_sens(unsigned short *sens); 87 | 88 | int mpu_get_sample_rate(unsigned short *rate); 89 | int mpu_set_sample_rate(unsigned short rate); 90 | int mpu_get_compass_sample_rate(unsigned short *rate); 91 | int mpu_set_compass_sample_rate(unsigned short rate); 92 | 93 | int mpu_get_fifo_config(unsigned char *sensors); 94 | int mpu_configure_fifo(unsigned char sensors); 95 | 96 | int mpu_get_power_state(unsigned char *power_on); 97 | int mpu_set_sensors(unsigned char sensors); 98 | 99 | int mpu_read_6500_accel_bias(long *accel_bias); 100 | int mpu_set_gyro_bias_reg(long * gyro_bias); 101 | int mpu_set_accel_bias_6500_reg(const long *accel_bias); 102 | int mpu_read_6050_accel_bias(long *accel_bias); 103 | int mpu_set_accel_bias_6050_reg(const long *accel_bias); 104 | 105 | /* Data getter/setter APIs */ 106 | int mpu_get_gyro_reg(short *data, unsigned long *timestamp); 107 | int mpu_get_accel_reg(short *data, unsigned long *timestamp); 108 | int mpu_get_compass_reg(short *data, unsigned long *timestamp); 109 | int mpu_get_temperature(long *data, unsigned long *timestamp); 110 | 111 | int mpu_get_int_status(short *status); 112 | int mpu_read_fifo(short *gyro, short *accel, unsigned long *timestamp, 113 | unsigned char *sensors, unsigned char *more); 114 | int mpu_read_fifo_stream(unsigned short length, unsigned char *data, 115 | unsigned char *more); 116 | int mpu_reset_fifo(void); 117 | 118 | int mpu_write_mem(unsigned short mem_addr, unsigned short length, 119 | unsigned char *data); 120 | int mpu_read_mem(unsigned short mem_addr, unsigned short length, 121 | unsigned char *data); 122 | int mpu_load_firmware(unsigned short length, const unsigned char *firmware, 123 | unsigned short start_addr, unsigned short sample_rate); 124 | 125 | int mpu_reg_dump(void); 126 | int mpu_read_reg(unsigned char reg, unsigned char *data); 127 | int mpu_run_self_test(long *gyro, long *accel); 128 | int mpu_run_6500_self_test(long *gyro, long *accel, unsigned char debug); 129 | int mpu_register_tap_cb(void (*func)(unsigned char, unsigned char)); 130 | 131 | #endif /* #ifndef _INV_MPU_H_ */ 132 | 133 | -------------------------------------------------------------------------------- /Libraries/eMPL/inv_mpu_dmp_motion_driver.h: -------------------------------------------------------------------------------- 1 | /* 2 | $License: 3 | Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. 4 | See included License.txt for License information. 5 | $ 6 | */ 7 | /** 8 | * @addtogroup DRIVERS Sensor Driver Layer 9 | * @brief Hardware drivers to communicate with sensors via I2C. 10 | * 11 | * @{ 12 | * @file inv_mpu_dmp_motion_driver.h 13 | * @brief DMP image and interface functions. 14 | * @details All functions are preceded by the dmp_ prefix to 15 | * differentiate among MPL and general driver function calls. 16 | */ 17 | #ifndef _INV_MPU_DMP_MOTION_DRIVER_H_ 18 | #define _INV_MPU_DMP_MOTION_DRIVER_H_ 19 | 20 | #define TAP_X (0x01) 21 | #define TAP_Y (0x02) 22 | #define TAP_Z (0x04) 23 | #define TAP_XYZ (0x07) 24 | 25 | #define TAP_X_UP (0x01) 26 | #define TAP_X_DOWN (0x02) 27 | #define TAP_Y_UP (0x03) 28 | #define TAP_Y_DOWN (0x04) 29 | #define TAP_Z_UP (0x05) 30 | #define TAP_Z_DOWN (0x06) 31 | 32 | #define ANDROID_ORIENT_PORTRAIT (0x00) 33 | #define ANDROID_ORIENT_LANDSCAPE (0x01) 34 | #define ANDROID_ORIENT_REVERSE_PORTRAIT (0x02) 35 | #define ANDROID_ORIENT_REVERSE_LANDSCAPE (0x03) 36 | 37 | #define DMP_INT_GESTURE (0x01) 38 | #define DMP_INT_CONTINUOUS (0x02) 39 | 40 | #define DMP_FEATURE_TAP (0x001) 41 | #define DMP_FEATURE_ANDROID_ORIENT (0x002) 42 | #define DMP_FEATURE_LP_QUAT (0x004) 43 | #define DMP_FEATURE_PEDOMETER (0x008) 44 | #define DMP_FEATURE_6X_LP_QUAT (0x010) 45 | #define DMP_FEATURE_GYRO_CAL (0x020) 46 | #define DMP_FEATURE_SEND_RAW_ACCEL (0x040) 47 | #define DMP_FEATURE_SEND_RAW_GYRO (0x080) 48 | #define DMP_FEATURE_SEND_CAL_GYRO (0x100) 49 | 50 | #define INV_WXYZ_QUAT (0x100) 51 | 52 | /* Set up functions. */ 53 | int dmp_load_motion_driver_firmware(void); 54 | int dmp_set_fifo_rate(unsigned short rate); 55 | int dmp_get_fifo_rate(unsigned short *rate); 56 | int dmp_enable_feature(unsigned short mask); 57 | int dmp_get_enabled_features(unsigned short *mask); 58 | int dmp_set_interrupt_mode(unsigned char mode); 59 | int dmp_set_orientation(unsigned short orient); 60 | int dmp_set_gyro_bias(long *bias); 61 | int dmp_set_accel_bias(long *bias); 62 | 63 | /* Tap functions. */ 64 | int dmp_register_tap_cb(void (*func)(unsigned char, unsigned char)); 65 | int dmp_set_tap_thresh(unsigned char axis, unsigned short thresh); 66 | int dmp_set_tap_axes(unsigned char axis); 67 | int dmp_set_tap_count(unsigned char min_taps); 68 | int dmp_set_tap_time(unsigned short time); 69 | int dmp_set_tap_time_multi(unsigned short time); 70 | int dmp_set_shake_reject_thresh(long sf, unsigned short thresh); 71 | int dmp_set_shake_reject_time(unsigned short time); 72 | int dmp_set_shake_reject_timeout(unsigned short time); 73 | 74 | /* Android orientation functions. */ 75 | int dmp_register_android_orient_cb(void (*func)(unsigned char)); 76 | 77 | /* LP quaternion functions. */ 78 | int dmp_enable_lp_quat(unsigned char enable); 79 | int dmp_enable_6x_lp_quat(unsigned char enable); 80 | 81 | /* Pedometer functions. */ 82 | int dmp_get_pedometer_step_count(unsigned long *count); 83 | int dmp_set_pedometer_step_count(unsigned long count); 84 | int dmp_get_pedometer_walk_time(unsigned long *time); 85 | int dmp_set_pedometer_walk_time(unsigned long time); 86 | 87 | /* DMP gyro calibration functions. */ 88 | int dmp_enable_gyro_cal(unsigned char enable); 89 | 90 | /* Read function. This function should be called whenever the MPU interrupt is 91 | * detected. 92 | */ 93 | int dmp_read_fifo(short *gyro, short *accel, long *quat, 94 | unsigned long *timestamp, short *sensors, unsigned char *more); 95 | 96 | #endif /* #ifndef _INV_MPU_DMP_MOTION_DRIVER_H_ */ 97 | 98 | -------------------------------------------------------------------------------- /Math/inc/FastMath.h: -------------------------------------------------------------------------------- 1 | /* 2 | The MIT License (MIT) 3 | 4 | Copyright (c) 2015-? suhetao 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | this software and associated documentation files (the "Software"), to deal in 8 | the Software without restriction, including without limitation the rights to 9 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | the Software, and to permit persons to whom the Software is furnished to do so, 11 | subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | #ifndef _FASTMATH_H_ 25 | #define _FASTMATH_H_ 26 | 27 | #include "Double.h" 28 | 29 | #define ROOT_HALF (0.70710678118654752440084436210485f) 30 | #define LOGA_COEF0 (-4.649062303464e-1f) 31 | #define LOGA_COEF1 (+1.360095468621e-2f) 32 | #define LOGB_COEF0 (-5.578873750242f) 33 | 34 | #define LOGDA_COEF0 (-6.4124943423745581147e1f) 35 | #define LOGDA_COEF1 (+1.6383943563021534222e1f) 36 | #define LOGDA_COEF2 (-7.8956112887491257267e-1f) 37 | #define LOGDB_COEF0 (-7.6949932108494879777e2f) 38 | #define LOGDB_COEF1 (+3.1203222091924532844e2f) 39 | #define LOGDB_COEF2 (-3.5667977739034646171e1f) 40 | #define LN2_DC1 (0.693359375f) 41 | #define LN2_DC2 (-2.121944400e-4f) 42 | #define LN2_DC3 (-5.46905827678e-14f) 43 | 44 | float FastLn(float x); 45 | ////////////////////////////////////////////////////////////////////////// 46 | ///Coefficients used for pow 47 | #define POWP_COEF1 (+8.33333286245e-2f) 48 | #define POWP_COEF2 (+1.25064850052e-2f) 49 | #define POWQ_COEF1 (+6.93147180556341e-1f) 50 | #define POWQ_COEF2 (+2.40226506144710e-1f) 51 | #define POWQ_COEF3 (+5.55040488130765e-2f) 52 | #define POWQ_COEF4 (+9.61620659583789e-3f) 53 | #define POWQ_COEF5 (+1.30525515942810e-3f) 54 | 55 | #define POW_BIGNUM (+2046.0f) 56 | #define POW_SMALLNUM (-2015.0f) 57 | 58 | #define LOG2E_MINUS1 (0.44269504088896340735992468100189f) 59 | // 60 | #define FLT_EPSILON 1.1920928955078125E-07F 61 | #define FLT_MAX 3.4028234663852886E+38F 62 | // 63 | float FastPow(float x,float y); 64 | ////////////////////////////////////////////////////////////////////////// 65 | // 66 | #define X_MAX (+9.099024257348e3f) 67 | #define INV_PI_2 ( 0.63661977236758134307553505349006f) 68 | #define PI_2_C1 ( 1.5703125f) 69 | #define PI_2_C2 ( 4.84466552734375e-4f) 70 | #define PI_2_C3 (-6.39757837755768678308360248557e-7f) 71 | 72 | #define TANP_COEF1 (-1.113614403566e-1f) 73 | #define TANP_COEF2 (+1.075154738488e-3f) 74 | #define TANQ_COEF0 (+1.000000000000f) 75 | #define TANQ_COEF1 (-4.446947720281e-1f) 76 | #define TANQ_COEF2 (+1.597339213300e-2f) 77 | float FastTan(float x); 78 | ////////////////////////////////////////////////////////////////////////// 79 | 80 | #define _2_PI 6.283185307179586476925286766559f 81 | #define RADTODEG(x) ((x) * 57.295779513082320876798154814105f) 82 | #define DEGTORAD(x) ((x) * 0.01745329251994329576923690768489f) 83 | 84 | //translate from the DSP instruction of a DSP Library. 85 | #ifndef PI 86 | #define PI (3.1415926535897932384626433832795f) 87 | #endif 88 | #define PI_2 (1.5707963267948966192313216916398f) 89 | #define PI_3 (1.0471975511965977461542144610932f) 90 | #define PI_4 (0.78539816339744830961566084581988f) 91 | #define PI_6 (0.52359877559829887307710723054658f) 92 | #define TWO_MINUS_ROOT3 (0.26794919243112270647255365849413f) 93 | #define SQRT3_MINUS_1 (0.73205080756887729352744634150587f) 94 | #define SQRT3 (1.7320508075688772935274463415059f) 95 | #define EPS_FLOAT (+3.452669830012e-4f) 96 | //Coefficients used for atan/atan2 97 | #define ATANP_COEF0 (-1.44008344874f) 98 | #define ATANP_COEF1 (-7.20026848898e-1f) 99 | #define ATANQ_COEF0 (+4.32025038919f) 100 | #define ATANQ_COEF1 (+4.75222584599f) 101 | //Coefficients used for asin/acos 102 | #define ASINP_COEF1 (-2.7516555290596f) 103 | #define ASINP_COEF2 (+2.9058762374859f) 104 | #define ASINP_COEF3 (-5.9450144193246e-1f) 105 | #define ASINQ_COEF0 (-1.6509933202424e+1f) 106 | #define ASINQ_COEF1 (+2.4864728969164e+1f) 107 | #define ASINQ_COEF2 (-1.0333867072113e+1f) 108 | 109 | float FastAsin(float x); 110 | float FastAtan2(float y, float x); 111 | 112 | float FastSqrtI(float x); 113 | float FastSqrt(float x); 114 | 115 | float FastSin(float x); 116 | float FastCos(float x); 117 | void FastSinCos(float x, float *sinVal, float *cosVal); 118 | 119 | __inline float FastAbs(float x){ 120 | union { unsigned int i; float f;} y; 121 | y.f = x; 122 | y.i = y.i & 0x7FFFFFFF; 123 | return (float)y.f; 124 | } 125 | 126 | __inline double FastAbsD(double x){ 127 | union { unsigned __int64 i; double d;} y; 128 | y.d = x; 129 | y.i = y.i & 0x7FFFFFFFFFFFFFFFLL; 130 | return (double)y.d; 131 | } 132 | 133 | ////////////////////////////////////////////////////////////////////////// 134 | Double FastSqrtID(Double dx); 135 | Double FastSqrtD(Double dx); 136 | 137 | #endif 138 | -------------------------------------------------------------------------------- /Matrix/inc/DoubleMatrix.h: -------------------------------------------------------------------------------- 1 | /* 2 | The MIT License (MIT) 3 | 4 | Copyright (c) 2015-? suhetao 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | this software and associated documentation files (the "Software"), to deal in 8 | the Software without restriction, including without limitation the rights to 9 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | the Software, and to permit persons to whom the Software is furnished to do so, 11 | subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | #ifndef _DOUBLEMATRIX_H_ 25 | #define _DOUBLEMATRIX_H_ 26 | 27 | #include "Double.h" 28 | 29 | #endif 30 | -------------------------------------------------------------------------------- /Matrix/inc/Matrix.h: -------------------------------------------------------------------------------- 1 | /* 2 | The MIT License (MIT) 3 | 4 | Copyright (c) 2015-? suhetao 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | this software and associated documentation files (the "Software"), to deal in 8 | the Software without restriction, including without limitation the rights to 9 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | the Software, and to permit persons to whom the Software is furnished to do so, 11 | subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | #ifndef _MATRIX_H_ 25 | #define _MATRIX_H_ 26 | 27 | #include "arm_math.h" 28 | 29 | void arm_mat_zero_f32(arm_matrix_instance_f32* s); 30 | arm_status mat_identity(float32_t *pData, uint16_t numRows, uint16_t numCols, float32_t value); 31 | arm_status arm_mat_identity_f32(arm_matrix_instance_f32* s, float32_t value); 32 | arm_status arm_mat_fill_f32(arm_matrix_instance_f32* s, float32_t *pData, uint32_t blockSize); 33 | arm_status arm_mat_chol_f32(arm_matrix_instance_f32* s); 34 | arm_status arm_mat_remainlower_f32(arm_matrix_instance_f32* s); 35 | 36 | void arm_mat_getsubmatrix_f32(arm_matrix_instance_f32* s, arm_matrix_instance_f32 *a, int row, int col); 37 | void arm_mat_setsubmatrix_f32(arm_matrix_instance_f32* a, arm_matrix_instance_f32 *s, int row, int col); 38 | 39 | void arm_mat_getcolumn_f32(arm_matrix_instance_f32* s, float32_t *x, uint32_t col); 40 | void arm_mat_setcolumn_f32(arm_matrix_instance_f32* s, float32_t *x, uint32_t col); 41 | void arm_mat_cumsum_f32(arm_matrix_instance_f32* s, float32_t *tmp, float32_t *x); 42 | int arm_mat_qr_decompositionT_f32(arm_matrix_instance_f32 *A, arm_matrix_instance_f32 *R); 43 | 44 | #endif 45 | -------------------------------------------------------------------------------- /Matrix/src/DoubleMatrix.c: -------------------------------------------------------------------------------- 1 | /* 2 | The MIT License (MIT) 3 | 4 | Copyright (c) 2015-? suhetao 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | this software and associated documentation files (the "Software"), to deal in 8 | the Software without restriction, including without limitation the rights to 9 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | the Software, and to permit persons to whom the Software is furnished to do so, 11 | subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | #include "DoubleMatrix.h" 25 | #include "FastMath.h" 26 | 27 | -------------------------------------------------------------------------------- /Project/JLinkSettings.ini: -------------------------------------------------------------------------------- 1 | [BREAKPOINTS] 2 | ForceImpTypeAny = 0 3 | ShowInfoWin = 1 4 | EnableFlashBP = 2 5 | BPDuringExecution = 0 6 | [CFI] 7 | CFISize = 0x00 8 | CFIAddr = 0x00 9 | [CPU] 10 | OverrideMemMap = 0 11 | AllowSimulation = 1 12 | ScriptFile="" 13 | [FLASH] 14 | CacheExcludeSize = 0x00 15 | CacheExcludeAddr = 0x00 16 | MinNumBytesFlashDL = 0 17 | SkipProgOnCRCMatch = 1 18 | VerifyDownload = 1 19 | AllowCaching = 1 20 | EnableFlashDL = 2 21 | Override = 0 22 | Device="UNSPECIFIED" 23 | [GENERAL] 24 | WorkRAMSize = 0x00 25 | WorkRAMAddr = 0x00 26 | RAMUsageLimit = 0x00 27 | [SWO] 28 | SWOLogFile="" 29 | [MEM] 30 | RdOverrideOrMask = 0x00 31 | RdOverrideAndMask = 0xFFFFFFFF 32 | RdOverrideAddr = 0xFFFFFFFF 33 | WrOverrideOrMask = 0x00 34 | WrOverrideAndMask = 0xFFFFFFFF 35 | WrOverrideAddr = 0xFFFFFFFF 36 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # stm32f4_mpu9250 2 | Access the data of 3-axis magnetometer and DMP from MPU9250 with SPI interface 3 | 4 | All data fusion (including the data of dmp output, such as the accelerometer data, 5 | gyroscope, 6-axis quaternion and internal magnetometer data) via a 7-state, 13-mesurement 6 | EKF(Extended Kalman filter) / Unscented Kalman Filter(UKF) / Cubature Kalman Filters (CKF) Algorithm / 7 | Square-Root Cubature Kalman Filters (SRCKF) Algorithm. 8 | 9 | 1.kalman feature: 10 | 11 | prediction state: q0 q1 q2 q3 wx wy wz 12 | mesurement:q0 q1 q2 q3(q0~q3 from dmp ouput) ax ay az wx wy wz mx my mz 13 | 14 | 2.Add a miniIMU for doctor's miniQuadrotor 15 | 16 | add a new fixed-point ekf algorithm for doctor's miniQuadrotor 17 | please check the miniIMU directory at the root and look the usage.txt for using! 18 | 19 | 3.Add a win32 application for serial port communication 20 | 21 | calibraion for accelerometer, gyroscope, magnetometer 22 | please check the "Calibration App" directory at the root 23 | 24 | 4.Add a miniAHRS for 9-axis fusion, such as ax ay az wx wy wz mx my mz form accelerometer,gyroscope,magnetometer 25 | 26 | 7-state ekf algorithm: quaternion and 3-axis gyroscope bais 27 | 3-mesurement for accelerometer 28 | 3-mesurement for magnetometer 29 | 30 | 5.Add a GPS/ins implement via ekf fusion algorithm 31 | 32 | 16-state ekf algorithm: 4 quaternion, 3D postion, 3D velocity, 3-axis gyroscope bais and 3-axis accelerometer bais 33 | 9-mesurement for magnetometer, 3D postion and 3D velocity from gps and barometer 34 | 35 | 6.Other stuff 36 | 37 | add ms5611 spi driver 38 | using 4-order Runge_Kutta to slove the quaternion differential equation. 39 | 40 | 7.Add a 9-axis sensor fusion with square-root cubature Kalman filter for testing 41 | 42 | 7-state srckf algorithm: quaternion and 3-axis gyroscope bais 43 | 6-mesurement for accelerometer, magnetometer ouput 44 | 45 | 8.add ublox-m8n uart4 driver 46 | 47 | 9.Emulated double precision Double single routine -------------------------------------------------------------------------------- /miniAHRS/miniAHRS.h: -------------------------------------------------------------------------------- 1 | /* 2 | The MIT License (MIT) 3 | 4 | Copyright (c) 2015-? suhetao 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | this software and associated documentation files (the "Software"), to deal in 8 | the Software without restriction, including without limitation the rights to 9 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | the Software, and to permit persons to whom the Software is furnished to do so, 11 | subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | #ifndef MINIAHRS_H_ 25 | #define MINIAHRS_H_ 26 | ////////////////////////////////////////////////////////////////////////// 27 | // 28 | #define EKF_STATE_DIM 7 //q0 q1 q2 q3 wxb wyb wzb 29 | #define EKF_MEASUREMENT_DIM 6 //ax ay az and mx my mz 30 | 31 | #define EKF_HALFPI 1.5707963267948966192313216916398f 32 | #define EKF_PI 3.1415926535897932384626433832795f 33 | #define EKF_TWOPI 6.283185307179586476925286766559f 34 | #define EKF_TODEG(x) ((x) * 57.2957796f) 35 | 36 | void EKF_AHRSInit(float *accel, float *mag); 37 | void EKF_AHRSUpdate(float *gyro, float *accel, float *mag, float dt); 38 | void EKF_AHRSGetAngle(float* rpy); 39 | 40 | void EKF_AHRSGetQ(float* Q); 41 | 42 | #endif 43 | -------------------------------------------------------------------------------- /miniIMU/FP/FP_Math.h: -------------------------------------------------------------------------------- 1 | /* 2 | The MIT License (MIT) 3 | 4 | Copyright (c) 2015-? suhetao 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | this software and associated documentation files (the "Software"), to deal in 8 | the Software without restriction, including without limitation the rights to 9 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | the Software, and to permit persons to whom the Software is furnished to do so, 11 | subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | #ifndef _FP_FASTMATH_H_ 25 | #define _FP_FASTMATH_H_ 26 | 27 | #define FIXED_POINT 28 | #define PRECISION 16 29 | 30 | #define OPTIMIZE_SDIV 31 | 32 | typedef int Q16; 33 | typedef int int32_t; 34 | typedef unsigned int uint32_t; 35 | typedef unsigned char uint8_t; 36 | typedef unsigned short uint16_t; 37 | typedef signed __int64 int64_t; 38 | typedef unsigned __int64 uint64_t; 39 | 40 | #define Q16_One 65536 41 | 42 | ////////////////////////////////////////////////////////////////////////// 43 | //S16.16 44 | #define FP_ADDS(a, b) ((a) + (b)) 45 | #define FP_SUBS(a, b) ((a) - (b)) 46 | 47 | #define FP_SMUL_FRAC(a, b, frac) \ 48 | ((int32_t)((int64_t)(a)*(int64_t)(b) >> (frac))) 49 | 50 | #define FP_UMUL_FRAC(a, b, frac) \ 51 | ((uint32_t)((uint64_t)(a)*(uint64_t)(b) >> (frac))) 52 | 53 | #define FP_SMUL(a, b) \ 54 | ((int32_t)((int64_t)(a)*(int64_t)(b) >> 16)) 55 | 56 | #define FP_UMUL(a, b) \ 57 | ((uint32_t)((uint64_t)(a)*(uint64_t)(b) >> 16)) 58 | 59 | Q16 FP_SDIV(Q16 a, Q16 b); 60 | 61 | ////////////////////////////////////////////////////////////////////////// 62 | #define FP_ISQRT_UPDATE(est, mant2) \ 63 | (((est) + ((est) >> 1)) - \ 64 | FP_UMUL_FRAC(FP_UMUL_FRAC(mant2, est, 31), \ 65 | FP_UMUL_FRAC(est, est, 31), 31)) 66 | 67 | Q16 FT_Q16(float f); 68 | 69 | Q16 FP_Sqrt(Q16 xval, uint32_t frac); 70 | Q16 FP_SqrtI(Q16 xval, int32_t frac); 71 | 72 | __inline float Q16_FT(Q16 x) 73 | { 74 | return ((float)x) / 65536.0f; 75 | } 76 | ////////////////////////////////////////////////////////////////////////// 77 | //translate from google's skia fixed-point dsp Library. 78 | // 79 | #define TO_FLOAT_DEGREE(x) (((float)(x)) * 0.000874264215087890625f) 80 | #define Q16_TWOPI 411775 81 | #define Q16_PI 205887 82 | #define Q16_HALFPI 102944 83 | 84 | Q16 FP_FastAsin(Q16 a); 85 | Q16 FP_FastAtan2(Q16 y, Q16 x); 86 | 87 | #endif 88 | -------------------------------------------------------------------------------- /miniIMU/FP/FP_Matrix.h: -------------------------------------------------------------------------------- 1 | /* 2 | The MIT License (MIT) 3 | 4 | Copyright (c) 2015-? suhetao 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | this software and associated documentation files (the "Software"), to deal in 8 | the Software without restriction, including without limitation the rights to 9 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | the Software, and to permit persons to whom the Software is furnished to do so, 11 | subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | #ifndef _FP_MAXTRIX_H_ 25 | #define _FP_MAXTRIX_H_ 26 | 27 | #include "FP_Math.h" 28 | ////////////////////////////////////////////////////////////////////////// 29 | // 30 | void FP_Matrix_Copy(Q16 *pSrc, int numRows, int numCols, Q16 *pDst); 31 | int FP_Maxtrix_Add(Q16 *pSrcA, unsigned short numRows, unsigned short numCols, Q16 *pSrcB, Q16 *pDst); 32 | int FP_Maxtrix_Sub(Q16 *pSrcA, unsigned short numRows, unsigned short numCols, Q16 *pSrcB, Q16 *pDst); 33 | int FP_Matrix_Multiply(Q16 *A, int nrows, int ncols, Q16 *B, int mcols, Q16 *C); 34 | void FP_Matrix_Multiply_With_Transpose(Q16 *A, int nrows, int ncols, Q16 *B, int mrows, Q16 *C); 35 | int FP_Matrix_Inverse(Q16* pSrc, unsigned short n, Q16* pDst); 36 | #endif 37 | -------------------------------------------------------------------------------- /miniIMU/FP/FP_miniIMU.h: -------------------------------------------------------------------------------- 1 | /* 2 | The MIT License (MIT) 3 | 4 | Copyright (c) 2015-? suhetao 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | this software and associated documentation files (the "Software"), to deal in 8 | the Software without restriction, including without limitation the rights to 9 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | the Software, and to permit persons to whom the Software is furnished to do so, 11 | subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | #ifndef _FP_MINIIMU_H_ 25 | #define _FP_MINIIMU_H_ 26 | 27 | ////////////////////////////////////////////////////////////////////////// 28 | //S16.16 29 | //#define FP_EKF_STATE_DIM 4 //q0 q1 q2 q3 30 | #define FP_EKF_STATE_DIM 7 //q0 q1 q2 q3 wxb wyb wzb 31 | #define FP_EKF_MEASUREMENT_DIM 3 //ax ay az 32 | 33 | void FP_EKF_IMUInit(float *accel, float *gyro); 34 | void FP_EKF_IMUUpdate(float *gyro, float *accel, float dt); 35 | void FP_EKF_IMUGetAngle(float* rpy); 36 | 37 | #endif 38 | -------------------------------------------------------------------------------- /miniIMU/Usage.txt: -------------------------------------------------------------------------------- 1 | /* 2 | The MIT License (MIT) 3 | 4 | Copyright (c) 2015-? suhetao 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | this software and associated documentation files (the "Software"), to deal in 8 | the Software without restriction, including without limitation the rights to 9 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | the Software, and to permit persons to whom the Software is furnished to do so, 11 | subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | ////////////////////////////////////////////////////////////////////////// 25 | //setp 1: 26 | Decide how many states do you want to use. 27 | Look into the filename of "miniIMU.h" 28 | 29 | either uncomment #define EKF_STATE_DIM 4 30 | or uncomment #define EKF_STATE_DIM 7 on your own, 31 | by default is "define EKF_STATE_DIM 7" 32 | 33 | ////////////////////////////////////////////////////////////////////////// 34 | //setp 2: 35 | // 36 | using the struct as below,In the main loop of the program 37 | ...... 38 | if EKF is not initialization 39 | EKF_IMUInit(accel, gyro); 40 | else 41 | EKF_IMUUpdate(gyro, accel, dt); 42 | ...... 43 | EKF_IMUGetAngle(rpy); 44 | 45 | 46 | Enjoy it! -------------------------------------------------------------------------------- /miniIMU/miniIMU.h: -------------------------------------------------------------------------------- 1 | /* 2 | The MIT License (MIT) 3 | 4 | Copyright (c) 2015-? suhetao 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | this software and associated documentation files (the "Software"), to deal in 8 | the Software without restriction, including without limitation the rights to 9 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | the Software, and to permit persons to whom the Software is furnished to do so, 11 | subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | #ifndef MINIIMU_H_ 25 | #define MINIIMU_H_ 26 | ////////////////////////////////////////////////////////////////////////// 27 | // 28 | //#define EKF_STATE_DIM 4 //q0 q1 q2 q3 29 | #define EKF_STATE_DIM 7 //q0 q1 q2 q3 wxb wyb wzb 30 | #define EKF_MEASUREMENT_DIM 3 //ax ay az 31 | 32 | #define EKF_HALFPI 1.5707963267948966192313216916398f 33 | #define EKF_PI 3.1415926535897932384626433832795f 34 | #define EKF_TWOPI 6.283185307179586476925286766559f 35 | #define EKF_TODEG(x) ((x) * 57.2957796f) 36 | 37 | void EKF_IMUInit(float *accel, float *gyro); 38 | void EKF_IMUUpdate(float *gyro, float *accel, float dt); 39 | void EKF_IMUGetAngle(float* rpy); 40 | 41 | void EKF_IMUGetQ(float *Q); 42 | 43 | #endif 44 | -------------------------------------------------------------------------------- /miniIMU/miniMatrix.h: -------------------------------------------------------------------------------- 1 | /* 2 | The MIT License (MIT) 3 | 4 | Copyright (c) 2015-? suhetao 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy of 7 | this software and associated documentation files (the "Software"), to deal in 8 | the Software without restriction, including without limitation the rights to 9 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 10 | the Software, and to permit persons to whom the Software is furnished to do so, 11 | subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 18 | FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 19 | COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 20 | IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 21 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 22 | */ 23 | 24 | #ifndef _MAXTRIX_H_ 25 | #define _MAXTRIX_H_ 26 | 27 | #include "FastMath.h" 28 | 29 | ////////////////////////////////////////////////////////////////////////// 30 | // 31 | 32 | void Matrix_Zero(float *A, unsigned short numRows, unsigned short numCols); 33 | void Matrix_Copy(float *pSrc, unsigned short numRows, unsigned short numCols, float *pDst); 34 | int Maxtrix_Add(float *pSrcA, unsigned short numRows, unsigned short numCols, float *pSrcB, float *pDst); 35 | int Maxtrix_Sub(float *pSrcA, unsigned short numRows, unsigned short numCols, float *pSrcB, float *pDst); 36 | int Matrix_Multiply(float* pSrcA, unsigned short numRowsA, unsigned short numColsA, float* pSrcB, unsigned short numColsB, float* pDst); 37 | void Matrix_Multiply_With_Transpose(float *A, unsigned short nrows, unsigned short ncols, float *B, unsigned short mrows, float *C); 38 | void Maxtrix_Transpose(float *pSrc, unsigned short nRows, unsigned short nCols, float *pDst); 39 | int Matrix_Inverse(float* pDst, unsigned short n, float * pSrc); 40 | 41 | #endif 42 | --------------------------------------------------------------------------------