ARDUNIO IMU processing姿态数据可视化

https://www.arduino.cn/thread-42852-1-1.html

关键数据打包

float roll, pitch, heading;

 Serial.print("Orientation: ");
    Serial.print(heading);
    Serial.print(" ");
    Serial.print(pitch);
    Serial.print(" ");
    Serial.println(roll);

　　接受判断关键 13 ascll码换行标志位

发射端

#include <ESP8266WiFi.h>
  
const char *ssid = "esp8266_666";
const char *password = "12345678";
WiFiServer server(8266);
void setup()
{
  Serial.begin(115200);
  Serial.println();
  Serial.print("Setting soft-AP ... ");
    
IPAddress softLocal(192,168,1,1);  
IPAddress softGateway(192,168,1,1);
IPAddress softSubnet(255,255,255,0);
  
WiFi.softAPConfig(softLocal, softGateway, softSubnet);  
WiFi.softAP(ssid, password);
    
IPAddress myIP = WiFi.softAPIP();
  Serial.print("AP IP address: ");
  Serial.println(myIP);
 server.begin();
 Serial.printf("Web server started, open %s in a web browser\n", WiFi.localIP().toString().c_str());
  
}
    float roll, pitch, heading;
void loop()
{
 WiFiClient client = server.available();
 if (client)
  {
    Serial.println("\n[Client connected]");
    while (client.connected())
    {
 
 //  将串口数据打印给TCP
  if(Serial.available()){
    size_t len = Serial.available();
    uint8_t sbuf[len];
    Serial.readBytes(sbuf, len);
   
    client.write(sbuf, len);
    delay(1);
     
    }

    
    roll=roll+10; pitch=pitch+10; heading=heading+10;
    Serial.print("Orientation: ");
    Serial.print(heading);
    Serial.print(" ");
    Serial.print(pitch);
    Serial.print(" ");
    Serial.println(roll);

     delay(1000);
         
  // 将TCP数据打印给串口
      if (client.available())
      {
       //  String line = client.readStringUntil(13);// arduino换行符号  ascll码 13
        String line = client.readStringUntil('\r');
        Serial.println(line);    
          
      }
    }
    delay(1);
 
  //  client.stop();
    Serial.println("[Client disonnected]");
  }
  
 }

接收端

#include <ESP8266WiFi.h>

const char* ssid = "esp8266_666";
const char* password = "12345678";

const char* host = "192.168.1.1";
  const int httpPort = 8266;
  
IPAddress staticIP(192,168,1,22);
IPAddress gateway(192,168,1,1);
IPAddress subnet(255,255,255,0);

WiFiClient client;


void setup(void)
{
  Serial.begin(115200);
  Serial.println();

  Serial.printf("Connecting to %s\n", ssid);
  WiFi.config(staticIP, gateway, subnet);
  WiFi.begin(ssid, password);
  while (WiFi.status() != WL_CONNECTED)
  {
    delay(500);
    Serial.print(".");
  }
  Serial.println();
  Serial.print("Connected, IP address: ");
  Serial.println(WiFi.localIP());


  
  if (!client.connect(host, httpPort)) {
    Serial.println("connection failed");
    return;
  }

  
}

void loop() {
  
 // client.print("abc\r");
 // delay(1000);
   while(client.available()){
   //int newLine = 13; 
    String line = client.readStringUntil(13);//结束标志 换行，新ascll码
   // String line = client.readStringUntil('\r');
    Serial.println(line);
  }
  
  }

IMU程序

注意修改波特率

]#include <CurieIMU.h>
#include <MadgwickAHRS.h>

Madgwick filter;
unsigned long microsPerReading, microsPrevious;
float accelScale, gyroScale;

void setup() {
  Serial.begin(115200);

  // 初始化IMU和滤波器
  CurieIMU.begin();
  CurieIMU.setGyroRate(25);
  CurieIMU.setAccelerometerRate(25);
  filter.begin(25);

  // 设置加速度计测量范围为2G
  CurieIMU.setAccelerometerRange(2);
  // 设置陀螺仪测量范围为+/-250°/s
  CurieIMU.setGyroRange(250);

  // 初始化用于调整更新速率的变量
  microsPerReading = 1000000 / 25;
  microsPrevious = micros();

  //陀螺仪校准
  Serial.print("Starting Gyroscope calibration and enabling offset compensation...");
  CurieIMU.autoCalibrateGyroOffset();
  Serial.println(" Done");

  //加速度计校准
  Serial.print("Starting Acceleration calibration and enabling offset compensation...");
  CurieIMU.autoCalibrateAccelerometerOffset(X_AXIS, 0);
  CurieIMU.autoCalibrateAccelerometerOffset(Y_AXIS, 0);
  CurieIMU.autoCalibrateAccelerometerOffset(Z_AXIS, 1);
  Serial.println(" Done");
}

