semin ahn / Mbed OS zeta_stm_kinetic

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   BufferedSerial

MPU9250/MPU9250_SPI.h@0:4ff8aeb3e4d1, 2021-04-02 (annotated)

Committer:
_seminahn
Date:
Fri Apr 02 05:24:49 2021 +0000
Revision:
0:4ff8aeb3e4d1
top_module

Who changed what in which revision?

UserRevisionLine numberNew contents of line
_seminahn 0:4ff8aeb3e4d1 1 /*
_seminahn 0:4ff8aeb3e4d1 2 * 20201024 - (MBED code) MPU-9250 AHRS
_seminahn 0:4ff8aeb3e4d1 3 *
_seminahn 0:4ff8aeb3e4d1 4 */
_seminahn 0:4ff8aeb3e4d1 5 #ifndef MPU9250_SPI_
_seminahn 0:4ff8aeb3e4d1 6 #define MPU9250_SPI_
_seminahn 0:4ff8aeb3e4d1 7
_seminahn 0:4ff8aeb3e4d1 8 #include "mbed.h"
_seminahn 0:4ff8aeb3e4d1 9 #include <cmath>
_seminahn 0:4ff8aeb3e4d1 10 #include "MPU9250/MPU9250RegisterMap.h"
_seminahn 0:4ff8aeb3e4d1 11 #include "MPU9250/MadgwickAHRS.h"
_seminahn 0:4ff8aeb3e4d1 12
_seminahn 0:4ff8aeb3e4d1 13 //#define PRINT_DETAILS
_seminahn 0:4ff8aeb3e4d1 14 #define DT 0.01f
_seminahn 0:4ff8aeb3e4d1 15 #define fc 0.5f
_seminahn 0:4ff8aeb3e4d1 16 #define Tc (1./(2.*M_PI*fc))
_seminahn 0:4ff8aeb3e4d1 17 #define ALPHA (Tc/(Tc+DT))
_seminahn 0:4ff8aeb3e4d1 18 #define BETA (1-ALPHA)
_seminahn 0:4ff8aeb3e4d1 19
_seminahn 0:4ff8aeb3e4d1 20 #if (NO_ROS)
_seminahn 0:4ff8aeb3e4d1 21 extern mbed::Serial pc;
_seminahn 0:4ff8aeb3e4d1 22 #endif
_seminahn 0:4ff8aeb3e4d1 23
_seminahn 0:4ff8aeb3e4d1 24 enum GyroRange { GYRO_RANGE_250DPS, GYRO_RANGE_500DPS, GYRO_RANGE_1000DPS, GYRO_RANGE_2000DPS };
_seminahn 0:4ff8aeb3e4d1 25 enum AccelRange { ACCEL_RANGE_2G, ACCEL_RANGE_4G, ACCEL_RANGE_8G, ACCEL_RANGE_16G };
_seminahn 0:4ff8aeb3e4d1 26 enum DlpfBandwidth { DLPF_BANDWIDTH_250HZ, DLPF_BANDWIDTH_184HZ, DLPF_BANDWIDTH_92HZ, DLPF_BANDWIDTH_41HZ,
_seminahn 0:4ff8aeb3e4d1 27 DLPF_BANDWIDTH_20HZ, DLPF_BANDWIDTH_10HZ, DLPF_BANDWIDTH_5HZ };
_seminahn 0:4ff8aeb3e4d1 28 enum MagnBits { MGN_14BITS, MGN_16BITS }; // CNTL1 offset 4 0:14bits, 1:16bits
_seminahn 0:4ff8aeb3e4d1 29 enum MagnMode { MGN_POWER_DN=0, MGN_SNGL_MEAS=1, MGN_CONT_MEAS1=2,MGN_CONT_MEAS2=6,MGN_EX_TRIG=4,
_seminahn 0:4ff8aeb3e4d1 30 MGN_SELF_TEST=8, MGN_FUSE_ROM=15}; // CNTL1 offset 0
_seminahn 0:4ff8aeb3e4d1 31 enum SampleRate {SR_1000HZ=0, SR_200HZ=4, SR_100HZ=9 }; // 1kHz/(1+SRD)
_seminahn 0:4ff8aeb3e4d1 32 enum FusionMethod { COMPLEMENTARY, MADGWICK};
_seminahn 0:4ff8aeb3e4d1 33
_seminahn 0:4ff8aeb3e4d1 34 struct Vect3 {float x,y,z;};
_seminahn 0:4ff8aeb3e4d1 35
_seminahn 0:4ff8aeb3e4d1 36 class MPU9250_SPI
_seminahn 0:4ff8aeb3e4d1 37 {
_seminahn 0:4ff8aeb3e4d1 38 float aRes ; // 가속도 (LSB 당의 값)
_seminahn 0:4ff8aeb3e4d1 39 float gRes ; // 자이로 (LSB 당의 값)
_seminahn 0:4ff8aeb3e4d1 40 float mRes ; // 지자기 (LSB 당의 값)
_seminahn 0:4ff8aeb3e4d1 41 AccelRange _accelRange;
_seminahn 0:4ff8aeb3e4d1 42 GyroRange _gyroRange;
_seminahn 0:4ff8aeb3e4d1 43 DlpfBandwidth _bandwidth;
_seminahn 0:4ff8aeb3e4d1 44 MagnMode _mMode;
_seminahn 0:4ff8aeb3e4d1 45 MagnBits _mBits;
_seminahn 0:4ff8aeb3e4d1 46 SampleRate _srd;
_seminahn 0:4ff8aeb3e4d1 47 Vect3 magCalibration ; // factory mag calibration
_seminahn 0:4ff8aeb3e4d1 48 Vect3 magBias ; //지자기 바이어스
_seminahn 0:4ff8aeb3e4d1 49 Vect3 magScale ; // 지자기 스케일
_seminahn 0:4ff8aeb3e4d1 50 Vect3 gyroBias ; // 자이로 바이어스
_seminahn 0:4ff8aeb3e4d1 51 Vect3 accelBias ; // 가속도 바이어스
_seminahn 0:4ff8aeb3e4d1 52 int16_t tempCount; // 원시 온도값
_seminahn 0:4ff8aeb3e4d1 53 float temperature; //실제 온도값 (도C)
_seminahn 0:4ff8aeb3e4d1 54 float SelfTestResult[6]; // 자이로 가속도 실험결과
_seminahn 0:4ff8aeb3e4d1 55 Vect3 a, g, m;
_seminahn 0:4ff8aeb3e4d1 56 float q[4]; // 사원수 배열
_seminahn 0:4ff8aeb3e4d1 57 float roll, pitch, yaw;
_seminahn 0:4ff8aeb3e4d1 58 float a12, a22, a31, a32, a33; // 회전행렬의 계수
_seminahn 0:4ff8aeb3e4d1 59 float magnetic_declination; // Seoul 2019.1.2
_seminahn 0:4ff8aeb3e4d1 60 SPI _spi;
_seminahn 0:4ff8aeb3e4d1 61 DigitalOut _csPin;
_seminahn 0:4ff8aeb3e4d1 62 InterruptIn _intPin;
_seminahn 0:4ff8aeb3e4d1 63 Timer _tmr;
_seminahn 0:4ff8aeb3e4d1 64 volatile static bool _dataReady;
_seminahn 0:4ff8aeb3e4d1 65 Madgwick qFilter;
_seminahn 0:4ff8aeb3e4d1 66
_seminahn 0:4ff8aeb3e4d1 67 public:
_seminahn 0:4ff8aeb3e4d1 68 // MPU9250_SPI(SPIClass& bus,uint8_t csPin,uint8_t intPin);
_seminahn 0:4ff8aeb3e4d1 69 MPU9250_SPI(PinName mosi,PinName miso,PinName sclk, PinName cs, PinName intpin );
_seminahn 0:4ff8aeb3e4d1 70 void setup() ;
_seminahn 0:4ff8aeb3e4d1 71 void update(FusionMethod) ;
_seminahn 0:4ff8aeb3e4d1 72 void update(Vect3& _a,Vect3& _g,Vect3& _m) ;
_seminahn 0:4ff8aeb3e4d1 73 Vect3 getAccBias() const { return accelBias; }
_seminahn 0:4ff8aeb3e4d1 74 Vect3 getGyroBias() const { return gyroBias; }
_seminahn 0:4ff8aeb3e4d1 75 Vect3 getMagBias() const { return magBias; }
_seminahn 0:4ff8aeb3e4d1 76 Vect3 getMagScale() const { return magScale; }
_seminahn 0:4ff8aeb3e4d1 77 void setAccBias(Vect3 v) { accelBias = v; }
_seminahn 0:4ff8aeb3e4d1 78 void setGyroBias(Vect3 v) { gyroBias = v; }
_seminahn 0:4ff8aeb3e4d1 79 void setMagBias(Vect3 v) { magBias = v; }
_seminahn 0:4ff8aeb3e4d1 80 void setMagScale(Vect3 v) { magScale = v; }
_seminahn 0:4ff8aeb3e4d1 81 void setMagneticDeclination(const float d) { magnetic_declination = d; }
_seminahn 0:4ff8aeb3e4d1 82 void setAccelRange(AccelRange range) ;
_seminahn 0:4ff8aeb3e4d1 83 void setGyroRange(GyroRange range);
_seminahn 0:4ff8aeb3e4d1 84 void setDlpfBandwidth(DlpfBandwidth bandwidth);
_seminahn 0:4ff8aeb3e4d1 85 void setSampleRate(SampleRate srd);
_seminahn 0:4ff8aeb3e4d1 86 void calibrateMag();
_seminahn 0:4ff8aeb3e4d1 87 void calibrateGyro() ;
_seminahn 0:4ff8aeb3e4d1 88
_seminahn 0:4ff8aeb3e4d1 89 float getRoll() const { return roll; }
_seminahn 0:4ff8aeb3e4d1 90 float getPitch() const { return pitch; }
_seminahn 0:4ff8aeb3e4d1 91 float getYaw() const { return yaw; }
_seminahn 0:4ff8aeb3e4d1 92
_seminahn 0:4ff8aeb3e4d1 93 Vect3 getAccelVect() {return a;}
_seminahn 0:4ff8aeb3e4d1 94 Vect3 getGyroVect() {return g;}
_seminahn 0:4ff8aeb3e4d1 95 Vect3 getMagVect() {return m;}
_seminahn 0:4ff8aeb3e4d1 96
_seminahn 0:4ff8aeb3e4d1 97 float getq0() {return q[0];} // DDD
_seminahn 0:4ff8aeb3e4d1 98 float getq1() {return q[1];} // DDD
_seminahn 0:4ff8aeb3e4d1 99 float getq2() {return q[2];} // DDD
_seminahn 0:4ff8aeb3e4d1 100 float getq3() {return q[3];} // DDD
_seminahn 0:4ff8aeb3e4d1 101
_seminahn 0:4ff8aeb3e4d1 102 void enableDataReadyInterrupt();
_seminahn 0:4ff8aeb3e4d1 103 bool isDataReady(){return _dataReady;}
_seminahn 0:4ff8aeb3e4d1 104
_seminahn 0:4ff8aeb3e4d1 105 private:
_seminahn 0:4ff8aeb3e4d1 106 uint8_t isConnectedMPU9250() ;
_seminahn 0:4ff8aeb3e4d1 107 uint8_t isConnectedAK8963() ;
_seminahn 0:4ff8aeb3e4d1 108 void initMPU9250() ;
_seminahn 0:4ff8aeb3e4d1 109 void initAK8963() ;
_seminahn 0:4ff8aeb3e4d1 110 void intService(){ _dataReady=true; }
_seminahn 0:4ff8aeb3e4d1 111 void updateSensors();
_seminahn 0:4ff8aeb3e4d1 112 void updateAccelGyro() ;
_seminahn 0:4ff8aeb3e4d1 113 void readMPU9250Data(int16_t * destination) ;
_seminahn 0:4ff8aeb3e4d1 114 void updateMag() ;
_seminahn 0:4ff8aeb3e4d1 115 void readMagData(int16_t * destination) ;
_seminahn 0:4ff8aeb3e4d1 116 void magCalMPU9250();
_seminahn 0:4ff8aeb3e4d1 117 void updateRPY();
_seminahn 0:4ff8aeb3e4d1 118 void compFilter(float dt);
_seminahn 0:4ff8aeb3e4d1 119 void writeByte(uint8_t subAddress, uint8_t data);
_seminahn 0:4ff8aeb3e4d1 120 uint8_t readByte(uint8_t subAddress);
_seminahn 0:4ff8aeb3e4d1 121 void readBytes(uint8_t subAddress, uint8_t count, uint8_t* dest);
_seminahn 0:4ff8aeb3e4d1 122 void replaceBlock(uint8_t address, uint8_t block, uint8_t at, uint8_t sz);
_seminahn 0:4ff8aeb3e4d1 123 void replaceBlockAK(uint8_t address, uint8_t block, uint8_t at, uint8_t sz);
_seminahn 0:4ff8aeb3e4d1 124 void writeAK8963Byte(uint8_t subAddress, uint8_t data);
_seminahn 0:4ff8aeb3e4d1 125 void readAK8963Bytes(uint8_t subAddress, uint8_t count, uint8_t* dest);
_seminahn 0:4ff8aeb3e4d1 126 uint8_t readAK8963Byte(uint8_t subAddress);
_seminahn 0:4ff8aeb3e4d1 127 };
_seminahn 0:4ff8aeb3e4d1 128
_seminahn 0:4ff8aeb3e4d1 129 #endif
_seminahn 0:4ff8aeb3e4d1 130
_seminahn 0:4ff8aeb3e4d1 131 /***** EOF *****/
_seminahn 0:4ff8aeb3e4d1 132