0

BN055 Vertical Orientation
Moderators: adafruit_support_bill, adafruit

Please be positive and constructive with your questions and comments.

Re: BN055 Vertical Orientation

by gammaburst on Wed Jun 16, 2021 7:38 pm

Reminder, the BNO055's Euler angles are broken, do not read them. Read its quaternion instead. If you really need Euler, you can convert the quaternion to Euler.

gammaburst
 
Posts: 579
Joined: Thu Dec 31, 2015 12:06 pm

Re: BN055 Vertical Orientation

by ppecch on Wed Jun 16, 2021 7:58 pm

gammaburst wrote:Reminder, the BNO055's Euler angles are broken, do not read them. Read its quaternion instead. If you really need Euler, you can convert the quaternion to Euler.


Yes that's what I'm doing, using the quaternions to get the angles, its the remapping I'm still confused about even after all the discuss, but I do appreciate everyone's help!

Code: Select all | TOGGLE FULL SIZE
imu::Quaternion quat = bno.getQuat(); //Request Quaternions From Sensor
 
  float q0 = quat.w();
  float q1 = quat.x();
  float q2 = quat.y();
  float q3 = quat.z();

  float roll = -1*atan2((2*q2*q3 + 2*q0*q1), pow(q0,2) - pow(q1,2) - pow(q2,2) - pow(q3,2));
  float pitch = -1*asin(2*q1*q3 - 2*q0*q2);
  float yaw = atan2((2*q1*q2 + 2*q0*q3), pow(q0,2) + pow(q1,2) - pow(q2,2) - pow(q3,2));
 
  float rollDeg  = 57.2958 * roll;
  float pitchDeg = 57.2958 * pitch;
  float yawDeg   = 57.2958 * yaw;

ppecch
 
Posts: 16
Joined: Mon Jun 14, 2021 12:11 pm

Re: BN055 Vertical Orientation

by sj_remington on Thu Jun 17, 2021 10:13 am

I believe that what you want is the SENSOR FRAME pitch and roll, so why not use the offset-removed raw accelerometer data, together with the simple formulas I posted way back near the start of this thread?

Make sure your sensor placement agrees with one of the three possibilities allowed by cyclic permutation, presented in that post.

sj_remington
 
Posts: 107
Joined: Mon Jul 27, 2020 4:51 pm

Re: BN055 Vertical Orientation

by gammaburst on Thu Jun 17, 2021 5:29 pm

Years ago when I began learning the BNO055, I needed to mount it vertically inside my project, but the axes flips were confusing me. So I temporarily mounted the BNO at its default orientation (laying flat), and confirmed that its output quaternions were correct. Good! Save that version of the project. Then I reattached the BNO at my desired vertical orientation, and changed its two axes remap registers as described in the datasheet. The BNO continued outputting the same correct quaternions even though it was now mounted vertically. Yay! The axis remap registers worked great.

Be careful reading the axis remap register diagrams in the datasheet. Bosch drew them with a confusing 90 degree rotation.

gammaburst
 
Posts: 579
Joined: Thu Dec 31, 2015 12:06 pm

Re: BN055 Vertical Orientation

by ppecch on Thu Jun 17, 2021 6:57 pm

sj_remington - I attempted what you suggested which is a viable solution however, I believe its application dependent as well. In my very very limited experience, using acceleration alone doesn't necessarily make for a stable gimbal in this application. I tried what you suggested and its extremely unstable due to the drift in the accelerometer. The problem could also be between the computer and chair (ie. me) but again I do appreciate the suggestion.

gammaburst - Remapping the axes is what I'd like to accomplish with the use of quaternions, but again, I just don't have enough knowledge or experience to make it happen.
I tried to modify the "Adafruit_BNO055.cpp" file and down around line #125, you'll see a section of code that is commented out:

/* Configure axis mapping (see section 3.4) */

