ryan lin / chair_BNO055

wrapper class for BNO055

Dependencies:   BNO055

Dependents:   wheelchaircontrol wheelchaircontrolRos

chair_BNO055.cpp@3:531a74cecb89, 2018-08-12 (annotated)

Committer:
ryanlin97
Date:
Sun Aug 12 00:33:19 2018 +0000
Revision:
3:531a74cecb89
Parent:
2:7d6467fa1977 
changed sda and scl definitions to d4 and d5 for small mbed board

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ryanlin97 0:ad7a811c859f 1 #include "chair_BNO055.h"
ryanlin97 0:ad7a811c859f 2
ryanlin97 1:3258d62af038 3 static float total_yaw = 0;
ryanlin97 1:3258d62af038 4 static float minGyroZ;
ryanlin97 1:3258d62af038 5 chair_BNO055::chair_BNO055(Serial* out, Timer* time)
ryanlin97 1:3258d62af038 6 {
ryanlin97 1:3258d62af038 7 imu = new BNO055(SDA, SCL);
ryanlin97 1:3258d62af038 8 usb = out;
ryanlin97 1:3258d62af038 9 t = time;
ryanlin97 1:3258d62af038 10 start = false;
ryanlin97 0:ad7a811c859f 11 }
ryanlin97 0:ad7a811c859f 12
ryanlin97 1:3258d62af038 13 chair_BNO055::chair_BNO055(PinName sda_pin, PinName scl_pin, Serial* out, Timer* time)
ryanlin97 1:3258d62af038 14 {
ryanlin97 1:3258d62af038 15 imu = new BNO055(sda_pin, scl_pin);
ryanlin97 1:3258d62af038 16 usb = out;
ryanlin97 1:3258d62af038 17 t = time;
ryanlin97 1:3258d62af038 18 start = false;
ryanlin97 0:ad7a811c859f 19 }
ryanlin97 0:ad7a811c859f 20
ryanlin97 1:3258d62af038 21 void chair_BNO055::setup()
ryanlin97 1:3258d62af038 22 {
ryanlin97 1:3258d62af038 23 imu->reset();
ryanlin97 1:3258d62af038 24 usb->printf("Bosch Sensortec BNO055 test program on \r\n");//" __DATE__ "/" __TIME__ "\r\n");
ryanlin97 1:3258d62af038 25 while (imu->check() == 0) {
ryanlin97 1:3258d62af038 26 usb->printf("Bosch BNO055 is NOT available!!\r\n");
ryanlin97 1:3258d62af038 27 wait(.5);
ryanlin97 1:3258d62af038 28 }
ryanlin97 1:3258d62af038 29 usb->printf("BNO055 found\r\n\r\n");
ryanlin97 1:3258d62af038 30 usb->printf("Chip ID: %0z\r\n",imu->ID.id);
ryanlin97 1:3258d62af038 31 usb->printf("Accelerometer ID: %0z\r\n",imu->ID.accel);
ryanlin97 1:3258d62af038 32 usb->printf("Gyroscope ID: %0z\r\n",imu->ID.gyro);
ryanlin97 1:3258d62af038 33 usb->printf("Magnetometer ID: %0z\r\n\r\n",imu->ID.mag);
ryanlin97 1:3258d62af038 34 usb->printf("Firmware version v%d.%0d\r\n",imu->ID.sw[0],imu->ID.sw[1]);
ryanlin97 1:3258d62af038 35 usb->printf("Bootloader version v%d\r\n\r\n",imu->ID.bootload);
ryanlin97 1:3258d62af038 36 imu->set_accel_units(MPERSPERS);
ryanlin97 1:3258d62af038 37 imu->setmode(OPERATION_MODE_AMG);
ryanlin97 1:3258d62af038 38 imu->read_calibration_data();
ryanlin97 1:3258d62af038 39 imu->write_calibration_data();
ryanlin97 1:3258d62af038 40 imu->set_angle_units(DEGREES);
ryanlin97 0:ad7a811c859f 41
ryanlin97 1:3258d62af038 42 t->start();
ryanlin97 1:3258d62af038 43
ryanlin97 0:ad7a811c859f 44 }
ryanlin97 0:ad7a811c859f 45
ryanlin97 1:3258d62af038 46 void chair_BNO055::calibrate_yaw() {
ryanlin97 1:3258d62af038 47 float start = t->read();
ryanlin97 1:3258d62af038 48
ryanlin97 1:3258d62af038 49 while( t->read()- start <= CAL_TIME ){
ryanlin97 1:3258d62af038 50 total_yaw = boxcar(chair_BNO055::angle_north());
ryanlin97 1:3258d62af038 51 usb->printf("total_yaw %f\n", total_yaw);
ryanlin97 1:3258d62af038 52 }
ryanlin97 1:3258d62af038 53 }
ryanlin97 1:3258d62af038 54
ryanlin97 1:3258d62af038 55
ryanlin97 1:3258d62af038 56 double chair_BNO055::accel_x()
ryanlin97 1:3258d62af038 57 {
ryanlin97 1:3258d62af038 58 imu->get_accel();
ryanlin97 1:3258d62af038 59 return (double)imu->accel.x;
ryanlin97 0:ad7a811c859f 60 }
ryanlin97 0:ad7a811c859f 61
ryanlin97 1:3258d62af038 62 double chair_BNO055::accel_y()
ryanlin97 1:3258d62af038 63 {
ryanlin97 1:3258d62af038 64 imu->get_accel();
ryanlin97 1:3258d62af038 65 return (double)imu->accel.y;
ryanlin97 0:ad7a811c859f 66 }
ryanlin97 0:ad7a811c859f 67
ryanlin97 1:3258d62af038 68 double chair_BNO055::accel_z()
ryanlin97 1:3258d62af038 69 {
ryanlin97 1:3258d62af038 70 imu->get_accel();
ryanlin97 1:3258d62af038 71 return (double)imu->accel.z;
ryanlin97 0:ad7a811c859f 72 }
ryanlin97 0:ad7a811c859f 73
ryanlin97 1:3258d62af038 74 double chair_BNO055::gyro_x()
ryanlin97 1:3258d62af038 75 {
ryanlin97 1:3258d62af038 76 imu->get_gyro();
ryanlin97 1:3258d62af038 77 return (double)imu->gyro.x;
ryanlin97 0:ad7a811c859f 78 }
ryanlin97 0:ad7a811c859f 79
ryanlin97 1:3258d62af038 80 double chair_BNO055::gyro_y()
ryanlin97 1:3258d62af038 81 {
ryanlin97 1:3258d62af038 82 imu->get_gyro();
ryanlin97 1:3258d62af038 83 return (double)imu->gyro.y;
ryanlin97 1:3258d62af038 84 }
ryanlin97 1:3258d62af038 85
ryanlin97 1:3258d62af038 86 double chair_BNO055::gyro_z()
ryanlin97 1:3258d62af038 87 {
ryanlin97 1:3258d62af038 88 imu->get_gyro();
ryanlin97 1:3258d62af038 89 return lowPass(imu->gyro.z);
ryanlin97 1:3258d62af038 90 }
ryanlin97 1:3258d62af038 91
ryanlin97 1:3258d62af038 92 double chair_BNO055::angle_north()
ryanlin97 1:3258d62af038 93 {
ryanlin97 1:3258d62af038 94 imu->get_mag();
ryanlin97 1:3258d62af038 95 float x = imu->mag.x - 520;
ryanlin97 1:3258d62af038 96 float y = imu->mag.y;
ryanlin97 1:3258d62af038 97
ryanlin97 1:3258d62af038 98 float result = x/y;
ryanlin97 1:3258d62af038 99
ryanlin97 1:3258d62af038 100 float angleToNorth;
ryanlin97 1:3258d62af038 101 if(imu->mag.y>0)
ryanlin97 1:3258d62af038 102 angleToNorth = 90.0 - atan(result)*180/PI;
ryanlin97 1:3258d62af038 103 else if(imu->mag.y<0)
ryanlin97 1:3258d62af038 104 angleToNorth = 270.0 - atan(result)*180/PI;
ryanlin97 1:3258d62af038 105 else if(y == 0 && x <= 0)
ryanlin97 1:3258d62af038 106 angleToNorth = 180;
ryanlin97 1:3258d62af038 107 else if(y == 0 && x > 0)
ryanlin97 1:3258d62af038 108 angleToNorth = 0;
ryanlin97 1:3258d62af038 109
ryanlin97 1:3258d62af038 110 return (double)angleToNorth;
ryanlin97 0:ad7a811c859f 111 }
ryanlin97 0:ad7a811c859f 112
ryanlin97 1:3258d62af038 113 double chair_BNO055::roll()
ryanlin97 1:3258d62af038 114 {
ryanlin97 1:3258d62af038 115 imu->get_accel();
ryanlin97 1:3258d62af038 116 imu->get_gyro();
ryanlin97 0:ad7a811c859f 117
ryanlin97 1:3258d62af038 118 float roll = atan2(-imu->accel.x ,( sqrt((imu->accel.y * imu->accel.y) +
ryanlin97 1:3258d62af038 119 (imu->accel.z * imu->accel.z))));
ryanlin97 1:3258d62af038 120 roll = roll*57.3;
ryanlin97 1:3258d62af038 121
ryanlin97 1:3258d62af038 122 t->reset();
ryanlin97 1:3258d62af038 123
ryanlin97 1:3258d62af038 124 return (double)roll;
ryanlin97 0:ad7a811c859f 125 }
ryanlin97 0:ad7a811c859f 126
ryanlin97 1:3258d62af038 127 double chair_BNO055::pitch()
ryanlin97 1:3258d62af038 128 {
ryanlin97 1:3258d62af038 129 imu->get_accel();
ryanlin97 1:3258d62af038 130 imu->get_gyro();
ryanlin97 0:ad7a811c859f 131
ryanlin97 1:3258d62af038 132 float pitch = atan2 (imu->accel.y ,( sqrt ((imu->accel.x * imu->accel.x) +
ryanlin97 1:3258d62af038 133 (imu->accel.z * imu->accel.z))));
ryanlin97 1:3258d62af038 134 pitch = pitch*57.3;
ryanlin97 1:3258d62af038 135
ryanlin97 1:3258d62af038 136 t->reset();
ryanlin97 1:3258d62af038 137
ryanlin97 1:3258d62af038 138 return (double)pitch;
ryanlin97 0:ad7a811c859f 139 }
ryanlin97 0:ad7a811c859f 140
ryanlin97 1:3258d62af038 141 double chair_BNO055::yaw()
ryanlin97 1:3258d62af038 142 {
ryanlin97 1:3258d62af038 143 float gyroZ = chair_BNO055::gyro_z();
ryanlin97 0:ad7a811c859f 144
ryanlin97 1:3258d62af038 145 if(abs(gyroZ) >= .3) {
ryanlin97 1:3258d62af038 146 total_yaw = (total_yaw - t->read()*gyroZ);
ryanlin97 1:3258d62af038 147 //usb->printf("gyroZ %f\n", gyroZ);
ryanlin97 1:3258d62af038 148 }
ryanlin97 1:3258d62af038 149 t->reset();
ryanlin97 1:3258d62af038 150
ryanlin97 1:3258d62af038 151 if(total_yaw > 360)
ryanlin97 1:3258d62af038 152 total_yaw -= 360;
ryanlin97 1:3258d62af038 153 if(total_yaw < 0)
ryanlin97 1:3258d62af038 154 total_yaw += 360;
ryanlin97 1:3258d62af038 155
ryanlin97 1:3258d62af038 156 //usb->printf("yaw %f\n", total_yaw);
ryanlin97 1:3258d62af038 157 return (double)total_yaw;
ryanlin97 0:ad7a811c859f 158 }