First draft HMC5883 magnetometer sensor using physical quantities, outputting via serial port using std::cout on mbed os 5
hmc5883.cpp@11:de7c9ae7ef65, 2020-03-26 (annotated)
- Committer:
- skyscraper
- Date:
- Thu Mar 26 22:58:21 2020 +0000
- Revision:
- 11:de7c9ae7ef65
- Parent:
- 9:87a7169b4d5c
Changed to use threads
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
skyscraper | 2:9ffb2f18756b | 1 | |
skyscraper | 2:9ffb2f18756b | 2 | |
skyscraper | 2:9ffb2f18756b | 3 | #include "mbed.h" |
skyscraper | 8:c5dc1ce10722 | 4 | #include "hmc5883.h" |
skyscraper | 8:c5dc1ce10722 | 5 | #include "resourceManager.h" |
skyscraper | 7:5d14da0b4c95 | 6 | |
skyscraper | 2:9ffb2f18756b | 7 | #include <iostream> |
skyscraper | 2:9ffb2f18756b | 8 | #include <quan/out/magnetic_flux_density.hpp> |
skyscraper | 2:9ffb2f18756b | 9 | #include <quan/three_d/out/vect.hpp> |
skyscraper | 2:9ffb2f18756b | 10 | |
skyscraper | 8:c5dc1ce10722 | 11 | namespace |
skyscraper | 8:c5dc1ce10722 | 12 | { |
skyscraper | 3:2834be4e10ef | 13 | |
skyscraper | 8:c5dc1ce10722 | 14 | template <int N, int D=1> struct mag_data_rate_id; |
skyscraper | 8:c5dc1ce10722 | 15 | // values are mag settings for each data rate |
skyscraper | 8:c5dc1ce10722 | 16 | template <> struct mag_data_rate_id<3,4> : std::integral_constant<uint8_t,(0b000U << 2U)> {}; |
skyscraper | 8:c5dc1ce10722 | 17 | template <> struct mag_data_rate_id<3,2> : std::integral_constant<uint8_t,(0b001U << 2U)> {}; |
skyscraper | 8:c5dc1ce10722 | 18 | template <> struct mag_data_rate_id<3> : std::integral_constant<uint8_t,(0b010U << 2U)> {}; |
skyscraper | 8:c5dc1ce10722 | 19 | template <> struct mag_data_rate_id<15,2> : std::integral_constant<uint8_t,(0b011U << 2U)> {}; |
skyscraper | 8:c5dc1ce10722 | 20 | template <> struct mag_data_rate_id<15> : std::integral_constant<uint8_t,(0b100U << 2U)> {}; |
skyscraper | 8:c5dc1ce10722 | 21 | template <> struct mag_data_rate_id<30> : std::integral_constant<uint8_t,(0b101U << 2U)> {}; |
skyscraper | 8:c5dc1ce10722 | 22 | template <> struct mag_data_rate_id<75> : std::integral_constant<uint8_t,(0b110U << 2U)> {}; |
skyscraper | 8:c5dc1ce10722 | 23 | |
skyscraper | 8:c5dc1ce10722 | 24 | }//namespace |
skyscraper | 8:c5dc1ce10722 | 25 | |
skyscraper | 8:c5dc1ce10722 | 26 | hmc5883L::hmc5883L(I2C& i2cIn,uint8_t addressIn) |
skyscraper | 8:c5dc1ce10722 | 27 | :I2CBusDevice{i2cIn,addressIn} |
skyscraper | 8:c5dc1ce10722 | 28 | ,mag_resolution{0.92_milli_gauss} |
skyscraper | 8:c5dc1ce10722 | 29 | ,mag_range{1.3_gauss} |
skyscraper | 8:c5dc1ce10722 | 30 | ,mag_gain{1.0,1.0,1.0} |
skyscraper | 8:c5dc1ce10722 | 31 | ,mag_offset{0.0_uT,0.0_uT,0.0_uT} |
skyscraper | 8:c5dc1ce10722 | 32 | {} |
skyscraper | 3:2834be4e10ef | 33 | |
skyscraper | 8:c5dc1ce10722 | 34 | bool hmc5883L::set_reg_idx(uint8_t idx_in)const |
skyscraper | 8:c5dc1ce10722 | 35 | { |
skyscraper | 8:c5dc1ce10722 | 36 | char const idx = static_cast<char>(idx_in); |
skyscraper | 8:c5dc1ce10722 | 37 | bool const result = this->i2c_write(&idx,1) == 0; |
skyscraper | 8:c5dc1ce10722 | 38 | if(result) { |
skyscraper | 8:c5dc1ce10722 | 39 | return true; |
skyscraper | 8:c5dc1ce10722 | 40 | } else { |
skyscraper | 8:c5dc1ce10722 | 41 | std::cout << "mag_set_reg_idx failed\n"; |
skyscraper | 8:c5dc1ce10722 | 42 | return false; |
skyscraper | 8:c5dc1ce10722 | 43 | } |
skyscraper | 8:c5dc1ce10722 | 44 | } |
skyscraper | 8:c5dc1ce10722 | 45 | |
skyscraper | 8:c5dc1ce10722 | 46 | bool hmc5883L::write_reg(uint8_t idx, uint8_t val)const |
skyscraper | 8:c5dc1ce10722 | 47 | { |
skyscraper | 8:c5dc1ce10722 | 48 | char const ar[2] = {idx,val}; |
skyscraper | 8:c5dc1ce10722 | 49 | bool const result = this->i2c_write(ar,2) == 0; |
skyscraper | 8:c5dc1ce10722 | 50 | if(result) { |
skyscraper | 8:c5dc1ce10722 | 51 | return true; |
skyscraper | 8:c5dc1ce10722 | 52 | } else { |
skyscraper | 8:c5dc1ce10722 | 53 | std::cout << " mag_write_reg failed\n"; |
skyscraper | 8:c5dc1ce10722 | 54 | return false; |
skyscraper | 8:c5dc1ce10722 | 55 | } |
skyscraper | 8:c5dc1ce10722 | 56 | } |
skyscraper | 8:c5dc1ce10722 | 57 | |
skyscraper | 8:c5dc1ce10722 | 58 | bool hmc5883L::get_reg(uint8_t idx_in, uint8_t& result)const |
skyscraper | 8:c5dc1ce10722 | 59 | { |
skyscraper | 8:c5dc1ce10722 | 60 | if ( this->set_reg_idx(idx_in)) { |
skyscraper | 8:c5dc1ce10722 | 61 | char temp_result = 0; |
skyscraper | 8:c5dc1ce10722 | 62 | bool const success = this->i2c_read(&temp_result,1,false) == 0; |
skyscraper | 8:c5dc1ce10722 | 63 | if (success) { |
skyscraper | 8:c5dc1ce10722 | 64 | result = temp_result; |
skyscraper | 2:9ffb2f18756b | 65 | return true; |
skyscraper | 2:9ffb2f18756b | 66 | } else { |
skyscraper | 8:c5dc1ce10722 | 67 | std::cout << "mag_get_reg read failed\n"; |
skyscraper | 2:9ffb2f18756b | 68 | return false; |
skyscraper | 2:9ffb2f18756b | 69 | } |
skyscraper | 8:c5dc1ce10722 | 70 | } else { |
skyscraper | 8:c5dc1ce10722 | 71 | return false; |
skyscraper | 2:9ffb2f18756b | 72 | } |
skyscraper | 8:c5dc1ce10722 | 73 | } |
skyscraper | 8:c5dc1ce10722 | 74 | |
skyscraper | 8:c5dc1ce10722 | 75 | bool hmc5883L::modify_reg(uint8_t idx, uint8_t and_val, uint8_t or_val)const |
skyscraper | 8:c5dc1ce10722 | 76 | { |
skyscraper | 8:c5dc1ce10722 | 77 | uint8_t cur_val = 0; |
skyscraper | 8:c5dc1ce10722 | 78 | if(this->get_reg(idx,cur_val)) { |
skyscraper | 8:c5dc1ce10722 | 79 | uint8_t const new_val = (cur_val & and_val ) | or_val; |
skyscraper | 8:c5dc1ce10722 | 80 | return this->write_reg(idx,new_val); |
skyscraper | 8:c5dc1ce10722 | 81 | } else { |
skyscraper | 8:c5dc1ce10722 | 82 | return false; |
skyscraper | 2:9ffb2f18756b | 83 | } |
skyscraper | 8:c5dc1ce10722 | 84 | } |
skyscraper | 2:9ffb2f18756b | 85 | |
skyscraper | 8:c5dc1ce10722 | 86 | bool hmc5883L::detected(bool verbose)const |
skyscraper | 2:9ffb2f18756b | 87 | { |
skyscraper | 8:c5dc1ce10722 | 88 | if ( this->set_reg_idx(id_regA) ) { |
skyscraper | 2:9ffb2f18756b | 89 | char id_input[4]; |
skyscraper | 8:c5dc1ce10722 | 90 | bool success = this->i2c_read(id_input,3) == 0; |
skyscraper | 3:2834be4e10ef | 91 | if(success) { |
skyscraper | 2:9ffb2f18756b | 92 | id_input[3] = '\0'; |
skyscraper | 2:9ffb2f18756b | 93 | bool const is_hmc = (strcmp(id_input,"H43") == 0); |
skyscraper | 2:9ffb2f18756b | 94 | if (is_hmc) { |
skyscraper | 2:9ffb2f18756b | 95 | return true; |
skyscraper | 2:9ffb2f18756b | 96 | } else { |
skyscraper | 8:c5dc1ce10722 | 97 | if( verbose) { |
skyscraper | 2:9ffb2f18756b | 98 | std::cout << "hmc5883 ID string didnt match\n"; |
skyscraper | 8:c5dc1ce10722 | 99 | } |
skyscraper | 2:9ffb2f18756b | 100 | return false; |
skyscraper | 2:9ffb2f18756b | 101 | } |
skyscraper | 2:9ffb2f18756b | 102 | } else { |
skyscraper | 8:c5dc1ce10722 | 103 | if( verbose) { |
skyscraper | 8:c5dc1ce10722 | 104 | std::cout << "id mag read failed\n"; |
skyscraper | 8:c5dc1ce10722 | 105 | } |
skyscraper | 2:9ffb2f18756b | 106 | return false; |
skyscraper | 2:9ffb2f18756b | 107 | } |
skyscraper | 2:9ffb2f18756b | 108 | } else { |
skyscraper | 2:9ffb2f18756b | 109 | return false; |
skyscraper | 2:9ffb2f18756b | 110 | } |
skyscraper | 2:9ffb2f18756b | 111 | } |
skyscraper | 2:9ffb2f18756b | 112 | |
skyscraper | 2:9ffb2f18756b | 113 | // only 1,2,4,8 available |
skyscraper | 8:c5dc1ce10722 | 114 | bool hmc5883L::set_samples_average(int n_samples)const |
skyscraper | 2:9ffb2f18756b | 115 | { |
skyscraper | 2:9ffb2f18756b | 116 | uint8_t or_val = 0; |
skyscraper | 2:9ffb2f18756b | 117 | switch (n_samples) { |
skyscraper | 2:9ffb2f18756b | 118 | case 1 : |
skyscraper | 2:9ffb2f18756b | 119 | or_val = 0b00U << 5U; |
skyscraper | 2:9ffb2f18756b | 120 | break; |
skyscraper | 2:9ffb2f18756b | 121 | case 2 : |
skyscraper | 2:9ffb2f18756b | 122 | or_val = 0b01U << 5U; |
skyscraper | 2:9ffb2f18756b | 123 | break; |
skyscraper | 2:9ffb2f18756b | 124 | case 4 : |
skyscraper | 2:9ffb2f18756b | 125 | or_val = 0b10U << 5U; |
skyscraper | 2:9ffb2f18756b | 126 | break; |
skyscraper | 2:9ffb2f18756b | 127 | case 8 : |
skyscraper | 2:9ffb2f18756b | 128 | or_val = 0b11U << 5U; |
skyscraper | 2:9ffb2f18756b | 129 | break; |
skyscraper | 2:9ffb2f18756b | 130 | default: |
skyscraper | 2:9ffb2f18756b | 131 | std::cout << "mag_set_samples_average : invalid n_samples (" << n_samples << ")\n"; |
skyscraper | 2:9ffb2f18756b | 132 | return false; |
skyscraper | 2:9ffb2f18756b | 133 | } |
skyscraper | 2:9ffb2f18756b | 134 | uint8_t constexpr and_val = ~(0b11 << 5U); |
skyscraper | 8:c5dc1ce10722 | 135 | return this->modify_reg(cfg_regA,and_val,or_val); |
skyscraper | 2:9ffb2f18756b | 136 | } |
skyscraper | 2:9ffb2f18756b | 137 | |
skyscraper | 2:9ffb2f18756b | 138 | template <int N, int D> |
skyscraper | 8:c5dc1ce10722 | 139 | bool hmc5883L::set_data_rate()const |
skyscraper | 2:9ffb2f18756b | 140 | { |
skyscraper | 2:9ffb2f18756b | 141 | uint8_t constexpr and_val = static_cast<uint8_t>(~(0b111U << 2U)); |
skyscraper | 2:9ffb2f18756b | 142 | uint8_t constexpr or_val = mag_data_rate_id<N,D>::value; |
skyscraper | 8:c5dc1ce10722 | 143 | return this->modify_reg(cfg_regA,and_val,or_val); |
skyscraper | 8:c5dc1ce10722 | 144 | } |
skyscraper | 8:c5dc1ce10722 | 145 | //data rate 0.75, 1.5, 3 ,7.5, 15 (Default) , 30, 75 |
skyscraper | 8:c5dc1ce10722 | 146 | template bool hmc5883L::set_data_rate<3,4>() const; |
skyscraper | 8:c5dc1ce10722 | 147 | template bool hmc5883L::set_data_rate<3,2>() const; |
skyscraper | 8:c5dc1ce10722 | 148 | template bool hmc5883L::set_data_rate<3>() const; |
skyscraper | 8:c5dc1ce10722 | 149 | template bool hmc5883L::set_data_rate<15,2>() const; |
skyscraper | 8:c5dc1ce10722 | 150 | template bool hmc5883L::set_data_rate<15>() const; |
skyscraper | 8:c5dc1ce10722 | 151 | template bool hmc5883L::set_data_rate<30>() const; |
skyscraper | 8:c5dc1ce10722 | 152 | template bool hmc5883L::set_data_rate<75>() const; |
skyscraper | 8:c5dc1ce10722 | 153 | |
skyscraper | 8:c5dc1ce10722 | 154 | bool hmc5883L::set_positive_bias()const |
skyscraper | 8:c5dc1ce10722 | 155 | { |
skyscraper | 8:c5dc1ce10722 | 156 | uint8_t constexpr and_val = static_cast<uint8_t>(~(0b11U )); |
skyscraper | 8:c5dc1ce10722 | 157 | uint8_t constexpr or_val = 0b01U; |
skyscraper | 8:c5dc1ce10722 | 158 | return this->modify_reg(cfg_regA,and_val,or_val); |
skyscraper | 2:9ffb2f18756b | 159 | } |
skyscraper | 2:9ffb2f18756b | 160 | |
skyscraper | 8:c5dc1ce10722 | 161 | bool hmc5883L::set_negative_bias()const |
skyscraper | 8:c5dc1ce10722 | 162 | { |
skyscraper | 8:c5dc1ce10722 | 163 | uint8_t constexpr and_val = static_cast<uint8_t>(~(0b11U )); |
skyscraper | 8:c5dc1ce10722 | 164 | uint8_t constexpr or_val = 0b10U; |
skyscraper | 8:c5dc1ce10722 | 165 | return this->modify_reg(cfg_regA,and_val,or_val); |
skyscraper | 2:9ffb2f18756b | 166 | } |
skyscraper | 2:9ffb2f18756b | 167 | |
skyscraper | 8:c5dc1ce10722 | 168 | bool hmc5883L::mag_clear_bias()const |
skyscraper | 8:c5dc1ce10722 | 169 | { |
skyscraper | 8:c5dc1ce10722 | 170 | uint8_t constexpr and_val = static_cast<uint8_t>(~(0b11U )); |
skyscraper | 8:c5dc1ce10722 | 171 | uint8_t constexpr or_val = 0b00U; |
skyscraper | 8:c5dc1ce10722 | 172 | return this->modify_reg(cfg_regA,and_val,or_val); |
skyscraper | 8:c5dc1ce10722 | 173 | } |
skyscraper | 8:c5dc1ce10722 | 174 | |
skyscraper | 8:c5dc1ce10722 | 175 | bool hmc5883L::set_range(quan::magnetic_flux_density::uT const & range_in) |
skyscraper | 2:9ffb2f18756b | 176 | { |
skyscraper | 2:9ffb2f18756b | 177 | uint8_t or_value = 0; |
skyscraper | 2:9ffb2f18756b | 178 | auto const range = abs(range_in); |
skyscraper | 2:9ffb2f18756b | 179 | |
skyscraper | 2:9ffb2f18756b | 180 | if ( range <= 0.88_gauss) { |
skyscraper | 2:9ffb2f18756b | 181 | or_value = 0b001U << 5U ; |
skyscraper | 2:9ffb2f18756b | 182 | mag_range = 0.88_gauss; |
skyscraper | 2:9ffb2f18756b | 183 | mag_resolution = 0.73_milli_gauss; |
skyscraper | 2:9ffb2f18756b | 184 | } else if (range <= 1.3_gauss) { |
skyscraper | 2:9ffb2f18756b | 185 | or_value = 0b001U << 5U ; |
skyscraper | 2:9ffb2f18756b | 186 | mag_range = 1.3_gauss; |
skyscraper | 2:9ffb2f18756b | 187 | mag_resolution = 0.92_milli_gauss; |
skyscraper | 2:9ffb2f18756b | 188 | } else if (range <= 1.9_gauss) { |
skyscraper | 2:9ffb2f18756b | 189 | or_value = 0b010U << 5U ; |
skyscraper | 2:9ffb2f18756b | 190 | mag_range = 1.9_gauss; |
skyscraper | 2:9ffb2f18756b | 191 | mag_resolution = 1.22_milli_gauss; |
skyscraper | 2:9ffb2f18756b | 192 | } else if (range <= 2.5_gauss) { |
skyscraper | 2:9ffb2f18756b | 193 | or_value = 0b011U << 5U ; |
skyscraper | 2:9ffb2f18756b | 194 | mag_range = 2.5_gauss; |
skyscraper | 2:9ffb2f18756b | 195 | mag_resolution = 1.52_milli_gauss; |
skyscraper | 2:9ffb2f18756b | 196 | } else if (range <= 4.0_gauss) { |
skyscraper | 2:9ffb2f18756b | 197 | or_value = 0b100U << 5U ; |
skyscraper | 2:9ffb2f18756b | 198 | mag_range = 4.0_gauss; |
skyscraper | 2:9ffb2f18756b | 199 | mag_resolution = 2.27_milli_gauss; |
skyscraper | 2:9ffb2f18756b | 200 | } else if (range <= 4.7_gauss) { |
skyscraper | 2:9ffb2f18756b | 201 | or_value = 0b101U << 5U ; |
skyscraper | 2:9ffb2f18756b | 202 | mag_range = 4.7_gauss; |
skyscraper | 2:9ffb2f18756b | 203 | mag_resolution = 2.56_milli_gauss; |
skyscraper | 2:9ffb2f18756b | 204 | } else if (range <=5.6_gauss) { |
skyscraper | 2:9ffb2f18756b | 205 | or_value = 0b110U << 5U ; |
skyscraper | 2:9ffb2f18756b | 206 | mag_range = 5.6_gauss; |
skyscraper | 2:9ffb2f18756b | 207 | mag_resolution = 3.03_milli_gauss; |
skyscraper | 2:9ffb2f18756b | 208 | } else if ( range <= 8.1_gauss) { |
skyscraper | 2:9ffb2f18756b | 209 | or_value = 0b111U << 5U ; |
skyscraper | 2:9ffb2f18756b | 210 | mag_range = 8.1_gauss; |
skyscraper | 2:9ffb2f18756b | 211 | mag_resolution = 4.35_milli_gauss; |
skyscraper | 2:9ffb2f18756b | 212 | } else { |
skyscraper | 2:9ffb2f18756b | 213 | quan::magnetic_flux_density::uT constexpr max_range = 8.1_gauss; |
skyscraper | 2:9ffb2f18756b | 214 | std::cout << "range too big: max +- range = " << max_range <<"\n"; |
skyscraper | 2:9ffb2f18756b | 215 | return false; |
skyscraper | 2:9ffb2f18756b | 216 | } |
skyscraper | 2:9ffb2f18756b | 217 | uint8_t constexpr and_val = static_cast<uint8_t>(~(0b111U << 5U)); |
skyscraper | 2:9ffb2f18756b | 218 | std::cout << "mag range set to : +- " << mag_range <<'\n'; |
skyscraper | 8:c5dc1ce10722 | 219 | return this->modify_reg(cfg_regB,and_val,or_value); |
skyscraper | 2:9ffb2f18756b | 220 | |
skyscraper | 2:9ffb2f18756b | 221 | } |
skyscraper | 2:9ffb2f18756b | 222 | |
skyscraper | 8:c5dc1ce10722 | 223 | quan::magnetic_flux_density::uT |
skyscraper | 8:c5dc1ce10722 | 224 | hmc5883L::get_flux_density_range()const |
skyscraper | 2:9ffb2f18756b | 225 | { |
skyscraper | 2:9ffb2f18756b | 226 | return mag_range; |
skyscraper | 2:9ffb2f18756b | 227 | } |
skyscraper | 2:9ffb2f18756b | 228 | |
skyscraper | 8:c5dc1ce10722 | 229 | bool hmc5883L::set_continuous_measurement_mode()const |
skyscraper | 2:9ffb2f18756b | 230 | { |
skyscraper | 2:9ffb2f18756b | 231 | uint8_t constexpr and_val = static_cast<uint8_t>(~(0b11U )); |
skyscraper | 2:9ffb2f18756b | 232 | uint8_t constexpr or_val = 0b00U; |
skyscraper | 8:c5dc1ce10722 | 233 | return this->modify_reg(mode_reg,and_val,or_val); |
skyscraper | 2:9ffb2f18756b | 234 | } |
skyscraper | 2:9ffb2f18756b | 235 | |
skyscraper | 8:c5dc1ce10722 | 236 | bool hmc5883L::start_measurement()const |
skyscraper | 2:9ffb2f18756b | 237 | { |
skyscraper | 2:9ffb2f18756b | 238 | uint8_t constexpr and_val = static_cast<uint8_t>(~(0b11U )); |
skyscraper | 2:9ffb2f18756b | 239 | uint8_t constexpr or_val = 0b01U; |
skyscraper | 8:c5dc1ce10722 | 240 | return this->modify_reg(mode_reg,and_val,or_val); |
skyscraper | 2:9ffb2f18756b | 241 | } |
skyscraper | 2:9ffb2f18756b | 242 | |
skyscraper | 8:c5dc1ce10722 | 243 | bool hmc5883L::set_idle_mode()const |
skyscraper | 2:9ffb2f18756b | 244 | { |
skyscraper | 2:9ffb2f18756b | 245 | uint8_t constexpr and_val = static_cast<uint8_t>(~(0b11U )); |
skyscraper | 2:9ffb2f18756b | 246 | uint8_t constexpr or_val = 0b10U; |
skyscraper | 8:c5dc1ce10722 | 247 | return this->modify_reg(mode_reg,and_val,or_val); |
skyscraper | 2:9ffb2f18756b | 248 | } |
skyscraper | 2:9ffb2f18756b | 249 | |
skyscraper | 8:c5dc1ce10722 | 250 | bool hmc5883L::data_ready()const |
skyscraper | 2:9ffb2f18756b | 251 | { |
skyscraper | 2:9ffb2f18756b | 252 | uint8_t result = 0; |
skyscraper | 8:c5dc1ce10722 | 253 | if ( this->get_reg(status_reg, result)) { |
skyscraper | 2:9ffb2f18756b | 254 | return (result & 0b1U) != 0U; |
skyscraper | 2:9ffb2f18756b | 255 | } else { |
skyscraper | 2:9ffb2f18756b | 256 | std::cout << "mag data ready failed\n"; |
skyscraper | 2:9ffb2f18756b | 257 | return false; |
skyscraper | 2:9ffb2f18756b | 258 | } |
skyscraper | 2:9ffb2f18756b | 259 | } |
skyscraper | 2:9ffb2f18756b | 260 | |
skyscraper | 8:c5dc1ce10722 | 261 | bool hmc5883L::data_locked()const |
skyscraper | 2:9ffb2f18756b | 262 | { |
skyscraper | 2:9ffb2f18756b | 263 | uint8_t result = 0; |
skyscraper | 8:c5dc1ce10722 | 264 | if ( this->get_reg(status_reg, result)) { |
skyscraper | 2:9ffb2f18756b | 265 | return (result & 0b10U) != 0U; |
skyscraper | 2:9ffb2f18756b | 266 | } else { |
skyscraper | 2:9ffb2f18756b | 267 | std::cout << "mag data locked failed\n"; |
skyscraper | 2:9ffb2f18756b | 268 | return false; |
skyscraper | 2:9ffb2f18756b | 269 | } |
skyscraper | 2:9ffb2f18756b | 270 | } |
skyscraper | 2:9ffb2f18756b | 271 | |
skyscraper | 9:87a7169b4d5c | 272 | // call data_ready and returned true before call |
skyscraper | 8:c5dc1ce10722 | 273 | bool hmc5883L::read(quan::three_d::vect<quan::magnetic_flux_density::uT> & v)const |
skyscraper | 2:9ffb2f18756b | 274 | { |
skyscraper | 8:c5dc1ce10722 | 275 | if( this->set_reg_idx(dout_reg)) { |
skyscraper | 2:9ffb2f18756b | 276 | char arr[7]; |
skyscraper | 8:c5dc1ce10722 | 277 | bool success= this->i2c_read(arr,7) == 0; |
skyscraper | 3:2834be4e10ef | 278 | if(success) { |
skyscraper | 2:9ffb2f18756b | 279 | // TODO check status reg arr[6] |
skyscraper | 2:9ffb2f18756b | 280 | quan::three_d::vect<int16_t> temp; |
skyscraper | 2:9ffb2f18756b | 281 | temp.x = static_cast<int16_t>(arr[1]) + ( static_cast<int16_t>(arr[0]) << 8U); |
skyscraper | 2:9ffb2f18756b | 282 | temp.y = static_cast<int16_t>(arr[5]) + ( static_cast<int16_t>(arr[4]) << 8U); |
skyscraper | 2:9ffb2f18756b | 283 | temp.z = static_cast<int16_t>(arr[3]) + ( static_cast<int16_t>(arr[2]) << 8U); |
skyscraper | 8:c5dc1ce10722 | 284 | |
skyscraper | 8:c5dc1ce10722 | 285 | quan::three_d::vect<quan::magnetic_flux_density::uT> result |
skyscraper | 8:c5dc1ce10722 | 286 | = temp * mag_resolution; |
skyscraper | 8:c5dc1ce10722 | 287 | |
skyscraper | 8:c5dc1ce10722 | 288 | result.x *= this->mag_gain.x; |
skyscraper | 8:c5dc1ce10722 | 289 | result.y *= this->mag_gain.y; |
skyscraper | 8:c5dc1ce10722 | 290 | result.z *= this->mag_gain.z; |
skyscraper | 8:c5dc1ce10722 | 291 | |
skyscraper | 8:c5dc1ce10722 | 292 | result -= this->mag_offset; |
skyscraper | 8:c5dc1ce10722 | 293 | |
skyscraper | 8:c5dc1ce10722 | 294 | v = result; |
skyscraper | 8:c5dc1ce10722 | 295 | |
skyscraper | 2:9ffb2f18756b | 296 | return true; |
skyscraper | 2:9ffb2f18756b | 297 | } else { |
skyscraper | 2:9ffb2f18756b | 298 | std::cout << "mag_read failed\n"; |
skyscraper | 2:9ffb2f18756b | 299 | return false; |
skyscraper | 2:9ffb2f18756b | 300 | } |
skyscraper | 2:9ffb2f18756b | 301 | } else { |
skyscraper | 2:9ffb2f18756b | 302 | return false; |
skyscraper | 2:9ffb2f18756b | 303 | } |
skyscraper | 2:9ffb2f18756b | 304 | } |
skyscraper | 2:9ffb2f18756b | 305 | |
skyscraper | 8:c5dc1ce10722 | 306 | bool hmc5883L::make_measurement(quan::three_d::vect<quan::magnetic_flux_density::uT>& result)const |
skyscraper | 2:9ffb2f18756b | 307 | { |
skyscraper | 8:c5dc1ce10722 | 308 | if ( ! this->start_measurement()) { |
skyscraper | 2:9ffb2f18756b | 309 | return false; |
skyscraper | 2:9ffb2f18756b | 310 | } |
skyscraper | 2:9ffb2f18756b | 311 | |
skyscraper | 8:c5dc1ce10722 | 312 | while (! this->data_ready()) { |
skyscraper | 2:9ffb2f18756b | 313 | ThisThread::sleep_for(5U); |
skyscraper | 2:9ffb2f18756b | 314 | } |
skyscraper | 8:c5dc1ce10722 | 315 | return this->read(result); |
skyscraper | 8:c5dc1ce10722 | 316 | } |
skyscraper | 8:c5dc1ce10722 | 317 | |
skyscraper | 8:c5dc1ce10722 | 318 | bool hmc5883L::set_gain( quan::three_d::vect<double> const & gain) |
skyscraper | 8:c5dc1ce10722 | 319 | { |
skyscraper | 8:c5dc1ce10722 | 320 | this->mag_gain = gain; |
skyscraper | 8:c5dc1ce10722 | 321 | return true; |
skyscraper | 2:9ffb2f18756b | 322 | } |
skyscraper | 2:9ffb2f18756b | 323 | |
skyscraper | 8:c5dc1ce10722 | 324 | bool hmc5883L::set_offset( |
skyscraper | 8:c5dc1ce10722 | 325 | quan::three_d::vect< |
skyscraper | 8:c5dc1ce10722 | 326 | quan::magnetic_flux_density::uT |
skyscraper | 8:c5dc1ce10722 | 327 | > const & offset |
skyscraper | 8:c5dc1ce10722 | 328 | ) |
skyscraper | 8:c5dc1ce10722 | 329 | { |
skyscraper | 8:c5dc1ce10722 | 330 | this->mag_offset = offset; |
skyscraper | 8:c5dc1ce10722 | 331 | return true; |
skyscraper | 8:c5dc1ce10722 | 332 | } |