Razor 9DOF IMU - AHRS从未同步过

时间:2015-02-07 23:37:10

标签: serial-port arduino processing serial-communication

我目前正在处理Processing sketch https://github.com/ptrbrtz/razor-9dof-ahrs/blob/master/Processing/Razor_AHRS_test/Razor_AHRS_test.pde 因为变量同步,所以永远不会到达第181行 绝不是 TRUE 。所以只有“连接到Razor ......”才出现,没有别的。我打印了frameCount并且它不断增加。

是什么让同步标志成为现实?我需要采取一些身体动作吗?

1 个答案:

答案 0 :(得分:0)

我不熟悉Razor_AHRS,但首先可以检查一下你是否使用了正确的串口?代码使用第一个串行端口,但根据您的配置/操作系统,可能需要更新。 (例如,在OSX上,这可能指向蓝牙端口)

尝试在Arduino中使用串行监视器或CoolTerm以57600波特率连接到Razor串口并尝试发送草图发送的相同信息:

#ob
#o1
#oe0
#s00

在您发送完最后一个数据包之后,您应该在串行监视器/冷却术语中接收这些数据:

#SYNCH00\r\n

至少我收集的是您链接到的源代码。

如果上述方法有效,则意味着您在Processing中发送/接收串行数据的方式有问题。否则,它根本不是处理问题,需要退一步检查您在此设备上的其他资源,并仔细检查电路是否正常。

