hmc5883.cpp

00001 
00002 
00003 #include "mbed.h"
00004 #include "hmc5883.h"
00005 #include "resourceManager.h"
00006 
00007 #include <iostream>
00008 #include <quan/out/magnetic_flux_density.hpp>
00009 #include <quan/three_d/out/vect.hpp>
00010 
00011 namespace
00012 {
00013 
00014 template <int N, int D=1> struct mag_data_rate_id;
00015 // values are mag settings for each data rate
00016 template <> struct mag_data_rate_id<3,4> : std::integral_constant<uint8_t,(0b000U << 2U)> {};
00017 template <> struct mag_data_rate_id<3,2> : std::integral_constant<uint8_t,(0b001U << 2U)> {};
00018 template <> struct mag_data_rate_id<3> : std::integral_constant<uint8_t,(0b010U << 2U)> {};
00019 template <> struct mag_data_rate_id<15,2> : std::integral_constant<uint8_t,(0b011U << 2U)> {};
00020 template <> struct mag_data_rate_id<15> : std::integral_constant<uint8_t,(0b100U << 2U)> {};
00021 template <> struct mag_data_rate_id<30> : std::integral_constant<uint8_t,(0b101U << 2U)> {};
00022 template <> struct mag_data_rate_id<75> : std::integral_constant<uint8_t,(0b110U << 2U)> {};
00023 
00024 }//namespace
00025 
00026 hmc5883L::hmc5883L(I2C& i2cIn,uint8_t addressIn)
00027     :I2CBusDevice{i2cIn,addressIn}
00028     ,mag_resolution{0.92_milli_gauss}
00029     ,mag_range{1.3_gauss}
00030     ,mag_gain{1.0,1.0,1.0}
00031     ,mag_offset{0.0_uT,0.0_uT,0.0_uT}
00032 {}
00033 
00034 bool hmc5883L::set_reg_idx(uint8_t idx_in)const
00035 {
00036     char const idx = static_cast<char>(idx_in);
00037     bool const result = this->i2c_write(&idx,1) == 0;
00038     if(result) {
00039         return true;
00040     } else {
00041         std::cout << "mag_set_reg_idx failed\n";
00042         return false;
00043     }
00044 }
00045 
00046 bool hmc5883L::write_reg(uint8_t idx, uint8_t val)const
00047 {
00048     char const ar[2] = {idx,val};
00049     bool const result = this->i2c_write(ar,2) == 0;
00050     if(result) {
00051         return true;
00052     } else {
00053         std::cout << " mag_write_reg failed\n";
00054         return false;
00055     }
00056 }
00057 
00058 bool hmc5883L::get_reg(uint8_t idx_in, uint8_t& result)const
00059 {
00060     if ( this->set_reg_idx(idx_in)) {
00061         char temp_result = 0;
00062         bool const success = this->i2c_read(&temp_result,1,false) == 0;
00063         if (success) {
00064             result = temp_result;
00065             return true;
00066         } else {
00067             std::cout << "mag_get_reg read failed\n";
00068             return false;
00069         }
00070     } else {
00071         return false;
00072     }
00073 }
00074 
00075 bool hmc5883L::modify_reg(uint8_t idx, uint8_t and_val, uint8_t or_val)const
00076 {
00077     uint8_t cur_val = 0;
00078     if(this->get_reg(idx,cur_val)) {
00079         uint8_t const new_val = (cur_val & and_val ) | or_val;
00080         return this->write_reg(idx,new_val);
00081     } else {
00082         return false;
00083     }
00084 }
00085 
00086 bool hmc5883L::detected(bool verbose)const
00087 {
00088     if ( this->set_reg_idx(id_regA) ) {
00089         char id_input[4];
00090         bool success = this->i2c_read(id_input,3) == 0;
00091         if(success) {
00092             id_input[3] = '\0';
00093             bool const is_hmc = (strcmp(id_input,"H43") == 0);
00094             if (is_hmc) {
00095                 return true;
00096             } else {
00097                 if( verbose) {
00098                 std::cout << "hmc5883 ID string didnt match\n";
00099                 }
00100                 return false;
00101             }
00102         } else {
00103             if( verbose) {
00104                 std::cout << "id mag read failed\n";
00105             }
00106             return false;
00107         }
00108     } else {
00109         return false;
00110     }
00111 }
00112 
00113 // only 1,2,4,8 available
00114 bool hmc5883L::set_samples_average(int n_samples)const
00115 {
00116     uint8_t or_val = 0;
00117     switch (n_samples) {
00118         case 1 :
00119             or_val = 0b00U << 5U;
00120             break;
00121         case 2 :
00122             or_val = 0b01U << 5U;
00123             break;
00124         case 4 :
00125             or_val = 0b10U << 5U;
00126             break;
00127         case 8 :
00128             or_val = 0b11U << 5U;
00129             break;
00130         default:
00131             std::cout << "mag_set_samples_average : invalid n_samples (" << n_samples << ")\n";
00132             return false;
00133     }
00134     uint8_t constexpr and_val = ~(0b11 << 5U);
00135     return this->modify_reg(cfg_regA,and_val,or_val);
00136 }
00137 
00138 template <int N, int D>
00139 bool hmc5883L::set_data_rate()const
00140 {
00141     uint8_t constexpr and_val = static_cast<uint8_t>(~(0b111U << 2U));
00142     uint8_t constexpr or_val = mag_data_rate_id<N,D>::value;
00143     return this->modify_reg(cfg_regA,and_val,or_val);
00144 }
00145 //data rate 0.75, 1.5, 3 ,7.5, 15 (Default) , 30, 75
00146 template bool hmc5883L::set_data_rate<3,4>() const;
00147 template bool hmc5883L::set_data_rate<3,2>() const;
00148 template bool hmc5883L::set_data_rate<3>() const;
00149 template bool hmc5883L::set_data_rate<15,2>() const;
00150 template bool hmc5883L::set_data_rate<15>() const;
00151 template bool hmc5883L::set_data_rate<30>() const;
00152 template bool hmc5883L::set_data_rate<75>() const;
00153 
00154 bool hmc5883L::set_positive_bias()const
00155 {
00156     uint8_t constexpr and_val = static_cast<uint8_t>(~(0b11U ));
00157     uint8_t constexpr or_val = 0b01U;
00158     return this->modify_reg(cfg_regA,and_val,or_val);
00159 }
00160 
00161 bool hmc5883L::set_negative_bias()const
00162 {
00163     uint8_t constexpr and_val = static_cast<uint8_t>(~(0b11U ));
00164     uint8_t constexpr or_val = 0b10U;
00165     return this->modify_reg(cfg_regA,and_val,or_val);
00166 }
00167 
00168 bool hmc5883L::mag_clear_bias()const
00169 {
00170     uint8_t constexpr and_val = static_cast<uint8_t>(~(0b11U ));
00171     uint8_t constexpr or_val = 0b00U;
00172     return this->modify_reg(cfg_regA,and_val,or_val);
00173 }
00174 
00175 bool hmc5883L::set_range(quan::magnetic_flux_density::uT const & range_in)
00176 {
00177     uint8_t or_value = 0;
00178     auto const range = abs(range_in);
00179 
00180     if ( range <= 0.88_gauss) {
00181         or_value = 0b001U << 5U ;
00182         mag_range = 0.88_gauss;
00183         mag_resolution = 0.73_milli_gauss;
00184     } else if (range <= 1.3_gauss) {
00185         or_value = 0b001U << 5U ;
00186         mag_range = 1.3_gauss;
00187         mag_resolution = 0.92_milli_gauss;
00188     } else if (range <= 1.9_gauss) {
00189         or_value = 0b010U << 5U ;
00190         mag_range = 1.9_gauss;
00191         mag_resolution = 1.22_milli_gauss;
00192     } else if (range <= 2.5_gauss) {
00193         or_value = 0b011U << 5U ;
00194         mag_range = 2.5_gauss;
00195         mag_resolution = 1.52_milli_gauss;
00196     } else if (range <= 4.0_gauss) {
00197         or_value = 0b100U << 5U ;
00198         mag_range = 4.0_gauss;
00199         mag_resolution = 2.27_milli_gauss;
00200     } else if (range <= 4.7_gauss) {
00201         or_value = 0b101U << 5U ;
00202         mag_range = 4.7_gauss;
00203         mag_resolution = 2.56_milli_gauss;
00204     } else if (range <=5.6_gauss) {
00205         or_value = 0b110U << 5U ;
00206         mag_range = 5.6_gauss;
00207         mag_resolution = 3.03_milli_gauss;
00208     } else if ( range <= 8.1_gauss) {
00209         or_value = 0b111U << 5U ;
00210         mag_range = 8.1_gauss;
00211         mag_resolution = 4.35_milli_gauss;
00212     } else {
00213         quan::magnetic_flux_density::uT constexpr max_range = 8.1_gauss;
00214         std::cout << "range too big: max +- range = " << max_range <<"\n";
00215         return false;
00216     }
00217     uint8_t constexpr and_val = static_cast<uint8_t>(~(0b111U << 5U));
00218     std::cout << "mag range set to : +- " <<  mag_range <<'\n';
00219     return this->modify_reg(cfg_regB,and_val,or_value);
00220 
00221 }
00222 
00223 quan::magnetic_flux_density::uT
00224 hmc5883L::get_flux_density_range()const
00225 {
00226     return mag_range;
00227 }
00228 
00229 bool hmc5883L::set_continuous_measurement_mode()const
00230 {
00231     uint8_t constexpr and_val = static_cast<uint8_t>(~(0b11U ));
00232     uint8_t constexpr or_val = 0b00U;
00233     return this->modify_reg(mode_reg,and_val,or_val);
00234 }
00235 
00236 bool hmc5883L::start_measurement()const
00237 {
00238     uint8_t constexpr and_val = static_cast<uint8_t>(~(0b11U ));
00239     uint8_t constexpr or_val = 0b01U;
00240     return this->modify_reg(mode_reg,and_val,or_val);
00241 }
00242 
00243 bool hmc5883L::set_idle_mode()const
00244 {
00245     uint8_t constexpr and_val = static_cast<uint8_t>(~(0b11U ));
00246     uint8_t constexpr or_val = 0b10U;
00247     return this->modify_reg(mode_reg,and_val,or_val);
00248 }
00249 
00250 bool hmc5883L::data_ready()const
00251 {
00252     uint8_t result = 0;
00253     if ( this->get_reg(status_reg, result)) {
00254         return (result & 0b1U) != 0U;
00255     } else {
00256         std::cout << "mag data ready failed\n";
00257         return false;
00258     }
00259 }
00260 
00261 bool hmc5883L::data_locked()const
00262 {
00263     uint8_t result = 0;
00264     if ( this->get_reg(status_reg, result)) {
00265         return (result & 0b10U) != 0U;
00266     } else {
00267         std::cout << "mag data locked failed\n";
00268         return false;
00269     }
00270 }
00271 
00272 // call data_ready and returned true before call
00273 bool hmc5883L::read(quan::three_d::vect<quan::magnetic_flux_density::uT> & v)const
00274 {
00275     if( this->set_reg_idx(dout_reg)) {
00276         char arr[7];
00277         bool success= this->i2c_read(arr,7) == 0;
00278         if(success) {
00279             // TODO check status reg arr[6]
00280             quan::three_d::vect<int16_t> temp;
00281             temp.x = static_cast<int16_t>(arr[1]) + ( static_cast<int16_t>(arr[0]) << 8U);
00282             temp.y = static_cast<int16_t>(arr[5]) + ( static_cast<int16_t>(arr[4]) << 8U);
00283             temp.z = static_cast<int16_t>(arr[3]) + ( static_cast<int16_t>(arr[2]) << 8U);
00284             
00285             quan::three_d::vect<quan::magnetic_flux_density::uT>  result
00286               = temp * mag_resolution;
00287               
00288             result.x *= this->mag_gain.x;
00289             result.y *= this->mag_gain.y;
00290             result.z *= this->mag_gain.z;
00291             
00292             result -= this->mag_offset;
00293             
00294             v = result;
00295             
00296             return true;
00297         } else {
00298             std::cout << "mag_read failed\n";
00299             return false;
00300         }
00301     } else {
00302         return false;
00303     }
00304 }
00305 
00306 bool hmc5883L::make_measurement(quan::three_d::vect<quan::magnetic_flux_density::uT>& result)const
00307 {
00308     if ( ! this->start_measurement()) {
00309         return false;
00310     }
00311 
00312     while (! this->data_ready()) {
00313         ThisThread::sleep_for(5U);
00314     }
00315     return this->read(result);
00316 }
00317 
00318 bool hmc5883L::set_gain( quan::three_d::vect<double> const & gain)
00319 {
00320     this->mag_gain = gain;
00321     return true;
00322 }
00323 
00324 bool hmc5883L::set_offset(
00325     quan::three_d::vect<
00326         quan::magnetic_flux_density::uT
00327     > const & offset
00328 )
00329 {
00330     this->mag_offset = offset;
00331     return true;
00332 }