I am trying to calculate the relative roll, pitch, yaw with two IMUs (BNO055, GY-85) using I2C. I am using bno library and wire library for this. i2c_scanner sketch shows the sensors correctly:
Scanning...
I2C device found at address 0x0E !
I2C device found at address 0x28 !
I2C device found at address 0x53 !
I2C device found at address 0x68 !
done
However, the results of the GY-85 are messed up after couple of sample:
if you can't see the embedded picture above: Imgur: The magic of the Internet
I am almost sure the bno library and wire library somehow intervene. Can you guys tell me what could be wrong?
I am using arduino DUE with pins 20/21.
I forgot the code:
#define USB_USBCON
#include <Arduino.h>
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>
#include <GY_85.h>
#define BNO055_SAMPLERATE_DELAY_MS (5)
Adafruit_BNO055 bno = Adafruit_BNO055(-1, 0x28);
bool state_odom = LOW;
int LED_odom = 4;
bool state_IMU = LOW;
int LED_IMU = 5;
void stateChange(bool & state, int LED){
state = !state;
digitalWrite(LED, state);
}
double ENCODEROUTPUT = 20.0;
#define HALLSEN_RA 3
#define HALLSEN_FA 2
double encoderValue_F = 0.0;
double encoderValue_R = 0.0;
double encoderValueOld_F = 0.0;
double encoderValueOld_R = 0.0;
double encoderValueTemp_F = 0.0;
double encoderValueTemp_R = 0.0;
void updateEncoder_FA()
{
encoderValue_F += 1.0;
digitalWrite(LED_odom, HIGH);
}
void updateEncoder_RA()
{
encoderValue_R += 1.0;
digitalWrite(LED_odom, HIGH);
}
double radii = 65.0/2.0;
double lr = 15.0/100.0;
double lf = 15.0/100.0;
double beta;
double fwa;
unsigned long interval = 40;
double interval_d = 40.0;
unsigned long previousMillis = 0;
unsigned long currentMillis = 0;
double factor_rpm_mps = ((2.0*3.14)*((radii)/1000.0))/60.0;
double rpm_F = 0.0;
double rpm_R = 0.0;
double vx_F = 0.0;
double vx_R = 0.0;
double vx_m = 0.0;
double vx = 0.0;
double vy = 0.0;
double vth = 0.0;
double dt = 0.0;
double delta_x = 0.0;
double delta_y = 0.0;
double delta_th = 0.0;
double x_driven = 0.0;
double y_driven = 0.0;
double th_driven = 0.0;
double lengthBetweenTwoWheels = 130.0/1000.0; //130 mm
int16_t ax_raw, ay_raw, az_raw;
int16_t gx_raw, gy_raw, gz_raw;
double ax_raw_SI, ay_raw_SI, az_raw_SI;
double gx_raw_SI, gy_raw_SI, gz_raw_SI;
double aa_SI[3];
double aaReal_SI[3];
double gyroReal_SI[3];
double aaWorld_SI[3];
double euler[3];
double ypr[3];
double quat_SI[4];
void receive(int numBytes){}
uint8_t displayCalStatus(void)
{
uint8_t bno_system, bno_gyro, bno_accel, bno_mag;
bno_system = bno_gyro = bno_accel = bno_mag = 0;
bno.getCalibration(&bno_system, &bno_gyro, &bno_accel, &bno_mag);
return bno_system;
}
GY_85 GY85; //create the object
double zeroValue[5] = { 0, 0, 0, 0, 0};
double gyroXangle = 180;
double gyroYangle = 180;
double gyroZangle = 180;
double accXangle = 180;
double accYangle = 180;
double accZangle = 180;
double compAngleX = 180;
double compAngleY = 180;
double compAngleZ = 180;
unsigned long timer;
uint8_t buffer[2]; // I2C buffer
int ax_fw;
int ay_fw;
int az_fw;
int cx_fw;
int cy_fw;
int cz_fw;
float gx_fw;
float gy_fw;
float gz_fw;
float gt_fw;
double getAngle(const int x_val, const int y_val) {
double accXval = (double)x_val;
double accYval = (double)y_val;
double angle = (atan2(accXval, accYval) + PI) * RAD_TO_DEG;
return angle;
}
void setup() {
Serial.begin(57600);
pinMode(LED_odom, OUTPUT);
pinMode(LED_IMU, OUTPUT);
while(!bno.begin())
{
}
delay(1000);
bno.setExtCrystalUse(true);
Wire.begin();
delay(10);
GY85.init();
delay(10);
pinMode(HALLSEN_FA, INPUT_PULLUP); // Set hall sensor A as input pullup
pinMode(HALLSEN_RA, INPUT_PULLUP); // Set hall sensor A as input pullup
attachInterrupt(digitalPinToInterrupt(HALLSEN_FA), updateEncoder_FA, RISING);
attachInterrupt(digitalPinToInterrupt(HALLSEN_RA), updateEncoder_RA, RISING);
timer = micros();
}
void loop() {
digitalWrite(LED_odom, LOW);
/* Get a new sensor event */
sensors_event_t imu_event;
bno.getEvent(&imu_event);
imu::Quaternion quat_imu = bno.getQuat();
imu::Vector<3> lgyro = bno.getVector(Adafruit_BNO055::VECTOR_GYROSCOPE);
imu::Vector<3> laccel = bno.getVector(Adafruit_BNO055::VECTOR_LINEARACCEL);
aaReal_SI[0] = laccel.x();
uint8_t bno_system_out = displayCalStatus();
if (bno_system_out>0){
delay(2);
digitalWrite(LED_IMU, HIGH);
}else{
stateChange(state_IMU, LED_IMU);
}
imu_data.publish(&imumsg_filtered);
currentMillis = millis();
if (currentMillis - previousMillis > interval) {
ax_fw = GY85.accelerometer_x( GY85.readFromAccelerometer() );
ay_fw = GY85.accelerometer_y( GY85.readFromAccelerometer() );
az_fw = GY85.accelerometer_z( GY85.readFromAccelerometer() );
Serial.print(ax_fw); Serial.print("\t");
Serial.print(ay_fw); Serial.print("\t");
Serial.print(az_fw); Serial.print("\n");
timer = micros();
rpm_F = (double)(encoderValue_F/(interval_d/1000.0) * 60.0 / ENCODEROUTPUT);
rpm_R = (double)(encoderValue_R/(interval_d/1000.0) * 60.0 / ENCODEROUTPUT);
vx_F = abs(rpm_F*factor_rpm_mps); // rpm_FR*2*PI/60*65/2.0/1000
vx_R = abs(rpm_R*factor_rpm_mps);
vx_m = (vx_F + vx_R)/2.0;
beta = atan((lf)/(lf+lr)*tan(fwa));
vth = vx_m/(lf+lr)*cos(beta)*tan(fwa);
vx = vx_m*cos(th_driven+beta);
vy = vx_m*sin(th_driven+beta);
dt = millis() - currentMillis;
delta_x = (vx * cos(th_driven)) * dt/1000.0;
delta_y = (vx * sin(th_driven)) * dt/1000.0;
delta_th = vth * dt/1000.0;
x_driven += delta_x;
y_driven += delta_y;
th_driven += delta_th;
while (th_driven > PI) {
th_driven -= 2.0 * PI;
}
while (th_driven < -PI) {
th_driven += 2.0 * PI;
}
encoderValue_F = 0.0;
encoderValue_R = 0.0;
}
}
Update: I have connected both of the sensors to 3.3 V and added 5.12K resistors to SDA/SCL. This seems to fixed the issue. But what I don't understand is why the sensors individually work but not together when connected to 5V