【得捷电子Follow Me第二季第4期】Arduino Nano RP2040 Connect 基础任务

鲜de芒果 · 发表于2024-12-29 15:50

【得捷电子Follow Me第二季第4期】Arduino Nano RP2040 Connect 基础任务 [复制链接]

本帖最后由鲜de芒果于 2024-12-29 13:33 编辑

1. 任务要求

学习IMU基础知识，调试IMU传感器，通过串口打印六轴原始数据；

2. 基础知识

IMU (Inertial Measurement Unit) 惯性测量单元是一种检测物体三轴姿态角和加速度的装置，广泛应用于运动控制和精密位移推算。

IMU 主要由三轴陀螺仪和三轴加速度计组成，有时也包括磁力计，形成所谓的 9轴IMU。它通过测量物体在三维空间中的角速度和加速度来解算出物体的姿态。加速度计用于检测载体坐标系统独立三轴的加速度信号，而陀螺仪则检测载体相对于导航坐标系的角速度信号。

IMU被广泛用于需要进行运动控制的设备，如汽车、机器人、潜艇、飞机、导弹和航天器等。在这些应用中，IMU提供的数据有助于实现自主导航和定位，提高设备的导航精度和稳定性。例如，在无人机领域，IMU可以确保飞行器在空中保持稳定，并在GPS信号弱或不可用时提供可靠的导航信息。

总的来说，IMU作为一种高精度的运动状态测量装置，在现代科技和工业领域发挥着重要作用。随着技术的不断进步，IMU的性能和应用范围有望进一步扩展和提升。

根据 Arduino Nano RP2040 Connect 官方手册，如上图所示。Arduino Nano RP2040 Connect 板载了一颗由 ST 公司生产的 LSM6DSOXTR 6轴惯性测量单元（IMU）。使用 Arduino 开发时，有很多的库可供选择用于读取 LSM6DSOXTR 数据。

3. 功能实现

3.1 IMU可视化

本任务要求只需要通过串口打印六轴原始数据，这样无法直观地感知IMU的状态。因此，考虑使用可视化的形式将数据数据展示出来，而IMU比较广泛的一个应用场景便是无人机应用。可以找到很多的无人机上位机来可视化IMU数据，这里我选择使用匿名上位机。匿名上位机通信协议已经定义好了数据格式，并且也是通过串口通信。我们只需要按照匿名上位机通信协议中的格式，将IMU数据通过串口进行发送即可使开发板模拟成无人机的飞行姿态，从而达到IMU数据可视化的目的。

3.2 代码实现

/**
 * FollowMe 2-4 任务2：
 * 1. 学习IMU基础知识，调试IMU传感器，通过串口打印六轴原始数据
 */
#include <Arduino.h>
#include <Arduino_LSM6DSOX.h>
#include "Madgwick.h"

#define DATA_MULTIPLIER 10000           // 数据放大倍数


Madgwick filter;                        // 初始化 Madgwic 算法过滤器，用于姿态估

float Ax, Ay, Az;                       // 三轴加速度计数据
float Gx, Gy, Gz;                       // 三轴陀螺仪数据

uint8_t send_buf[32];                   // 发送数据缓存
uint8_t cnt = 0;                        // 打包数据索引

/**
 * 根据匿名上位机通信协议打包数据，并发送至上位机
 * 
 */
void packData(uint8_t fun, uint8_t *data, uint8_t len);
/**
 * 四元数据 数据打包发送
 */
void sendQuaternionData(int16_t dat1, int16_t dat2, int16_t dat3, int16_t dat4, uint8_t dat5);


void setup() {
  // 初始化串口
  Serial.begin(115200);
  delay(1500);

  // IMU 初始化并启动通信
  while (!IMU.begin()) {
    Serial.println("IMU(LSM6DSOXTR) 初始化失败, 延时1秒后重试!");
    IMU.end();
    delay(1000);
  }

  // 打印采样速率
  Serial.print("加速度计采样速率 ");
  Serial.print(IMU.accelerationSampleRate());
  Serial.println(" Hz");

  Serial.print("陀螺仪采样速率 ");  
  Serial.print(IMU.gyroscopeSampleRate());
  Serial.println(" Hz");
}

void loop() {
  // 读取加速度计数据
  if (IMU.accelerationAvailable()) {
    IMU.readAcceleration(Ax, Ay, Az);
  }

  // 读取陀螺仪数据
  if (IMU.gyroscopeAvailable()) {
    IMU.readGyroscope(Gx, Gy, Gz);
  }

  // 更新IMU数据
  filter.updateIMU(Gx, Gy, Gz, Ax, Ay, Az);

  // 获取四元数据
  float w = filter.getQuatW();
  float x = filter.getQuatX();
  float y = filter.getQuatY();
  float z = filter.getQuatZ();
  sendQuaternionData(w * DATA_MULTIPLIER, x * DATA_MULTIPLIER, y * DATA_MULTIPLIER, z * DATA_MULTIPLIER, 0); // 发送数据至上位机

  delay(50);
}


/**
 * 根据匿名上位机通信协议打包数据，并发送至上位机
 */
void packData(uint8_t fun, uint8_t *data, uint8_t len) {
  cnt = 0;
  uint8_t i = 0;
  if (len > 28) return;                                 // 最多28字节
  send_buf[cnt++] = 0XAA;                               // 帧头
  send_buf[cnt++] = 0xAA;                               // 目标地址
  send_buf[cnt++] = fun;                                // 功能字
  send_buf[cnt++] = len;                                // 数据长度
  for (i = 0; i < len; i ++) send_buf[cnt++] = data[i]; // 复制数据
  uint8_t sumcheck = 0;
  uint8_t addcheck = 0;
  for (i = 0; i < (len + 4); i ++) {
    sumcheck += send_buf[i];                            // 从帧头开始，对每一字节进行求和，直到DATA区结束
    addcheck += sumcheck;                               // 每一字节的求和操作，进行一次sumcheck的累加
  }
  send_buf[cnt++] = sumcheck;                           // 和校验
  send_buf[cnt++] = addcheck;                           // 附加校验

  for (i = 0; i < cnt; i ++) {                          // 通过串口发送至上位机
    Serial.write(send_buf[i]);
  }
}

/**
 * 四元数据 数据打包发送
 */
void sendQuaternionData(int16_t dat1, int16_t dat2, int16_t dat3, int16_t dat4, uint8_t dat5) {
  uint8_t tbuf[18];
  tbuf[0] = dat1 & 0XFF;
  tbuf[1] = (dat1 >> 8) & 0XFF;
  tbuf[2] = dat2 & 0XFF;
  tbuf[3] = (dat2 >> 8) & 0XFF;
  tbuf[4] = dat3 & 0XFF;
  tbuf[5] = (dat3 >> 8) & 0XFF;
  tbuf[6] = dat4 & 0XFF;
  tbuf[7] = (dat4 >> 8) & 0XFF;
  tbuf[8] = dat5 & 0XFF;
  packData(0x04, tbuf, 9);
}