File content as of revision 3:2834be4e10ef:

#include "mbed.h"
#include <iostream>
#include <quan/out/magnetic_flux_density.hpp>
#include <quan/three_d/out/vect.hpp>

/*

   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)());
   };
*/

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);
        i2c.lock();
        bool const result = i2c.write(i2c_addr,&idx,1) == 0;
        i2c.unlock();
        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};
        i2c.lock();
        bool const result = i2c.write(i2c_addr,ar,2) == 0;
        i2c.unlock();
        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 temp_result = 0;
            i2c.lock();
            bool const success = i2c.read(i2c_addr,&temp_result,1) == 0;
            i2c.unlock();
            if (success) {
                result = temp_result;
                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;
        }
    }

}  // namespace

// probe for the HMC5883 on I2C
// return true if found
bool mag_detected()
{
    if ( mag_set_reg_idx(id_regA) ) {
        char id_input[4];
        i2c.lock();
        bool success = i2c.read(i2c_addr,id_input,3) == 0;
        i2c.unlock();
        if(success) {
            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;
    }
}

// only 1,2,4,8 available
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 {
    
    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()
{
    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);
}

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;
}

// 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);

}

quan::magnetic_flux_density::uT 
mag_get_range()
{
    return mag_range;
}

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];
        i2c.lock();
        bool success= i2c.read(i2c_addr,arr,7) == 0;
        i2c.unlock();
        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;
            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);
}

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