How to convert a string of Euler angles characters read by the 9DOF Razor IMU board into numeric values.

To guide the quadcopter in the space I will use the 9DOF Razor IMU board with the 9 Degree of Measurement Attitude and Heading Reference System (AHRS) code base. This board with AHRS code has the advantage of doing all the calculations and corrections for me. All I need to do is work out the interface. With the code one can chooses three options for receiving the data according to the constants that we define. If one sets the defined value "PRINT_DCM" to 1 then one will print the whole direction cosine matrix. If one sets the defined value "PRINT_ANALOG" to 1 then one will print the analog raw data. If one sets the defined value "PRINT_EULER" to 1 then one will print the Euler angles roll, pitch and yaw. These options are selected in the AHRS code before being compiled.

But first one needs to get the data in a format that can then be used by the Seeeduino Mega board to do the calculations to orient and stabilize the quadcopter. Reading the stream data from the 9DOF Razor IMU to the Seeeduino Mega board is a simple task and it has been shown in a previous post in which the data was transferred via a serial connection between the two boards - the diagram of the assembly can be seen in the image of the this previous post.

The data that I choose consist of the Euler angles roll, pitch and yaw and are sent by the 9DOF Razor IMU board sequentially in a text line. The line has the following format: "!ANG:0.00,0.00,0.00\r\n". To use the angles are necessary to split the line and then convert the numeric characters string parts to a numeric value (in this case a floating point number). After several failed attempts to read stream data sequentially I discovered that if I did not blocking the serial reads until the 9DOF Razor IMU board send the data often do not find the end of the text line which results in incorrect values and buffer overflow problems. To solve the problem of the serial reads may block permanently due to faulty connection with the 9DOF Razor IMU board was added a timeout. If at the end of the timeout has not been read the line the program continues its execution whiles keeping the previous angles values. This, procedure is important because if the Seeeduino Mega board lost the connection with 9DOF Razor IMU board the program can predicts code for example that here the motors are turned off and is activated a parachute, that save the quadcopter from crashing.

In the following sketch we can check the path followed to obtain the Euler angles in a numerical format from the 9DOF Razor IMU board.

#define TIMEOUT 100

// { roll, pitch, yaw }
float angles[3] = {0.0, 0.0, 0.0};

char read_char() {
  unsigned long starttime = millis();
  do {
    if(Serial1.available() > 0)
      return((char)Serial1.read());
  } while((millis() - starttime) < TIMEOUT);
  return NULL;
}

void get_angles() {
  for(int n=0; n<3; n++) {
    char buffer[9];
    int received = 0;
    char ch;
    do {
      if((ch = read_char()) == NULL) return;
    } while(ch != ',' && ch != '\r' && (buffer[received++] = ch));   
    buffer[received] = '\0';
    angles[n] = atof(buffer);
  }
}

void read_9dofahrs() {
  char beginstr[6] = {'!','A','N','G','\:','\0'};
  for(int i=0; i<5; i++)
    if(read_char() != beginstr[i]) return;
  get_angles();
}

void setup() {
  Serial.begin(57600);
  Serial1.begin(57600);
}

void loop() {
  read_9dofahrs();

  Serial.print(angles[0]);
  Serial.print(" | ");
  Serial.print(angles[1]);
  Serial.print(" | ");
  Serial.print(angles[2]);
  Serial.println("");
}

The result of this sketch can be seen in the following figure.

This short sketch will then be added to the main program that will control the stabilization and orientation in space of the quadcopter.