File content as of revision 0:ad7a811c859f:

#include "chair_BNO055.h"

Timer t;


chair_BNO055::chair_BNO055(){
  imu = new BNO055(SDA, SCL);
}

chair_BNO055::chair_BNO055(PinName sda_pin, PinName scl_pin){
  imu = new BNO055(sda_pin, scl_pin);
}

void chair_BNO055::setup(){
  imu->reset();
  while (imu->check() == 0) {
    wait(.5);
  }
  imu->set_accel_units(MPERSPERS);
  imu->setmode(OPERATION_MODE_AMG);
  imu->get_calib();
  imu->write_calibration_data();
  imu->set_angle_units(DEGREES);
  imu->setmode(OPERATION_MODE_AMG); //put into while loop 
  t.start();
}

double chair_BNO055::accel_x(){
  imu->get_accel();
  return (double)imu->accel.x;
}

double chair_BNO055::accel_y(){
  imu->get_accel();
  return (double)imu->accel.y;
}

double chair_BNO055::accel_z(){
  imu->get_accel();
  return (double)imu->accel.z;
}

double chair_BNO055::gyro_x(){
  imu->get_gyro();
  return (double)imu->gyro.x;
}

double chair_BNO055::gyro_y(){
  imu->get_gyro();
  return (double)imu->gyro.y;
}

double chair_BNO055::gyro_z(){
  imu->get_gyro();
  return (double)imu->gyro.z;
}

double chair_BNO055::angle_north(){
  imu->get_mag();
  float x = imu->mag.x;
  float y = imu->mag.y;
  
  float result = x/y;
  
  float angleToNorth;
  if(imu->mag.y>0)
    angleToNorth = 90.0 - atan(result)*180/PI;
  else if(imu->mag.y<0)
    angleToNorth = 270.0 - atan(result)*180/PI;
  else if(y == 0 && x <= 0)
    angleToNorth = 180;
  else if(y == 0 && x > 0)
    angleToNorth = 0;
  
  return (double)angleToNorth;
}

double chair_BNO055::roll(){
  imu->get_accel();
  imu->get_gyro();
  
  float roll = atan2(-imu->accel.x ,( sqrt((imu->accel.y * imu->accel.y) + 
  (imu->accel.z * imu->accel.z))));
  roll = roll*57.3;
  
  t.reset();

  return (double)roll;
}

double chair_BNO055::pitch(){
  imu->get_accel();
  imu->get_gyro();
  
  float pitch = atan2 (imu->accel.y ,( sqrt ((imu->accel.x * imu->accel.x) + 
  (imu->accel.z * imu->accel.z))));
  pitch = pitch*57.3;
  
  t.reset();

  return (double)pitch;
}

double chair_BNO055::yaw(){
  imu->get_gyro();
  float yaw = (yaw - t.read()*imu->gyro.z);
  
  if(yaw > 360)
    yaw -= 360;
  if(yaw < 0)
    yaw += 360;
    
  t.reset();

  return (double)yaw;
}