Diff: main.cpp

Revision:

9:72ad545ce54f

Parent:

7:811949acf593

--- a/main.cpp	Tue Feb 07 00:24:31 2017 +0000
+++ b/main.cpp	Mon Jun 12 04:13:35 2017 +0000
@@ -35,56 +35,45 @@
 #include "max32630fthr.h"
 #include "bmi160.h"
 
-
-void dumpImuRegisters(BMI160 &imu);
-void printRegister(BMI160 &imu, BMI160::Registers reg);
-void printBlock(BMI160 &imu, BMI160::Registers startReg, BMI160::Registers stopReg);
+#define LOG(...) { printf(__VA_ARGS__); }
 
-
-int main()
-{
-    MAX32630FTHR pegasus(MAX32630FTHR::VIO_3V3);
-    
+int main() {
     DigitalOut rLED(LED1, LED_OFF);
     DigitalOut gLED(LED2, LED_OFF);
     DigitalOut bLED(LED3, LED_OFF);
-    
+
     I2C i2cBus(P5_7, P6_0);
     i2cBus.frequency(400000);
     BMI160_I2C imu(i2cBus, BMI160_I2C::I2C_ADRS_SDO_LO);
     
-    printf("\033[H");  //home
-    printf("\033[0J");  //erase from cursor to end of screen
-    
+    I2C dataOut(P3_4, P3_5);
+    dataOut.frequency(10000);
+
     uint32_t failures = 0;
-    
-    if(imu.setSensorPowerMode(BMI160::GYRO, BMI160::NORMAL) != BMI160::RTN_NO_ERROR)
-    {
-        printf("Failed to set gyroscope power mode\n");
+
+    MAX32630FTHR pegasus(MAX32630FTHR::VIO_3V3);
+
+    if(imu.setSensorPowerMode(BMI160::GYRO, BMI160::NORMAL) != BMI160::RTN_NO_ERROR) {
+        LOG("Failed to set gyroscope power mode\n");
         failures++;
     }
     wait_ms(100);
     
-    if(imu.setSensorPowerMode(BMI160::ACC, BMI160::NORMAL) != BMI160::RTN_NO_ERROR)
-    {
-        printf("Failed to set accelerometer power mode\n");
+    if(imu.setSensorPowerMode(BMI160::ACC, BMI160::NORMAL) != BMI160::RTN_NO_ERROR) {
+        LOG("Failed to set accelerometer power mode\n");
         failures++;
     }
     wait_ms(100);
-    
-    
+
     BMI160::AccConfig accConfig;
     //example of using getSensorConfig
-    if(imu.getSensorConfig(accConfig) == BMI160::RTN_NO_ERROR)
-    {
-        printf("ACC Range = %d\n", accConfig.range);
-        printf("ACC UnderSampling = %d\n", accConfig.us);
-        printf("ACC BandWidthParam = %d\n", accConfig.bwp);
-        printf("ACC OutputDataRate = %d\n\n", accConfig.odr);
-    }
-    else
-    {
-        printf("Failed to get accelerometer configuration\n");
+    if(imu.getSensorConfig(accConfig) == BMI160::RTN_NO_ERROR) {
+        LOG("ACC Range = %d\n", accConfig.range);
+        LOG("ACC UnderSampling = %d\n", accConfig.us);
+        LOG("ACC BandWidthParam = %d\n", accConfig.bwp);
+        LOG("ACC OutputDataRate = %d\n\n", accConfig.odr);
+    } else {
+        LOG("Failed to get accelerometer configuration\n");
         failures++;
     }
     
@@ -93,118 +82,50 @@
     accConfig.us = BMI160::ACC_US_OFF;
     accConfig.bwp = BMI160::ACC_BWP_2;
     accConfig.odr = BMI160::ACC_ODR_8;
-    if(imu.setSensorConfig(accConfig) == BMI160::RTN_NO_ERROR)
-    {
-        printf("ACC Range = %d\n", accConfig.range);
-        printf("ACC UnderSampling = %d\n", accConfig.us);
-        printf("ACC BandWidthParam = %d\n", accConfig.bwp);
-        printf("ACC OutputDataRate = %d\n\n", accConfig.odr);
-    }
-    else
-    {
-        printf("Failed to set accelerometer configuration\n");
+    if(imu.setSensorConfig(accConfig) == BMI160::RTN_NO_ERROR) {
+        LOG("ACC Range = %d\n", accConfig.range);
+        LOG("ACC UnderSampling = %d\n", accConfig.us);
+        LOG("ACC BandWidthParam = %d\n", accConfig.bwp);
+        LOG("ACC OutputDataRate = %d\n\n", accConfig.odr);
+    } else {
+        LOG("Failed to set accelerometer configuration\n");
         failures++;
     }
     
     BMI160::GyroConfig gyroConfig;
-    if(imu.getSensorConfig(gyroConfig) == BMI160::RTN_NO_ERROR)
-    {
-        printf("GYRO Range = %d\n", gyroConfig.range);
-        printf("GYRO BandWidthParam = %d\n", gyroConfig.bwp);
-        printf("GYRO OutputDataRate = %d\n\n", gyroConfig.odr);
-    }
-    else
-    {
-        printf("Failed to get gyroscope configuration\n");
+    if(imu.getSensorConfig(gyroConfig) == BMI160::RTN_NO_ERROR) {
+        LOG("GYRO Range = %d\n", gyroConfig.range);
+        LOG("GYRO BandWidthParam = %d\n", gyroConfig.bwp);
+        LOG("GYRO OutputDataRate = %d\n\n", gyroConfig.odr);
+    } else {
+        LOG("Failed to get gyroscope configuration\n");
         failures++;
     }
     
     wait(1.0);
-    printf("\033[H");  //home
-    printf("\033[0J");  //erase from cursor to end of screen
     
-    if(failures == 0)
-    {
+    if(failures == 0) {
         float imuTemperature; 
         BMI160::SensorData accData;
         BMI160::SensorData gyroData;
         BMI160::SensorTime sensorTime;
         
-        while(1)
-        {
+        char buffer[256] = {0};
+        
+        while(1) {
             imu.getGyroAccXYZandSensorTime(accData, gyroData, sensorTime, accConfig.range, gyroConfig.range);
             imu.getTemperature(&imuTemperature);
             
-            printf("ACC xAxis = %s%4.3f\n", "\033[K", accData.xAxis.scaled);
-            printf("ACC yAxis = %s%4.3f\n", "\033[K", accData.yAxis.scaled);
-            printf("ACC zAxis = %s%4.3f\n\n", "\033[K", accData.zAxis.scaled);
-            
-            printf("GYRO xAxis = %s%5.1f\n", "\033[K", gyroData.xAxis.scaled);
-            printf("GYRO yAxis = %s%5.1f\n", "\033[K", gyroData.yAxis.scaled);
-            printf("GYRO zAxis = %s%5.1f\n\n", "\033[K", gyroData.zAxis.scaled);
-            
-            printf("Sensor Time = %s%f\n", "\033[K", sensorTime.seconds);
-            printf("Sensor Temperature = %s%5.3f\n", "\033[K", imuTemperature);
-            
-            printf("\033[H");  //home
+            sprintf(buffer, "%d,%d,%d\n", accData.xAxis.raw, accData.yAxis.raw, accData.zAxis.raw);
+            LOG(buffer);
+            dataOut.write(0x8<<1, buffer, strlen(buffer) + 1);
+
             gLED = !gLED;
         }
-    }
-    else
-    {
-        while(1)
-        {
+    } else {
+        while(1) {
             rLED = !rLED;
             wait(0.25);
         }
     }
 }
-
-
-//*****************************************************************************
-void dumpImuRegisters(BMI160 &imu)
-{
-    printRegister(imu, BMI160::CHIP_ID);
-    printBlock(imu, BMI160::ERR_REG,BMI160::FIFO_DATA);
-    printBlock(imu, BMI160::ACC_CONF, BMI160::FIFO_CONFIG_1);
-    printBlock(imu, BMI160::MAG_IF_0, BMI160::SELF_TEST);
-    printBlock(imu, BMI160::NV_CONF, BMI160::STEP_CONF_1);
-    printRegister(imu, BMI160::CMD);
-    printf("\n");
-}
-
-
-//*****************************************************************************
-void printRegister(BMI160 &imu, BMI160::Registers reg)
-{
-    uint8_t data;
-    if(imu.readRegister(reg, &data) == BMI160::RTN_NO_ERROR)
-    {
-        printf("IMU Register 0x%02x = 0x%02x\n", reg, data);
-    }
-    else
-    {
-        printf("Failed to read register\n");
-    }
-}
-
-
-//*****************************************************************************
-void printBlock(BMI160 &imu, BMI160::Registers startReg, BMI160::Registers stopReg)
-{
-    uint8_t numBytes = ((stopReg - startReg) + 1);
-    uint8_t buff[numBytes];
-    uint8_t offset = static_cast<uint8_t>(startReg);
-    
-    if(imu.readBlock(startReg, stopReg, buff) == BMI160::RTN_NO_ERROR)
-    {
-        for(uint8_t idx = offset; idx < (numBytes + offset); idx++)
-        {
-            printf("IMU Register 0x%02x = 0x%02x\n", idx, buff[idx - offset]);
-        }
-    }
-    else
-    {
-        printf("Failed to read block\n");
-    }
-}