void loop() {
  int aix, aiy, aiz;
  int gix, giy, giz;
  float ax, ay, az;
  float gx, gy, gz;
  float roll, pitch, heading;
  unsigned long microsNow;

  // 按设定读取频率，读取数据并更新滤波器
  microsNow = micros();
  if (microsNow - microsPrevious >= microsPerReading) {

    // 读取IMU原始数据
    CurieIMU.readMotionSensor(aix, aiy, aiz, gix, giy, giz);

    // convert from raw data to gravity and degrees/second units
    ax = convertRawAcceleration(aix);
    ay = convertRawAcceleration(aiy);
    az = convertRawAcceleration(aiz);
    gx = convertRawGyro(gix);
    gy = convertRawGyro(giy);
    gz = convertRawGyro(giz);

    // 更新滤波器，并进行相关运算
    filter.updateIMU(gx, gy, gz, ax, ay, az);

    // 获取并输出AHRS姿态数据
    roll = filter.getRoll();
    pitch = filter.getPitch();
    heading = filter.getYaw();
    Serial.print("Orientation: ");
    Serial.print(heading);
    Serial.print(" ");
    Serial.print(pitch);
    Serial.print(" ");
    Serial.println(roll);

    // 计时
    microsPrevious = microsPrevious + microsPerReading;
  }
}

float convertRawAcceleration(int aRaw) {
  float a = (aRaw * 2.0) / 32768.0;
  return a;
}

float convertRawGyro(int gRaw) {
  float g = (gRaw * 250.0) / 32768.0;
  return g;
}

上位机processing可视化

注意修改波特率和串口

myPort = new Serial(this, "COM19", 115200);

import processing.serial.*;
Serial myPort;
 
float yaw = 0.0;
float pitch = 0.0;
float roll = 0.0;
 
void setup()
{
  size(600, 500, P3D);
 
  // if you have only ONE serial port active
  //myPort = new Serial(this, Serial.list()[0], 9600); // if you have only ONE serial port active
 
  // if you know the serial port name
  myPort = new Serial(this, "COM19", 115200);                    // Windows
  //myPort = new Serial(this, "/dev/ttyACM0", 9600);             // Linux
  //myPort = new Serial(this, "/dev/cu.usbmodem1217321", 9600);  // Mac
 
  textSize(16); // set text size
  textMode(SHAPE); // set text mode to shape
}
 
void draw()
{
  serialEvent();  // read and parse incoming serial message
  background(255); // set background to white
  lights();
 
  translate(width/2, height/2); // set position to centre
 
  pushMatrix(); // begin object
 
  float c1 = cos(radians(roll));
  float s1 = sin(radians(roll));
  float c2 = cos(radians(pitch));
  float s2 = sin(radians(pitch));
  float c3 = cos(radians(yaw));
  float s3 = sin(radians(yaw));
  applyMatrix( c2*c3, s1*s3+c1*c3*s2, c3*s1*s2-c1*s3, 0,
               -s2, c1*c2, c2*s1, 0,
               c2*s3, c1*s2*s3-c3*s1, c1*c3+s1*s2*s3, 0,
               0, 0, 0, 1);
 
  drawArduino();
 
  popMatrix(); // end of object
 
  // Print values to console
  print(roll);
  print("\t");
  print(pitch);
  print("\t");
  print(yaw);
  println();
}
 
void serialEvent()
{
  int newLine = 13; //13 new line character in ASCII
  String message;
  do {
    message = myPort.readStringUntil(newLine); // read from port until new line
    if (message != null) {
      String[] list = split(trim(message), " ");
      if (list.length >= 4 && list[0].equals("Orientation:")) {
        yaw = float(list[1]); // convert to float yaw
        pitch = float(list[2]); // convert to float pitch
        roll = float(list[3]); // convert to float roll
      }
    }
  } while (message != null);
}
 
void drawArduino()
{
  /* function contains shape(s) that are rotated with the IMU */
  stroke(0, 90, 90); // set outline colour to darker teal
  fill(0, 130, 130); // set fill colour to lighter teal
  box(300, 10, 200); // draw Arduino board base shape
 
  stroke(0); // set outline colour to black
  fill(80); // set fill colour to dark grey
 
  translate(60, -10, 90); // set position to edge of Arduino box
  box(170, 20, 10); // draw pin header as box
 
  translate(-20, 0, -180); // set position to other edge of Arduino box
  box(210, 20, 10); // draw other pin header as box
}

ARDUNIO IMU processing姿态数据可视化

猜你喜欢