Исправление KivApple, (текущая версия) :
void sensors_init() {
_delay_ms(20);
mpu6050_write(MPU6050_PWR_MGMT_1, MPU6050_CLKSEL_INTERNAL);
_delay_ms(10);
mpu6050_write(MPU6050_SMPLRT_DIV, 0);
mpu6050_write(MPU6050_CONFIG, MPU6050_EXT_SYNC_DISABLED | MPU6050_DLPF_44HZ);
mpu6050_write(MPU6050_GYRO_CONFIG, MPU6050_FS_SEL_500);
mpu6050_write(MPU6050_ACCEL_CONFIG, MPU6050_AFS_SEL_4G);
_delay_ms(10);
sensors_read(true);
}
float normalize_angle(float angle) {
if (angle > M_PI) {
angle = -M_PI + (angle - M_PI);
} else if (angle < -M_PI) {
angle = M_PI + (angle + M_PI);
}
return angle;
}
void sensors_read(bool first) {
sensors_need_read = false;
sensors_read_count++;
// Read data
temperature = mpu6050_read_data(MPU6050_TEMP_OUT_H) / 340.0 + 36.53;
float gyro_dx = (float)mpu6050_read_data(MPU6050_GYRO_XOUT_H) / 65.5 / 180.0 * M_PI;
float gyro_dy = (float)mpu6050_read_data(MPU6050_GYRO_YOUT_H) / 65.5 / 180.0 * M_PI;
float gyro_dz = (float)mpu6050_read_data(MPU6050_GYRO_ZOUT_H) / 65.5 / 180.0 * M_PI;
accel_x = (float)mpu6050_read_data(MPU6050_ACCEL_XOUT_H) / 8192.0;
accel_y = (float)mpu6050_read_data(MPU6050_ACCEL_YOUT_H) / 8192.0;
accel_z = (float)mpu6050_read_data(MPU6050_ACCEL_ZOUT_H) / 8192.0;
// Calculate angle by accelerometer
float accel_rx = atan(accel_x / sqrt(accel_y * accel_y + accel_z * accel_z));
float accel_ry = atan(accel_y / sqrt(accel_x * accel_x + accel_z * accel_z));
// Check NaN and INF
if ((accel_rx != accel_rx) || (accel_rx == INFINITY) || (accel_rx == -INFINITY)) accel_rx = gyro_x;
if ((accel_ry != accel_ry) || (accel_ry == INFINITY) || (accel_ry == -INFINITY)) accel_ry = gyro_y;
// Appy gyroscope value to current angle
if (first) {
gyro_x = accel_rx;
gyro_y = accel_ry;
gyro_z = 0.0;
} else {
gyro_x += gyro_dx / SENSORS_FREQ;
gyro_y += gyro_dy / SENSORS_FREQ;
gyro_z += gyro_dz / SENSORS_FREQ;
gyro_x = normalize_angle(gyro_x);
gyro_y = normalize_angle(gyro_y);
gyro_z = normalize_angle(gyro_z);
}
// Filter angle
gyro_x = 0.9 * gyro_x + 0.1 * accel_rx;
gyro_y = 0.9 * gyro_y + 0.1 * accel_ry;
}
Алгоритм может и примитивный, но он точно работает. Если ускорение не большое, то точность +-1 градус и никуда не уплывает (просто может колебаться чуть-чуть на одном месте в пределах этого диапазона), если есть большое ускорение точность падает, но лишь пока оно не прекратиться.
У кода есть один минус - он сходит с ума, если угол наклона более +-90 градусов (то есть плату совсем перевернули к верх ногами) и выдаёт бред, пока угол не станет меньше +-90 градусов. Буду признателен, если мне укажут как его доработать.
Исправление KivApple, :
float normalize_angle(float angle) {
if (angle > M_PI) {
angle = -M_PI + (angle - M_PI);
} else if (angle < -M_PI) {
angle = M_PI + (angle + M_PI);
}
return angle;
}
void sensors_read(bool first) {
sensors_need_read = false;
sensors_read_count++;
// Read data
temperature = mpu6050_read_data(MPU6050_TEMP_OUT_H) / 340.0 + 36.53;
float gyro_dx = (float)mpu6050_read_data(MPU6050_GYRO_XOUT_H) / 65.5 / 180.0 * M_PI;
float gyro_dy = (float)mpu6050_read_data(MPU6050_GYRO_YOUT_H) / 65.5 / 180.0 * M_PI;
float gyro_dz = (float)mpu6050_read_data(MPU6050_GYRO_ZOUT_H) / 65.5 / 180.0 * M_PI;
accel_x = (float)mpu6050_read_data(MPU6050_ACCEL_XOUT_H) / 8192.0;
accel_y = (float)mpu6050_read_data(MPU6050_ACCEL_YOUT_H) / 8192.0;
accel_z = (float)mpu6050_read_data(MPU6050_ACCEL_ZOUT_H) / 8192.0;
// Calculate angle by accelerometer
float accel_rx = atan(accel_x / sqrt(accel_y * accel_y + accel_z * accel_z));
float accel_ry = atan(accel_y / sqrt(accel_x * accel_x + accel_z * accel_z));
// Check NaN and INF
if ((accel_rx != accel_rx) || (accel_rx == INFINITY) || (accel_rx == -INFINITY)) accel_rx = gyro_x;
if ((accel_ry != accel_ry) || (accel_ry == INFINITY) || (accel_ry == -INFINITY)) accel_ry = gyro_y;
// Appy gyroscope value to current angle
if (first) {
gyro_x = accel_rx;
gyro_y = accel_ry;
gyro_z = 0.0;
} else {
gyro_x += gyro_dx / SENSORS_FREQ;
gyro_y += gyro_dy / SENSORS_FREQ;
gyro_z += gyro_dz / SENSORS_FREQ;
gyro_x = normalize_angle(gyro_x);
gyro_y = normalize_angle(gyro_y);
gyro_z = normalize_angle(gyro_z);
}
// Filter angle
gyro_x = 0.9 * gyro_x + 0.1 * accel_rx;
gyro_y = 0.9 * gyro_y + 0.1 * accel_ry;
}
Алгоритм может и примитивный, но он точно работает. Если ускорение не большое, то точность +-1 градус и никуда не уплывает (просто может колебаться чуть-чуть на одном месте в пределах этого диапазона), если есть большое ускорение точность падает, но лишь пока оно не прекратиться.
У кода есть один минус - он сходит с ума, если угол наклона более +-90 градусов (то есть плату совсем перевернули к верх ногами) и выдаёт бред, пока угол не станет меньше +-90 градусов. Буду признателен, если мне укажут как его доработать.
Исходная версия KivApple, :
void sensors_read(bool first) {
sensors_need_read = false;
sensors_read_count++;
// Read data
temperature = mpu6050_read_data(MPU6050_TEMP_OUT_H) / 340.0 + 36.53;
float gyro_dx = (float)mpu6050_read_data(MPU6050_GYRO_XOUT_H) / 65.5 / 180.0 * M_PI;
float gyro_dy = (float)mpu6050_read_data(MPU6050_GYRO_YOUT_H) / 65.5 / 180.0 * M_PI;
float gyro_dz = (float)mpu6050_read_data(MPU6050_GYRO_ZOUT_H) / 65.5 / 180.0 * M_PI;
accel_x = (float)mpu6050_read_data(MPU6050_ACCEL_XOUT_H) / 8192.0;
accel_y = (float)mpu6050_read_data(MPU6050_ACCEL_YOUT_H) / 8192.0;
accel_z = (float)mpu6050_read_data(MPU6050_ACCEL_ZOUT_H) / 8192.0;
// Calculate angle by accelerometer
float accel_rx = atan(accel_x / sqrt(accel_y * accel_y + accel_z * accel_z));
float accel_ry = atan(accel_y / sqrt(accel_x * accel_x + accel_z * accel_z));
// Check NaN and INF
if ((accel_rx != accel_rx) || (accel_rx == INFINITY) || (accel_rx == -INFINITY)) accel_rx = gyro_x;
if ((accel_ry != accel_ry) || (accel_ry == INFINITY) || (accel_ry == -INFINITY)) accel_ry = gyro_y;
// Appy gyroscope value to current angle
if (first) {
gyro_x = accel_rx;
gyro_y = accel_ry;
gyro_z = 0.0;
} else {
gyro_x += gyro_dx / SENSORS_FREQ;
gyro_y += gyro_dy / SENSORS_FREQ;
gyro_z += gyro_dz / SENSORS_FREQ;
gyro_x = normalize_angle(gyro_x);
gyro_y = normalize_angle(gyro_y);
gyro_z = normalize_angle(gyro_z);
}
// Filter angle
gyro_x = 0.9 * gyro_x + 0.1 * accel_rx;
gyro_y = 0.9 * gyro_y + 0.1 * accel_ry;
}
Алгоритм может и примитивный, но он точно работает. Если ускорение не большое, то точность +-1 градус и никуда не уплывает (просто может колебаться чуть-чуть на одном месте в пределах этого диапазона), если есть большое ускорение точность падает, но лишь пока оно не прекратиться.