nRF52832 SSM - 9axis IMU ICM-20948(MPU9250) BLE 전송 테스트 (Roll, Pitch, Yaw 그래프 출력)

nRF52832 SSM 보드에는 9축 IMU센서 ICM-20948(MPU9250)가 실장되어 있다. 3축 자세 정보를 측정해서 BLE를 통해 전송하는 테스트를 해 보자.

 

BLE 서비스는 BLE-UART를 이용하고 ICM-20948(MPU9250)의 센서 데이터를 주기적(10ms)로 전송하는 코드

#include <bluefruit.h>
#include <MPU9250.h>


#define LED_BUILTIN 22

// BLE Service
BLEUart bleuart; // uart over ble
MPU9250 mpu;


void InitSensor(void)
{
  Wire.begin();
  delay(2000);
  
  if (!mpu.setup(0x68)) {  // change to your own address
      while (1) {
          Serial.println("MPU connection failed. Please check your connection with `connection_check` example.");
          delay(5000);
      }
  }

  //print_calibration();
  mpu.verbose(false);  
}

void print_roll_pitch_yaw(void) {
    //Serial.print("Yaw, Pitch, Roll: ");

    float Yaw, Pitch, Roll;
    Roll = mpu.getRoll();
    Pitch = mpu.getPitch();
    Yaw = mpu.getYaw();

    Serial.print(Roll, 2);
    Serial.print(", ");
    Serial.print(Pitch, 2);
    Serial.print(", ");
    Serial.println(Yaw, 2);

    bleuart.print(Roll, 2);
    bleuart.print(", ");
    bleuart.print(Pitch, 2);
    bleuart.print(", ");
    bleuart.println(Yaw, 2);
}

void setup()
{
  pinMode(LED_BUILTIN, OUTPUT);
  digitalWrite(LED_BUILTIN, LOW);

  Serial.begin(115200);
  Serial.println("nRF52832 SSM BLE-UAR Test");


  InitSensor();

  // Setup the BLE LED to be enabled on CONNECT
  Bluefruit.autoConnLed(true);

  // Config the peripheral connection with maximum bandwidth 
  // more SRAM required by SoftDevice
  // Note: All config***() function must be called before begin()
  Bluefruit.configPrphBandwidth(BANDWIDTH_MAX);

  Bluefruit.begin();
  Bluefruit.setTxPower(4); 
  
  //콜백 함수 등록  
  Bluefruit.Periph.setConnectCallback(connect_callback);
  Bluefruit.Periph.setDisconnectCallback(disconnect_callback);

  // Configure and Start BLE Uart Service
  bleuart.begin();

  // Set up and start advertising
  startAdv();
  Serial.println("start advertising");
}

void startAdv(void)
{
  // Advertising packet
  Bluefruit.Advertising.addFlags(BLE_GAP_ADV_FLAGS_LE_ONLY_GENERAL_DISC_MODE);
  Bluefruit.Advertising.addTxPower();

  //bleuart 서비스 등록
  Bluefruit.Advertising.addService(bleuart);
  Bluefruit.ScanResponse.addName();

  Bluefruit.Advertising.restartOnDisconnect(true);
  Bluefruit.Advertising.setInterval(32, 244);    // in unit of 0.625 ms
  Bluefruit.Advertising.setFastTimeout(30);      // number of seconds in fast mode
  Bluefruit.Advertising.start(0);                // 0 = Don't stop advertising after n seconds  
}

void loop()
{
  if (mpu.update()) {
      static uint32_t prev_ms = millis();
      if (millis() > prev_ms + 10) {
          
          //print_rawdata();
          print_roll_pitch_yaw();
          
          //print_roll_pitch_yaw();
          prev_ms = millis();
      }
  }

  //BLE에서 데이터가 전송되면
  while ( bleuart.available() )
  {
    uint8_t ch;
    ch = (uint8_t) bleuart.read();
    
    //시리얼 포트로 표시
    Serial.write(ch);

    if(ch=='a')digitalWrite(LED_BUILTIN, 0);
    else if(ch=='b')digitalWrite(LED_BUILTIN, 1);
  }
}

//연결시 수행되는 콜백 함수
void connect_callback(uint16_t conn_handle)
{
  // Get the reference to current connection
  BLEConnection* connection = Bluefruit.Connection(conn_handle);

  char central_name[32] = { 0 };
  connection->getPeerName(central_name, sizeof(central_name));

  Serial.print("Connected to ");
  Serial.println(central_name);
}

//연결 해제시 수행되는 콜백함수
void disconnect_callback(uint16_t conn_handle, uint8_t reason)
{
  (void) conn_handle;
  (void) reason;

  Serial.println();
  Serial.print("Disconnected, reason = 0x"); Serial.println(reason, HEX);
}

 

프로그램 실행후 접속해서 그래프로 출력하면 ICM-20948(MPU9250)의 센서의 Roll, Pitch, Yaw 값이 출력 되는 것을 확인 할 수 있다.