Diff: main.cpp

Revision:

0:37dbfb036586

Child:

1:e11ab941748b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Mar 21 23:18:28 2020 +0000
@@ -0,0 +1,464 @@
+
+#include "mbed.h"
+
+#include <string>
+#include <array>
+#include <iostream>
+#include <ratio>
+#include <type_traits>
+
+#include <quan/out/magnetic_flux_density.hpp>
+#include <quan/three_d/out/vect.hpp>
+#include <quan/max.hpp>
+#include <quan/min.hpp>
+
+namespace
+{
+DigitalOut led1(LED1);
+} // namespace
+
+/*
+ paradigm
+   input stream
+synchronous/asynchronous
+Ideally it updates in background
+You can be notified of update in callback
+or just read
+*/
+template<typename SensorUnit>
+struct Sensor {
+    typedef SensorUnit sensor_unit_t;
+    
+    // TODO status_t get_status()const;
+    virtual bool open() = 0;
+    virtual bool read(sensor_unit_t const & current_value) const = 0;
+    virtual bool close() = 0;
+};
+
+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 );
+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) {
+        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";
+        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 result1 = 0;
+        if (i2c.read(i2c_addr,&result1,1) == 0) {
+            result = result1;
+            return true;
+        } else {
+            std::cout << "mag_get_reg read failed\n";
+            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;
+    }
+}
+
+// probe for the HMC5883 on I2C
+// return true if found
+bool mag_detected()
+{
+    if ( mag_set_reg_idx(id_regA) ) {
+        char id_input[4];
+        if(i2c.read(i2c_addr,id_input,3) == 0) {
+            id_input[3] = '\0';
+            bool const is_hmc = (strcmp(id_input,"H43") == 0);
+            if (is_hmc) {
+                return true;
+            } else {
+                std::cout << "hmc5883 ID string didnt match\n";
+                return false;
+            }
+        } else {
+            std::cout << "id mag read failed\n";
+            return false;
+        }
+    } else {
+        return false;
+    }
+}
+
+// terminal loop, printing message periodically
+void loop_forever(std::string const & str)
+{
+    // stop but print error dynamically
+    int count = 0;
+    for (;;) {
+        led1 = !led1;
+        std::cout << str << " " << count++ << '\n';
+        ThisThread::sleep_for(1000U);
+    }
+}
+
+bool mag_set_samples_average(int n_samples)
+{
+    uint8_t or_val = 0;
+    switch (n_samples) {
+        case 1 :
+            or_val = 0b00U << 5U;
+            break;
+        case 2 :
+            or_val = 0b01U << 5U;
+            break;
+        case 4 :
+            or_val = 0b10U << 5U;
+            break;
+        case 8 :
+            or_val = 0b11U << 5U;
+            break;
+        default:
+            std::cout << "mag_set_samples_average : invalid n_samples (" << n_samples << ")\n";
+            return false;
+    }
+    uint8_t constexpr and_val = ~(0b11 << 5U);
+    return mag_modify_reg(cfg_regA,and_val,or_val);
+}
+
+/*
+data rate 0.75, 1.5, 3 ,7.5, 15 (Default) , 30, 75
+*/
+
+namespace detail
+{
+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)> {};
+} // detail
+
+template <int N, int D=1>
+inline bool mag_set_data_rate()
+{
+    uint8_t constexpr and_val = static_cast<uint8_t>(~(0b111U << 2U));
+    uint8_t constexpr or_val = detail::mag_data_rate_id<N,D>::value;
+    return mag_modify_reg(cfg_regA,and_val,or_val);
+}
+
+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;
+// set +- range
+// sets the nearest greater equal +-range to abs(range_in)
+bool mag_set_range(quan::magnetic_flux_density::uT const & range_in)
+{
+    uint8_t or_value = 0;
+    auto const range = abs(range_in);
+
+    if ( range <= 0.88_gauss) {
+        or_value = 0b001U << 5U ;
+        mag_range = 0.88_gauss;
+        mag_resolution = 0.73_milli_gauss;
+    } else if (range <= 1.3_gauss) {
+        or_value = 0b001U << 5U ;
+        mag_range = 1.3_gauss;
+        mag_resolution = 0.92_milli_gauss;
+    } else if (range <= 1.9_gauss) {
+        or_value = 0b010U << 5U ;
+        mag_range = 1.9_gauss;
+        mag_resolution = 1.22_milli_gauss;
+    } else if (range <= 2.5_gauss) {
+        or_value = 0b011U << 5U ;
+        mag_range = 2.5_gauss;
+        mag_resolution = 1.52_milli_gauss;
+    } else if (range <= 4.0_gauss) {
+        or_value = 0b100U << 5U ;
+        mag_range = 4.0_gauss;
+        mag_resolution = 2.27_milli_gauss;
+    } else if (range <= 4.7_gauss) {
+        or_value = 0b101U << 5U ;
+        mag_range = 4.7_gauss;
+        mag_resolution = 2.56_milli_gauss;
+    } else if (range <=5.6_gauss) {
+        or_value = 0b110U << 5U ;
+        mag_range = 5.6_gauss;
+        mag_resolution = 3.03_milli_gauss;
+    } else if ( range <= 8.1_gauss) {
+        or_value = 0b111U << 5U ;
+        mag_range = 8.1_gauss;
+        mag_resolution = 4.35_milli_gauss;
+    } else {
+        quan::magnetic_flux_density::uT constexpr max_range = 8.1_gauss;
+        std::cout << "range too big: max +- range = " << max_range <<"\n";
+        return false;
+    }
+    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);
+
+}
+
+bool mag_set_continuous_measurement_mode()
+{
+    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);
+}
+
+bool mag_set_single_measurement_mode()
+{
+    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);
+}
+
+bool mag_set_idle_mode()
+{
+    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);
+}
+
+bool mag_data_ready()
+{
+    uint8_t result = 0;
+    if ( mag_get_reg(status_reg, result)) {
+        return (result & 0b1U) != 0U;
+    } else {
+        std::cout << "mag data ready failed\n";
+        return false;
+    }
+}
+
+bool mag_data_locked()
+{
+    uint8_t result = 0;
+    if ( mag_get_reg(status_reg, result)) {
+        return (result & 0b10U) != 0U;
+    } else {
+        std::cout << "mag data locked failed\n";
+        return false;
+    }
+}
+
+// assume mag_data_ready has returned true before call
+bool mag_read(quan::three_d::vect<quan::magnetic_flux_density::uT> & v)
+{
+    if( mag_set_reg_idx(dout_reg)) {
+        char arr[7];
+        if(i2c.read(i2c_addr,arr,7) == 0) {
+            // 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;
+            return true;
+        } else {
+            std::cout << "mag_read failed\n";
+            return false;
+        }
+    } else {
+        return false;
+    }
+}
+
+bool mag_do_single_measurement(quan::three_d::vect<quan::magnetic_flux_density::uT>& result)
+{
+    if ( ! mag_set_single_measurement_mode()) {
+        return false;
+    }
+
+    while (! mag_data_ready()) {
+        ThisThread::sleep_for(5U);
+    }
+    return mag_read(result);
+}
+
+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;
+}
+
+}// namespace
+
+int main()
+{
+
+    std::cout << "HMC5883 test\n";
+
+    //wait for mag to init
+    ThisThread::sleep_for(500U);
+
+    bool success = false;
+    if ( mag_detected()) {
+        success = true;
+        std::cout << "Detected a HMC5883\n";
+    } else {
+        loop_forever("Failed to detect HMC5883");
+    }
+    /*
+
+    for ( int i = 0; i < 5; ++i){
+        mag_get_offsets(offsets);
+        ThisThread::sleep_for(100U);
+    }
+    */
+    // N.b after offsets removed mag was reading around 33.6 uT, so not bad!
+    constexpr auto earth_magnetic_field_flux_density = 31.869_uT;
+
+    success =
+        mag_set_samples_average(8) &&
+        mag_set_data_rate<3,4>() &&
+        // N.B if offsets are large then may need to set larger range
+        // prob need to cycle through looking for best range
+        // so this may not work
+        mag_set_range( earth_magnetic_field_flux_density * 2U);
+
+    if ( !success) {
+        loop_forever("HMC5883 setup failed");
+    }
+
+    // calculate the offsets dynamically by averaging 
+    // the min and max over time
+    quan::three_d::vect<quan::magnetic_flux_density::uT> voffsets;
+    quan::three_d::vect<quan::magnetic_flux_density::uT> vmax;
+    quan::three_d::vect<quan::magnetic_flux_density::uT> vmin;
+
+    for (;;) {
+
+        quan::three_d::vect<quan::magnetic_flux_density::uT> values;
+        if(mag_do_single_measurement(values)) {
+
+            vmax.x = quan::max(vmax.x,values.x);
+            vmax.y = quan::max(vmax.y,values.y);
+            vmax.z = quan::max(vmax.z,values.z);
+
+            vmin.x = quan::min(vmin.x,values.x);
+            vmin.y = quan::min(vmin.y,values.y);
+            vmin.z = quan::min(vmin.z,values.z);
+
+            voffsets = (vmin + vmax)/2.f;
+
+            values -= voffsets;
+
+            std::cout << "val = " << values << '\n';
+            std::cout << "off = " << voffsets << "\n\n";
+        } else {
+            std::cout << "mag read failed\n";
+        }
+        ThisThread::sleep_for(10U);
+    }
+}
\ No newline at end of file