LINUX.ORG.RU

История изменений

Исправление 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 градус и никуда не уплывает (просто может колебаться чуть-чуть на одном месте в пределах этого диапазона), если есть большое ускорение точность падает, но лишь пока оно не прекратиться.