First draft HMC5883 magnetometer sensor using physical quantities, outputting via serial port using std::cout on mbed os 5
hmc5883.cpp
- Committer:
- skyscraper
- Date:
- 2020-03-26
- Revision:
- 8:c5dc1ce10722
- Parent:
- 7:5d14da0b4c95
- Child:
- 9:87a7169b4d5c
File content as of revision 8:c5dc1ce10722:
#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; } } // assume mag_data_ready has 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; }