1) Выбор датчиков для применения. Здесь есть варианты использования модулей, где напаяно несколько датчиков в одном корпусе http://www.gearbest.com/sensors/pp_244849.html , а можно использовать датчик, у которого все это в одном корпусе, например, MPU9250 http://www.gearbest.com/sensors/pp_244846.html?wid=21 (именно этот датчик я приобрел и пытаюсь использовать), однако, для измерения высоты все-равно нужен дополнительно еще один датчик – BMP180 или bmp280 http://www.gearbest.com/sensors/pp_340012.html .
2) Получение данных из датчика. Здесь сложности должны возникать мало у кого. Подключаем датчик к микроконтроллеру и пишем код для работы с ним (по i2c или spi в зависимости от выбранного).
Спойлер
Код: Выделить всё
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
}
//3) Обработка данных. Для того чтобы подобную систему датчиков успешно использовать, необходимо обработать «сырые» данные, полученные от датчиков. Акселерометр измеряет ускорение, действующее на датчик, с помощью чего мы можем получить проекцию тела относительно земли – отклонение (вращение) по оси X и отклонение (вращение) по оси Y. Вращение по оси Z по данным акселерометра получить не удастся. Вращение по любой из осей координат можно получить при помощи гироскопа. Этот датчик измеряет угловую скорость, то есть скорость поворота вокруг оси. Показания этого датчика сильно зависят от обработки получаемых данных датчика – чем больше скорость опроса, тем более точные результаты. Каждый опрос датчика даст нам мгновенные значения угловой скорости, но между опросами угловая скорость может быть неравномерна, что не даст нам полностью достоверных данных, кроме этого необходимо знать точное время между опросами датчика для интегрирования данных по углу поворота, а раз мы многократно интегрируем данные, которые содержат ошибки, то и они будут накапливаться, что приведет к «уплыванию» показаний. Для фиксации вращения по оси Z хорошо подходит магнитометр – датчик, измеряющий магнитное поле. Как известно, земля имеет свое магнитное поле, а магнитометр как раз в роли компаса может определять направление на север. Но магнитометр также чувствителен и к другим магнитным полям, создаваемым металлическими предметами, различной электронной техникой, радиоаппаратурой и так далее, поэтому применение магнитометра часто проблематично в городских условиях. Кроме этого, измерение магнитометром можно проводить достоверно только в положении ровно перпендикулярном оси к земле. Таким образом, каждый датчик по отдельности не даст полного набора данных, по которым можно определить ориентацию тела в пространстве, но если данные от каждого датчика использовать вместе, то можно добиться очень неплохих результатов.
4) Далее вопросы, возникающие в процессе как это сделать (собственно цель создания темы). От простого к сложному. Если мы получаем данные с акселерометра и, предположим, они нас устраивают своей стабильностью. Далее минимум нам нужно получить вращение по оси Z. Используем магнитометр и компенсируем отклонение оси от перпендикулярного земле положения по данным акселерометра.
Алгоритм компенсации наклона магнитомтера:
Спойлер
Код: Выделить всё
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;Обработка данных производится в радианах, а перевод в градусы только в самом конце. Мои результаты использования этого алгоритма: компенсация почти не работает, даже при минимальных отклонениях показания магнитометра остаются недостоверными при наклонах датчика.
Почему? Вроде как метод обоснован и должен хотя бы минимально работать.
Для получения положения датчиков в пространстве можно воспользоваться методом Маджвика, называемый фильтром Маджвика. Метод не прост, используем код, близкий к авторскому https://github.com/kriswiner/MPU-9250/tree/master/STM32F401.
Спойлер
Код: Выделить всё
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);Результат снова недостоверный – кажется, что показания стали еще хуже. Углы плавают. Что здесь не так?