Code: Select all | TOGGLE FULL SIZE
write8(BNO055_AXIS_MAP_CONFIG_ADDR, REMAP_CONFIG_P2); // P0-P7, Default is P1
delay(10);
write8(BNO055_AXIS_MAP_SIGN_ADDR, REMAP_SIGN_P2); // P0-P7, Default is P1
delay(10);


I uncommented that code and plugged in the correct P number from page 25 of the Bosch Data Sheet for BNO055, saved the "Adafruit_BNO055.cpp" file back into the library and it had no effect.

ppecch
 
Posts: 16
Joined: Mon Jun 14, 2021 12:11 pm

Re: BN055 Vertical Orientation

by sj_remington on Thu Jun 17, 2021 8:28 pm

extremely unstable due to the drift in the accelerometer
Something else is wrong. Accelerometer drift is usually negligible, unless there are significant changes in the sensor temperature.

In your first attempts, use the tilt code to just measure the pitch and roll angles, with the sensor held still at various angles. You will find that the measurements are very reproducible and stable, although there will be measurement noise of up to 1 or 2 degrees.

Once you have convinced yourself that axis permutation works as you expect, and that the angles make sense, move on to your application.

Finally, measurement of tilt angles using just the accelerometer assumes that the sensor is still. If the sensor is rotating rapidly, the assumption breaks down and the rate gyro measurements are used to compensate for the error. I find that the Mahony fusion filter conveniently and accurately integrates the gyro and accelerometer data, which avoids the errors introduced by filters using the magnetometer as well.

sj_remington
 
Posts: 107
Joined: Mon Jul 27, 2020 4:51 pm

Re: BN055 Vertical Orientation

by ppecch on Thu Jun 17, 2021 10:32 pm

gammaburst wrote: Yay! The axis remap registers worked great.

Be careful reading the axis remap register diagrams in the datasheet. Bosch drew them with a confusing 90 degree rotation.


Remapping the axes is what I'd like to accomplish with the use of quaternions, but again, I just don't have enough knowledge or experience to make it happen.
I tried to modify the "Adafruit_BNO055.cpp" file and down around line #125, you'll see a section of code that is commented out:

Code: Select all | TOGGLE FULL SIZE
/* Configure axis mapping (see section 3.4) */

write8(BNO055_AXIS_MAP_CONFIG_ADDR, REMAP_CONFIG_P2); // P0-P7, Default is P1
delay(10);
write8(BNO055_AXIS_MAP_SIGN_ADDR, REMAP_SIGN_P2); // P0-P7, Default is P1
delay(10);


I uncommented that code and plugged in the correct P number from page 25 of the Bosch Data Sheet for BNO055, saved the "Adafruit_BNO055.cpp" file back into the library and it had no effect. How did you accomplish this?

ppecch
 
Posts: 16
Joined: Mon Jun 14, 2021 12:11 pm

Re: BN055 Vertical Orientation

by gammaburst on Fri Jun 18, 2021 7:40 am

The library also contains setAxisRemap() and setAxisSign(). Give them a try.

Years ago, I had mysterious trouble using Adafruit's BNO055 library. Rather than debugging it, I replaced the entire library with a few lines of code that did only what I needed. Here's an example Arduino sketch:
viewtopic.php?f=19&p=842231#p842134

My sketch does extra things that you probably don't need, such as reading dozens of BNO registers and converting to Euler. Strip them out to reduce processing time and memory.

gammaburst
 
Posts: 579
Joined: Thu Dec 31, 2015 12:06 pm

Re: BN055 Vertical Orientation

by jps2000 on Fri Jun 18, 2021 8:54 am

When you have quaternions (qw,qx,qy,qz) you can rotate them to your needs by multiplying with an appropriate (constant) quaternion.
A table with all these necessary quaternions you find in the BNO080 data sheet chapter 4 figure 4.3

X Y Z Qw Qx Qy Qz
East North Up 1 0 0 0
North West Up (√2)/2 0 0 (√2)/2
West South Up 0 0 0 1
South East Up (√2)/2 0 0 −(√2)/2
....

