/*
*
Note that by default the axis orientation of the BNO chip looks like
the following (taken from section 3.4, page 24 of the datasheet). Notice
the dot in the corner that corresponds to the dot on the BNO chip:
| Z axis
|
| / X axis
____|__/____
Y axis / * | / /|
_________ /______|/ //
/___________ //
|____________|/
*/
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>
#include <EEPROM.h>
#include <uNavINS.h>
#include <ubx.h>
Adafruit_BNO055 bno = Adafruit_BNO055(55, 0x28); //3 lugar en EEPROM de la calibracion.
// a uNavINS object
uNavINS Filter;
bfs::Ubx gnss(&Serial1);
bool newGpsData;
unsigned long prevTOW;
// timers to measure performance
unsigned long tstart, tstop;
float acc_x, acc_y, acc_z;
float gyro_x, gyro_z, gyro_y;
float mag_x, mag_y, mag_z;
void setup() {
Serial.begin(57600);
if (!gnss.AutoBegin()) {
Serial.println("Unable to establish communication and configure GNSS RX");
while (1) {}
}
if (!bno.begin(bno.OPERATION_MODE_AMG))
{
/* There was a problem detecting the BNO055 ... check your connections */
Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!");
while (1);
}
//Set to axis remap p5
//bno.setAxisRemap(bno.REMAP_CONFIG_P5);
bno.setAxisSign(bno.REMAP_SIGN_P5);
int eeAddress = 0;
long bnoID;
bool foundCalib = false;
EEPROM.get(eeAddress, bnoID);
adafruit_bno055_offsets_t calibrationData;
sensor_t sensor;
bno.getSensor(&sensor);
if (bnoID != sensor.sensor_id)
{
Serial.println("\nNo Calibration Data for this sensor exists in EEPROM");
delay(500);
}
else
{
Serial.println("\nFound Calibration for this sensor in EEPROM.");
eeAddress += sizeof(long);
EEPROM.get(eeAddress, calibrationData);
//displaySensorOffsets(calibrationData);
Serial.println("\n\nRestoring Calibration data to the BNO055...");
bno.setSensorOffsets(calibrationData);
Serial.println("\n\nCalibration data loaded into BNO055");
foundCalib = true;
}
}
void loop() {
/* Get a new sensor event */
sensors_event_t angVelocityData , magnetometerData, accelerometerData;
if (gnss.Read() && gnss.num_sv() >= 3) {
bno.getEvent(&angVelocityData, Adafruit_BNO055::VECTOR_GYROSCOPE);
bno.getEvent(&magnetometerData, Adafruit_BNO055::VECTOR_MAGNETOMETER);
bno.getEvent(&accelerometerData, Adafruit_BNO055::VECTOR_ACCELEROMETER);
getIMU(&angVelocityData);
getIMU(&magnetometerData);
getIMU(&accelerometerData);
tstart = micros();
// update the filter
Filter.update(gnss.gps_tow_s(),gnss.north_vel_mps(),gnss.east_vel_mps(),gnss.down_vel_mps(),gnss.lat_rad(),gnss.lon_rad(),gnss.alt_msl_m(), gyro_x,gyro_y,gyro_z,acc_x,acc_y,acc_z,mag_x,mag_y,mag_z);
Serial.print(Filter.getPitch_rad()*180.0f/PI);
Serial.print(",");
Serial.print(Filter.getRoll_rad()*180.0f/PI);
Serial.print(",");
Serial.println(Filter.getYaw_rad()*180.0f/PI);
tstop = micros();
}
}
void getIMU(sensors_event_t* event) {
if (event->type == SENSOR_TYPE_ACCELEROMETER) {
//Serial.print("Accl:");
acc_x = event->acceleration.x;
acc_y = event->acceleration.y;
acc_z = event->acceleration.z;
//Serial.print(acc_x, 4); Serial.print("\t");
//Serial.print(acc_y, 4); Serial.print("\t");
//Serial.println(acc_z, 4);
}
else if (event->type == SENSOR_TYPE_MAGNETIC_FIELD) {
//Serial.print("Mag:");
mag_x = event->magnetic.x;
mag_y = event->magnetic.y;
mag_z = event->magnetic.z;
//Serial.print(mag_x, 4); Serial.print("\t");
//Serial.print(mag_y, 4); Serial.print("\t");
//Serial.println(mag_z, 4);
}
else if (event->type == SENSOR_TYPE_GYROSCOPE) {
//Serial.print("Gyro:");
gyro_x = event->gyro.x;
gyro_y = event->gyro.y;
gyro_z = event->gyro.z;
//Serial.print(gyro_x, 4); Serial.print("\t");
//Serial.print(gyro_y, 4); Serial.print("\t");
//Serial.println(gyro_z, 4);
}
}
