File content as of revision 11:de7c9ae7ef65:

#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>

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

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

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_get_reg read failed\n";
            return false;
        }
    } 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;
    }
}

bool hmc5883L::detected(bool verbose)const
{
    if ( this->set_reg_idx(id_regA) ) {
        char id_input[4];
        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 {
            if( verbose) {
                std::cout << "id mag read failed\n";
            }
            return false;
        }
    } else {
        return false;
    }
}

// only 1,2,4,8 available
bool hmc5883L::set_samples_average(int n_samples)const
{
    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 this->modify_reg(cfg_regA,and_val,or_val);
}

template <int N, int D>
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 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);
}

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

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

    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 this->modify_reg(cfg_regB,and_val,or_value);

}

quan::magnetic_flux_density::uT
hmc5883L::get_flux_density_range()const
{
    return mag_range;
}

bool hmc5883L::set_continuous_measurement_mode()const
{
    uint8_t constexpr and_val = static_cast<uint8_t>(~(0b11U ));
    uint8_t constexpr or_val = 0b00U;
    return this->modify_reg(mode_reg,and_val,or_val);
}

bool hmc5883L::start_measurement()const
{
    uint8_t constexpr and_val = static_cast<uint8_t>(~(0b11U ));
    uint8_t constexpr or_val = 0b01U;
    return this->modify_reg(mode_reg,and_val,or_val);
}

bool hmc5883L::set_idle_mode()const
{
    uint8_t constexpr and_val = static_cast<uint8_t>(~(0b11U ));
    uint8_t constexpr or_val = 0b10U;
    return this->modify_reg(mode_reg,and_val,or_val);
}

bool hmc5883L::data_ready()const
{
    uint8_t result = 0;
    if ( this->get_reg(status_reg, result)) {
        return (result & 0b1U) != 0U;
    } else {
        std::cout << "mag data ready failed\n";
        return false;
    }
}

bool hmc5883L::data_locked()const
{
    uint8_t result = 0;
    if ( this->get_reg(status_reg, result)) {
        return (result & 0b10U) != 0U;
    } else {
        std::cout << "mag data locked failed\n";
        return false;
    }
}

// call data_ready and returned true before call
bool hmc5883L::read(quan::three_d::vect<quan::magnetic_flux_density::uT> & v)const
{
    if( this->set_reg_idx(dout_reg)) {
        char arr[7];
        bool success= this->i2c_read(arr,7) == 0;
        if(success) {
            // TODO check status reg arr[6]
            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);
            
            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";
            return false;
        }
    } else {
        return false;
    }
}

bool hmc5883L::make_measurement(quan::three_d::vect<quan::magnetic_flux_density::uT>& result)const
{
    if ( ! this->start_measurement()) {
        return false;
    }

    while (! this->data_ready()) {
        ThisThread::sleep_for(5U);
    }
    return this->read(result);
}

bool hmc5883L::set_gain( quan::three_d::vect<double> const & gain)
{
    this->mag_gain = gain;
    return true;
}

bool hmc5883L::set_offset(
    quan::three_d::vect<
        quan::magnetic_flux_density::uT
    > const & offset
)
{
    this->mag_offset = offset;
    return true;
}