Diff: main.cpp

Revision:

8:c5dc1ce10722

Parent:

6:cc08f2d64773

Child:

9:87a7169b4d5c

--- a/main.cpp	Thu Mar 26 12:27:43 2020 +0000
+++ b/main.cpp	Thu Mar 26 21:13:44 2020 +0000
@@ -1,6 +1,6 @@
 
 #include "mbed.h"
-#include "hmc5883.h"
+#include "magnetometer.h"
 #include <quan/max.hpp>
 #include <quan/min.hpp>
 #include <quan/time.hpp>
@@ -15,59 +15,62 @@
 
 namespace {
     
-    QUAN_QUANTITY_LITERAL(magnetic_flux_density,gauss);
-    QUAN_QUANTITY_LITERAL(magnetic_flux_density,milli_gauss);
-    QUAN_QUANTITY_LITERAL(magnetic_flux_density,uT);
     QUAN_QUANTITY_LITERAL(time,ms);
 
     // terminal loop, printing message periodically
     void loop_forever(std::string const & str)
     {
-        DigitalOut led2(LED2);
+        DigitalOut led1(LED1,1);
         // stop but print error dynamically
         int count = 0;
         for (;;) {
-            led2 = 1;
+            led1 = 1;
             std::cout << str << " " << count++ << '\n';
             ThisThread::sleep_for(200U);
-            led2 = 0;
+            led1 = 0;
             ThisThread::sleep_for(800U);
         }
     }
+    
+    // do something with mag output
+    void callback(quan::three_d::vect<quan::magnetic_flux_density::uT> const & v)
+    {
+         std::cout << "val = " << v << '\n';
+    }
+    
 }// namespace
 
 int main()
 {
-    std::cout << "HMC5883 test\n";
-    //wait for mag to init
-    ThisThread::sleep_for(500U);
-
-    if ( mag_detected() ) {
-        std::cout << "Detected a HMC5883\n";
-    } else {
-        loop_forever("Failed to detect HMC5883");
-    }
+    DigitalOut led2(LED2,1);
+    std::cout << "magnetometer test\n";
 
-    // N.b after offsets removed mag was reading around 34.49 uT, so not bad!
-    constexpr auto earth_magnetic_field_flux_density = 31.869_uT;
-    bool const success =
-        mag_set_samples_average(8) &&
-        mag_set_range( earth_magnetic_field_flux_density * 2U);
-    if ( !success) {
-        loop_forever("HMC5883 setup failed");
+    if ( !mag_init()){
+        loop_forever("failed to init magnetometer\n");
     }
-    std::cout << " mag range set to " << mag_get_range() << '\n';
     
-    //gains form calib engine
-    quan::three_d::vect<double> gains{1.37689,1.35057,1.56647};
-    
-    // offsets from calib engine
-    quan::three_d::vect<
-        quan::magnetic_flux_density::uT
-    > offsets{10.5724_uT,-10.869_uT,2.241468_uT};
-     
-    mag_adjusted_values(20_ms,gains,offsets);
-   // mag_raw_values(20_ms);
-   // mag_offset_calibrate();
-    
+    uint64_t constexpr update_rate_ms = 20U;
+        
+    auto prev_led = Kernel::get_ms_count();
+    auto now = Kernel::get_ms_count();
+    // TODO Thread for this
+    for (;;){
+        mag_start_measurement();
+        do{
+           ThisThread::sleep_for(10U);
+        }  while ( ! mag_data_ready() ); 
+        
+        quan::three_d::vect<quan::magnetic_flux_density::uT> v;
+        if (mag_read(v)){
+          callback(v);
+        }
+        if ( (now - prev_led) >= 500U){
+            prev_led = now;
+            led2 = (led2 == 0) ? 1: 0;
+        }
+        auto const next_wake = now +
+            static_cast<uint64_t>(update_rate_ms);
+        ThisThread::sleep_until(next_wake);
+        now = next_wake;
+    }
 }
\ No newline at end of file