then you have to multiply (matrix multiplication) the quaternions from the BNO with the above

qw = qw * Qw - qx * Qx - qy * Qy - qz * Qz; // multiplication matrix
qx = q0 * Qx + qx * Qw + qy * Qz - qz * Qy;
qy = q0 * Qy - qx * Qz + qy * Qw + qz * Qx;
qz = q0 * Qz + qx * Qy - qy * Qx + qz * Qw;

In fact you can find out what quaternion you need for the rotation by orienting your imu such that you get the quaternions (1,0,0,0) which is ENU orientation then you rotate the IMU in the desired new orientation. Note down the result and thats it.
Note that for exact 90 degrees rotations only quaternions of 0, +/-1, +/-0.5, +/- sqrt(2)/2 are valid values so replace your notes with these exact values

But of course setting the remap registers is preferred.

jps2000
 
Posts: 639
Joined: Fri Jun 02, 2017 4:12 pm

Re: BN055 Vertical Orientation

by ppecch on Fri Jun 18, 2021 11:41 am

gammaburst wrote:The library also contains setAxisRemap() and setAxisSign(). Give them a try.

Years ago, I had mysterious trouble using Adafruit's BNO055 library. Rather than debugging it, I replaced the entire library with a few lines of code that did only what I needed. Here's an example Arduino sketch:
viewtopic.php?f=19&p=842231#p842134

My sketch does extra things that you probably don't need, such as reading dozens of BNO registers and converting to Euler. Strip them out to reduce processing time and memory.


So I tried the setAxisRemap() and setAxisSign() and that doesn't seem to work.
I'll have a look at your Arduino Sketch and try to implement that instead.
Can I incorporate your sketch into what I have already? Or are you setting it up as a library that you're referencing? These are the libraries I'm referencing and my setup:

Code: Select all | TOGGLE FULL SIZE
#include <Wire.h>
#include <math.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>
#include <Adafruit_BMP280.h>
#include <Servo.h>


#define BNO055_SAMPLERATE_DELAY_MS (50)
Adafruit_BNO055 bno = Adafruit_BNO055();

Adafruit_BMP280 bmp;

float altitude, descent;
float altPressure = 980; //1013.25 is Pressure(hPa) at sea level on the launch day of your region
long currentTime, lastTime;

Servo rollServo;
Servo pitchServo;

float desired_angle = 0;
float rollServoAdj = 90;
float pitchServoAdj = 95;
float rollServoStart = rollServoAdj;
float pitchServoStart = pitchServoAdj;
float servo_start_offset = 20;

float pid_p_x=0;
float pid_p_y=0;
float pid_i_x=0;
float pid_i_y=0;
float pid_d_x=0;
float pid_d_y=0;

float kp=3;
float ki=.0001;
float kd=.1;

float PIDX;
float PIDY;
float error_x;
float error_y;
float previous_error_x;
float previous_error_y;

float deltaT, time, timePrev;

int LEDred = 3;   
int LEDgrn = 5;   
int LEDblu = 6;   
int buzzer = 8;
int LEDarduino = 13;
int pyro = 11;

//System State:
// 0 - Launch Detection
// 1 - PID Control
// 2 - MECO
// 3 - Recovery

int state = 0;



void setup() {

  Serial.begin(115200);
 
  if (!bno.begin())
  {
    Serial.print("No BNO055 Sensor Detected");
  }
  delay(100);
 
  bno.begin();
  bno.setExtCrystalUse(true);
 
  if (!bmp.begin(0x76)) {
  Serial.print("No BMP280 Sensor Detected");
  while (1);
  }

  bmp.begin(0x76);
     
  rollServo.attach(9);
  pitchServo.attach(10);
  rollServo.write(rollServoStart);
  pitchServo.write(pitchServoStart);
     
  pinMode(LEDblu, OUTPUT);
  pinMode(LEDgrn, OUTPUT);
  pinMode(LEDred, OUTPUT);
  pinMode(buzzer, OUTPUT);
  pinMode(LEDarduino, OUTPUT);
  pinMode(pyro, OUTPUT);

  startup();
}

