First draft HMC5883 magnetometer sensor using physical quantities, outputting via serial port using std::cout on mbed os 5
main.cpp@0:37dbfb036586, 2020-03-21 (annotated)
- Committer:
- skyscraper
- Date:
- Sat Mar 21 23:18:28 2020 +0000
- Revision:
- 0:37dbfb036586
- Child:
- 1:e11ab941748b
Working version of hmc5883 sensor with imported cut down quan library
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
skyscraper | 0:37dbfb036586 | 1 | |
skyscraper | 0:37dbfb036586 | 2 | #include "mbed.h" |
skyscraper | 0:37dbfb036586 | 3 | |
skyscraper | 0:37dbfb036586 | 4 | #include <string> |
skyscraper | 0:37dbfb036586 | 5 | #include <array> |
skyscraper | 0:37dbfb036586 | 6 | #include <iostream> |
skyscraper | 0:37dbfb036586 | 7 | #include <ratio> |
skyscraper | 0:37dbfb036586 | 8 | #include <type_traits> |
skyscraper | 0:37dbfb036586 | 9 | |
skyscraper | 0:37dbfb036586 | 10 | #include <quan/out/magnetic_flux_density.hpp> |
skyscraper | 0:37dbfb036586 | 11 | #include <quan/three_d/out/vect.hpp> |
skyscraper | 0:37dbfb036586 | 12 | #include <quan/max.hpp> |
skyscraper | 0:37dbfb036586 | 13 | #include <quan/min.hpp> |
skyscraper | 0:37dbfb036586 | 14 | |
skyscraper | 0:37dbfb036586 | 15 | namespace |
skyscraper | 0:37dbfb036586 | 16 | { |
skyscraper | 0:37dbfb036586 | 17 | DigitalOut led1(LED1); |
skyscraper | 0:37dbfb036586 | 18 | } // namespace |
skyscraper | 0:37dbfb036586 | 19 | |
skyscraper | 0:37dbfb036586 | 20 | /* |
skyscraper | 0:37dbfb036586 | 21 | paradigm |
skyscraper | 0:37dbfb036586 | 22 | input stream |
skyscraper | 0:37dbfb036586 | 23 | synchronous/asynchronous |
skyscraper | 0:37dbfb036586 | 24 | Ideally it updates in background |
skyscraper | 0:37dbfb036586 | 25 | You can be notified of update in callback |
skyscraper | 0:37dbfb036586 | 26 | or just read |
skyscraper | 0:37dbfb036586 | 27 | */ |
skyscraper | 0:37dbfb036586 | 28 | template<typename SensorUnit> |
skyscraper | 0:37dbfb036586 | 29 | struct Sensor { |
skyscraper | 0:37dbfb036586 | 30 | typedef SensorUnit sensor_unit_t; |
skyscraper | 0:37dbfb036586 | 31 | |
skyscraper | 0:37dbfb036586 | 32 | // TODO status_t get_status()const; |
skyscraper | 0:37dbfb036586 | 33 | virtual bool open() = 0; |
skyscraper | 0:37dbfb036586 | 34 | virtual bool read(sensor_unit_t const & current_value) const = 0; |
skyscraper | 0:37dbfb036586 | 35 | virtual bool close() = 0; |
skyscraper | 0:37dbfb036586 | 36 | }; |
skyscraper | 0:37dbfb036586 | 37 | |
skyscraper | 0:37dbfb036586 | 38 | namespace |
skyscraper | 0:37dbfb036586 | 39 | { |
skyscraper | 0:37dbfb036586 | 40 | /* |
skyscraper | 0:37dbfb036586 | 41 | 00 Configuration Register A R/W |
skyscraper | 0:37dbfb036586 | 42 | 01 Configuration Register B R/W |
skyscraper | 0:37dbfb036586 | 43 | 02 Mode Register R/W |
skyscraper | 0:37dbfb036586 | 44 | 03 Data Output X MSB Register R |
skyscraper | 0:37dbfb036586 | 45 | 04 Data Output X LSB Register R |
skyscraper | 0:37dbfb036586 | 46 | 05 Data Output Z MSB Register R |
skyscraper | 0:37dbfb036586 | 47 | 06 Data Output Z LSB Register R |
skyscraper | 0:37dbfb036586 | 48 | 07 Data Output Y MSB Register R |
skyscraper | 0:37dbfb036586 | 49 | 08 Data Output Y LSB Register R |
skyscraper | 0:37dbfb036586 | 50 | 09 Status Register R |
skyscraper | 0:37dbfb036586 | 51 | 10 Identification Register A R |
skyscraper | 0:37dbfb036586 | 52 | 11 Identification Register B R |
skyscraper | 0:37dbfb036586 | 53 | 12 Identification Register C R |
skyscraper | 0:37dbfb036586 | 54 | */ |
skyscraper | 0:37dbfb036586 | 55 | |
skyscraper | 0:37dbfb036586 | 56 | I2C i2c(I2C_SDA, I2C_SCL ); |
skyscraper | 0:37dbfb036586 | 57 | constexpr uint8_t i2c_addr = 0x3D; |
skyscraper | 0:37dbfb036586 | 58 | constexpr char cfg_regA = 0; |
skyscraper | 0:37dbfb036586 | 59 | constexpr char cfg_regB = 1; |
skyscraper | 0:37dbfb036586 | 60 | constexpr char mode_reg = 2; |
skyscraper | 0:37dbfb036586 | 61 | constexpr char dout_reg = 3; |
skyscraper | 0:37dbfb036586 | 62 | constexpr char status_reg = 9; |
skyscraper | 0:37dbfb036586 | 63 | constexpr char id_regA = 10U; |
skyscraper | 0:37dbfb036586 | 64 | |
skyscraper | 0:37dbfb036586 | 65 | // Set reg index to idx_in |
skyscraper | 0:37dbfb036586 | 66 | // return true if successful |
skyscraper | 0:37dbfb036586 | 67 | bool mag_set_reg_idx(uint8_t idx_in) |
skyscraper | 0:37dbfb036586 | 68 | { |
skyscraper | 0:37dbfb036586 | 69 | char const idx = static_cast<char>(idx_in); |
skyscraper | 0:37dbfb036586 | 70 | bool const result = (i2c.write(i2c_addr,&idx,1) == 0); |
skyscraper | 0:37dbfb036586 | 71 | if(result) { |
skyscraper | 0:37dbfb036586 | 72 | return true; |
skyscraper | 0:37dbfb036586 | 73 | } else { |
skyscraper | 0:37dbfb036586 | 74 | std::cout << "mag_set_reg_idx failed\n"; |
skyscraper | 0:37dbfb036586 | 75 | return false; |
skyscraper | 0:37dbfb036586 | 76 | } |
skyscraper | 0:37dbfb036586 | 77 | } |
skyscraper | 0:37dbfb036586 | 78 | |
skyscraper | 0:37dbfb036586 | 79 | // Write reg at idx with val |
skyscraper | 0:37dbfb036586 | 80 | // return true if successful |
skyscraper | 0:37dbfb036586 | 81 | bool mag_write_reg(uint8_t idx, uint8_t val) |
skyscraper | 0:37dbfb036586 | 82 | { |
skyscraper | 0:37dbfb036586 | 83 | char ar[2] = {idx,val}; |
skyscraper | 0:37dbfb036586 | 84 | bool const result = (i2c.write(i2c_addr,ar,2) == 0); |
skyscraper | 0:37dbfb036586 | 85 | if(result) { |
skyscraper | 0:37dbfb036586 | 86 | return true; |
skyscraper | 0:37dbfb036586 | 87 | } else { |
skyscraper | 0:37dbfb036586 | 88 | std::cout << " mag_write_reg failed\n"; |
skyscraper | 0:37dbfb036586 | 89 | return false; |
skyscraper | 0:37dbfb036586 | 90 | } |
skyscraper | 0:37dbfb036586 | 91 | } |
skyscraper | 0:37dbfb036586 | 92 | |
skyscraper | 0:37dbfb036586 | 93 | // Read reg at idx to result |
skyscraper | 0:37dbfb036586 | 94 | // return true if successfull |
skyscraper | 0:37dbfb036586 | 95 | bool mag_get_reg(uint8_t idx_in, uint8_t& result) |
skyscraper | 0:37dbfb036586 | 96 | { |
skyscraper | 0:37dbfb036586 | 97 | if ( mag_set_reg_idx(idx_in)) { |
skyscraper | 0:37dbfb036586 | 98 | char result1 = 0; |
skyscraper | 0:37dbfb036586 | 99 | if (i2c.read(i2c_addr,&result1,1) == 0) { |
skyscraper | 0:37dbfb036586 | 100 | result = result1; |
skyscraper | 0:37dbfb036586 | 101 | return true; |
skyscraper | 0:37dbfb036586 | 102 | } else { |
skyscraper | 0:37dbfb036586 | 103 | std::cout << "mag_get_reg read failed\n"; |
skyscraper | 0:37dbfb036586 | 104 | return false; |
skyscraper | 0:37dbfb036586 | 105 | } |
skyscraper | 0:37dbfb036586 | 106 | } else { |
skyscraper | 0:37dbfb036586 | 107 | return false; |
skyscraper | 0:37dbfb036586 | 108 | } |
skyscraper | 0:37dbfb036586 | 109 | } |
skyscraper | 0:37dbfb036586 | 110 | |
skyscraper | 0:37dbfb036586 | 111 | // Update value in reg using and and or masks |
skyscraper | 0:37dbfb036586 | 112 | // reg <-- (reg & and_val) | or_val |
skyscraper | 0:37dbfb036586 | 113 | // return true if successfull |
skyscraper | 0:37dbfb036586 | 114 | bool mag_modify_reg(uint8_t idx, uint8_t and_val, uint8_t or_val) |
skyscraper | 0:37dbfb036586 | 115 | { |
skyscraper | 0:37dbfb036586 | 116 | uint8_t cur_val = 0; |
skyscraper | 0:37dbfb036586 | 117 | if(mag_get_reg(idx,cur_val)) { |
skyscraper | 0:37dbfb036586 | 118 | uint8_t const new_val = (cur_val & and_val ) | or_val; |
skyscraper | 0:37dbfb036586 | 119 | return mag_write_reg(idx,new_val); |
skyscraper | 0:37dbfb036586 | 120 | } else { |
skyscraper | 0:37dbfb036586 | 121 | return false; |
skyscraper | 0:37dbfb036586 | 122 | } |
skyscraper | 0:37dbfb036586 | 123 | } |
skyscraper | 0:37dbfb036586 | 124 | |
skyscraper | 0:37dbfb036586 | 125 | // probe for the HMC5883 on I2C |
skyscraper | 0:37dbfb036586 | 126 | // return true if found |
skyscraper | 0:37dbfb036586 | 127 | bool mag_detected() |
skyscraper | 0:37dbfb036586 | 128 | { |
skyscraper | 0:37dbfb036586 | 129 | if ( mag_set_reg_idx(id_regA) ) { |
skyscraper | 0:37dbfb036586 | 130 | char id_input[4]; |
skyscraper | 0:37dbfb036586 | 131 | if(i2c.read(i2c_addr,id_input,3) == 0) { |
skyscraper | 0:37dbfb036586 | 132 | id_input[3] = '\0'; |
skyscraper | 0:37dbfb036586 | 133 | bool const is_hmc = (strcmp(id_input,"H43") == 0); |
skyscraper | 0:37dbfb036586 | 134 | if (is_hmc) { |
skyscraper | 0:37dbfb036586 | 135 | return true; |
skyscraper | 0:37dbfb036586 | 136 | } else { |
skyscraper | 0:37dbfb036586 | 137 | std::cout << "hmc5883 ID string didnt match\n"; |
skyscraper | 0:37dbfb036586 | 138 | return false; |
skyscraper | 0:37dbfb036586 | 139 | } |
skyscraper | 0:37dbfb036586 | 140 | } else { |
skyscraper | 0:37dbfb036586 | 141 | std::cout << "id mag read failed\n"; |
skyscraper | 0:37dbfb036586 | 142 | return false; |
skyscraper | 0:37dbfb036586 | 143 | } |
skyscraper | 0:37dbfb036586 | 144 | } else { |
skyscraper | 0:37dbfb036586 | 145 | return false; |
skyscraper | 0:37dbfb036586 | 146 | } |
skyscraper | 0:37dbfb036586 | 147 | } |
skyscraper | 0:37dbfb036586 | 148 | |
skyscraper | 0:37dbfb036586 | 149 | // terminal loop, printing message periodically |
skyscraper | 0:37dbfb036586 | 150 | void loop_forever(std::string const & str) |
skyscraper | 0:37dbfb036586 | 151 | { |
skyscraper | 0:37dbfb036586 | 152 | // stop but print error dynamically |
skyscraper | 0:37dbfb036586 | 153 | int count = 0; |
skyscraper | 0:37dbfb036586 | 154 | for (;;) { |
skyscraper | 0:37dbfb036586 | 155 | led1 = !led1; |
skyscraper | 0:37dbfb036586 | 156 | std::cout << str << " " << count++ << '\n'; |
skyscraper | 0:37dbfb036586 | 157 | ThisThread::sleep_for(1000U); |
skyscraper | 0:37dbfb036586 | 158 | } |
skyscraper | 0:37dbfb036586 | 159 | } |
skyscraper | 0:37dbfb036586 | 160 | |
skyscraper | 0:37dbfb036586 | 161 | bool mag_set_samples_average(int n_samples) |
skyscraper | 0:37dbfb036586 | 162 | { |
skyscraper | 0:37dbfb036586 | 163 | uint8_t or_val = 0; |
skyscraper | 0:37dbfb036586 | 164 | switch (n_samples) { |
skyscraper | 0:37dbfb036586 | 165 | case 1 : |
skyscraper | 0:37dbfb036586 | 166 | or_val = 0b00U << 5U; |
skyscraper | 0:37dbfb036586 | 167 | break; |
skyscraper | 0:37dbfb036586 | 168 | case 2 : |
skyscraper | 0:37dbfb036586 | 169 | or_val = 0b01U << 5U; |
skyscraper | 0:37dbfb036586 | 170 | break; |
skyscraper | 0:37dbfb036586 | 171 | case 4 : |
skyscraper | 0:37dbfb036586 | 172 | or_val = 0b10U << 5U; |
skyscraper | 0:37dbfb036586 | 173 | break; |
skyscraper | 0:37dbfb036586 | 174 | case 8 : |
skyscraper | 0:37dbfb036586 | 175 | or_val = 0b11U << 5U; |
skyscraper | 0:37dbfb036586 | 176 | break; |
skyscraper | 0:37dbfb036586 | 177 | default: |
skyscraper | 0:37dbfb036586 | 178 | std::cout << "mag_set_samples_average : invalid n_samples (" << n_samples << ")\n"; |
skyscraper | 0:37dbfb036586 | 179 | return false; |
skyscraper | 0:37dbfb036586 | 180 | } |
skyscraper | 0:37dbfb036586 | 181 | uint8_t constexpr and_val = ~(0b11 << 5U); |
skyscraper | 0:37dbfb036586 | 182 | return mag_modify_reg(cfg_regA,and_val,or_val); |
skyscraper | 0:37dbfb036586 | 183 | } |
skyscraper | 0:37dbfb036586 | 184 | |
skyscraper | 0:37dbfb036586 | 185 | /* |
skyscraper | 0:37dbfb036586 | 186 | data rate 0.75, 1.5, 3 ,7.5, 15 (Default) , 30, 75 |
skyscraper | 0:37dbfb036586 | 187 | */ |
skyscraper | 0:37dbfb036586 | 188 | |
skyscraper | 0:37dbfb036586 | 189 | namespace detail |
skyscraper | 0:37dbfb036586 | 190 | { |
skyscraper | 0:37dbfb036586 | 191 | template <int N, int D=1> struct mag_data_rate_id; |
skyscraper | 0:37dbfb036586 | 192 | // values are mag settings for each data rate |
skyscraper | 0:37dbfb036586 | 193 | template <> struct mag_data_rate_id<3,4> : std::integral_constant<uint8_t,(0b000U << 2U)> {}; |
skyscraper | 0:37dbfb036586 | 194 | template <> struct mag_data_rate_id<3,2> : std::integral_constant<uint8_t,(0b001U << 2U)> {}; |
skyscraper | 0:37dbfb036586 | 195 | template <> struct mag_data_rate_id<3> : std::integral_constant<uint8_t,(0b010U << 2U)> {}; |
skyscraper | 0:37dbfb036586 | 196 | template <> struct mag_data_rate_id<15,2> : std::integral_constant<uint8_t,(0b011U << 2U)> {}; |
skyscraper | 0:37dbfb036586 | 197 | template <> struct mag_data_rate_id<15> : std::integral_constant<uint8_t,(0b100U << 2U)> {}; |
skyscraper | 0:37dbfb036586 | 198 | template <> struct mag_data_rate_id<30> : std::integral_constant<uint8_t,(0b101U << 2U)> {}; |
skyscraper | 0:37dbfb036586 | 199 | template <> struct mag_data_rate_id<75> : std::integral_constant<uint8_t,(0b110U << 2U)> {}; |
skyscraper | 0:37dbfb036586 | 200 | } // detail |
skyscraper | 0:37dbfb036586 | 201 | |
skyscraper | 0:37dbfb036586 | 202 | template <int N, int D=1> |
skyscraper | 0:37dbfb036586 | 203 | inline bool mag_set_data_rate() |
skyscraper | 0:37dbfb036586 | 204 | { |
skyscraper | 0:37dbfb036586 | 205 | uint8_t constexpr and_val = static_cast<uint8_t>(~(0b111U << 2U)); |
skyscraper | 0:37dbfb036586 | 206 | uint8_t constexpr or_val = detail::mag_data_rate_id<N,D>::value; |
skyscraper | 0:37dbfb036586 | 207 | return mag_modify_reg(cfg_regA,and_val,or_val); |
skyscraper | 0:37dbfb036586 | 208 | } |
skyscraper | 0:37dbfb036586 | 209 | |
skyscraper | 0:37dbfb036586 | 210 | bool mag_set_positive_bias() |
skyscraper | 0:37dbfb036586 | 211 | { |
skyscraper | 0:37dbfb036586 | 212 | uint8_t constexpr and_val = static_cast<uint8_t>(~(0b11U )); |
skyscraper | 0:37dbfb036586 | 213 | uint8_t constexpr or_val = 0b01U; |
skyscraper | 0:37dbfb036586 | 214 | return mag_modify_reg(cfg_regA,and_val,or_val); |
skyscraper | 0:37dbfb036586 | 215 | } |
skyscraper | 0:37dbfb036586 | 216 | |
skyscraper | 0:37dbfb036586 | 217 | bool mag_set_negative_bias() |
skyscraper | 0:37dbfb036586 | 218 | { |
skyscraper | 0:37dbfb036586 | 219 | uint8_t constexpr and_val = static_cast<uint8_t>(~(0b11U )); |
skyscraper | 0:37dbfb036586 | 220 | uint8_t constexpr or_val = 0b10U; |
skyscraper | 0:37dbfb036586 | 221 | return mag_modify_reg(cfg_regA,and_val,or_val); |
skyscraper | 0:37dbfb036586 | 222 | } |
skyscraper | 0:37dbfb036586 | 223 | |
skyscraper | 0:37dbfb036586 | 224 | bool mag_clear_bias() |
skyscraper | 0:37dbfb036586 | 225 | { |
skyscraper | 0:37dbfb036586 | 226 | uint8_t constexpr and_val = static_cast<uint8_t>(~(0b11U )); |
skyscraper | 0:37dbfb036586 | 227 | uint8_t constexpr or_val = 0b00U; |
skyscraper | 0:37dbfb036586 | 228 | return mag_modify_reg(cfg_regA,and_val,or_val); |
skyscraper | 0:37dbfb036586 | 229 | } |
skyscraper | 0:37dbfb036586 | 230 | |
skyscraper | 0:37dbfb036586 | 231 | QUAN_QUANTITY_LITERAL(magnetic_flux_density,gauss); |
skyscraper | 0:37dbfb036586 | 232 | QUAN_QUANTITY_LITERAL(magnetic_flux_density,milli_gauss); |
skyscraper | 0:37dbfb036586 | 233 | QUAN_QUANTITY_LITERAL(magnetic_flux_density,uT); |
skyscraper | 0:37dbfb036586 | 234 | |
skyscraper | 0:37dbfb036586 | 235 | // per lsb defualt resolution |
skyscraper | 0:37dbfb036586 | 236 | quan::magnetic_flux_density::uT mag_resolution = 0.92_milli_gauss; |
skyscraper | 0:37dbfb036586 | 237 | // range before saturation |
skyscraper | 0:37dbfb036586 | 238 | quan::magnetic_flux_density::uT mag_range = 1.3_gauss; |
skyscraper | 0:37dbfb036586 | 239 | // set +- range |
skyscraper | 0:37dbfb036586 | 240 | // sets the nearest greater equal +-range to abs(range_in) |
skyscraper | 0:37dbfb036586 | 241 | bool mag_set_range(quan::magnetic_flux_density::uT const & range_in) |
skyscraper | 0:37dbfb036586 | 242 | { |
skyscraper | 0:37dbfb036586 | 243 | uint8_t or_value = 0; |
skyscraper | 0:37dbfb036586 | 244 | auto const range = abs(range_in); |
skyscraper | 0:37dbfb036586 | 245 | |
skyscraper | 0:37dbfb036586 | 246 | if ( range <= 0.88_gauss) { |
skyscraper | 0:37dbfb036586 | 247 | or_value = 0b001U << 5U ; |
skyscraper | 0:37dbfb036586 | 248 | mag_range = 0.88_gauss; |
skyscraper | 0:37dbfb036586 | 249 | mag_resolution = 0.73_milli_gauss; |
skyscraper | 0:37dbfb036586 | 250 | } else if (range <= 1.3_gauss) { |
skyscraper | 0:37dbfb036586 | 251 | or_value = 0b001U << 5U ; |
skyscraper | 0:37dbfb036586 | 252 | mag_range = 1.3_gauss; |
skyscraper | 0:37dbfb036586 | 253 | mag_resolution = 0.92_milli_gauss; |
skyscraper | 0:37dbfb036586 | 254 | } else if (range <= 1.9_gauss) { |
skyscraper | 0:37dbfb036586 | 255 | or_value = 0b010U << 5U ; |
skyscraper | 0:37dbfb036586 | 256 | mag_range = 1.9_gauss; |
skyscraper | 0:37dbfb036586 | 257 | mag_resolution = 1.22_milli_gauss; |
skyscraper | 0:37dbfb036586 | 258 | } else if (range <= 2.5_gauss) { |
skyscraper | 0:37dbfb036586 | 259 | or_value = 0b011U << 5U ; |
skyscraper | 0:37dbfb036586 | 260 | mag_range = 2.5_gauss; |
skyscraper | 0:37dbfb036586 | 261 | mag_resolution = 1.52_milli_gauss; |
skyscraper | 0:37dbfb036586 | 262 | } else if (range <= 4.0_gauss) { |
skyscraper | 0:37dbfb036586 | 263 | or_value = 0b100U << 5U ; |
skyscraper | 0:37dbfb036586 | 264 | mag_range = 4.0_gauss; |
skyscraper | 0:37dbfb036586 | 265 | mag_resolution = 2.27_milli_gauss; |
skyscraper | 0:37dbfb036586 | 266 | } else if (range <= 4.7_gauss) { |
skyscraper | 0:37dbfb036586 | 267 | or_value = 0b101U << 5U ; |
skyscraper | 0:37dbfb036586 | 268 | mag_range = 4.7_gauss; |
skyscraper | 0:37dbfb036586 | 269 | mag_resolution = 2.56_milli_gauss; |
skyscraper | 0:37dbfb036586 | 270 | } else if (range <=5.6_gauss) { |
skyscraper | 0:37dbfb036586 | 271 | or_value = 0b110U << 5U ; |
skyscraper | 0:37dbfb036586 | 272 | mag_range = 5.6_gauss; |
skyscraper | 0:37dbfb036586 | 273 | mag_resolution = 3.03_milli_gauss; |
skyscraper | 0:37dbfb036586 | 274 | } else if ( range <= 8.1_gauss) { |
skyscraper | 0:37dbfb036586 | 275 | or_value = 0b111U << 5U ; |
skyscraper | 0:37dbfb036586 | 276 | mag_range = 8.1_gauss; |
skyscraper | 0:37dbfb036586 | 277 | mag_resolution = 4.35_milli_gauss; |
skyscraper | 0:37dbfb036586 | 278 | } else { |
skyscraper | 0:37dbfb036586 | 279 | quan::magnetic_flux_density::uT constexpr max_range = 8.1_gauss; |
skyscraper | 0:37dbfb036586 | 280 | std::cout << "range too big: max +- range = " << max_range <<"\n"; |
skyscraper | 0:37dbfb036586 | 281 | return false; |
skyscraper | 0:37dbfb036586 | 282 | } |
skyscraper | 0:37dbfb036586 | 283 | uint8_t constexpr and_val = static_cast<uint8_t>(~(0b111U << 5U)); |
skyscraper | 0:37dbfb036586 | 284 | std::cout << "mag range set to : +- " << mag_range <<'\n'; |
skyscraper | 0:37dbfb036586 | 285 | return mag_modify_reg(cfg_regB,and_val,or_value); |
skyscraper | 0:37dbfb036586 | 286 | |
skyscraper | 0:37dbfb036586 | 287 | } |
skyscraper | 0:37dbfb036586 | 288 | |
skyscraper | 0:37dbfb036586 | 289 | bool mag_set_continuous_measurement_mode() |
skyscraper | 0:37dbfb036586 | 290 | { |
skyscraper | 0:37dbfb036586 | 291 | uint8_t constexpr and_val = static_cast<uint8_t>(~(0b11U )); |
skyscraper | 0:37dbfb036586 | 292 | uint8_t constexpr or_val = 0b00U; |
skyscraper | 0:37dbfb036586 | 293 | return mag_modify_reg(mode_reg,and_val,or_val); |
skyscraper | 0:37dbfb036586 | 294 | } |
skyscraper | 0:37dbfb036586 | 295 | |
skyscraper | 0:37dbfb036586 | 296 | bool mag_set_single_measurement_mode() |
skyscraper | 0:37dbfb036586 | 297 | { |
skyscraper | 0:37dbfb036586 | 298 | uint8_t constexpr and_val = static_cast<uint8_t>(~(0b11U )); |
skyscraper | 0:37dbfb036586 | 299 | uint8_t constexpr or_val = 0b01U; |
skyscraper | 0:37dbfb036586 | 300 | return mag_modify_reg(mode_reg,and_val,or_val); |
skyscraper | 0:37dbfb036586 | 301 | } |
skyscraper | 0:37dbfb036586 | 302 | |
skyscraper | 0:37dbfb036586 | 303 | bool mag_set_idle_mode() |
skyscraper | 0:37dbfb036586 | 304 | { |
skyscraper | 0:37dbfb036586 | 305 | uint8_t constexpr and_val = static_cast<uint8_t>(~(0b11U )); |
skyscraper | 0:37dbfb036586 | 306 | uint8_t constexpr or_val = 0b10U; |
skyscraper | 0:37dbfb036586 | 307 | return mag_modify_reg(mode_reg,and_val,or_val); |
skyscraper | 0:37dbfb036586 | 308 | } |
skyscraper | 0:37dbfb036586 | 309 | |
skyscraper | 0:37dbfb036586 | 310 | bool mag_data_ready() |
skyscraper | 0:37dbfb036586 | 311 | { |
skyscraper | 0:37dbfb036586 | 312 | uint8_t result = 0; |
skyscraper | 0:37dbfb036586 | 313 | if ( mag_get_reg(status_reg, result)) { |
skyscraper | 0:37dbfb036586 | 314 | return (result & 0b1U) != 0U; |
skyscraper | 0:37dbfb036586 | 315 | } else { |
skyscraper | 0:37dbfb036586 | 316 | std::cout << "mag data ready failed\n"; |
skyscraper | 0:37dbfb036586 | 317 | return false; |
skyscraper | 0:37dbfb036586 | 318 | } |
skyscraper | 0:37dbfb036586 | 319 | } |
skyscraper | 0:37dbfb036586 | 320 | |
skyscraper | 0:37dbfb036586 | 321 | bool mag_data_locked() |
skyscraper | 0:37dbfb036586 | 322 | { |
skyscraper | 0:37dbfb036586 | 323 | uint8_t result = 0; |
skyscraper | 0:37dbfb036586 | 324 | if ( mag_get_reg(status_reg, result)) { |
skyscraper | 0:37dbfb036586 | 325 | return (result & 0b10U) != 0U; |
skyscraper | 0:37dbfb036586 | 326 | } else { |
skyscraper | 0:37dbfb036586 | 327 | std::cout << "mag data locked failed\n"; |
skyscraper | 0:37dbfb036586 | 328 | return false; |
skyscraper | 0:37dbfb036586 | 329 | } |
skyscraper | 0:37dbfb036586 | 330 | } |
skyscraper | 0:37dbfb036586 | 331 | |
skyscraper | 0:37dbfb036586 | 332 | // assume mag_data_ready has returned true before call |
skyscraper | 0:37dbfb036586 | 333 | bool mag_read(quan::three_d::vect<quan::magnetic_flux_density::uT> & v) |
skyscraper | 0:37dbfb036586 | 334 | { |
skyscraper | 0:37dbfb036586 | 335 | if( mag_set_reg_idx(dout_reg)) { |
skyscraper | 0:37dbfb036586 | 336 | char arr[7]; |
skyscraper | 0:37dbfb036586 | 337 | if(i2c.read(i2c_addr,arr,7) == 0) { |
skyscraper | 0:37dbfb036586 | 338 | // TODO check status reg arr[6] |
skyscraper | 0:37dbfb036586 | 339 | // if |
skyscraper | 0:37dbfb036586 | 340 | quan::three_d::vect<int16_t> temp; |
skyscraper | 0:37dbfb036586 | 341 | temp.x = static_cast<int16_t>(arr[1]) + ( static_cast<int16_t>(arr[0]) << 8U); |
skyscraper | 0:37dbfb036586 | 342 | temp.y = static_cast<int16_t>(arr[5]) + ( static_cast<int16_t>(arr[4]) << 8U); |
skyscraper | 0:37dbfb036586 | 343 | temp.z = static_cast<int16_t>(arr[3]) + ( static_cast<int16_t>(arr[2]) << 8U); |
skyscraper | 0:37dbfb036586 | 344 | v = temp * mag_resolution; |
skyscraper | 0:37dbfb036586 | 345 | return true; |
skyscraper | 0:37dbfb036586 | 346 | } else { |
skyscraper | 0:37dbfb036586 | 347 | std::cout << "mag_read failed\n"; |
skyscraper | 0:37dbfb036586 | 348 | return false; |
skyscraper | 0:37dbfb036586 | 349 | } |
skyscraper | 0:37dbfb036586 | 350 | } else { |
skyscraper | 0:37dbfb036586 | 351 | return false; |
skyscraper | 0:37dbfb036586 | 352 | } |
skyscraper | 0:37dbfb036586 | 353 | } |
skyscraper | 0:37dbfb036586 | 354 | |
skyscraper | 0:37dbfb036586 | 355 | bool mag_do_single_measurement(quan::three_d::vect<quan::magnetic_flux_density::uT>& result) |
skyscraper | 0:37dbfb036586 | 356 | { |
skyscraper | 0:37dbfb036586 | 357 | if ( ! mag_set_single_measurement_mode()) { |
skyscraper | 0:37dbfb036586 | 358 | return false; |
skyscraper | 0:37dbfb036586 | 359 | } |
skyscraper | 0:37dbfb036586 | 360 | |
skyscraper | 0:37dbfb036586 | 361 | while (! mag_data_ready()) { |
skyscraper | 0:37dbfb036586 | 362 | ThisThread::sleep_for(5U); |
skyscraper | 0:37dbfb036586 | 363 | } |
skyscraper | 0:37dbfb036586 | 364 | return mag_read(result); |
skyscraper | 0:37dbfb036586 | 365 | } |
skyscraper | 0:37dbfb036586 | 366 | |
skyscraper | 0:37dbfb036586 | 367 | bool mag_get_offsets(quan::three_d::vect<quan::magnetic_flux_density::uT> & result) |
skyscraper | 0:37dbfb036586 | 368 | { |
skyscraper | 0:37dbfb036586 | 369 | // set single measurement mode |
skyscraper | 0:37dbfb036586 | 370 | |
skyscraper | 0:37dbfb036586 | 371 | // to prevent saturation |
skyscraper | 0:37dbfb036586 | 372 | mag_set_range(5.7_gauss); |
skyscraper | 0:37dbfb036586 | 373 | quan::three_d::vect<quan::magnetic_flux_density::uT> mSet; |
skyscraper | 0:37dbfb036586 | 374 | // throw away first |
skyscraper | 0:37dbfb036586 | 375 | mag_do_single_measurement(mSet); |
skyscraper | 0:37dbfb036586 | 376 | |
skyscraper | 0:37dbfb036586 | 377 | mag_set_positive_bias(); |
skyscraper | 0:37dbfb036586 | 378 | mag_do_single_measurement(mSet); |
skyscraper | 0:37dbfb036586 | 379 | std::cout << "mSet = " << mSet <<'\n'; |
skyscraper | 0:37dbfb036586 | 380 | |
skyscraper | 0:37dbfb036586 | 381 | mag_set_negative_bias(); |
skyscraper | 0:37dbfb036586 | 382 | quan::three_d::vect<quan::magnetic_flux_density::uT> mReset; |
skyscraper | 0:37dbfb036586 | 383 | mag_do_single_measurement(mReset); |
skyscraper | 0:37dbfb036586 | 384 | mag_clear_bias(); |
skyscraper | 0:37dbfb036586 | 385 | |
skyscraper | 0:37dbfb036586 | 386 | std::cout << "mReset = " << mReset <<'\n'; |
skyscraper | 0:37dbfb036586 | 387 | |
skyscraper | 0:37dbfb036586 | 388 | result = (mSet - mReset )/2; |
skyscraper | 0:37dbfb036586 | 389 | |
skyscraper | 0:37dbfb036586 | 390 | std::cout << "result = " << result << '\n'; |
skyscraper | 0:37dbfb036586 | 391 | |
skyscraper | 0:37dbfb036586 | 392 | return true; |
skyscraper | 0:37dbfb036586 | 393 | } |
skyscraper | 0:37dbfb036586 | 394 | |
skyscraper | 0:37dbfb036586 | 395 | }// namespace |
skyscraper | 0:37dbfb036586 | 396 | |
skyscraper | 0:37dbfb036586 | 397 | int main() |
skyscraper | 0:37dbfb036586 | 398 | { |
skyscraper | 0:37dbfb036586 | 399 | |
skyscraper | 0:37dbfb036586 | 400 | std::cout << "HMC5883 test\n"; |
skyscraper | 0:37dbfb036586 | 401 | |
skyscraper | 0:37dbfb036586 | 402 | //wait for mag to init |
skyscraper | 0:37dbfb036586 | 403 | ThisThread::sleep_for(500U); |
skyscraper | 0:37dbfb036586 | 404 | |
skyscraper | 0:37dbfb036586 | 405 | bool success = false; |
skyscraper | 0:37dbfb036586 | 406 | if ( mag_detected()) { |
skyscraper | 0:37dbfb036586 | 407 | success = true; |
skyscraper | 0:37dbfb036586 | 408 | std::cout << "Detected a HMC5883\n"; |
skyscraper | 0:37dbfb036586 | 409 | } else { |
skyscraper | 0:37dbfb036586 | 410 | loop_forever("Failed to detect HMC5883"); |
skyscraper | 0:37dbfb036586 | 411 | } |
skyscraper | 0:37dbfb036586 | 412 | /* |
skyscraper | 0:37dbfb036586 | 413 | |
skyscraper | 0:37dbfb036586 | 414 | for ( int i = 0; i < 5; ++i){ |
skyscraper | 0:37dbfb036586 | 415 | mag_get_offsets(offsets); |
skyscraper | 0:37dbfb036586 | 416 | ThisThread::sleep_for(100U); |
skyscraper | 0:37dbfb036586 | 417 | } |
skyscraper | 0:37dbfb036586 | 418 | */ |
skyscraper | 0:37dbfb036586 | 419 | // N.b after offsets removed mag was reading around 33.6 uT, so not bad! |
skyscraper | 0:37dbfb036586 | 420 | constexpr auto earth_magnetic_field_flux_density = 31.869_uT; |
skyscraper | 0:37dbfb036586 | 421 | |
skyscraper | 0:37dbfb036586 | 422 | success = |
skyscraper | 0:37dbfb036586 | 423 | mag_set_samples_average(8) && |
skyscraper | 0:37dbfb036586 | 424 | mag_set_data_rate<3,4>() && |
skyscraper | 0:37dbfb036586 | 425 | // N.B if offsets are large then may need to set larger range |
skyscraper | 0:37dbfb036586 | 426 | // prob need to cycle through looking for best range |
skyscraper | 0:37dbfb036586 | 427 | // so this may not work |
skyscraper | 0:37dbfb036586 | 428 | mag_set_range( earth_magnetic_field_flux_density * 2U); |
skyscraper | 0:37dbfb036586 | 429 | |
skyscraper | 0:37dbfb036586 | 430 | if ( !success) { |
skyscraper | 0:37dbfb036586 | 431 | loop_forever("HMC5883 setup failed"); |
skyscraper | 0:37dbfb036586 | 432 | } |
skyscraper | 0:37dbfb036586 | 433 | |
skyscraper | 0:37dbfb036586 | 434 | // calculate the offsets dynamically by averaging |
skyscraper | 0:37dbfb036586 | 435 | // the min and max over time |
skyscraper | 0:37dbfb036586 | 436 | quan::three_d::vect<quan::magnetic_flux_density::uT> voffsets; |
skyscraper | 0:37dbfb036586 | 437 | quan::three_d::vect<quan::magnetic_flux_density::uT> vmax; |
skyscraper | 0:37dbfb036586 | 438 | quan::three_d::vect<quan::magnetic_flux_density::uT> vmin; |
skyscraper | 0:37dbfb036586 | 439 | |
skyscraper | 0:37dbfb036586 | 440 | for (;;) { |
skyscraper | 0:37dbfb036586 | 441 | |
skyscraper | 0:37dbfb036586 | 442 | quan::three_d::vect<quan::magnetic_flux_density::uT> values; |
skyscraper | 0:37dbfb036586 | 443 | if(mag_do_single_measurement(values)) { |
skyscraper | 0:37dbfb036586 | 444 | |
skyscraper | 0:37dbfb036586 | 445 | vmax.x = quan::max(vmax.x,values.x); |
skyscraper | 0:37dbfb036586 | 446 | vmax.y = quan::max(vmax.y,values.y); |
skyscraper | 0:37dbfb036586 | 447 | vmax.z = quan::max(vmax.z,values.z); |
skyscraper | 0:37dbfb036586 | 448 | |
skyscraper | 0:37dbfb036586 | 449 | vmin.x = quan::min(vmin.x,values.x); |
skyscraper | 0:37dbfb036586 | 450 | vmin.y = quan::min(vmin.y,values.y); |
skyscraper | 0:37dbfb036586 | 451 | vmin.z = quan::min(vmin.z,values.z); |
skyscraper | 0:37dbfb036586 | 452 | |
skyscraper | 0:37dbfb036586 | 453 | voffsets = (vmin + vmax)/2.f; |
skyscraper | 0:37dbfb036586 | 454 | |
skyscraper | 0:37dbfb036586 | 455 | values -= voffsets; |
skyscraper | 0:37dbfb036586 | 456 | |
skyscraper | 0:37dbfb036586 | 457 | std::cout << "val = " << values << '\n'; |
skyscraper | 0:37dbfb036586 | 458 | std::cout << "off = " << voffsets << "\n\n"; |
skyscraper | 0:37dbfb036586 | 459 | } else { |
skyscraper | 0:37dbfb036586 | 460 | std::cout << "mag read failed\n"; |
skyscraper | 0:37dbfb036586 | 461 | } |
skyscraper | 0:37dbfb036586 | 462 | ThisThread::sleep_for(10U); |
skyscraper | 0:37dbfb036586 | 463 | } |
skyscraper | 0:37dbfb036586 | 464 | } |