AVR IMU

관성 측정 장치(Inertial Measurement Unit)
GY-9250(MPU-9250/6500)
MPU9250: 가속도, 자이로, 지자기 각각의 센서의 3축을 담은 모듈

프로젝트의 flow chart

센서의 Raw 데이터를 읽어와서 처리를 진행

• 보드와 GY-9250간 I2C 통신.

int16_t read_mpu9250_register16(uint8_t reg_addr) {
	I2C_START();
	I2C_WRITE((MPU9250_ADDRESS << 1) | 0);    // LSB 0: WRITE
	I2C_WRITE(reg_addr);
	I2C_START();
	I2C_WRITE((MPU9250_ADDRESS << 1) | 1);    // LSB 1: READ
	int16_t data = (I2C_READ_ACK() << 8) | I2C_READ_NACK();
	I2C_STOP();
	return data;
}

• 센서 모듈을 납땜을 하지 않아 연결이 처음부터 안되어 있을 경우가 많았음. 그래서 연결이 끊어져있다가 연결을 시도했을 경우에 대한 처리를 해보려 함.

uint8_t I2C_READ_ACK() {
	TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWEA);

	TCNT1 = 0;  // Time out Counter
	TCCR1B |= (1 << CS12) | (1 << CS10);  // Timer 1024 prescaler

	while (!(TWCR & (1 << TWINT))) {
		if (TCNT1 > 1000) {	// 1sec
			break;
		}
	}
	
	TCCR1B = 0;
	return TWDR;    // Higher 8bit
}

uint8_t I2C_READ_NACK() {
	TWCR = (1 << TWINT) | (1 << TWEN);
	
	TCNT1 = 0;
	TCCR1B |= (1 << CS12) | (1 << CS10);

	while (!(TWCR & (1 << TWINT))) {
		if (TCNT1 > 1000) {
			break;
		}
	}

	TCCR1B = 0;
	return TWDR;    // Lower 8bit
}

ACK와 NACK를 받는 부분에서 TWINT부분에 대한 처리가 이루어 지지 않는 것을 디버깅을 통해 알아냄.
READ_ACK, READ_NACK 내부에 타이머를 설정하여 TWINT가 정상적으로 쓰여졌을 경우에 빠져나오게 코드를 짬.
(통신이 완전히 끊겨있다가 연결되는 경우는 처리 가능하지만, 통신 중간에 끊길 경우는 해결 X)

• ACC, GYRO 센서 데이터를 상보 필터(Complementary Filter)를 거쳐 Roll(x), Pitch(y), Yaw(z) 값을 얻어냄.

float accel_scale = 2.0;    // set accel scale ±2g
float gyro_scale = 2000.0; // set gyro scale ±2000°/sec
float dt = 0.01;	// Time interval(PC)

void ComplementaryFilter(float accel_x, float accel_y, float accel_z, float gyro_x, float gyro_y, float gyro_z, float dt) {
	roll = atan2(accel_y, accel_z) * (180.0 / M_PI);
	pitch = atan(-accel_x / sqrt(accel_y * accel_y + accel_z * accel_z)) * (180.0 / M_PI);

	yaw += gyro_z * dt;
}

• PC에서 시리얼 통신으로 10ms마다 MCU에게 특정 문자를 보냄으로서 처리가 이루어짐.

ISR(USART_RX_vect) {
	Received_char = UDR0;
	Data_Received = 1;
}

while (1) {
		int16_t accel_x_raw = read_mpu9250_register16(ACCEL_XOUT);
		int16_t accel_y_raw = read_mpu9250_register16(ACCEL_YOUT);
		int16_t accel_z_raw = read_mpu9250_register16(ACCEL_ZOUT);
		int16_t gyro_x_raw = read_mpu9250_register16(GYRO_XOUT);
		int16_t gyro_y_raw = read_mpu9250_register16(GYRO_YOUT);
		int16_t gyro_z_raw = read_mpu9250_register16(GYRO_ZOUT);
		
		float gyro_x_data = gyro_x_raw / gyro_scale;
		float gyro_y_data = gyro_y_raw / gyro_scale;
		float gyro_z_data = gyro_z_raw / gyro_scale;
		float accel_x_data = accel_x_raw / accel_scale;
		float accel_y_data = accel_y_raw / accel_scale;
		float accel_z_data = accel_z_raw / accel_scale;
		// pass to filter
		ComplementaryFilter(accel_x_data, accel_y_data, accel_z_data, gyro_x_data, gyro_y_data, gyro_z_data, dt);
		
		// MPU9250 raw data Print LCD
		LCD_setPosition(0, 0);
		char IMU_Roll_str[7];
		snprintf(IMU_Roll_str, sizeof(IMU_Roll_str), "%+05.3f", roll);
		LCD_sendString(IMU_Roll_str);

		
		LCD_setPosition(8, 0);
		char IMU_Pitch_str[7];
		snprintf(IMU_Pitch_str, sizeof(IMU_Pitch_str), "%+05.3f", pitch);
		LCD_sendString(IMU_Pitch_str);
		
		LCD_setPosition(0, 1);
		char IMU_Yaw_str[7];
		snprintf(IMU_Yaw_str, sizeof(IMU_Yaw_str),"%+05.3f", yaw);
		LCD_sendString(IMU_Yaw_str);
		
		LCD_setPosition(8, 1);
		LCD_sendString("IMU_XYZ");
		
		if (Data_Received) {
			//_delay_ms(20);
			UART_Transmit_data("Roll", roll);
			UART_Transmit_data("Pitch", pitch);
			UART_Transmit_data("Yaw", yaw);
			UART_Transmit_char('\n');	// if no line break, looping
			Data_Received = 0;
		}
		
	}

참고 :https://blog.naver.com/speedprinse/221192173624

github: https://github.com/NGU516/AVR/blob/main/IMU/IMU/main.c