ppecch
 
Posts: 16
Joined: Mon Jun 14, 2021 12:11 pm

Re: BN055 Vertical Orientation

by ppecch on Fri Jun 18, 2021 1:14 pm

jps2000 wrote:When you have quaternions (qw,qx,qy,qz) you can rotate them to your needs by multiplying with an appropriate (constant) quaternion.
A table with all these necessary quaternions you find in the BNO080 data sheet chapter 4 figure 4.3

X Y Z Qw Qx Qy Qz
East North Up 1 0 0 0
North West Up (√2)/2 0 0 (√2)/2
West South Up 0 0 0 1
South East Up (√2)/2 0 0 −(√2)/2
....

then you have to multiply (matrix multiplication) the quaternions from the BNO with the above

qw = qw * Qw - qx * Qx - qy * Qy - qz * Qz; // multiplication matrix
qx = q0 * Qx + qx * Qw + qy * Qz - qz * Qy;
qy = q0 * Qy - qx * Qz + qy * Qw + qz * Qx;
qz = q0 * Qz + qx * Qy - qy * Qx + qz * Qw;



Ok that's another approach, this is what I tried:

Code: Select all | TOGGLE FULL SIZE
 imu::Quaternion quat = bno.getQuat(); //Request Quaternions From Sensor
 
float qw = quat.w();
  float qx = quat.x();
  float qy = quat.y();
  float qz = quat.z();

  float Qw = sqrt(2)/2;
  float Qx = 0;
  float Qy = sqrt(-1*2)/2;
  float Qz = 0;
 
  float q0 = qw * Qw - qx * Qx - qy * Qy - qz * Qz;
  float q1 = qw * Qx + qx * Qw + qy * Qz - qz * Qy;
  float q2 = qw * Qy - qx * Qz + qy * Qw + qz * Qx;
  float q3 = qw * Qz + qx * Qy - qy * Qx + qz * Qw;

  float roll = -1*atan2((2*q2*q3 + 2*q0*q1), pow(q0,2) - pow(q1,2) - pow(q2,2) - pow(q3,2));
  float pitch = -1*asin(2*q1*q3 - 2*q0*q2);
  float yaw = atan2((2*q1*q2 + 2*q0*q3), pow(q0,2) + pow(q1,2) - pow(q2,2) - pow(q3,2));
 
  float rollDeg  = 57.2958 * roll;
  float pitchDeg = 57.2958 * pitch;
  float yawDeg   = 57.2958 * yaw;

I'm trying to get a X - East, Y - Up and Z - South position, but still having trouble.

ppecch
 
Posts: 16
Joined: Mon Jun 14, 2021 12:11 pm

Re: BN055 Vertical Orientation

by jps2000 on Fri Jun 18, 2021 1:52 pm

float Qy = sqrt(-1*2)/2;
should be float Qy = - sqrt(2)/2; or just Qy = - Qw; (no sqrt of a negative value!!!)

Can you specify what it mean "having trouble"?

Do you first get Q = (1,0,0,0 ) in ENU orientation?
Then you apply the formula
Then you should get Q=(1,0,0,0) in the new EUS orientation
can you check this?

Precondition is that Status register shows 3 (full calibrated) and remain there.
Your sensor must not be influenced by external magnetic fields.
I remember having troubles playing around with the sensor in front of my laptop not considering that there was a magnetic lock for the lid.

When you experiment indoors it can be that earth magnetic field is distorted or attenuated.
Try with a magnetic compass if reading is correct. But then move away that compass . it is also magnetic!

