EEPW论坛

Arduino Nano RP2040 Connect开发板上安装了一颗6轴惯性测量单元传感器，其位置如下：

LSM6DSOXTR 是一款 6 轴 IMU（惯性测量单元）系统级封装，具有 3 轴数字加速度计和 3 轴数字陀螺仪，在高性能模式下以 0.55 mA 的速度提高性能，并支持始终开启的低功耗功能，为消费者提供最佳运动体验。

LSM6DSOXTR支持主要的操作系统要求，提供 9 KB 的真实、虚拟和批处理传感器，用于动态数据批处理。意法半导体的 MEMS 传感器模块系列利用了已经用于生产微机械加速度计和陀螺仪的稳健而成熟的制造工艺。各种传感元件采用专门的微加工工艺制造，而 IC 接口则使用 CMOS 技术开发，允许设计专用电路，该电路经过调整以更好地匹配传感元件的特性。

LSM6DSOXTR的满量程加速度范围为 ±2/±4/±8/±16 g，角速率范围为 ±125/±250/±500/±1000/±2000 dps。

3轴加速度计是用来测量加速度的机电装置，可以是静态的重力加速度或者运动物体的时刻变化的加速度等。利用加速度传感器能够计算出板子倾斜的方向或者角度。

3轴数字陀螺仪：

陀螺仪传感器是一种能够测量和保持物体的方向和角速度的装置。陀螺仪比加速度计更先进，因为它们可以测量物体的倾斜和横向方向，而加速度计只能测量物体的直线运动。

陀螺仪图示。

这颗传感器能测出3轴加速度，3轴角速度。后续根据这6个值根据特定算法（Madgwick）可以计算出开发板相对于初始位置时候的欧拉角。

注：Madgwick算法是另外一种九轴融合的方法，广泛应用在旋翼飞行器上，效果也蛮不错的。网上已经有很多madgwick算法的源代码了。在Arduino环境下，需要安装如下库。

据此进而旋转TFT屏幕上的小立方体，同时RP2040会把计算出来的欧拉角通过UART发送给PC上的上位机串口工具，进而旋转上位机上的小立方体。实现的效果是TFT屏幕上的小立方体与上位机一起旋转。

程序流程图：

TFT屏幕上绘制简单的立方体是比较简单的，8个顶点，12条边。立方体是通过矩阵的方式描述，下面是8个顶点与12条边。

pt << -1, -1, 1, 1,  //A 0
    1, -1, 1, 1,       //B 1
    1, 1, 1, 1,        //c 2
    -1, 1, 1, 1,       //d 3
    -1, -1, -1, 1,     //e 4
    1, -1, -1, 1,      //f 5
    1, 1, -1, 1,       //g 6
    -1, 1, -1, 1;      //h 7

  tp << 0, 1,  //A-B
    1, 2,      //b-c
    2, 3,      //c-d
    3, 0,      //d-a
    0, 4,      //a-e
    1, 5,      //b-f
    2, 6,      //c-g
    3, 7,      //d-h
    4, 5,      //e-f
    5, 6,      //f-g
    6, 7,      //g-h
    7, 4;      //h-e

在TFT屏幕上绘制立方体：

void drawTube(const MatrixXd& m, uint16_t color) {
  int nr = tp.rows();  //行数
  for (int i = 0; i < nr; i++) {
    auto start = m.row(tp(i, 0));
    auto end = m.row(tp(i, 1));
    tft.drawLine(start(0), start(1), end(0), end(1), color);
  }
}

之后的顶点需要根据欧拉角进行旋转，这就需要使用Eigen库。一个线性代数库。

核心代码：

// Includes
#include <Arduino_LSM6DS3.h>
#include <ArduinoEigen.h>
#include <MadgwickAHRS.h>
#include <Adafruit_GFX.h>     // Core graphics library
#include <Adafruit_ST7789.h>  // Hardware-specific library for ST7789
#include <SPI.h>

float ax, ay, az;
float gx, gy, gz;

// Defines
#define SAMPLE_RATE 10  // in Hz

#define TFT_CS D10  // Chip select control pin
#define TFT_RST D4  // Reset pin (could connect to Arduino RESET pin)
#define TFT_DC D12  // Data Command control pin
#define TFT_MOSI D11
#define TFT_SCLK D13
#define TFT_MISO D15

//Adafruit_ST7789 tft = Adafruit_ST7789(TFT_CS, TFT_DC, TFT_MOSI, TFT_SCLK, TFT_RST);
Adafruit_ST7789 tft = Adafruit_ST7789(TFT_CS, TFT_DC, TFT_RST);

// Constructors
Madgwick filter;  // Madgwick algorithm for roll, pitch, and yaw calculations

/*********************************Cube Related Begin***************************/
using namespace Eigen;
struct Line {
  uint8_t start;
  uint8_t end;
};

