Diff: main.cpp

Revision:

1:a2c5a8a74527

Parent:

0:06fc5a68ead2

Child:

2:a0fb0b785bd6

--- a/main.cpp	Sat Apr 08 04:15:22 2017 +0000
+++ b/main.cpp	Sat Apr 08 08:10:19 2017 -0400
@@ -6,6 +6,7 @@
 Thread mems_thread;
 Thread range_thread;
 Mutex serial_mutex;
+//Mutex sensor_mutex;
 
 DigitalOut led(D3);
 
@@ -14,7 +15,7 @@
 /* Instantiate the expansion board */
 static X_NUCLEO_IKS01A1 *mems_expansion_board = X_NUCLEO_IKS01A1::Instance(&i2c);
 // MAKE SURE YOU JUMPER PIN D7 TO PIN D8!
-static X_NUCLEO_53L0A1 *range = X_NUCLEO_53L0A1::Instance(&i2c, A2, D7, D2);
+static X_NUCLEO_53L0A1 *range_board = X_NUCLEO_53L0A1::Instance(&i2c, A2, D7, D2);
 
 /* Retrieve the composing elements of the expansion board */
 static GyroSensor *gyroscope = mems_expansion_board->GetGyroscope();
@@ -26,9 +27,11 @@
 static TempSensor *temp_sensor2 = mems_expansion_board->pt_sensor;
 
 /* Helper function for printing from multiple threads */
-void ts_printf() {
+void ts_printf(const char* format, ...) {
     serial_mutex.lock();
-    printf();
+    va_list args;
+    va_start(args, format);
+    printf(format, args);
     serial_mutex.unlock();
 }
 
@@ -71,36 +74,87 @@
     float value1, value2;
     char buffer1[32], buffer2[32];
     int32_t axes[3];
+    int ret;
 
     ts_printf("\r\n--- Starting mems board ---\r\n");
 
+    //sensor_mutex.lock();
     humidity_sensor->read_id(&id);
+    //sensor_mutex.unlock();
     ts_printf("HTS221  humidity & temperature    = 0x%X\r\n", id);
+
+    //sensor_mutex.lock();
     pressure_sensor->read_id(&id);
+    //sensor_mutex.unlock();
     ts_printf("LPS25H  pressure & temperature    = 0x%X\r\n", id);
+
+    //sensor_mutex.lock();
     magnetometer->read_id(&id);
+    //sensor_mutex.unlock();
     ts_printf("LIS3MDL magnetometer              = 0x%X\r\n", id);
+
+    //sensor_mutex.lock();
     gyroscope->read_id(&id);
+    //sensor_mutex.unlock();
     ts_printf("LSM6DS0 accelerometer & gyroscope = 0x%X\r\n", id);
 
     wait(3);
 
     while (true) {
-        temp_sensor1->get_temperature(&value1);
-        humidity_sensor->get_humidity(&value2);
-        ts_printf("HTS221: [temp] %7s°C,   [hum] %s%%\r\n", printDouble(buffer1, value1), printDouble(buffer2, value2));
+        //sensor_mutex.lock();
+        ret = temp_sensor1->get_temperature(&value1);
+        //sensor_mutex.unlock();
+        if (ret) {
+            ts_printf("failed to get temp C\r\n");
+        }
+        //sensor_mutex.lock();
+        ret = humidity_sensor->get_humidity(&value2);
+        //sensor_mutex.unlock();
+        if (ret) {
+            ts_printf("failed to get humidity\r\n");
+        }
+        //ts_printf("HTS221: [temp] %7s°C,   [hum] %s%%\r\n", printDouble(buffer1, value1), printDouble(buffer2, value2));
+        ts_printf("HTS221: [temp] %f°C\r\n", value1);
+        ts_printf("HTS221: [hum] %f%%\r\n", value2);
 
-        temp_sensor2->get_fahrenheit(&value1);
-        pressure_sensor->get_pressure(&value2);
-        ts_printf("LPS25H: [temp] %7s°F, [press] %smbar\r\n", printDouble(buffer1, value1), printDouble(buffer2, value2));
+        //sensor_mutex.lock();
+        ret = temp_sensor2->get_fahrenheit(&value1);
+        //sensor_mutex.unlock();
+        if (ret) {
+            ts_printf("failed to get temp F\r\n");
+        }
+        //sensor_mutex.lock();
+        ret = pressure_sensor->get_pressure(&value2);
+        //sensor_mutex.unlock();
+        if (ret) {
+            ts_printf("failed to get pressure F\r\n");
+        }
+        //ts_printf("LPS25H: [temp] %7s°F, [press] %smbar\r\n", printDouble(buffer1, value1), printDouble(buffer2, value2));
+        ts_printf("LPS25H: [temp] %f°F\r\n", value1);
+        ts_printf("LPS25H: [press] %fmbar\r\n", value2);
 
-        magnetometer->get_m_axes(axes);
+        //sensor_mutex.lock();
+        ret = magnetometer->get_m_axes(axes);
+        //sensor_mutex.unlock();
+        if (ret) {
+            ts_printf("failed to get magnetometer\r\n");
+        }
         ts_printf("LIS3MDL [mag/mgauss]:  %7ld, %7ld, %7ld\r\n", axes[0], axes[1], axes[2]);
         
-        accelerometer->get_x_axes(axes);
+        //sensor_mutex.lock();
+        ret = accelerometer->get_x_axes(axes);
+        //sensor_mutex.unlock();
+        if (ret) {
+            ts_printf("failed to get accelerometer\r\n");
+        }
         ts_printf("LSM6DS0 [acc/mg]:      %7ld, %7ld, %7ld\r\n", axes[0], axes[1], axes[2]);
 
-        gyroscope->get_g_axes(axes);
+        //sensor_mutex.lock();
+        ret = gyroscope->get_g_axes(axes);
+        //sensor_mutex.unlock();
+        if (ret) {
+            ts_printf("failed to get gyroscope\r\n");
+        }
         ts_printf("LSM6DS0 [gyro/mdps]:   %7ld, %7ld, %7ld\r\n", axes[0], axes[1], axes[2]);
 
         wait(1.5);
@@ -116,7 +170,9 @@
 
     /* init the 53L0A1 expansion board with default values */
     while (true) {
-        status=board->InitBoard();
+        //sensor_mutex.lock();
+        status=range_board->InitBoard();
+        //sensor_mutex.unlock();
         if (status) {
             ts_printf("Failed to init range board!\r\n");
             wait(1);
@@ -127,23 +183,25 @@
     }
 
     while (true) {
-        status = board->sensor_centre->GetDistance(&distance);
+        //sensor_mutex.lock();
+        status = range_board->sensor_centre->GetDistance(&distance);
+        //sensor_mutex.unlock();
         if (status == VL53L0X_ERROR_NONE) {
             ts_printf("Distance : %ld\n", distance);
         }
 
-        wait(0.5);
+        wait(0.8);
     }
 }
 
 int main() {
     ts_printf("\r\n--- Application starting up ---\r\n");
 
-    mems_thread.start(mems_func);
-    range_thread.start(range_func);
+    mems_thread.start(mbed::Callback<void()> (mems_func));
+    range_thread.start(mbed::Callback<void()> (range_func));
 
     while (true) {
         led = !led;
         wait(0.25);
     }
-}
\ No newline at end of file
+}