Diff: hmc5883.cpp

Revision:

8:c5dc1ce10722

Parent:

7:5d14da0b4c95

Child:

9:87a7169b4d5c

--- a/hmc5883.cpp	Thu Mar 26 12:27:43 2020 +0000
+++ b/hmc5883.cpp	Thu Mar 26 21:13:44 2020 +0000
@@ -1,150 +1,108 @@
 
 
 #include "mbed.h"
+#include "hmc5883.h"
+#include "resourceManager.h"
 
 #include <iostream>
 #include <quan/out/magnetic_flux_density.hpp>
 #include <quan/three_d/out/vect.hpp>
 
-#include "resourceManager.h"
-/*
+namespace
+{
 
-   MySensor sensor;
-   
-   "HMC5883" -> HMC5883
-   I2C ->
-         list of I2CBuses 
-            I2CBusID -> I2C(I2C_SCL,I2C_SDA) 
-               I2CAddress ->  "0x3d" atoi -> 0x3D 
-   
-   if (!sensor.open("HMC5883L.I2C[I2CBusID,0x3d]")){
-       loop_forever("failed to open \"HMC5883L.I2C[I2CBusID,0x3d]\"");
-   }
-   sensor.Attach(50.0_Hz, onMagDataUpdated);
-   sensor.run();
-   
-   template <typename Quantity>
-   struct Sensor{
-      static bool open(Sensor& sensor, const char* name);
-      bool connected()const;
-      bool running() const;
-      bool idle() const
-      void close();
-      bool read(Quantity & q);
-      setUpdateCallback(void(*pFun)());
-   };
-*/
+template <int N, int D=1> struct mag_data_rate_id;
+// values are mag settings for each data rate
+template <> struct mag_data_rate_id<3,4> : std::integral_constant<uint8_t,(0b000U << 2U)> {};
+template <> struct mag_data_rate_id<3,2> : std::integral_constant<uint8_t,(0b001U << 2U)> {};
+template <> struct mag_data_rate_id<3> : std::integral_constant<uint8_t,(0b010U << 2U)> {};
+template <> struct mag_data_rate_id<15,2> : std::integral_constant<uint8_t,(0b011U << 2U)> {};
+template <> struct mag_data_rate_id<15> : std::integral_constant<uint8_t,(0b100U << 2U)> {};
+template <> struct mag_data_rate_id<30> : std::integral_constant<uint8_t,(0b101U << 2U)> {};
+template <> struct mag_data_rate_id<75> : std::integral_constant<uint8_t,(0b110U << 2U)> {};
+
+}//namespace
+
+hmc5883L::hmc5883L(I2C& i2cIn,uint8_t addressIn)
+    :I2CBusDevice{i2cIn,addressIn}
+    ,mag_resolution{0.92_milli_gauss}
+    ,mag_range{1.3_gauss}
+    ,mag_gain{1.0,1.0,1.0}
+    ,mag_offset{0.0_uT,0.0_uT,0.0_uT}
+{}
 