MatrixXd pt(8, 4);   //立边体顶点
MatrixXi tp(12, 2);  //立方体边

MatrixXd tm0(4, 4);  //放大60位
MatrixXd tm1(4, 4);  //平移
Matrix4d txr;
Matrix4d tyr;
Matrix4d tzr;

double xa;
double ya;
double za;
char buffer[8];  // string buffer for use with dtostrf() function

void drawTube(uint16_t color) {
  int nr = tp.rows();  //行数
  for (int i = 0; i < nr; i++) {
    auto start = pt.row(tp(i, 0));
    auto end = pt.row(tp(i, 1));
    tft.drawLine(start(0), start(1), end(0), end(1), color);
  }
}

void drawTube(const MatrixXd& m, uint16_t color) {
  int nr = tp.rows();  //行数
  for (int i = 0; i < nr; i++) {
    auto start = m.row(tp(i, 0));
    auto end = m.row(tp(i, 1));
    tft.drawLine(start(0), start(1), end(0), end(1), color);
  }
}

//angle是角度
void SetXRotationTranslation(double angle) {
  double tmp = 3.1415926 * angle / 180.0;
  double c = cos(tmp);
  double s = sin(tmp);
  txr(1, 1) = c;
  txr(1, 2) = s;
  txr(2, 1) = -s;
  txr(2, 2) = c;
}

//angle是角度
void SetYRotationTranslation(double angle) {
  double tmp = 3.1415926 * angle / 180.0;
  double c = cos(tmp);
  double s = sin(tmp);
  tyr(0, 0) = c;
  tyr(0, 2) = -s;
  tyr(2, 0) = s;
  tyr(2, 2) = c;
}

//angle是角度
void SetZRotationTranslation(double angle) {
  double tmp = 3.1415926 * angle / 180.0;
  double c = cos(tmp);
  double s = sin(tmp);
  tzr(0, 0) = c;
  tzr(0, 1) = s;
  tzr(1, 0) = -s;
  tzr(1, 1) = c;
}
/*********************************Cube Related End***************************/

void setup() {
  Serial.begin(9600);  // initialize serial bus (Serial Monitor)
  // while (!Serial)
  //   ;  // wait for serial initialization
  Serial.print("LSM6DS3 IMU initialization ");
  if (IMU.begin()) {  // initialize IMU
    Serial.println("completed successfully.");
  } else {
    Serial.println("FAILED.");
    IMU.end();
    while (1)
      ;
  }
  Serial.println();
  filter.begin(SAMPLE_RATE);  // initialize Madgwick filter

  // OR use this initializer (uncomment) if using a 1.14" 240x135 TFT:
  tft.init(135, 240);  // Init ST7789 240x135

  // SPI speed defaults to SPI_DEFAULT_FREQ defined in the library, you can override it here
  // Note that speed allowable depends on chip and quality of wiring, if you go too fast, you
  // may end up with a black screen some times, or all the time.
  //tft.setSPISpeed(40000000);

  Serial.println(F("Initialized"));

  uint16_t time = millis();
  tft.fillScreen(ST77XX_BLACK);
  time = millis() - time;

  Serial.println(time, DEC);
  delay(500);

  Serial.println("done");
  delay(1000);

  tft.fillScreen(ST77XX_BLACK);
  /*********************************Cube Related End***************************/
  pt << -1, -1, 1, 1,  //A 0
    1, -1, 1, 1,       //B 1
    1, 1, 1, 1,        //c 2
    -1, 1, 1, 1,       //d 3
    -1, -1, -1, 1,     //e 4
    1, -1, -1, 1,      //f 5
    1, 1, -1, 1,       //g 6
    -1, 1, -1, 1;      //h 7

  tp << 0, 1,  //A-B
    1, 2,      //b-c
    2, 3,      //c-d
    3, 0,      //d-a
    0, 4,      //a-e
    1, 5,      //b-f
    2, 6,      //c-g
    3, 7,      //d-h
    4, 5,      //e-f
    5, 6,      //f-g
    6, 7,      //g-h
    7, 4;      //h-e

  tm0 << 30, 0, 0, 0,
    0, 30, 0, 0,
    0, 0, 30, 0,
    0, 0, 0, 1;

  tm1 << 1, 0, 0, 0,
    0, 1, 0, 0,
    0, 0, 1, 0,
    60, 60, 60, 1;

  txr.setIdentity(4, 4);
  tyr.setIdentity(4, 4);
  tzr.setIdentity(4, 4);

  pt *= tm0;  //这个就是我们最初的大个立方体
  /*********************************Cube Related End***************************/
}


