エラー判定用 '\0' + クォータニオンのfloat型4成分,合計17byteのデータをひたすらバイナリ形式で送り続ける.最初のバイトが '\0' でなかったらズレているので,適当なバイト数見送って先頭が '\0' になるよう合わせること.
Diff: MPU9250/MPU9250.cpp
- Revision:
- 0:2a7221ee9861
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MPU9250/MPU9250.cpp Wed Mar 21 07:58:57 2018 +0000 @@ -0,0 +1,388 @@ +/** Daqn change log + * 2018/02/16 : Replace words "MPU6050" with "MPU9250". + * 2018/02/16 : L57, flip T/F by adding "!". + * 2018/02/21 : L + */ + +/** + * Includes + */ +#include "MPU9250.h" + +MPU9250::MPU9250(PinName sda, PinName scl) : connection(sda, scl) { + this->setSleepMode(false); + + //Initializations: + currentGyroRange = 0; + currentAcceleroRange=0; + alpha = ALPHA; +} + +//-------------------------------------------------- +//-------------------General------------------------ +//-------------------------------------------------- + +void MPU9250::write(char address, char data) { + char temp[2]; + temp[0]=address; + temp[1]=data; + + connection.write(MPU9250_ADDRESS * 2,temp,2); +} + +char MPU9250::read(char address) { + char retval; + connection.write(MPU9250_ADDRESS * 2, &address, 1, true); + connection.read(MPU9250_ADDRESS * 2, &retval, 1); + return retval; +} + +void MPU9250::read(char address, char *data, int length) { + connection.write(MPU9250_ADDRESS * 2, &address, 1, true); + connection.read(MPU9250_ADDRESS * 2, data, length); +} + +void MPU9250::setSleepMode(bool state) { + char temp; + temp = this->read(MPU9250_PWR_MGMT_1_REG); + if (state == true) + temp |= 1<<MPU9250_SLP_BIT; + if (state == false) + temp &= ~(1<<MPU9250_SLP_BIT); + this->write(MPU9250_PWR_MGMT_1_REG, temp); +} + +bool MPU9250::testConnection( void ) { + char temp; + temp = this->read(MPU9250_WHO_AM_I_REG); + return !(temp == (MPU9250_ADDRESS & 0xFE)); // Daqn 2018/02/16 +} + +void MPU9250::setBW(char BW) { + char temp; + BW=BW & 0x07; + temp = this->read(MPU9250_CONFIG_REG); + temp &= 0xF8; + temp = temp + BW; + this->write(MPU9250_CONFIG_REG, temp); +} + +void MPU9250::setI2CBypass(bool state) { + char temp; + temp = this->read(MPU9250_INT_PIN_CFG); + if (state == true) + temp |= 1<<MPU9250_BYPASS_BIT; + if (state == false) + temp &= ~(1<<MPU9250_BYPASS_BIT); + this->write(MPU9250_INT_PIN_CFG, temp); +} + +//-------------------------------------------------- +//----------------Accelerometer--------------------- +//-------------------------------------------------- + +void MPU9250::setAcceleroRange( char range ) { + char temp; + range = range & 0x03; + currentAcceleroRange = range; + + temp = this->read(MPU9250_ACCELERO_CONFIG_REG); + temp &= ~(3<<3); + temp = temp + (range<<3); + this->write(MPU9250_ACCELERO_CONFIG_REG, temp); +} + +int MPU9250::getAcceleroRawX( void ) { + short retval; + char data[2]; + this->read(MPU9250_ACCEL_XOUT_H_REG, data, 2); + retval = (data[0]<<8) + data[1]; + return (int)retval; +} + +int MPU9250::getAcceleroRawY( void ) { + short retval; + char data[2]; + this->read(MPU9250_ACCEL_YOUT_H_REG, data, 2); + retval = (data[0]<<8) + data[1]; + return (int)retval; +} + +int MPU9250::getAcceleroRawZ( void ) { + short retval; + char data[2]; + this->read(MPU9250_ACCEL_ZOUT_H_REG, data, 2); + retval = (data[0]<<8) + data[1]; + return (int)retval; +} + +void MPU9250::getAcceleroRaw( int *data ) { + char temp[6]; + this->read(MPU9250_ACCEL_XOUT_H_REG, temp, 6); + data[0] = (int)(short)((temp[0]<<8) + temp[1]); + data[1] = (int)(short)((temp[2]<<8) + temp[3]); + data[2] = (int)(short)((temp[4]<<8) + temp[5]); +} + +void MPU9250::getAccelero( float *data ) { + int temp[3]; + this->getAcceleroRaw(temp); + if (currentAcceleroRange == MPU9250_ACCELERO_RANGE_2G) { + data[0]=(float)temp[0] / 16384.0 * 9.81; + data[1]=(float)temp[1] / 16384.0 * 9.81; + data[2]=(float)temp[2] / 16384.0 * 9.81; + } + if (currentAcceleroRange == MPU9250_ACCELERO_RANGE_4G){ + data[0]=(float)temp[0] / 8192.0 * 9.81; + data[1]=(float)temp[1] / 8192.0 * 9.81; + data[2]=(float)temp[2] / 8192.0 * 9.81; + } + if (currentAcceleroRange == MPU9250_ACCELERO_RANGE_8G){ + data[0]=(float)temp[0] / 4096.0 * 9.81; + data[1]=(float)temp[1] / 4096.0 * 9.81; + data[2]=(float)temp[2] / 4096.0 * 9.81; + } + if (currentAcceleroRange == MPU9250_ACCELERO_RANGE_16G){ + data[0]=(float)temp[0] / 2048.0 * 9.81; + data[1]=(float)temp[1] / 2048.0 * 9.81; + data[2]=(float)temp[2] / 2048.0 * 9.81; + } + + #ifdef DOUBLE_ACCELERO + data[0]*=2; + data[1]*=2; + data[2]*=2; + #endif +} +//-------------------------------------------------- +//------------------Gyroscope----------------------- +//-------------------------------------------------- +void MPU9250::setGyroRange( char range ) { + char temp; + currentGyroRange = range; + range = range & 0x03; + temp = this->read(MPU9250_GYRO_CONFIG_REG); + temp &= ~(3<<3); + temp = temp + range<<3; + this->write(MPU9250_GYRO_CONFIG_REG, temp); +} + +int MPU9250::getGyroRawX( void ) { + short retval; + char data[2]; + this->read(MPU9250_GYRO_XOUT_H_REG, data, 2); + retval = (data[0]<<8) + data[1]; + return (int)retval; +} + +int MPU9250::getGyroRawY( void ) { + short retval; + char data[2]; + this->read(MPU9250_GYRO_YOUT_H_REG, data, 2); + retval = (data[0]<<8) + data[1]; + return (int)retval; +} + +int MPU9250::getGyroRawZ( void ) { + short retval; + char data[2]; + this->read(MPU9250_GYRO_ZOUT_H_REG, data, 2); + retval = (data[0]<<8) + data[1]; + return (int)retval; +} + +void MPU9250::getGyroRaw( int *data ) { + char temp[6]; + this->read(MPU9250_GYRO_XOUT_H_REG, temp, 6); + data[0] = (int)(short)((temp[0]<<8) + temp[1]); + data[1] = (int)(short)((temp[2]<<8) + temp[3]); + data[2] = (int)(short)((temp[4]<<8) + temp[5]); +} + +void MPU9250::getGyro( float *data ) { + int temp[3]; + this->getGyroRaw(temp); + if (currentGyroRange == MPU9250_GYRO_RANGE_250) { + data[0]=(float)temp[0] / 301.0; + data[1]=(float)temp[1] / 301.0; + data[2]=(float)temp[2] / 301.0; + } //7505.5 + if (currentGyroRange == MPU9250_GYRO_RANGE_500){ + data[0]=(float)temp[0] / 3752.9; + data[1]=(float)temp[1] / 3752.9; + data[2]=(float)temp[2] / 3752.9; + } + if (currentGyroRange == MPU9250_GYRO_RANGE_1000){ + data[0]=(float)temp[0] / 1879.3;; + data[1]=(float)temp[1] / 1879.3; + data[2]=(float)temp[2] / 1879.3; + } + if (currentGyroRange == MPU9250_GYRO_RANGE_2000){ + data[0]=(float)temp[0] / 939.7; + data[1]=(float)temp[1] / 939.7; + data[2]=(float)temp[2] / 939.7; + } +} +//-------------------------------------------------- +//-------------------Temperature-------------------- +//-------------------------------------------------- +int MPU9250::getTempRaw( void ) { + short retval; + char data[2]; + this->read(MPU9250_TEMP_H_REG, data, 2); + retval = (data[0]<<8) + data[1]; + return (int)retval; +} + +float MPU9250::getTemp( void ) { + float retval; + retval=(float)this->getTempRaw(); + retval=(retval+521.0)/340.0+35.0; + return retval; +} + + +//-------------------------------------------------- +//------------------Magnetometer-------------------- +//-------------------------------------------------- +void MPU9250::read(char address, char subaddress, char *data, int length) +{ +// connection.write(MPU9250_ADDRESS * 2, &address, 1, true); +// connection.read(MPU9250_ADDRESS * 2, data, length); + connection.write(address, &subaddress, 1, true); + connection.read(address, data, length); +} + +//void MPU9250::setMagnetoRange( char range ) { +// char temp; +// currentMagnetoRange = range; +// range = range & 0x03; +// temp = this->read(MPU9250_MAGNETO_CONFIG_REG); +// temp &= ~(3<<3); +// temp = temp + range<<3; +// this->write(MPU9250_MAGNETO_CONFIG_REG, temp); +//} + + + +int MPU9250::getMagnetoRawX( void ) +{ + short retval; + char data[2]; + this->read(AK8963_ADDRESS, AK8963_XOUT_L, data, 2); + retval = (data[0]<<8) + data[1]; + return (int)retval; +} + +int MPU9250::getMagnetoRawY( void ) +{ + short retval; + char data[2]; + this->read(AK8963_ADDRESS, AK8963_YOUT_L, data, 2); + retval = (data[0]<<8) + data[1]; + return (int)retval; +} +// +int MPU9250::getMagnetoRawZ( void ) +{ + short retval; + char data[2]; + this->read(AK8963_ADDRESS, AK8963_ZOUT_L, data, 2); + retval = (data[0]<<8) + data[1]; + return (int)retval; +} + +void MPU9250::getMagnetoRaw( int *data ) { + char temp[6]; + this->read(AK8963_ADDRESS, AK8963_XOUT_L, temp, 6); + data[0] = (int)(short)((temp[1]<<8) + temp[0]); + data[1] = (int)(short)((temp[3]<<8) + temp[2]); + data[2] = (int)(short)((temp[5]<<8) + temp[4]); +} + +void MPU9250::getMagneto( float *data ) { + int temp[3]; + this->getMagnetoRaw(temp); + data[0]=(float)temp[0] * .15f; + data[1]=(float)temp[1] * .15f; + data[2]=(float)temp[2] * .15f; +} + +/**Additional function added by Montvydas Klumbys, which will allow easy offset, angle calculation and much more. + function for getting angles in degrees from accelerometer +*/ +void MPU9250::getAcceleroAngle( float *data ) { + float temp[3]; + this->getAccelero(temp); + + data[X_AXIS] = atan (temp[Y_AXIS]/sqrt(pow(temp[X_AXIS], 2) + pow(temp[Z_AXIS], 2))) * RADIANS_TO_DEGREES; //calculate angle x(pitch/roll?) from accellerometer reading + data[Y_AXIS] = atan (-1*temp[X_AXIS]/sqrt(pow(temp[Y_AXIS], 2) + pow(temp[Z_AXIS], 2))) * RADIANS_TO_DEGREES; //calculate angle x(pitch/roll?) from accellerometer reading + data[Z_AXIS] = atan (sqrt(pow(temp[X_AXIS], 2) + pow(temp[Y_AXIS], 2))/temp[Z_AXIS]) * RADIANS_TO_DEGREES; //This one is not used anywhere later on + +// data[Y_AXIS] = atan2 (temp[Y_AXIS],temp[Z_AXIS]) * RADIANS_TO_DEGREES; //This spits out values between -180 to 180 (360 degrees) +// data[X_AXIS] = atan2 (-1*temp[X_AXIS], temp[Z_AXIS]) * RADIANS_TO_DEGREES; //but it takes longer and system gets unstable when angles ~90 degrees +} + +///function for getting offset values for the gyro & accelerometer +void MPU9250::getOffset(float *accOffset, float *gyroOffset, int sampleSize){ + float gyro[3]; + float accAngle[3]; + + for (int i = 0; i < 3; i++) { + accOffset[i] = 0.0; //initialise offsets to 0.0 + gyroOffset[i] = 0.0; + } + + for (int i = 0; i < sampleSize; i++){ + this->getGyro(gyro); //take real life measurements + this->getAcceleroAngle (accAngle); + + for (int j = 0; j < 3; j++){ + *(accOffset+j) += accAngle[j]/sampleSize; //average measurements + *(gyroOffset+j) += gyro[j]/sampleSize; + } + wait (0.01); //wait between each reading for accuracy + } +} + +///function for computing angles for roll, pitch anf yaw +void MPU9250::computeAngle (float *angle, float *accOffset, float *gyroOffset, float interval){ + float gyro[3]; + float accAngle[3]; + + this->getGyro(gyro); //get gyro value in rad/s + this->getAcceleroAngle(accAngle); //get angle from accelerometer + + for (int i = 0; i < 3; i++){ + gyro[i] -= gyroOffset[i]; //substract offset values + accAngle[i] -= accOffset[i]; + } + + //apply filters on pitch and roll to get accurate angle values + angle[X_AXIS] = alpha * (angle[X_AXIS] + GYRO_SCALE*gyro[X_AXIS]*interval) + (1-alpha)*accAngle[X_AXIS]; + angle[Y_AXIS] = alpha * (angle[Y_AXIS] + GYRO_SCALE*gyro[Y_AXIS]*interval) + (1-alpha)*accAngle[Y_AXIS]; + + //calculate Yaw using just the gyroscope values - inaccurate + angle[Z_AXIS] = angle[Z_AXIS] + GYRO_SCALE*gyro[Z_AXIS]*interval; +} + +///function for setting a different Alpha value, which is used in complemetary filter calculations +void MPU9250::setAlpha(float val){ + alpha = val; +} + +///function for enabling interrupts on MPU9250 INT pin, when the data is ready to take +void MPU9250::enableInt( void ){ + char temp; + temp = this->read(MPU9250_RA_INT_ENABLE); + temp |= 0x01; + this->write(MPU9250_RA_INT_ENABLE, temp); +} + +///function for disbling interrupts on MPU9250 INT pin, when the data is ready to take +void MPU9250::disableInt ( void ){ + char temp; + temp = this->read(MPU9250_RA_INT_ENABLE); + temp &= 0xFE; + this->write(MPU9250_RA_INT_ENABLE, temp); +} \ No newline at end of file