main.cpp

00001 //-----TOBY GALAL AND TOM ALLEN FIGHTING CRIME-----//
00002 
00003 /*
00004  * mbed Application program for the mbed Nucleo F401
00005  *  BNO055 Intelligent 9-axis absolute orientation sensor
00006  *  by Bosch Sensortec
00007  *
00008  * Copyright (c) 2015 Kenji Arai / JH1PJL
00009  *  http://www.page.sannet.ne.jp/kenjia/index.html
00010  *  http://mbed.org/users/kenjiArai/
00011  *      Created: March     30th, 2015
00012  *      Revised: April     16th, 2015
00013  *
00014  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
00015  * INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE
00016  * AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
00017  * DAMAGES OR OTHER  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
00018  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
00019  */
00020 
00021 //  Include ---------------------------------------------------------------------------------------
00022 #include    "mbed.h"
00023 #include    "BNO055.h"
00024 #include    "MODSERIAL.h"
00025 //#include    "arm_math.h"
00026 
00027 //  Definition ------------------------------------------------------------------------------------
00028 #define NUM_LOOP    100
00029 
00030 //  Object ----------------------------------------------------------------------------------------
00031 MODSERIAL  pc(USBTX,USBRX);
00032 
00033 volatile bool RAW = true;
00034 volatile bool FUS = false;
00035 bool printed = false;
00036 int Time;
00037 int TimeStart;
00038 int TimeStamp = 0;
00039 int TimeStamp2 = 0;
00040 int TimeOfStart = 0;
00041 int TimeOfStartms = 0;
00042 int TimeNow;
00043 int sync = 1;
00044 bool direction = false;
00045 
00046 //string sdata = "";
00047 
00048 DigitalOut led1(LED1);
00049 DigitalOut led2(LED2);
00050 DigitalOut led3(LED3);
00051 DigitalOut led4(LED4);
00052 DigitalOut pwr_onoff(p30);
00053 DigitalOut oscill(p20);
00054 
00055 I2C    i2c(p28, p27); // SDA, SCL
00056 BNO055 imu(i2c, p29);  // Reset =D7, addr = BNO055_G_CHIP_ADDR, mode = MODE_NDOF <- as default
00057 LocalFileSystem local("local");                 // Create the local filesystem under the name "local"
00058 
00059 Timer t;
00060 
00061 PwmOut PWM (p21);
00062 
00063 //  RAM -------------------------------------------------------------------------------------------
00064 BNO055_ID_INF_TypeDef       bno055_id_inf;
00065 BNO055_EULER_TypeDef        euler_angles;
00066 BNO055_QUATERNION_TypeDef   quaternion;
00067 BNO055_LIN_ACC_TypeDef      linear_acc;
00068 BNO055_GRAVITY_TypeDef      gravity;
00069 BNO055_TEMPERATURE_TypeDef  chip_temp;
00070 
00071 //  ROM / Constant data ---------------------------------------------------------------------------
00072 
00073 //  Function prototypes ---------------------------------------------------------------------------
00074 
00075 
00076 //-------------------------------------------------------------------------------------------------
00077 //  Serial Interrupt
00078 //-------------------------------------------------------------------------------------------------
00079 void rxCallback(MODSERIAL_IRQ_INFO *q)
00080 {
00081     MODSERIAL *serial = q->serial;
00082     if ( serial->rxGetLastChar() == 'f') {
00083         imu.write_reg0(0x3d, 0x0c);
00084         RAW = false;
00085         FUS = true;
00086         pc.printf("\r\nSWITCHING TO FUSION DATA!\r\n");
00087     }
00088     if ( serial->rxGetLastChar() == 'r') {
00089         imu.write_reg0(0x3d, 0x07);
00090         RAW = true;
00091         FUS = false;
00092         pc.printf("\r\nSWITCHING TO RAW DATA!\r\n");
00093     }
00094     if ( serial->rxGetLastChar() == '*') {
00095         TimeOfStart = t.read_us();
00096         TimeOfStartms = t.read_ms();
00097         TimeStamp = 0;
00098         pc.printf("RESETTING TIME");
00099     }
00100 }
00101 
00102 //-------------------------------------------------------------------------------------------------
00103 //  Control Program
00104 //-------------------------------------------------------------------------------------------------
00105 // Calibration
00106 //  Please refer BNO055 Data sheet 3.10 Calibration & 3.6.4 Sensor calibration data
00107 void bno055_calbration(void)
00108 {
00109     uint8_t d;
00110     pc.printf("------ Enter BNO055 Manual Calibration Mode ------\r\n");
00111     //---------- Gyroscope Caliblation ------------------------------------------------------------
00112     // (a) Place the device in a single stable position for a period of few seconds to allow the
00113     //     gyroscope to calibrate
00114     pc.printf("Step1) Please wait few seconds\r\n");
00115     t.start();
00116     while (t.read() < 10) {
00117         d = imu.read_calib_status();
00118         pc.printf("calibrating = 0x%x target  = 0x30(at least)\r\n", d);
00119         if ((d & 0x30) == 0x30) {
00120             break;
00121         }
00122         wait(1.0);
00123     }
00124     pc.printf("-> Step1) is done\r\n\r\n");
00125     //---------- Magnetometer Caliblation ---------------------------------------------------------
00126     // (a) Make some random movements (for example: writing the number ‘8’ on air) until the
00127     //     CALIB_STAT register indicates fully calibrated.
00128     // (b) It takes more calibration movements to get the magnetometer calibrated than in the
00129     //     NDOF mode.
00130     pc.printf("Step2) random moving (try to change the BNO055 axis)\r\n");
00131     t.start();
00132     while (t.read() < 30) {
00133         d = imu.read_calib_status();
00134         pc.printf("calibrating, = 0x%x target  = 0x33(at least)\r\n", d);
00135         if ((d & 0x03) == 0x03) {
00136             break;
00137         }
00138         wait(1.0);
00139     }
00140     pc.printf("-> Step2) is done\r\n\r\n");
00141     //---------- Magnetometer Caliblation ---------------------------------------------------------
00142     // a) Place the device in 6 different stable positions for a period of few seconds
00143     //    to allow the accelerometer to calibrate.
00144     // b) Make sure that there is slow movement between 2 stable positions
00145     //    The 6 stable positions could be in any direction, but make sure that the device is
00146     //    lying at least once perpendicular to the x, y and z axis.
00147     pc.printf("Step3) Change rotation each X,Y,Z axis KEEP SLOWLY!!");
00148     pc.printf(" Each 90deg stay a 5 sec and set at least 6 position.\r\n");
00149     pc.printf(" e.g. (1)ACC:X0,Y0,Z-9,(2)ACC:X9,Y0,Z0,(3)ACC:X0,Y0,Z9,");
00150     pc.printf("(4)ACC:X-9,Y0,Z0,(5)ACC:X0,Y-9,Z0,(6)ACC:X0,Y9,Z0,\r\n");
00151     pc.printf(" If you will give up, hit any key.\r\n", d);
00152     t.stop();
00153     while (true) {
00154         d = imu.read_calib_status();
00155         imu.get_gravity(&gravity);
00156         pc.printf("calibrating = 0x%x target  = 0xff ACC:X %4.1f, Y %4.1f, Z %4.1f\r\n",
00157                   d, gravity.x, gravity.y, gravity.z);
00158         if (d == 0xff) {
00159             break;
00160         }
00161         if (pc.readable()) {
00162             break;
00163         }
00164         wait(1.0);
00165     }
00166     if (imu.read_calib_status() == 0xff) {
00167         pc.printf("-> All of Calibration steps are done successfully!\r\n\r\n");
00168     } else {
00169         pc.printf("-> Calibration steps are suspended!\r\n\r\n");
00170     }
00171     t.stop();
00172 }
00173 
00174 //-------------------------------------------------------------------------------------------------
00175 //-------------------------------------------------------------------------------------------------
00176 //-------------------------------------------------------------------------------------------------
00177 //-------------------------------------------------------------------------------------------------
00178 //  Main Program
00179 //-------------------------------------------------------------------------------------------------
00180 //-------------------------------------------------------------------------------------------------
00181 //-------------------------------------------------------------------------------------------------
00182 //-------------------------------------------------------------------------------------------------
00183 int main()
00184 {
00185     oscill=0;
00186     pc.baud(921600);
00187     pc.attach(&rxCallback, MODSERIAL::RxIrq);
00188     pc.printf("BNO055 Hello World/\r\n\r\n");
00189     imu.set_mounting_position(MT_P6);
00190     pwr_onoff = 0;
00191     pc.printf("\r\n\r\nIf pc terminal soft is ready, please hit any key!/\r\n");
00192     char c = pc.getc();
00193     pc.printf("Bosch Sensortec BNO055 test program on " __DATE__ "/" __TIME__ "\r\n");
00194     // Is BNO055 avairable?
00195     if (imu.chip_ready() == 0) {
00196         do {
00197             pc.printf("Bosch BNO055 is NOT avirable!!\r\n Reset/\r\n");
00198             pwr_onoff = 1;  // Power off
00199             wait(0.1);
00200             pwr_onoff = 0;  // Power on
00201             wait(0.02);
00202         } while(imu.reset());
00203     }
00204     pc.printf("Bosch BNO055 is available now!!/\r\n");
00205     pc.printf("AXIS_REMAP_CONFIG:0x%02x, AXIS_REMAP_SIGN:0x%02x/\r\n",
00206               imu.read_reg0(BNO055_AXIS_MAP_CONFIG), imu.read_reg0(BNO055_AXIS_MAP_SIGN));
00207     imu.read_id_inf(&bno055_id_inf);
00208     pc.printf("CHIP ID:0x%02x, ACC ID:0x%02x, MAG ID:0x%02x, GYR ID:0x%02x,/",
00209               bno055_id_inf.chip_id, bno055_id_inf.acc_id, bno055_id_inf.mag_id, bno055_id_inf.gyr_id);
00210     pc.printf("SW REV:0x%04x, BL REV:0x%02x/\r\n",
00211               bno055_id_inf.sw_rev_id, bno055_id_inf.bootldr_rev_id);
00212     pc.printf("If you would like to calibrate the BNO055, please hit 'y' (No: any other key)/\r\n");
00213     c = pc.getc();
00214     if (c == 'y') {
00215         bno055_calbration();
00216     }
00217     
00218     pc.printf("If you would like to Synchronize the System, please hit 'y' (No: any other key)\r\n/");
00219     c = pc.getc();
00220     if (c == 'y') {
00221         sync=0;
00222         pc.printf("Synchronise/");
00223         wait(0.5);
00224     }    
00225     
00226     while (sync == 0) {
00227         pc.printf("HELLO/");
00228         if (pc.readable()) {
00229             if (pc.getc() == 72) {
00230                 while (sync == 0) {
00231                     if (pc.readable()) {
00232                         if (pc.getc() == 73) {
00233                             sync = 1;
00234                         }
00235                     }
00236                 }
00237             }    
00238         }       
00239     }
00240     pc.printf("START/");
00241     t.start();
00242     imu.write_reg0(0x3d, 0x07); // Default FUS
00243     imu.write_reg0(0x3b, 0x01);
00244     imu.write_reg0(0x08, 0x03);
00245     int units = imu.read_reg0(0x08);
00246     while (1) {
00247         while (RAW == true) {
00248             Time = t.read_ms();
00249             if (Time > TimeStamp + 5) {
00250                 pc.printf("RAW,xLSB:,%d,xMSB:,%d,yLSB:%d,yMSB:,%d,zLSB:,%d,zMSB:,%d,"
00251                           ,imu.read_reg0(0x08),imu.read_reg0(0x09)
00252                           ,imu.read_reg0(0x0A),imu.read_reg0(0x0B)
00253                           ,imu.read_reg0(0x0C),imu.read_reg0(0x0D));
00254                 pc.printf("Time:,%d,/",t.read_ms());
00255                 TimeStamp = Time;
00256             }
00257         }
00258         while (RAW == false) {
00259             //Time = t.read_ms();
00260             //TimeNow = + t.read_ms() - TimeOfStart;
00261             
00262             //TimeStart = t.read_us() - TimeOfStart;
00263             
00264             if (t.read_us () - TimeOfStart > TimeStamp + 7060) {                              
00265                           
00266                 //oscill = 1;
00267                           
00268                 imu.get_linear_accel(&linear_acc);
00269                 imu.get_Euler_Angles(&euler_angles);
00270                 imu.get_quaternion(&quaternion);
00271                 imu.get_gravity(&gravity);
00272                 pc.printf("FUS,T:,%d,x:,%+6.1f,y:,%+6.1f,z:,%+6.1f, , ,Y,%+6.1f,R,%+6.1f,P,%+6.1f,W,%d,X,%d,Y,%d,Z,%d,X,%+6.1f,Y,%+6.1f,Z,%+6.1f,/"
00273                           ,t.read_ms() - TimeOfStartms,linear_acc.x ,linear_acc.y ,linear_acc.z,
00274                           euler_angles.h, euler_angles.r, euler_angles.p,
00275                           quaternion.w, quaternion.x, quaternion.y, quaternion.z,
00276                           gravity.x, gravity.y, gravity.z); 
00277                           
00278                 //oscill = 0;
00279                 //wait(1);
00280                                          
00281                 TimeStamp = t.read_us()- TimeOfStart;
00282                 //pc.printf("TimeStart:,%d,/",TimeStart); 
00283                 //pc.printf("Timend:,%d,/",+ t.read_us() - TimeOfStart);
00284             }
00285 
00286             /*if (Time > TimeStamp2 + 999) {
00287                 if (direction == false) {
00288                     PWM.pulsewidth_us(2400);
00289                     //pc.printf("DIR, UP,");
00290                     //pc.printf("Time:,%d,/",t.read_ms());
00291                     direction = true;
00292                     TimeStamp2 = Time;
00293                 }
00294 
00295                 else if (direction == true) {
00296                     PWM.pulsewidth_us(600);
00297                     //pc.printf("DIR, DOWN,");
00298                     //pc.printf("Time:,%d,/",t.read_ms());
00299                     direction = false;
00300                     TimeStamp2 = Time;
00301                 }
00302             }
00303             */
00304         }
00305     }
00306 }