在处理草图中,我可以看到一些可能出错的事情:

  1. 此条件:if (frameCount == 2)
  2. 此条件:if (serial.available() < token.length())
  3. 理论上的第一个条件应该因延迟调用而起作用,但我建议使用基于millis()的方法来延迟()并使用单独的标志,而不是frameCount本身,以防万一。 同样,第二个条件也应该有效,但是如果由于某种原因在串行通信中出现延迟并且您错过了具有您期望的确切字节数的数据包,那么您将错过此处的同步标志的提示。我建议一次缓冲一个字节,直到你到达最后一个字节。幸运的是,在你的情况下,你知道那是什么(\n)并且有一个函数可以为你做到这一点:Serial bufferUntil()。 在读完SYNC字符串后,需要取消此操作。 或者,您可以手动将字符附加到字符串,直到找到同步时到达\n

    这是我的头脑,没有设备我无法正常测试,但我暗示着这些方面的东西:

    /******************************************************************************************
    * Test Sketch for Razor AHRS v1.4.2
    * 9 Degree of Measurement Attitude and Heading Reference System
    * for Sparkfun "9DOF Razor IMU" and "9DOF Sensor Stick"
    *
    * Released under GNU GPL (General Public License) v3.0
    * Copyright (C) 2013 Peter Bartz [http://ptrbrtz.net]
    * Copyright (C) 2011-2012 Quality & Usability Lab, Deutsche Telekom Laboratories, TU Berlin
    * Written by Peter Bartz (peter-bartz@gmx.de)
    *
    * Infos, updates, bug reports, contributions and feedback:
    *     https://github.com/ptrbrtz/razor-9dof-ahrs
    ******************************************************************************************/
    
    /*
      NOTE: There seems to be a bug with the serial library in Processing versions 1.5
      and 1.5.1: "WARNING: RXTX Version mismatch ...".
      Processing 2.0.x seems to work just fine. Later versions may too.
      Alternatively, the older version 1.2.1 also works and is still available on the web.
    */
    
    import processing.opengl.*;
    import processing.serial.*;
    
    // IF THE SKETCH CRASHES OR HANGS ON STARTUP, MAKE SURE YOU ARE USING THE RIGHT SERIAL PORT:
    // 1. Have a look at the Processing console output of this sketch.
    // 2. Look for the serial port list and find the port you need (it's the same as in Arduino).
    // 3. Set your port number here:
    final static int SERIAL_PORT_NUM = 0;
    // 4. Try again.
    
    
    final static int SERIAL_PORT_BAUD_RATE = 57600;
    
    float yaw = 0.0f;
    float pitch = 0.0f;
    float roll = 0.0f;
    float yawOffset = 0.0f;
    
    PFont font;
    Serial serial;
    
    boolean synched = false;
    boolean isSetup = false;
    int now,wait = 3000;//wait 3s for Razor to bootup
    
    String serialString = ""; 
    int    serialDataLen= 12;
    int[]  serialBytes  = new int[serialDataLen];//4 bytes pair float value * 3 values
    int    byteCount    = 0;
    
    
    // Global setup
    void setup() {
      // Setup graphics
      size(640, 480, OPENGL);
      smooth();
      noStroke();
      frameRate(50);
    
      // Load font
    //  font = loadFont("Univers-66.vlw");
    //  textFont(font);
    
      // Setup serial port I/O
      println("AVAILABLE SERIAL PORTS:");
      println(Serial.list());
      String portName = Serial.list()[SERIAL_PORT_NUM];
      println();
      println("HAVE A LOOK AT THE LIST ABOVE AND SET THE RIGHT SERIAL PORT NUMBER IN THE CODE!");
      println("  -> Using port " + SERIAL_PORT_NUM + ": " + portName);
      try{
        serial = new Serial(this, portName, SERIAL_PORT_BAUD_RATE);
        now = millis();
      }catch(Exception e){
        System.err.println("Error opening serial port " + portName+"!\nPlease double check your port");
        e.printStackTrace();
        exit();
      }
    }
    
    void setupRazor() {
      println("Trying to setup and synch Razor...");
      if(millis() - now >= wait){
      // On Mac OSX and Linux (Windows too?) the board will do a reset when we connect, which is really bad.
      // See "Automatic (Software) Reset" on http://www.arduino.cc/en/Main/ArduinoBoardProMini
      // So we have to wait until the bootloader is finished and the Razor firmware can receive commands.
      // To prevent this, disconnect/cut/unplug the DTR line going to the board. This also has the advantage,
      // that the angles you receive are stable right from the beginning. 
    //  delay(3000);  // 3 seconds should be enough
    
      // Set Razor output parameters
      serial.write("#ob");  // Turn on binary output
      serial.write("#o1");  // Turn on continuous streaming output
      serial.write("#oe0"); // Disable error message output
    
      // Synch with Razor
      serial.clear();  // Clear input buffer up to here
      serial.write("#s00");  // Request synch token
      isSetup = true;
      }
    }
    
    float readFloat(int b0,int b1, int b2, int b3) {
      // Convert from little endian (Razor) to big endian (Java) and interpret as float
      return Float.intBitsToFloat(b0 + (b1 << 8) + (b2 << 16) + (b3 << 24));
    }
    
    void draw() {
       // Reset scene
      background(0);
      lights();
    
      // Sync with Razor 
      if (!synched) {
        textAlign(CENTER);
        fill(255);
        text("Connecting to Razor...", width/2, height/2, -200);
    
        if (!isSetup) setupRazor();  // Set ouput params and request synch token
        return;
      }
    
      // Draw board
      pushMatrix();
      translate(width/2, height/2, -350);
      drawBoard();
      popMatrix();
    
      textFont(font, 20);
      fill(255);
      textAlign(LEFT);
    
      // Output info text
      text("Point FTDI connector towards screen and press 'a' to align", 10, 25);
    
      // Output angles
      pushMatrix();
      translate(10, height - 10);
      textAlign(LEFT);
      text("Yaw: " + ((int) yaw), 0, 0);
      text("Pitch: " + ((int) pitch), 150, 0);
      text("Roll: " + ((int) roll), 300, 0);
      popMatrix();
    }
    void serialEvent(Serial p) {
      if(p.available() > 0){
        if(!synched){
          //wait for \n
          char c = p.readChar();
          serialString += c;
          if(c == '\n' && serialString.endsWith("#SYNCH00\r\n")) synched = true;
        }else{
          //synched, wait for 12 or more bytes
          int b = p.read();
          serialBytes[byteCount++] = b;
          if(byteCount >= serialDataLen){//we've got enough data, parse it
            yaw   = readFloat(serialBytes[0],serialBytes[1],serialBytes[2],serialBytes[3]);
            pitch = readFloat(serialBytes[4],serialBytes[5],serialBytes[6],serialBytes[7]);
            roll  = readFloat(serialBytes[8],serialBytes[9],serialBytes[10],serialBytes[11]);
            byteCount = 0;
          }
        }
      }
    }
    
    
    void keyPressed() {
      switch (key) {
        case '0':  // Turn Razor's continuous output stream off
          serial.write("#o0");
          break;
        case '1':  // Turn Razor's continuous output stream on
          serial.write("#o1");
          break;
        case 'f':  // Request one single yaw/pitch/roll frame from Razor (use when continuous streaming is off)
          serial.write("#f");
          break;
        case 'a':  // Align screen with Razor
          yawOffset = yaw;
      }
    }
    void drawArrow(float headWidthFactor, float headLengthFactor) {
      float headWidth = headWidthFactor * 200.0f;
      float headLength = headLengthFactor * 200.0f;
    
      pushMatrix();
    
      // Draw base
      translate(0, 0, -100);
      box(100, 100, 200);
    
      // Draw pointer
      translate(-headWidth/2, -50, -100);
      beginShape(QUAD_STRIP);
        vertex(0, 0 ,0);
        vertex(0, 100, 0);
        vertex(headWidth, 0 ,0);
        vertex(headWidth, 100, 0);
        vertex(headWidth/2, 0, -headLength);
        vertex(headWidth/2, 100, -headLength);
        vertex(0, 0 ,0);
        vertex(0, 100, 0);
      endShape();
      beginShape(TRIANGLES);
        vertex(0, 0, 0);
        vertex(headWidth, 0, 0);
        vertex(headWidth/2, 0, -headLength);
        vertex(0, 100, 0);
        vertex(headWidth, 100, 0);
        vertex(headWidth/2, 100, -headLength);
      endShape();
    
      popMatrix();
    }
    
    void drawBoard() {
      pushMatrix();
    
      rotateY(-radians(yaw - yawOffset));
      rotateX(-radians(pitch));
      rotateZ(radians(roll)); 
    
      // Board body
      fill(255, 0, 0);
      box(250, 20, 400);
    
      // Forward-arrow
      pushMatrix();
      translate(0, 0, -200);
      scale(0.5f, 0.2f, 0.25f);
      fill(0, 255, 0);
      drawArrow(1.0f, 2.0f);
      popMatrix();
    
      popMatrix();
    }