-namespace {
-    /*
-    00 Configuration Register A       R/W
-    01 Configuration Register B       R/W
-    02 Mode Register                  R/W
-    03 Data Output X MSB Register     R
-    04 Data Output X LSB Register     R
-    05 Data Output Z MSB Register     R
-    06 Data Output Z LSB Register     R
-    07 Data Output Y MSB Register     R
-    08 Data Output Y LSB Register     R
-    09 Status Register                R
-    10 Identification Register A      R
-    11 Identification Register B      R
-    12 Identification Register C      R
-    */
-    
-   // I2C i2c(I2C_SDA, I2C_SCL );
-   
-    I2C& i2c = resource::i2c1;
-   
-    constexpr uint8_t i2c_addr = 0x3D;
-    constexpr char cfg_regA = 0;
-    constexpr char cfg_regB = 1;
-    constexpr char mode_reg = 2;
-    constexpr char dout_reg = 3;
-    constexpr char status_reg = 9;
-    constexpr char id_regA = 10U;
-    
-    // Set reg index to idx_in
-    // return true if successful
-    bool mag_set_reg_idx(uint8_t idx_in)
-    {
-        char const idx = static_cast<char>(idx_in);
-        bool const result = i2c.write(i2c_addr,&idx,1) == 0;
-        if(result) {
+bool hmc5883L::set_reg_idx(uint8_t idx_in)const
+{
+    char const idx = static_cast<char>(idx_in);
+    bool const result = this->i2c_write(&idx,1) == 0;
+    if(result) {
+        return true;
+    } else {
+        std::cout << "mag_set_reg_idx failed\n";
+        return false;
+    }
+}
+
+bool hmc5883L::write_reg(uint8_t idx, uint8_t val)const
+{
+    char const ar[2] = {idx,val};
+    bool const result = this->i2c_write(ar,2) == 0;
+    if(result) {
+        return true;
+    } else {
+        std::cout << " mag_write_reg failed\n";
+        return false;
+    }
+}
+
+bool hmc5883L::get_reg(uint8_t idx_in, uint8_t& result)const
+{
+    if ( this->set_reg_idx(idx_in)) {
+        char temp_result = 0;
+        bool const success = this->i2c_read(&temp_result,1,false) == 0;
+        if (success) {
+            result = temp_result;
             return true;
         } else {
-            std::cout << "mag_set_reg_idx failed\n";
-            return false;
-        }
-    }
-    
-    // Write reg at idx with val
-    // return true if successful
-    bool mag_write_reg(uint8_t idx, uint8_t val)
-    {
-        char ar[2] = {idx,val};
-        bool const result = i2c.write(i2c_addr,ar,2) == 0;
-        if(result) {
-            return true;
-        } else {
-            std::cout << " mag_write_reg failed\n";
+            std::cout << "mag_get_reg read failed\n";
             return false;
         }
-    }
-    
-    // Read reg at idx to result
-    // return true if successfull
-    bool mag_get_reg(uint8_t idx_in, uint8_t& result)
-    {
-        if ( mag_set_reg_idx(idx_in)) {
-            char temp_result = 0;
-            bool const success = i2c.read(i2c_addr,&temp_result,1) == 0;
-            if (success) {
-                result = temp_result;
-                return true;
-            } else {
-                std::cout << "mag_get_reg read failed\n";
-                return false;
-            }
-        } else {
-            return false;
-        }
+    } else {
+        return false;
     }
-    
-    // Update value in reg using and and or masks
-    // reg <-- (reg & and_val) | or_val
-    // return true if successfull
-    bool mag_modify_reg(uint8_t idx, uint8_t and_val, uint8_t or_val)
-    {
-        uint8_t cur_val = 0;
-        if(mag_get_reg(idx,cur_val)) {
-            uint8_t const new_val = (cur_val & and_val ) | or_val;
-            return mag_write_reg(idx,new_val);
-        } else {
-            return false;
-        }
+}
+
+bool hmc5883L::modify_reg(uint8_t idx, uint8_t and_val, uint8_t or_val)const
+{
+    uint8_t cur_val = 0;
+    if(this->get_reg(idx,cur_val)) {
+        uint8_t const new_val = (cur_val & and_val ) | or_val;
+        return this->write_reg(idx,new_val);
+    } else {
+        return false;
     }
-
-}  // namespace
+}
 
-// probe for the HMC5883 on I2C
-// return true if found
-bool mag_detected()
+bool hmc5883L::detected(bool verbose)const
 {
-    if ( mag_set_reg_idx(id_regA) ) {
+    if ( this->set_reg_idx(id_regA) ) {
         char id_input[4];
-        bool success = i2c.read(i2c_addr,id_input,3) == 0;
+        bool success = this->i2c_read(id_input,3) == 0;
         if(success) {
             id_input[3] = '\0';
             bool const is_hmc = (strcmp(id_input,"H43") == 0);
             if (is_hmc) {
                 return true;
             } else {
+                if( verbose) {
                 std::cout << "hmc5883 ID string didnt match\n";
+                }
                 return false;
             }
         } else {
-            std::cout << "id mag read failed\n";
+            if( verbose) {
+                std::cout << "id mag read failed\n";
+            }
             return false;
         }
     } else {
@@ -153,7 +111,7 @@
 }
 
 // only 1,2,4,8 available
-bool mag_set_samples_average(int n_samples)
+bool hmc5883L::set_samples_average(int n_samples)const
 {
     uint8_t or_val = 0;
     switch (n_samples) {
@@ -174,78 +132,47 @@
             return false;
     }
     uint8_t constexpr and_val = ~(0b11 << 5U);
-    return mag_modify_reg(cfg_regA,and_val,or_val);
+    return this->modify_reg(cfg_regA,and_val,or_val);
 }
 
-/*
-data rate 0.75, 1.5, 3 ,7.5, 15 (Default) , 30, 75
-*/
-namespace {
-    
-    template <int N, int D=1> struct mag_data_rate_id;
-    // values are mag settings for each data rate
-    template <> struct mag_data_rate_id<3,4> : std::integral_constant<uint8_t,(0b000U << 2U)> {};
-    template <> struct mag_data_rate_id<3,2> : std::integral_constant<uint8_t,(0b001U << 2U)> {};
-    template <> struct mag_data_rate_id<3> : std::integral_constant<uint8_t,(0b010U << 2U)> {};
-    template <> struct mag_data_rate_id<15,2> : std::integral_constant<uint8_t,(0b011U << 2U)> {};
-    template <> struct mag_data_rate_id<15> : std::integral_constant<uint8_t,(0b100U << 2U)> {};
-    template <> struct mag_data_rate_id<30> : std::integral_constant<uint8_t,(0b101U << 2U)> {};
-    template <> struct mag_data_rate_id<75> : std::integral_constant<uint8_t,(0b110U << 2U)> {};
-
-}//namespace
-
 template <int N, int D>
-bool mag_set_data_rate()
+bool hmc5883L::set_data_rate()const
 {
     uint8_t constexpr and_val = static_cast<uint8_t>(~(0b111U << 2U));
     uint8_t constexpr or_val = mag_data_rate_id<N,D>::value;
-    return mag_modify_reg(cfg_regA,and_val,or_val);
+    return this->modify_reg(cfg_regA,and_val,or_val);
+}
+//data rate 0.75, 1.5, 3 ,7.5, 15 (Default) , 30, 75
+template bool hmc5883L::set_data_rate<3,4>() const;
+template bool hmc5883L::set_data_rate<3,2>() const;
+template bool hmc5883L::set_data_rate<3>() const;
+template bool hmc5883L::set_data_rate<15,2>() const;
+template bool hmc5883L::set_data_rate<15>() const;
+template bool hmc5883L::set_data_rate<30>() const;
+template bool hmc5883L::set_data_rate<75>() const;
+
+bool hmc5883L::set_positive_bias()const
+{
+    uint8_t constexpr and_val = static_cast<uint8_t>(~(0b11U ));
+    uint8_t constexpr or_val = 0b01U;
+    return this->modify_reg(cfg_regA,and_val,or_val);
 }
 
-template bool mag_set_data_rate<3,4>();
-template bool mag_set_data_rate<3,2>();
-template bool mag_set_data_rate<3,1>();
-template bool mag_set_data_rate<15,2>();
-template bool mag_set_data_rate<15,1>();
-template bool mag_set_data_rate<30,1>();
-template bool mag_set_data_rate<75,1>();
-
-namespace {
-    
-    bool mag_set_positive_bias()
-    {
-        uint8_t constexpr and_val = static_cast<uint8_t>(~(0b11U ));
-        uint8_t constexpr or_val = 0b01U;
-        return mag_modify_reg(cfg_regA,and_val,or_val);
-    }
-    
-    bool mag_set_negative_bias()
-    {
-        uint8_t constexpr and_val = static_cast<uint8_t>(~(0b11U ));
-        uint8_t constexpr or_val = 0b10U;
-        return mag_modify_reg(cfg_regA,and_val,or_val);
-    }
-    
-    bool mag_clear_bias()
-    {
-        uint8_t constexpr and_val = static_cast<uint8_t>(~(0b11U ));
-        uint8_t constexpr or_val = 0b00U;
-        return mag_modify_reg(cfg_regA,and_val,or_val);
-    }
-    
-    QUAN_QUANTITY_LITERAL(magnetic_flux_density,gauss);
-    QUAN_QUANTITY_LITERAL(magnetic_flux_density,milli_gauss);
-    QUAN_QUANTITY_LITERAL(magnetic_flux_density,uT);
-    
-    // per lsb defualt resolution
-    quan::magnetic_flux_density::uT mag_resolution = 0.92_milli_gauss;
-    // range before saturation
-    quan::magnetic_flux_density::uT mag_range = 1.3_gauss;
+bool hmc5883L::set_negative_bias()const
+{
+    uint8_t constexpr and_val = static_cast<uint8_t>(~(0b11U ));
+    uint8_t constexpr or_val = 0b10U;
+    return this->modify_reg(cfg_regA,and_val,or_val);
 }
 
-// set +- range
-// sets the nearest greater equal +-range to abs(range_in)
-bool mag_set_range(quan::magnetic_flux_density::uT const & range_in)
+bool hmc5883L::mag_clear_bias()const
+{
+    uint8_t constexpr and_val = static_cast<uint8_t>(~(0b11U ));
+    uint8_t constexpr or_val = 0b00U;
+    return this->modify_reg(cfg_regA,and_val,or_val);
+}
+
+bool hmc5883L::set_range(quan::magnetic_flux_density::uT const & range_in)
 {
     uint8_t or_value = 0;
     auto const range = abs(range_in);
@@ -289,41 +216,41 @@
     }
     uint8_t constexpr and_val = static_cast<uint8_t>(~(0b111U << 5U));
     std::cout << "mag range set to : +- " <<  mag_range <<'\n';
-    return mag_modify_reg(cfg_regB,and_val,or_value);
+    return this->modify_reg(cfg_regB,and_val,or_value);
 
 }
 
-quan::magnetic_flux_density::uT 
-mag_get_range()
+quan::magnetic_flux_density::uT
+hmc5883L::get_flux_density_range()const
 {
     return mag_range;
 }
 
-bool mag_set_continuous_measurement_mode()
+bool hmc5883L::set_continuous_measurement_mode()const
 {
     uint8_t constexpr and_val = static_cast<uint8_t>(~(0b11U ));
     uint8_t constexpr or_val = 0b00U;
-    return mag_modify_reg(mode_reg,and_val,or_val);
+    return this->modify_reg(mode_reg,and_val,or_val);
 }
 
-bool mag_set_single_measurement_mode()
+bool hmc5883L::start_measurement()const
 {
     uint8_t constexpr and_val = static_cast<uint8_t>(~(0b11U ));
     uint8_t constexpr or_val = 0b01U;
-    return mag_modify_reg(mode_reg,and_val,or_val);
+    return this->modify_reg(mode_reg,and_val,or_val);
 }
 
-bool mag_set_idle_mode()
+bool hmc5883L::set_idle_mode()const
 {
     uint8_t constexpr and_val = static_cast<uint8_t>(~(0b11U ));
     uint8_t constexpr or_val = 0b10U;
-    return mag_modify_reg(mode_reg,and_val,or_val);
+    return this->modify_reg(mode_reg,and_val,or_val);
 }
 
-bool mag_data_ready()
+bool hmc5883L::data_ready()const
 {
     uint8_t result = 0;
-    if ( mag_get_reg(status_reg, result)) {
+    if ( this->get_reg(status_reg, result)) {
         return (result & 0b1U) != 0U;
     } else {
         std::cout << "mag data ready failed\n";
@@ -331,10 +258,10 @@
     }
 }
 
-bool mag_data_locked()
+bool hmc5883L::data_locked()const
 {
     uint8_t result = 0;
-    if ( mag_get_reg(status_reg, result)) {
+    if ( this->get_reg(status_reg, result)) {
         return (result & 0b10U) != 0U;
     } else {
         std::cout << "mag data locked failed\n";
@@ -343,21 +270,29 @@
 }
 
 // assume mag_data_ready has returned true before call
-bool mag_read(quan::three_d::vect<quan::magnetic_flux_density::uT> & v)
+bool hmc5883L::read(quan::three_d::vect<quan::magnetic_flux_density::uT> & v)const
 {
-    if( mag_set_reg_idx(dout_reg)) {
+    if( this->set_reg_idx(dout_reg)) {
         char arr[7];
-        i2c.lock();
-        bool success= i2c.read(i2c_addr,arr,7) == 0;
-        i2c.unlock();
+        bool success= this->i2c_read(arr,7) == 0;
         if(success) {
             // TODO check status reg arr[6]
-            // if
             quan::three_d::vect<int16_t> temp;
             temp.x = static_cast<int16_t>(arr[1]) + ( static_cast<int16_t>(arr[0]) << 8U);
             temp.y = static_cast<int16_t>(arr[5]) + ( static_cast<int16_t>(arr[4]) << 8U);
             temp.z = static_cast<int16_t>(arr[3]) + ( static_cast<int16_t>(arr[2]) << 8U);
-            v = temp * mag_resolution;
+            
+            quan::three_d::vect<quan::magnetic_flux_density::uT>  result
+              = temp * mag_resolution;
+              
+            result.x *= this->mag_gain.x;
+            result.y *= this->mag_gain.y;
+            result.z *= this->mag_gain.z;
+            
+            result -= this->mag_offset;
+            
+            v = result;
+            
             return true;
         } else {
             std::cout << "mag_read failed\n";
@@ -368,45 +303,32 @@
     }
 }
 
-bool mag_do_single_measurement(quan::three_d::vect<quan::magnetic_flux_density::uT>& result)
+bool hmc5883L::make_measurement(quan::three_d::vect<quan::magnetic_flux_density::uT>& result)const
 {
-    if ( ! mag_set_single_measurement_mode()) {
+    if ( ! this->start_measurement()) {
         return false;
     }
 
-    while (! mag_data_ready()) {
+    while (! this->data_ready()) {
         ThisThread::sleep_for(5U);
     }
-    return mag_read(result);
+    return this->read(result);
+}
+
+bool hmc5883L::set_gain( quan::three_d::vect<double> const & gain)
+{
+    this->mag_gain = gain;
+    return true;
 }
 
-namespace {
-        //TODO raname to mag_self_test
-    bool mag_get_offsets(quan::three_d::vect<quan::magnetic_flux_density::uT> & result)
-    {
-        // set single measurement mode
-    
-        // to prevent saturation
-        mag_set_range(5.7_gauss);
-        quan::three_d::vect<quan::magnetic_flux_density::uT> mSet;
-        // throw away first
-        mag_do_single_measurement(mSet);
-    
-        mag_set_positive_bias();
-        mag_do_single_measurement(mSet);
-        std::cout << "mSet = " << mSet <<'\n';
-    
-        mag_set_negative_bias();
-        quan::three_d::vect<quan::magnetic_flux_density::uT> mReset;
-        mag_do_single_measurement(mReset);
-        mag_clear_bias();
-    
-        std::cout << "mReset = " << mReset <<'\n';
-    
-        result = (mSet - mReset )/2;
-    
-        std::cout << "result = " << result << '\n';
-    
-        return true;
-    }
-} // namepsace
\ No newline at end of file
+bool hmc5883L::set_offset(
+    quan::three_d::vect<
+        quan::magnetic_flux_density::uT
+    > const & offset
+)
+{
+    this->mag_offset = offset;
+    return true;
+}
+
+