If you do not need absolute orientation (north direction is not relevant) than you should select the 6DOF mode. It is much more stable.
Then Q = 1,0,0,0 appears in any horizontal position at the moment of reset ( north is assumed to be where the y axis points to at the time of switching on.

By the way: If you have tried now several methods an nothing work it is likely that there is a totally different problem.

jps2000
 
Posts: 639
Joined: Fri Jun 02, 2017 4:12 pm

Re: BN055 Vertical Orientation

by gammaburst on Fri Jun 18, 2021 2:51 pm

Yes, what jps2000 says!

How do you test the quaternions for correctness? Do you feed them into an animated 3D graphics display of the rocket? Do you feed them into servos and watching them move as you tilt the rocket?

You're welcome to use my example sketches any way you like, change them, laugh at them. I wrote this one years ago. It's probably an intermediate step as I was transforming one project into another, so it includes extra stuff that I was adding or removing. No, I wasn't creating my own library. I don't like libraries, especially other people's libraries. I prefer putting everything into one source file so that I have full visibility and easy control of everything. Yeah, I'm weird!

I'd be concerned that the rocket's acceleration would saturate the BNO's accelerometer or gyro and spoil its fusion math (quaternions). I can saturate it by merely shaking the sensor in my hand. It makes the BNO dizzy.

gammaburst
 
Posts: 579
Joined: Thu Dec 31, 2015 12:06 pm

Re: BN055 Vertical Orientation

by ppecch on Fri Jun 18, 2021 3:49 pm

gammaburst wrote:Yes, what jps2000 says!

How do you test the quaternions for correctness? Do you feed them into an animated 3D graphics display of the rocket? Do you feed them into servos and watching them move as you tilt the rocket?

You're welcome to use my example sketches any way you like, change them, laugh at them. I wrote this one years ago. It's probably an intermediate step as I was transforming one project into another, so it includes extra stuff that I was adding or removing. No, I wasn't creating my own library. I don't like libraries, especially other people's libraries. I prefer putting everything into one source file so that I have full visibility and easy control of everything. Yeah, I'm weird!

I'd be concerned that the rocket's acceleration would saturate the BNO's accelerometer or gyro and spoil its fusion math (quaternions). I can saturate it by merely shaking the sensor in my hand. It makes the BNO dizzy.



Correct I'm just reading them into the servos and watching them move as I tilt the test gimbal, not scientific at all. I like your approach of having just the one source file as well.
I was reading on another forum that I'm part of ,that your comment about the acceleration could be an issue as the rocket thrusts upward. Others are suggesting reading the gyro's instead. They are susceptible to drift as well but the flight is time is so short (2.5-3 sec) it shouldn't impact it all that much. I'm going to also try to incorporate your script into my setup as see what happens.

ppecch
 
Posts: 16
Joined: Mon Jun 14, 2021 12:11 pm

Re: BN055 Vertical Orientation

by ppecch on Fri Jun 18, 2021 3:55 pm

jps2000 wrote:float Qy = sqrt(-1*2)/2;
should be float Qy = - sqrt(2)/2; or just Qy = - Qw; (no sqrt of a negative value!!!)

Can you specify what it mean "having trouble"?

Do you first get Q = (1,0,0,0 ) in ENU orientation?
Then you apply the formula
Then you should get Q=(1,0,0,0) in the new EUS orientation
can you check this?

Precondition is that Status register shows 3 (full calibrated) and remain there.



Ah, mistake on my part, -1*(sqrt(2))/2 is what I meant !!!

The trouble Im having is the gimbal is immediately positioning itself to 90 deg, could be something else going on.

When I put in Q = 1,0,0,0 with the calibration at 3 across the board it working. How can I check the EUS orientation?
Is it just a matter of printing these values?

float q0 = qw * Qw - qx * Qx - qy * Qy - qz * Qz;
float q1 = qw * Qx + qx * Qw + qy * Qz - qz * Qy;
float q2 = qw * Qy - qx * Qz + qy * Qw + qz * Qx;
float q3 = qw * Qz + qx * Qy - qy * Qx + qz * Qw;

ppecch
 
Posts: 16
Joined: Mon Jun 14, 2021 12:11 pm

Please be positive and constructive with your questions and comments.