First draft HMC5883 magnetometer sensor using physical quantities, outputting via serial port using std::cout on mbed os 5
Diff: hmc5883.cpp
- Revision:
- 8:c5dc1ce10722
- Parent:
- 7:5d14da0b4c95
- Child:
- 9:87a7169b4d5c
--- a/hmc5883.cpp Thu Mar 26 12:27:43 2020 +0000 +++ b/hmc5883.cpp Thu Mar 26 21:13:44 2020 +0000 @@ -1,150 +1,108 @@ #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> -#include "resourceManager.h" -/* +namespace +{ - 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)()); - }; -*/ +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} +{} -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 ); - - I2C& i2c = resource::i2c1; - - 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) { +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_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"; + std::cout << "mag_get_reg read 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; - bool const success = i2c.read(i2c_addr,&temp_result,1) == 0; - if (success) { - result = temp_result; - return true; - } else { - std::cout << "mag_get_reg read failed\n"; - return false; - } - } else { - 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; - } +} + +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; } - -} // namespace +} -// probe for the HMC5883 on I2C -// return true if found -bool mag_detected() +bool hmc5883L::detected(bool verbose)const { - if ( mag_set_reg_idx(id_regA) ) { + if ( this->set_reg_idx(id_regA) ) { char id_input[4]; - bool success = i2c.read(i2c_addr,id_input,3) == 0; + 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 { - std::cout << "id mag read failed\n"; + if( verbose) { + std::cout << "id mag read failed\n"; + } return false; } } else { @@ -153,7 +111,7 @@ } // only 1,2,4,8 available -bool mag_set_samples_average(int n_samples) +bool hmc5883L::set_samples_average(int n_samples)const { uint8_t or_val = 0; switch (n_samples) { @@ -174,78 +132,47 @@ return false; } uint8_t constexpr and_val = ~(0b11 << 5U); - return mag_modify_reg(cfg_regA,and_val,or_val); + return this->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() +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 mag_modify_reg(cfg_regA,and_val,or_val); + 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); } -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; +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); } -// set +- range -// sets the nearest greater equal +-range to abs(range_in) -bool mag_set_range(quan::magnetic_flux_density::uT const & range_in) +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); @@ -289,41 +216,41 @@ } 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); + return this->modify_reg(cfg_regB,and_val,or_value); } -quan::magnetic_flux_density::uT -mag_get_range() +quan::magnetic_flux_density::uT +hmc5883L::get_flux_density_range()const { return mag_range; } -bool mag_set_continuous_measurement_mode() +bool hmc5883L::set_continuous_measurement_mode()const { 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); + return this->modify_reg(mode_reg,and_val,or_val); } -bool mag_set_single_measurement_mode() +bool hmc5883L::start_measurement()const { 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); + return this->modify_reg(mode_reg,and_val,or_val); } -bool mag_set_idle_mode() +bool hmc5883L::set_idle_mode()const { 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); + return this->modify_reg(mode_reg,and_val,or_val); } -bool mag_data_ready() +bool hmc5883L::data_ready()const { uint8_t result = 0; - if ( mag_get_reg(status_reg, result)) { + if ( this->get_reg(status_reg, result)) { return (result & 0b1U) != 0U; } else { std::cout << "mag data ready failed\n"; @@ -331,10 +258,10 @@ } } -bool mag_data_locked() +bool hmc5883L::data_locked()const { uint8_t result = 0; - if ( mag_get_reg(status_reg, result)) { + if ( this->get_reg(status_reg, result)) { return (result & 0b10U) != 0U; } else { std::cout << "mag data locked failed\n"; @@ -343,21 +270,29 @@ } // assume mag_data_ready has returned true before call -bool mag_read(quan::three_d::vect<quan::magnetic_flux_density::uT> & v) +bool hmc5883L::read(quan::three_d::vect<quan::magnetic_flux_density::uT> & v)const { - if( mag_set_reg_idx(dout_reg)) { + if( this->set_reg_idx(dout_reg)) { char arr[7]; - i2c.lock(); - bool success= i2c.read(i2c_addr,arr,7) == 0; - i2c.unlock(); + bool success= this->i2c_read(arr,7) == 0; 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; + + 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"; @@ -368,45 +303,32 @@ } } -bool mag_do_single_measurement(quan::three_d::vect<quan::magnetic_flux_density::uT>& result) +bool hmc5883L::make_measurement(quan::three_d::vect<quan::magnetic_flux_density::uT>& result)const { - if ( ! mag_set_single_measurement_mode()) { + if ( ! this->start_measurement()) { return false; } - while (! mag_data_ready()) { + while (! this->data_ready()) { ThisThread::sleep_for(5U); } - return mag_read(result); + return this->read(result); +} + +bool hmc5883L::set_gain( quan::three_d::vect<double> const & gain) +{ + this->mag_gain = gain; + return true; } -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 \ No newline at end of file +bool hmc5883L::set_offset( + quan::three_d::vect< + quan::magnetic_flux_density::uT + > const & offset +) +{ + this->mag_offset = offset; + return true; +} + +