ai_car
Embed:
(wiki syntax)
Show/hide line numbers
MPU9250_SPI.h
00001 #ifndef MPU9250_SPI_ 00002 #define MPU9250_SPI_ 00003 #include "mbed.h" 00004 #include <cmath> 00005 #include "MPU9250RegisterMap.h" 00006 #define DT 0.01f 00007 #define fc 0.5f 00008 #define Tc (1./(2.*M_PI*fc)) 00009 enum GyroRange { GYRO_RANGE_250DPS, GYRO_RANGE_500DPS, GYRO_RANGE_1000DPS, GYRO_RANGE_2000DPS }; 00010 enum AccelRange { ACCEL_RANGE_2G, ACCEL_RANGE_4G, ACCEL_RANGE_8G, ACCEL_RANGE_16G }; 00011 enum DlpfBandwidth { DLPF_BANDWIDTH_250HZ, DLPF_BANDWIDTH_184HZ, DLPF_BANDWIDTH_92HZ, DLPF_BANDWIDTH_41HZ, 00012 DLPF_BANDWIDTH_20HZ, DLPF_BANDWIDTH_10HZ, DLPF_BANDWIDTH_5HZ }; 00013 enum MagnBits { MGN_14BITS, MGN_16BITS }; // CNTL1 offset 4 0:14bits, 1:16bits 00014 enum MagnMode { MGN_POWER_DN=0, MGN_SNGL_MEAS=1, MGN_CONT_MEAS1=2,MGN_CONT_MEAS2=6,MGN_EX_TRIG=4, 00015 MGN_SELF_TEST=8, MGN_FUSE_ROM=15}; // CNTL1 offset 0 00016 enum SampleRate {SR_1000HZ=0, SR_200HZ=4, SR_100HZ=9 }; // 1kHz/(1+SRD) 00017 struct Vect3 {float x,y,z;}; 00018 class MPU9250_SPI { 00019 float aRes ; // 가속도 (LSB 당의 값) 00020 float gRes ; // 자이로 (LSB 당의 값) 00021 float mRes ; // 지자기 (LSB 당의 값) 00022 AccelRange _accelRange; 00023 GyroRange _gyroRange; 00024 DlpfBandwidth _bandwidth; 00025 MagnMode _mMode; 00026 MagnBits _mBits; 00027 SampleRate _srd; 00028 Vect3 magCalibration ; // factory mag calibration 00029 Vect3 magBias ; //지자기 바이어스 00030 Vect3 magScale ; // 지자기 스케일 00031 Vect3 gyroBias ; // 자이로 바이어스 00032 Vect3 accelBias ; // 가속도 바이어스 00033 int16_t tempCount; // 원시 온도값 00034 float temperature; //실제 온도값 (도C) 00035 float SelfTestResult[6]; // 자이로 가속도 실험결과 00036 Vect3 a, g, m; 00037 float roll, pitch, yaw; 00038 float magnetic_declination; // Seoul 2019.1.2 00039 SPI _spi; 00040 DigitalOut _csPin; 00041 InterruptIn _intPin; 00042 Timer _tmr; 00043 volatile static bool _dataReady; 00044 public: 00045 // MPU9250_SPI(SPIClass& bus,uint8_t csPin,uint8_t intPin); 00046 MPU9250_SPI(PinName mosi,PinName miso,PinName sclk, PinName cs, PinName intpin ); 00047 void setup() ; 00048 void update(Vect3& _a,Vect3& _g,Vect3& _m) ; 00049 Vect3 getAccBias() const { return accelBias; } 00050 Vect3 getGyroBias() const { return gyroBias; } 00051 Vect3 getMagBias() const { return magBias; } 00052 Vect3 getMagScale() const { return magScale; } 00053 void setAccBias(Vect3 v) { accelBias = v; } 00054 void setGyroBias(Vect3 v) { gyroBias = v; } 00055 void setMagBias(Vect3 v) { magBias = v; } 00056 void setMagScale(Vect3 v) { magScale = v; } 00057 void setMagneticDeclination(const float d) { magnetic_declination = d; } 00058 void setAccelRange(AccelRange range) ; 00059 void setGyroRange(GyroRange range); 00060 void setDlpfBandwidth(DlpfBandwidth bandwidth); 00061 void setSampleRate(SampleRate srd); 00062 float getRoll() const { return roll; } 00063 float getPitch() const { return pitch; } 00064 float getYaw() const { return yaw; } 00065 Vect3 getAccelVect() {return a;} 00066 Vect3 getGyroVect() {return g;} 00067 Vect3 getMagVect() {return m;} 00068 void enableDataReadyInterrupt(); 00069 bool isDataReady(){return _dataReady;} 00070 private: 00071 uint8_t isConnectedMPU9250() ; 00072 uint8_t isConnectedAK8963() ; 00073 void initMPU9250() ; 00074 void initAK8963() ; 00075 void intService(){ _dataReady=true; } 00076 void updateSensors(); 00077 void updateAccelGyro() ; 00078 void readMPU9250Data(int16_t * destination) ; 00079 void updateMag() ; 00080 void readMagData(int16_t * destination) ; 00081 void writeByte(uint8_t subAddress, uint8_t data); 00082 uint8_t readByte(uint8_t subAddress); 00083 void readBytes(uint8_t subAddress, uint8_t count, uint8_t* dest); 00084 void replaceBlock(uint8_t address, uint8_t block, uint8_t at, uint8_t sz); 00085 void replaceBlockAK(uint8_t address, uint8_t block, uint8_t at, uint8_t sz); 00086 void writeAK8963Byte(uint8_t subAddress, uint8_t data); 00087 void readAK8963Bytes(uint8_t subAddress, uint8_t count, uint8_t* dest); 00088 uint8_t readAK8963Byte(uint8_t subAddress); 00089 }; 00090 #endif
Generated on Thu Jul 14 2022 22:00:05 by
1.7.2