0

I've got a gyro hooked up to an arduino and I'm getting angular rate out in rad/sec in all three axes.

I want to be able to get out yaw, pitch, roll in body coordinates so the three axes of rotation are fixed to the body. The problem I'm having now is that when I roll the sensor, the yaw and pitch I get out become swapped. As I roll the sensor 90 degrees, the yaw and pitch change places. Anywhere in between, the yaw and pitch are a mixture between the two.

Instead, I want to keep the pitch and yaw relative to the new body rotation rather than the initial position.

Here is my code:

void loop() {
  currentTime = millis();
  dt = ((currentTime - prevTime) / 1000.0 );
  
  // Puts gyro data into data[2], data[4], data[5]
  readBMI();

  if(firstPass == false) {
    
    omega[0] = (data[3]);
    omega[1] = (data[4]);
    omega[2] = (data[5]);

    wLength = sqrt(sq(omega[0]) + sq(omega[1]) + sq(omega[2]));
    
    theta = wLength * dt;
    q_new[0] = cos(theta/2);
    q_new[1] = (omega[0] / wLength * sin(theta / 2));
    q_new[2] = (omega[1] / wLength * sin(theta / 2));
    q_new[3] = (omega[2] / wLength * sin(theta / 2));

    q[0] = q[0] * q_new[0] - q[1] * q_new[1] - q[2] * q_new[2] - q[3] * q_new[3];
    q[1] = q[0] * q_new[1] + q[1] * q_new[0] + q[2] * q_new[3] - q[3] * q_new[2];
    q[2] = q[0] * q_new[2] - q[1] * q_new[3] + q[2] * q_new[0] + q[3] * q_new[1];
    q[3] = q[0] * q_new[3] + q[1] * q_new[2] - q[2] * q_new[1] + q[3] * q_new[0];

    float sinr_cosp = 2 * (q[0] * q[1] + q[2] * q[3]);
    float cosr_cosp = 1 - 2 * (sq(q[1]) + sq(q[2]));
    roll = atan2(sinr_cosp, cosr_cosp)  * 180 / PI;

    pitch = asin(2 * (q[0] * q[2] - q[3] * q[1]))  * 180 / PI;

    double siny_cosp = 2 * (q[0] * q[3] + q[1] * q[2]);
    double cosy_cosp = 1 - 2 * (sq(q[2]) + sq(q[3]));
    yaw = atan2(siny_cosp, cosy_cosp)  * 180 / PI;

  }

  Serial.print(roll);
  Serial.print(" ");
  Serial.print(pitch);
  Serial.print(" ");
  Serial.print(yaw);
  Serial.print(" ");
  Serial.println();

  delay(20);
  prevTime = currentTime;
}

I'm getting the angles out correctly but my only problem is the yaw and pitch swap when it rolls. So I'm guessing I need a way to convert from world to body coodrinates?

Adam Marciniak
  • 41
  • 1
  • 6
  • Just as a first observation, you can't do q[0] = q[0]*etc and then follow up the next line with q[1] = q[0]*etc because the q[0] has already been changed by the previous line. Similar comments for the other q elements. You need to put the results into a temporary variable so that consistent q values are used on the rhs. – James Tursa Nov 10 '20 at 23:21
  • Yes thank you, I caught that after I posted this question. Alas, it still has the same issue though. The yaw and pitch swap when roll is changed. – Adam Marciniak Nov 11 '20 at 00:09
  • this (slightly of topic) might interest you [My Algorithm to Calculate Position of Smartphone - GPS and Sensors](https://stackoverflow.com/a/19764828/2521214) if the case read the whole thread.... To get back to your question you got angular speeds in your device local coordinate system so you need to have a [cumulative transform matrix](https://stackoverflow.com/a/28084380/2521214) ... and use that for the transforms (the incremental rotations included)... so get rid of your euler angles and use matrix instead... from there you can get your actual euler angles – Spektre Nov 11 '20 at 11:37
  • see my [reper 4D](https://stackoverflow.com/a/62466260/2521214) its a cumulative transform matrix in 4D you need the same in 3D (see the difference between global/local rotations) ... – Spektre Nov 11 '20 at 11:41

0 Answers0