Đăng vào

STM32 GNU MCU Eclipse: AHRS MPU9255

Giới thiệu

Tiếp tục với các tutorial lập trình trên kit STM32F4 với bộ công cụ GNU MCU Eclipse, bài viết này giới thiệu việc áp dụng giải thuật Attitude and Heading Reference System (AHRS) lên MPU9255 sử dụng bộ thư viện STM32F4 HAL và thiết lập mode Debug và Release.

Sử dụng code trong chương trình này chúng ta có thể lập trình các module trên STM32F4

  • Giao tiếp I2C
  • Giao tiếp USART
  • Giải thuật AHRS

Software và Hardware sử dụng

Sơ đồ đấu dây

Module USB UART CP2102 - STM32F4 Discovery

  • 5V - VCC
  • GND - GND
  • RXD - PC6
  • TXD - PC7

Waveshare 10DOF IMU - STM32F4 Discovery

  • VCC - VCC
  • GND - GND
  • SDA - PB9
  • SCL - PB8

Code main.c

Trong chương trình mình sử dụng macro

#ifdef TRACE
        trace_printf("Device error!\n");
#endif

Tức code trong macro sẽ được thực thi ở Build config Debug, cụ thể là printf lên debug console. Khi build ở Build config Release thì đoạn code ở trên sẽ không thực thi. Ngược lại thì đoan code trong macro dưới đây sẽ được thực thi ở Build config Release mà không được thực thi ở Build config Debug cụ thể ở macro này là truyền chuỗi qua chuẩn UART ở chế độ Release

#ifdef NDEBUG
    sprintf(mesg,"Device error \n");
    USART_Puts(USART6, mesg);
#endif

Cấu trúc chương trình chính main.c

/**
 * GNU MCU Eclipse project example for MPU9255
 * @author    nvtienanh
 * @email     nvtienanh@gmail.com
 * @ide       GNU MCU Eclipse
 * @packs     STM32F4xx/STM32F7xx Keil packs are requred with HAL driver support
 * @stdperiph STM32F4xx/STM32F7xx HAL drivers required
 */
/* Include core modules */
#include #include #include "diag/Trace.h"
#include "stm32fxxx_hal.h"
#include "defines.h"
#include "stm32_disco.h"
#include "stm32_delay.h"
#include "stm32_mpu9250.h"
#include "stm32_usart.h"
#include "stm32_exti.h"
#include "stm32_ahrs_imu.h"

MPU9250_t MPU9250;

AHRSIMU_t IMU;
// ----- main() ---------------------------------------------------------------

// Sample pragmas to cope with warnings. Please note the related line at
// the end of this function, used to pop the compiler diagnostics status.
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-parameter"
#pragma GCC diagnostic ignored "-Wmissing-declarations"
#pragma GCC diagnostic ignored "-Wreturn-type"

char mesg[20];

int main(int argc, char* argv[])
{
	/* Init system clock for maximum system speed */
	RCC_InitSystem();
    HAL_Init();
	/* Delay init */
	DELAY_Init();
    /* Init LEDs */
    DISCO_LedInit();
    /* Init USART, TX = PA2, RX = PA3 */
    USART_Init(USART6, USART_PinsPack_1, 115200);
#ifdef TRACE
        trace_printf("Start MUP9250\n");
#endif
#ifdef NDEBUG
    	sprintf(mesg,"Start MUP9250\n");
    	USART_Puts(USART6, mesg);
#endif
    /* Init MPU9250 */
    if (MPU9250_Init(&MPU9250, MPU9250_Device_0) != MPU9250_Result_Ok)
    {
#ifdef TRACE
        trace_printf("Device error!\n");
#endif
#ifdef NDEBUG
    	sprintf(mesg,"Device error \n");
    	USART_Puts(USART6, mesg);
#endif
        while (1);
    }
#ifdef TRACE
    trace_printf("Device connected \r\n");
#endif
#ifdef NDEBUG
	sprintf(mesg,"Device connected \n");
	USART_Puts(USART6, mesg);
#endif
    EXTI_Attach(GPIOA, GPIO_PIN_0, EXTI_Trigger_Rising);
    AHRSIMU_Init(&IMU, 1000, 0.5, 0);
    while (1)
    {
        if (MPU9250_DataReady(&MPU9250) == MPU9250_Result_Ok)
        {
            MPU9250_ReadAcce(&MPU9250);
            MPU9250_ReadGyro(&MPU9250);
            MPU9250_ReadMag(&MPU9250);
            AHRSIMU_UpdateIMU(&IMU, MPU9250.Gx, MPU9250.Gy, MPU9250.Gz, MPU9250.Ax, MPU9250.Ay, MPU9250.Az);
#ifdef TRACE
            trace_printf("R: %3.3f, P: %3.3f, Y: %3.3f\n", IMU.Roll, IMU.Pitch, IMU.Yaw);
            trace_printf("Ax: %f, Ay: %f, Az: %f, Gx: %f, Gy: %f, Gz: %f\n", \
            		MPU9250.Ax, MPU9250.Ay, MPU9250.Az, MPU9250.Gx, MPU9250.Gy, MPU9250.Gz);
            trace_printf("Mx: %d, My: %d, Mz: %d\r\n", MPU9250.Mx, MPU9250.My, MPU9250.Mz);
#endif
#ifdef NDEBUG
            sprintf(mesg,"R: %3.3f, P: %3.3f, Y: %3.3f\r\n", IMU.Roll, IMU.Pitch, IMU.Yaw);
            USART_Puts(USART6, mesg);
#endif

        }
	}
}

Chương trình trên có thể có nhiều bug..., bài viết này chỉ giới thiệu cách sử dụng là chính, tùy yêu cầu và ứng dụng có thể tối ưu hoặc chỉnh sửa theo nhu cầu riêng.

CHIA SẺ