Diff: main.cpp

Revision:

2:9ffb2f18756b

Parent:

1:e11ab941748b

Child:

3:2834be4e10ef

--- a/main.cpp	Sun Mar 22 09:46:50 2020 +0000
+++ b/main.cpp	Tue Mar 24 15:21:34 2020 +0000
@@ -1,426 +1,48 @@
 
 #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 "hmc5883.h"
 #include <quan/max.hpp>
 #include <quan/min.hpp>
 
-namespace
-{
-DigitalOut led2(LED2);
-} // 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;
+namespace {
+    
+    DigitalOut led2(LED2);
     
-    // 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 (;;) {
-        led2 = 1;
-        std::cout << str << " " << count++ << '\n';
-        ThisThread::sleep_for(200U);
-        led2 = 0;
-        ThisThread::sleep_for(800U);
-    }
-}
-
-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
+    QUAN_QUANTITY_LITERAL(magnetic_flux_density,gauss);
+    QUAN_QUANTITY_LITERAL(magnetic_flux_density,milli_gauss);
+    QUAN_QUANTITY_LITERAL(magnetic_flux_density,uT);
 
-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;
+    // terminal loop, printing message periodically
+    void loop_forever(std::string const & str)
+    {
+        // stop but print error dynamically
+        int count = 0;
+        for (;;) {
+            led2 = 1;
+            std::cout << str << " " << count++ << '\n';
+            ThisThread::sleep_for(200U);
+            led2 = 0;
+            ThisThread::sleep_for(800U);
+        }
     }
-    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;
+    bool success = mag_detected();
+    if ( success ) {
         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>() &&
@@ -428,10 +50,11 @@
         // 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");
     }
+    
+    std::cout << " mag range set to " << mag_get_range() << '\n';
 
     // calculate the offsets dynamically by averaging 
     // the min and max over time