void loop() {
  static unsigned long previousTime = millis();
  unsigned long currentTime = millis();
  if (currentTime - previousTime >= 1000 / SAMPLE_RATE) {
    // printValues();
    printRotationAngles();
    previousTime = millis();
  }

  MatrixXd tmp(8, 4);
  double xxa, yya, zza;

  //临界资源处理
  xxa = filter.getRoll();
  yya = filter.getPitch();
  zza = filter.getYaw();

  SetXRotationTranslation(xxa);
  SetYRotationTranslation(yya);
  SetZRotationTranslation(zza);

  tmp = pt;
  tmp *= txr;
  tmp *= tyr;
  tmp *= tzr;
  tmp *= tm1;
drawTube(tmp, ST77XX_GREEN);
#if 0
  tft.setCursor(0, 120);
  tft.setTextSize(2);
  tft.setTextColor(ST77XX_CYAN);
  tft.print("Roll ");
  //tft.println(dtostrf(filter.getRoll(), 4, 0, buffer));
  tft.println(xxa, 1);
  //tft.println(" degree, ");

  tft.print("Pitch ");
  tft.println(yya, 1);
  //tft.println(dtostrf(filter.getPitch(), 4, 0, buffer));
  //tft.println(" degree, ");
  tft.print("Yaw ");
  tft.println(zza, 1);
  //tft.println(dtostrf(filter.getYaw(), 4, 0, buffer));
  //tft.println(" degree");
#endif
  
  delay(30);
  

  #if 0
  tft.setCursor(0, 120);
  tft.setTextSize(2);
  tft.setTextColor(ST77XX_BLACK);
  tft.print("Roll ");
  //tft.println(dtostrf(filter.getRoll(), 4, 0, buffer));
  tft.println(xxa, 1);
  //tft.println(" degree, ");

  tft.print("Pitch ");
  tft.println(yya, 1);
  //tft.println(dtostrf(filter.getPitch(), 4, 0, buffer));
  //tft.println(" degree, ");
  tft.print("Yaw ");
  tft.println(zza, 1);
  //tft.println(dtostrf(filter.getYaw(), 4, 0, buffer));
  //tft.println(" degree");
#endif
drawTube(tmp, ST77XX_BLACK);
  //tft.fillRect(0, 120, 135, 60, ST77XX_BLACK);
}


// Prints IMU values.
void printValues() {


  // Retrieve and print IMU values
  if (IMU.accelerationAvailable() && IMU.gyroscopeAvailable()
      && IMU.readAcceleration(ax, ay, az) && IMU.readGyroscope(gx, gy, gz)) {
    Serial.print("ax = ");
    Serial.print(dtostrf(ax, 4, 1, buffer));
    Serial.print(" g, ");
    Serial.print("ay = ");
    Serial.print(dtostrf(ay, 4, 1, buffer));
    Serial.print(" g, ");
    Serial.print("az = ");
    Serial.print(dtostrf(az, 4, 1, buffer));
    Serial.print(" g, ");
    Serial.print("gx = ");
    Serial.print(dtostrf(gx, 7, 1, buffer));
    Serial.print(" °/s, ");
    Serial.print("gy = ");
    Serial.print(dtostrf(gy, 7, 1, buffer));
    Serial.print(" °/s, ");
    Serial.print("gz = ");
    Serial.print(dtostrf(gz, 7, 1, buffer));
    Serial.println(" °/s");
  }
}


// Prints rotation angles (roll, pitch, and yaw) calculated using the
// Madgwick algorithm.
// Note: Yaw is relative, not absolute, based on initial starting position.
// Calculating a true yaw (heading) angle requires an additional data source,
// such as a magnometer.
void printRotationAngles() {
  char buffer[5];  // string buffer for use with dtostrf() function

  if (IMU.accelerationAvailable() && IMU.gyroscopeAvailable()
      && IMU.readAcceleration(ax, ay, az) && IMU.readGyroscope(gx, gy, gz)) {
    filter.updateIMU(gx, gy, gz, ax, ay, az);  // update roll, pitch, and yaw values

    // Print rotation angles
#if 0  
    Serial.print("Roll = ");
    Serial.print(dtostrf(filter.getRoll(), 4, 0, buffer));
    Serial.print(" °, ");
    Serial.print("Pitch = ");
    Serial.print(dtostrf(filter.getPitch(), 4, 0, buffer));
    Serial.print(" °, ");
    Serial.print("Yaw = ");
    Serial.print(dtostrf(filter.getYaw(), 4, 0, buffer));
    Serial.println(" °");
#endif
#if 1
    float data[3];
    data[0] = filter.getRoll();
    data[1] = filter.getPitch();
    data[2] = filter.getYaw();
    Serial.write((char *)data, sizeof(float) * 3); 
    // 发送帧尾
    char tail[4] = {0x00, 0x00, 0x80, 0x7f};
    Serial.write(tail, 4);
    //Serial.println(filter.getRoll());
    //Serial.println(filter.getPitch());
    //Serial.println(filter.getYaw());
#endif

  }
}

在上位机上调试一下能否正常获取IMU传感器数据：

进而调用上位机的立方体控件，实物效果如下：