Сб окт 08, 2016 12:49:32
void MPU_init (void) {
uint8_t R;
uint8_t c;
HAL_I2C_Mem_Write(&hi2c1, MPU9250_ADDRESS_W, PWR_MGMT_1, 1, 0x00, 1, 100); // Clear sleep mode bit (6), enable all sensors
HAL_Delay(100);
R = 0x01;
HAL_I2C_Mem_Write(&hi2c1, MPU9250_ADDRESS_W, PWR_MGMT_1, 1, &R, 1, 100); // Set clock source to be PLL with x-axis gyroscope reference, bits 2:0 = 001
// Configure Gyro and Accelerometer
// Disable FSYNC and set accelerometer and gyro bandwidth to 44 and 42 Hz, respectively;
// DLPF_CFG = bits 2:0 = 010; this sets the sample rate at 1 kHz for both
// Maximum delay is 4.9 ms which is just over a 200 Hz maximum rate
R = 0x03;
HAL_I2C_Mem_Write(&hi2c1, MPU9250_ADDRESS_W, CONFIG, 1, &R, 1, 100);
// Set sample rate = gyroscope output rate/(1 + SMPLRT_DIV)
R = 0x04;
HAL_I2C_Mem_Write(&hi2c1, MPU9250_ADDRESS_W, SMPLRT_DIV, 1, &R, 1, 100); // Use a 200 Hz rate; the same rate set in CONFIG above
// Set gyroscope full scale range
// Range selects FS_SEL and AFS_SEL are 0 - 3, so 2-bit values are left-shifted into positions 4:3
HAL_I2C_Mem_Read(&hi2c1, MPU9250_ADDRESS_R, GYRO_CONFIG, 1, &c, 6, 100); // get current GYRO_CONFIG register value
// c = c & ~0xE0; // Clear self-test bits [7:5]
c = c & ~0x02; // Clear Fchoice bits [1:0]
c = c & ~0x18; // Clear AFS bits [4:3]
c = c | 0 << 3; // Set full scale range for the gyro - 0=+250dps
// c =| 0x00; // Set Fchoice for the gyro to 11 by writing its inverse to bits 1:0 of GYRO_CONFIG
HAL_I2C_Mem_Write(&hi2c1, MPU9250_ADDRESS_W, GYRO_CONFIG, 1, &c, 1, 100); // Write new GYRO_CONFIG value to register
// Set accelerometer full-scale range configuration
HAL_I2C_Mem_Read(&hi2c1, MPU9250_ADDRESS_R, ACCEL_CONFIG, 1, &c, 6, 100); // get current ACCEL_CONFIG register value
// c = c & ~0xE0; // Clear self-test bits [7:5]
c = c & ~0x18; // Clear AFS bits [4:3]
c = c | 0 << 3; // Set full scale range for the accelerometer - 0=2g
HAL_I2C_Mem_Write(&hi2c1, MPU9250_ADDRESS_W, ACCEL_CONFIG, 1, &c, 1, 100); // Write new ACCEL_CONFIG register value
// Set accelerometer sample rate configuration
// It is possible to get a 4 kHz sample rate from the accelerometer by choosing 1 for
// accel_fchoice_b bit [3]; in this case the bandwidth is 1.13 kHz
HAL_I2C_Mem_Read(&hi2c1, MPU9250_ADDRESS_R, ACCEL_CONFIG2, 1, &c, 6, 100); // get current ACCEL_CONFIG2 register value
c = c & ~0x0F; // Clear accel_fchoice_b (bit 3) and A_DLPFG (bits [2:0])
c = c | 0x03; // Set accelerometer rate to 1 kHz and bandwidth to 41 Hz
HAL_I2C_Mem_Write(&hi2c1, MPU9250_ADDRESS_W, ACCEL_CONFIG2, 1, &c, 1, 100); // Write new ACCEL_CONFIG2 register value
// The accelerometer, gyro, and thermometer are set to 1 kHz sample rates,
// but all these rates are further reduced by a factor of 5 to 200 Hz because of the SMPLRT_DIV setting
// Configure Interrupts and Bypass Enable
// Set interrupt pin active high, push-pull, and clear on read of INT_STATUS, enable I2C_BYPASS_EN so additional chips
// can join the I2C bus and all can be controlled by the Arduino as master
R=0x22;
HAL_I2C_Mem_Write(&hi2c1, MPU9250_ADDRESS_W, INT_PIN_CFG, 1, &R, 1, 100);
R=0x01;
HAL_I2C_Mem_Write(&hi2c1, MPU9250_ADDRESS_W, INT_ENABLE, 1, &R, 1, 100); // Enable data ready (bit 0) interrupt
}
//
void init_MAGN (float * destination) {
uint8_t R;
uint8_t rawData[3]; // x/y/z gyro calibration data stored here
// First extract the factory calibration for each magnetometer axis
R=0x00;
HAL_I2C_Mem_Write(&hi2c1, AK8963_ADDRESS_W, AK8963_CNTL, 1, &R, 1, 100); // Power down magnetometer
HAL_Delay(10);
R=0x0F;
HAL_I2C_Mem_Write(&hi2c1, AK8963_ADDRESS_W, AK8963_CNTL, 1, &R, 1, 100); // Enter Fuse ROM access mode
HAL_Delay(10);
HAL_I2C_Mem_Read(&hi2c1, AK8963_ADDRESS_R, AK8963_ASAX, 1, rawData, 3, 100); // Read the x-, y-, and z-axis calibration values
destination[0] = (float)(rawData[0] - 128)/256.0f + 1.0f; // Return x-axis sensitivity adjustment values, etc.
destination[1] = (float)(rawData[1] - 128)/256.0f + 1.0f;
destination[2] = (float)(rawData[2] - 128)/256.0f + 1.0f;
R=0x00;
HAL_I2C_Mem_Write(&hi2c1, AK8963_ADDRESS_W, AK8963_CNTL, 1, &R, 1, 100); // Power down magnetometer
HAL_Delay(10);
// Configure the magnetometer for continuous read and highest resolution
// set Mscale bit 4 to 1 (0) to enable 16 (14) bit resolution in CNTL register,
// and enable continuous mode data acquisition Mmode (bits [3:0]), 0010 for 8 Hz and 0110 for 100 Hz sample rates
R = 0 << 4 | 0x06;
HAL_I2C_Mem_Write(&hi2c1, AK8963_ADDRESS_W, AK8963_CNTL, 1, &R, 1, 100); // Set magnetometer data resolution and sample ODR
HAL_Delay(10);
R=0x40;
HAL_I2C_Mem_Write(&hi2c1, AK8963_ADDRESS_W, AK8963_ASTC, 1, &R, 1, 100); // set self-test
//
}
//
void MPU_get_accel (int16_t * destination) {
uint8_t rawData[6];
HAL_I2C_Mem_Read(&hi2c1, MPU9250_ADDRESS_R, ACCEL_XOUT_H, 1, rawData, 6, 100);
destination[0] = (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value
destination[1] = (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
destination[2] = (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
}
//
void MPU_get_gyro (int16_t * destination) {
uint8_t rawData[6]; // x/y/z gyro register data stored here
HAL_I2C_Mem_Read(&hi2c1, MPU9250_ADDRESS_R, GYRO_XOUT_H, 1, rawData, 6, 100); // Read the six raw data registers sequentially into data array
destination[0] = (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value
destination[1] = (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
destination[2] = (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
}
//
void MPU_get_magn (int16_t * destination) {
uint8_t rawData[7]; // x/y/z gyro register data, ST2 register stored here, must read ST2 at end of data acquisition
uint8_t c;
HAL_I2C_Mem_Read(&hi2c1, AK8963_ADDRESS_R, AK8963_ST1, 1, &c, 1, 100);
if(c >= 0x01) { // wait for magnetometer data ready bit to be set
HAL_I2C_Mem_Read(&hi2c1, AK8963_ADDRESS_R, AK8963_XOUT_L, 1, rawData, 7, 100); // Read the six raw data and ST2 registers sequentially into data array
c = rawData[6]; // End data read by reading ST2 register
if(!(c & 0x08)) { // Check if magnetic sensor overflow set, if not then report data
destination[0] = (int16_t)(((int16_t)rawData[1] << 8) | rawData[0]); // Turn the MSB and LSB into a signed 16-bit value
destination[1] = (int16_t)(((int16_t)rawData[3] << 8) | rawData[2]) ; // Data stored as little Endian
destination[2] = (int16_t)(((int16_t)rawData[5] << 8) | rawData[4]) ;
}
}
}
//
int16_t MPU_get_temper (void) {
uint8_t rawData[2]; // x/y/z gyro register data stored here
HAL_I2C_Mem_Read(&hi2c1, MPU9250_ADDRESS_R, TEMP_OUT_H, 1, rawData, 2, 100); // Read the two raw data registers sequentially into data array
return (int16_t)(((int16_t)rawData[0]) << 8 | rawData[1]) ; // Turn the MSB and LSB into a 16-bit value
}
//
float Axn = ACCEL[0]/sqrt(pow(ACCEL[0], 2) + pow(ACCEL[1], 2) + pow(ACCEL[2], 2));
float Ayn = ACCEL[1]/sqrt(pow(ACCEL[0], 2) + pow(ACCEL[1], 2) + pow(ACCEL[2], 2));
pitch = asin(-Axn);
roll = asin(Ayn/cos(pitch));
tilted_X = MAGN[0]*cos(pitch) + MAGN[2]*sin(pitch);
tilted_Y = MAGN[0]*sin(roll)*sin(pitch) + MAGN[1]*cos(roll) - MAGN[2]*sin(roll)*cos(pitch);
yaw = atan2(tilted_Y,tilted_X)*57.295;
pitch *= 57.295;
roll *= 57.295;
void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz)
{
float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3]; // short name local variable for readability
float norm;
float hx, hy, _2bx, _2bz;
float s1, s2, s3, s4;
float qDot1, qDot2, qDot3, qDot4;
// Auxiliary variables to avoid repeated arithmetic
float _2q1mx;
float _2q1my;
float _2q1mz;
float _2q2mx;
float _4bx;
float _4bz;
float _2q1 = 2.0f * q1;
float _2q2 = 2.0f * q2;
float _2q3 = 2.0f * q3;
float _2q4 = 2.0f * q4;
float _2q1q3 = 2.0f * q1 * q3;
float _2q3q4 = 2.0f * q3 * q4;
float q1q1 = q1 * q1;
float q1q2 = q1 * q2;
float q1q3 = q1 * q3;
float q1q4 = q1 * q4;
float q2q2 = q2 * q2;
float q2q3 = q2 * q3;
float q2q4 = q2 * q4;
float q3q3 = q3 * q3;
float q3q4 = q3 * q4;
float q4q4 = q4 * q4;
// Normalise accelerometer measurement
norm = sqrt(ax * ax + ay * ay + az * az);
if (norm == 0.0f) return; // handle NaN
norm = 1.0f/norm;
ax *= norm;
ay *= norm;
az *= norm;
// Normalise magnetometer measurement
norm = sqrt(mx * mx + my * my + mz * mz);
if (norm == 0.0f) return; // handle NaN
norm = 1.0f/norm;
mx *= norm;
my *= norm;
mz *= norm;
// Reference direction of Earth's magnetic field
_2q1mx = 2.0f * q1 * mx;
_2q1my = 2.0f * q1 * my;
_2q1mz = 2.0f * q1 * mz;
_2q2mx = 2.0f * q2 * mx;
hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 + _2q2 * mz * q4 - mx * q3q3 - mx * q4q4;
hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4;
_2bx = sqrt(hx * hx + hy * hy);
_2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4;
_4bx = 2.0f * _2bx;
_4bz = 2.0f * _2bz;
// Gradient decent algorithm corrective step
s1 = -_2q3 * (2.0f * q2q4 - _2q1q3 - ax) + _2q2 * (2.0f * q1q2 + _2q3q4 - ay) - _2bz * q3 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q4 + _2bz * q2) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q3 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
s2 = _2q4 * (2.0f * q2q4 - _2q1q3 - ax) + _2q1 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q2 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + _2bz * q4 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q3 + _2bz * q1) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q4 - _4bz * q2) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
s3 = -_2q1 * (2.0f * q2q4 - _2q1q3 - ax) + _2q4 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q3 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + (-_4bx * q3 - _2bz * q1) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q2 + _2bz * q4) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q1 - _4bz * q3) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
s4 = _2q2 * (2.0f * q2q4 - _2q1q3 - ax) + _2q3 * (2.0f * q1q2 + _2q3q4 - ay) + (-_4bx * q4 + _2bz * q2) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q1 + _2bz * q3) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q2 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
norm = sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4); // normalise step magnitude
norm = 1.0f/norm;
s1 *= norm;
s2 *= norm;
s3 *= norm;
s4 *= norm;
// Compute rate of change of quaternion
qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - beta * s1;
qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - beta * s2;
qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - beta * s3;
qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - beta * s4;
// Integrate to yield quaternion
q1 += qDot1 * deltat;
q2 += qDot2 * deltat;
q3 += qDot3 * deltat;
q4 += qDot4 * deltat;
norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); // normalise quaternion
norm = 1.0f/norm;
q[0] = q1 * norm;
q[1] = q2 * norm;
q[2] = q3 * norm;
q[3] = q4 * norm;
}
// вызываем функцию и по результатам определяем ориентацию
MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, mx, my, mz);
MadgwickAHRSupdate( gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, ax, ay, az, mx, my, mz);
yaw = atan2(2.0f * (q1 * q2 + q0 * q3), q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3);
pitch = -asin(2.0f * (q1 * q3 - q0 * q2));
roll = atan2(2.0f * (q0 * q1 + q2 * q3), q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3);
Сб окт 08, 2016 13:06:22
Сб окт 08, 2016 13:29:51
Сб окт 08, 2016 18:22:31
Ссылок у меня нет - просто качал проекты. Но они на работе, могу в понедельник глянуть.didim писал(а):Serj_K, а есть ссылки на эти проекты с кодом?
PS нашел на гитхабе - в Oculus Rift только магнитометр используется получается?
Сб окт 08, 2016 18:44:30
Вс окт 09, 2016 15:32:15
Пн окт 10, 2016 10:49:33
Пн окт 10, 2016 19:13:24
Ср окт 19, 2016 18:26:59
Ср окт 19, 2016 19:26:48
Пт ноя 11, 2016 15:14:24
Пт ноя 11, 2016 20:13:13
Пн ноя 28, 2016 20:15:41
Сб дек 03, 2016 17:56:55
didim писал(а):Обработка данных производится в радианах, а перевод в градусы только в самом конце. Мои результаты использования этого алгоритма: компенсация почти не работает, даже при минимальных отклонениях показания магнитометра остаются недостоверными при наклонах датчика.
Почему? Вроде как метод обоснован и должен хотя бы минимально работать.
Результат снова недостоверный – кажется, что показания стали еще хуже. Углы плавают. Что здесь не так?
Пт дек 09, 2016 13:47:48
Сб дек 10, 2016 09:16:51
Сб дек 10, 2016 09:23:03
Сб дек 10, 2016 12:27:56
Ср дек 14, 2016 15:27:22
Пн дек 19, 2016 10:24:40