Re: BN055 Vertical Orientation
Re: BN055 Vertical Orientation
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.
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;
Re: BN055 Vertical Orientation
Re: BN055 Vertical Orientation
Re: BN055 Vertical Orientation
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);
Re: BN055 Vertical Orientation
Something else is wrong. Accelerometer drift is usually negligible, unless there are significant changes in the sensor temperature.extremely unstable due to the drift in the accelerometer
Re: BN055 Vertical Orientation
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.
/* 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);
Re: BN055 Vertical Orientation
Re: BN055 Vertical Orientation
Re: BN055 Vertical Orientation
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.
#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();
}
Re: BN055 Vertical Orientation
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;
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;
Re: BN055 Vertical Orientation
Re: BN055 Vertical Orientation
Re: BN055 Vertical Orientation
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.
Re: BN055 Vertical Orientation
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.