Andy Little / Mbed OS mag_test3

First draft HMC5883 magnetometer sensor using physical quantities, outputting via serial port using std::cout on mbed os 5

main.cpp@1:e11ab941748b, 2020-03-22 (annotated)

Committer:
skyscraper
Date:
Sun Mar 22 09:46:50 2020 +0000
Revision:
1:e11ab941748b
Parent:
0:37dbfb036586
Child:
2:9ffb2f18756b
change led to led2 and tested on NUCLEO-F72RB. Since F072 doesnt have a led1

Who changed what in which revision?

UserRevisionLine numberNew 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 1:e11ab941748b 17 DigitalOut led2(LED2);
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 1:e11ab941748b 155 led2 = 1;
skyscraper 0:37dbfb036586 156 std::cout << str << " " << count++ << '\n';
skyscraper 1:e11ab941748b 157 ThisThread::sleep_for(200U);
skyscraper 1:e11ab941748b 158 led2 = 0;
skyscraper 1:e11ab941748b 159 ThisThread::sleep_for(800U);
skyscraper 0:37dbfb036586 160 }
skyscraper 0:37dbfb036586 161 }
skyscraper 0:37dbfb036586 162
skyscraper 0:37dbfb036586 163 bool mag_set_samples_average(int n_samples)
skyscraper 0:37dbfb036586 164 {
skyscraper 0:37dbfb036586 165 uint8_t or_val = 0;
skyscraper 0:37dbfb036586 166 switch (n_samples) {
skyscraper 0:37dbfb036586 167 case 1 :
skyscraper 0:37dbfb036586 168 or_val = 0b00U << 5U;
skyscraper 0:37dbfb036586 169 break;
skyscraper 0:37dbfb036586 170 case 2 :
skyscraper 0:37dbfb036586 171 or_val = 0b01U << 5U;
skyscraper 0:37dbfb036586 172 break;
skyscraper 0:37dbfb036586 173 case 4 :
skyscraper 0:37dbfb036586 174 or_val = 0b10U << 5U;
skyscraper 0:37dbfb036586 175 break;
skyscraper 0:37dbfb036586 176 case 8 :
skyscraper 0:37dbfb036586 177 or_val = 0b11U << 5U;
skyscraper 0:37dbfb036586 178 break;
skyscraper 0:37dbfb036586 179 default:
skyscraper 0:37dbfb036586 180 std::cout << "mag_set_samples_average : invalid n_samples (" << n_samples << ")\n";
skyscraper 0:37dbfb036586 181 return false;
skyscraper 0:37dbfb036586 182 }
skyscraper 0:37dbfb036586 183 uint8_t constexpr and_val = ~(0b11 << 5U);
skyscraper 0:37dbfb036586 184 return mag_modify_reg(cfg_regA,and_val,or_val);
skyscraper 0:37dbfb036586 185 }
skyscraper 0:37dbfb036586 186
skyscraper 0:37dbfb036586 187 /*
skyscraper 0:37dbfb036586 188 data rate 0.75, 1.5, 3 ,7.5, 15 (Default) , 30, 75
skyscraper 0:37dbfb036586 189 */
skyscraper 0:37dbfb036586 190
skyscraper 0:37dbfb036586 191 namespace detail
skyscraper 0:37dbfb036586 192 {
skyscraper 0:37dbfb036586 193 template <int N, int D=1> struct mag_data_rate_id;
skyscraper 0:37dbfb036586 194 // values are mag settings for each data rate
skyscraper 0:37dbfb036586 195 template <> struct mag_data_rate_id<3,4> : std::integral_constant<uint8_t,(0b000U << 2U)> {};
skyscraper 0:37dbfb036586 196 template <> struct mag_data_rate_id<3,2> : std::integral_constant<uint8_t,(0b001U << 2U)> {};
skyscraper 0:37dbfb036586 197 template <> struct mag_data_rate_id<3> : std::integral_constant<uint8_t,(0b010U << 2U)> {};
skyscraper 0:37dbfb036586 198 template <> struct mag_data_rate_id<15,2> : std::integral_constant<uint8_t,(0b011U << 2U)> {};
skyscraper 0:37dbfb036586 199 template <> struct mag_data_rate_id<15> : std::integral_constant<uint8_t,(0b100U << 2U)> {};
skyscraper 0:37dbfb036586 200 template <> struct mag_data_rate_id<30> : std::integral_constant<uint8_t,(0b101U << 2U)> {};
skyscraper 0:37dbfb036586 201 template <> struct mag_data_rate_id<75> : std::integral_constant<uint8_t,(0b110U << 2U)> {};
skyscraper 0:37dbfb036586 202 } // detail
skyscraper 0:37dbfb036586 203
skyscraper 0:37dbfb036586 204 template <int N, int D=1>
skyscraper 0:37dbfb036586 205 inline bool mag_set_data_rate()
skyscraper 0:37dbfb036586 206 {
skyscraper 0:37dbfb036586 207 uint8_t constexpr and_val = static_cast<uint8_t>(~(0b111U << 2U));
skyscraper 0:37dbfb036586 208 uint8_t constexpr or_val = detail::mag_data_rate_id<N,D>::value;
skyscraper 0:37dbfb036586 209 return mag_modify_reg(cfg_regA,and_val,or_val);
skyscraper 0:37dbfb036586 210 }
skyscraper 0:37dbfb036586 211
skyscraper 0:37dbfb036586 212 bool mag_set_positive_bias()
skyscraper 0:37dbfb036586 213 {
skyscraper 0:37dbfb036586 214 uint8_t constexpr and_val = static_cast<uint8_t>(~(0b11U ));
skyscraper 0:37dbfb036586 215 uint8_t constexpr or_val = 0b01U;
skyscraper 0:37dbfb036586 216 return mag_modify_reg(cfg_regA,and_val,or_val);
skyscraper 0:37dbfb036586 217 }
skyscraper 0:37dbfb036586 218
skyscraper 0:37dbfb036586 219 bool mag_set_negative_bias()
skyscraper 0:37dbfb036586 220 {
skyscraper 0:37dbfb036586 221 uint8_t constexpr and_val = static_cast<uint8_t>(~(0b11U ));
skyscraper 0:37dbfb036586 222 uint8_t constexpr or_val = 0b10U;
skyscraper 0:37dbfb036586 223 return mag_modify_reg(cfg_regA,and_val,or_val);
skyscraper 0:37dbfb036586 224 }
skyscraper 0:37dbfb036586 225
skyscraper 0:37dbfb036586 226 bool mag_clear_bias()
skyscraper 0:37dbfb036586 227 {
skyscraper 0:37dbfb036586 228 uint8_t constexpr and_val = static_cast<uint8_t>(~(0b11U ));
skyscraper 0:37dbfb036586 229 uint8_t constexpr or_val = 0b00U;
skyscraper 0:37dbfb036586 230 return mag_modify_reg(cfg_regA,and_val,or_val);
skyscraper 0:37dbfb036586 231 }
skyscraper 0:37dbfb036586 232
skyscraper 0:37dbfb036586 233 QUAN_QUANTITY_LITERAL(magnetic_flux_density,gauss);
skyscraper 0:37dbfb036586 234 QUAN_QUANTITY_LITERAL(magnetic_flux_density,milli_gauss);
skyscraper 0:37dbfb036586 235 QUAN_QUANTITY_LITERAL(magnetic_flux_density,uT);
skyscraper 0:37dbfb036586 236
skyscraper 0:37dbfb036586 237 // per lsb defualt resolution
skyscraper 0:37dbfb036586 238 quan::magnetic_flux_density::uT mag_resolution = 0.92_milli_gauss;
skyscraper 0:37dbfb036586 239 // range before saturation
skyscraper 0:37dbfb036586 240 quan::magnetic_flux_density::uT mag_range = 1.3_gauss;
skyscraper 0:37dbfb036586 241 // set +- range
skyscraper 0:37dbfb036586 242 // sets the nearest greater equal +-range to abs(range_in)
skyscraper 0:37dbfb036586 243 bool mag_set_range(quan::magnetic_flux_density::uT const & range_in)
skyscraper 0:37dbfb036586 244 {
skyscraper 0:37dbfb036586 245 uint8_t or_value = 0;
skyscraper 0:37dbfb036586 246 auto const range = abs(range_in);
skyscraper 0:37dbfb036586 247
skyscraper 0:37dbfb036586 248 if ( range <= 0.88_gauss) {
skyscraper 0:37dbfb036586 249 or_value = 0b001U << 5U ;
skyscraper 0:37dbfb036586 250 mag_range = 0.88_gauss;
skyscraper 0:37dbfb036586 251 mag_resolution = 0.73_milli_gauss;
skyscraper 0:37dbfb036586 252 } else if (range <= 1.3_gauss) {
skyscraper 0:37dbfb036586 253 or_value = 0b001U << 5U ;
skyscraper 0:37dbfb036586 254 mag_range = 1.3_gauss;
skyscraper 0:37dbfb036586 255 mag_resolution = 0.92_milli_gauss;
skyscraper 0:37dbfb036586 256 } else if (range <= 1.9_gauss) {
skyscraper 0:37dbfb036586 257 or_value = 0b010U << 5U ;
skyscraper 0:37dbfb036586 258 mag_range = 1.9_gauss;
skyscraper 0:37dbfb036586 259 mag_resolution = 1.22_milli_gauss;
skyscraper 0:37dbfb036586 260 } else if (range <= 2.5_gauss) {
skyscraper 0:37dbfb036586 261 or_value = 0b011U << 5U ;
skyscraper 0:37dbfb036586 262 mag_range = 2.5_gauss;
skyscraper 0:37dbfb036586 263 mag_resolution = 1.52_milli_gauss;
skyscraper 0:37dbfb036586 264 } else if (range <= 4.0_gauss) {
skyscraper 0:37dbfb036586 265 or_value = 0b100U << 5U ;
skyscraper 0:37dbfb036586 266 mag_range = 4.0_gauss;
skyscraper 0:37dbfb036586 267 mag_resolution = 2.27_milli_gauss;
skyscraper 0:37dbfb036586 268 } else if (range <= 4.7_gauss) {
skyscraper 0:37dbfb036586 269 or_value = 0b101U << 5U ;
skyscraper 0:37dbfb036586 270 mag_range = 4.7_gauss;
skyscraper 0:37dbfb036586 271 mag_resolution = 2.56_milli_gauss;
skyscraper 0:37dbfb036586 272 } else if (range <=5.6_gauss) {
skyscraper 0:37dbfb036586 273 or_value = 0b110U << 5U ;
skyscraper 0:37dbfb036586 274 mag_range = 5.6_gauss;
skyscraper 0:37dbfb036586 275 mag_resolution = 3.03_milli_gauss;
skyscraper 0:37dbfb036586 276 } else if ( range <= 8.1_gauss) {
skyscraper 0:37dbfb036586 277 or_value = 0b111U << 5U ;
skyscraper 0:37dbfb036586 278 mag_range = 8.1_gauss;
skyscraper 0:37dbfb036586 279 mag_resolution = 4.35_milli_gauss;
skyscraper 0:37dbfb036586 280 } else {
skyscraper 0:37dbfb036586 281 quan::magnetic_flux_density::uT constexpr max_range = 8.1_gauss;
skyscraper 0:37dbfb036586 282 std::cout << "range too big: max +- range = " << max_range <<"\n";
skyscraper 0:37dbfb036586 283 return false;
skyscraper 0:37dbfb036586 284 }
skyscraper 0:37dbfb036586 285 uint8_t constexpr and_val = static_cast<uint8_t>(~(0b111U << 5U));
skyscraper 0:37dbfb036586 286 std::cout << "mag range set to : +- " << mag_range <<'\n';
skyscraper 0:37dbfb036586 287 return mag_modify_reg(cfg_regB,and_val,or_value);
skyscraper 0:37dbfb036586 288
skyscraper 0:37dbfb036586 289 }
skyscraper 0:37dbfb036586 290
skyscraper 0:37dbfb036586 291 bool mag_set_continuous_measurement_mode()
skyscraper 0:37dbfb036586 292 {
skyscraper 0:37dbfb036586 293 uint8_t constexpr and_val = static_cast<uint8_t>(~(0b11U ));
skyscraper 0:37dbfb036586 294 uint8_t constexpr or_val = 0b00U;
skyscraper 0:37dbfb036586 295 return mag_modify_reg(mode_reg,and_val,or_val);
skyscraper 0:37dbfb036586 296 }
skyscraper 0:37dbfb036586 297
skyscraper 0:37dbfb036586 298 bool mag_set_single_measurement_mode()
skyscraper 0:37dbfb036586 299 {
skyscraper 0:37dbfb036586 300 uint8_t constexpr and_val = static_cast<uint8_t>(~(0b11U ));
skyscraper 0:37dbfb036586 301 uint8_t constexpr or_val = 0b01U;
skyscraper 0:37dbfb036586 302 return mag_modify_reg(mode_reg,and_val,or_val);
skyscraper 0:37dbfb036586 303 }
skyscraper 0:37dbfb036586 304
skyscraper 0:37dbfb036586 305 bool mag_set_idle_mode()
skyscraper 0:37dbfb036586 306 {
skyscraper 0:37dbfb036586 307 uint8_t constexpr and_val = static_cast<uint8_t>(~(0b11U ));
skyscraper 0:37dbfb036586 308 uint8_t constexpr or_val = 0b10U;
skyscraper 0:37dbfb036586 309 return mag_modify_reg(mode_reg,and_val,or_val);
skyscraper 0:37dbfb036586 310 }
skyscraper 0:37dbfb036586 311
skyscraper 0:37dbfb036586 312 bool mag_data_ready()
skyscraper 0:37dbfb036586 313 {
skyscraper 0:37dbfb036586 314 uint8_t result = 0;
skyscraper 0:37dbfb036586 315 if ( mag_get_reg(status_reg, result)) {
skyscraper 0:37dbfb036586 316 return (result & 0b1U) != 0U;
skyscraper 0:37dbfb036586 317 } else {
skyscraper 0:37dbfb036586 318 std::cout << "mag data ready failed\n";
skyscraper 0:37dbfb036586 319 return false;
skyscraper 0:37dbfb036586 320 }
skyscraper 0:37dbfb036586 321 }
skyscraper 0:37dbfb036586 322
skyscraper 0:37dbfb036586 323 bool mag_data_locked()
skyscraper 0:37dbfb036586 324 {
skyscraper 0:37dbfb036586 325 uint8_t result = 0;
skyscraper 0:37dbfb036586 326 if ( mag_get_reg(status_reg, result)) {
skyscraper 0:37dbfb036586 327 return (result & 0b10U) != 0U;
skyscraper 0:37dbfb036586 328 } else {
skyscraper 0:37dbfb036586 329 std::cout << "mag data locked failed\n";
skyscraper 0:37dbfb036586 330 return false;
skyscraper 0:37dbfb036586 331 }
skyscraper 0:37dbfb036586 332 }
skyscraper 0:37dbfb036586 333
skyscraper 0:37dbfb036586 334 // assume mag_data_ready has returned true before call
skyscraper 0:37dbfb036586 335 bool mag_read(quan::three_d::vect<quan::magnetic_flux_density::uT> & v)
skyscraper 0:37dbfb036586 336 {
skyscraper 0:37dbfb036586 337 if( mag_set_reg_idx(dout_reg)) {
skyscraper 0:37dbfb036586 338 char arr[7];
skyscraper 0:37dbfb036586 339 if(i2c.read(i2c_addr,arr,7) == 0) {
skyscraper 0:37dbfb036586 340 // TODO check status reg arr[6]
skyscraper 0:37dbfb036586 341 // if
skyscraper 0:37dbfb036586 342 quan::three_d::vect<int16_t> temp;
skyscraper 0:37dbfb036586 343 temp.x = static_cast<int16_t>(arr[1]) + ( static_cast<int16_t>(arr[0]) << 8U);
skyscraper 0:37dbfb036586 344 temp.y = static_cast<int16_t>(arr[5]) + ( static_cast<int16_t>(arr[4]) << 8U);
skyscraper 0:37dbfb036586 345 temp.z = static_cast<int16_t>(arr[3]) + ( static_cast<int16_t>(arr[2]) << 8U);
skyscraper 0:37dbfb036586 346 v = temp * mag_resolution;
skyscraper 0:37dbfb036586 347 return true;
skyscraper 0:37dbfb036586 348 } else {
skyscraper 0:37dbfb036586 349 std::cout << "mag_read failed\n";
skyscraper 0:37dbfb036586 350 return false;
skyscraper 0:37dbfb036586 351 }
skyscraper 0:37dbfb036586 352 } else {
skyscraper 0:37dbfb036586 353 return false;
skyscraper 0:37dbfb036586 354 }
skyscraper 0:37dbfb036586 355 }
skyscraper 0:37dbfb036586 356
skyscraper 0:37dbfb036586 357 bool mag_do_single_measurement(quan::three_d::vect<quan::magnetic_flux_density::uT>& result)
skyscraper 0:37dbfb036586 358 {
skyscraper 0:37dbfb036586 359 if ( ! mag_set_single_measurement_mode()) {
skyscraper 0:37dbfb036586 360 return false;
skyscraper 0:37dbfb036586 361 }
skyscraper 0:37dbfb036586 362
skyscraper 0:37dbfb036586 363 while (! mag_data_ready()) {
skyscraper 0:37dbfb036586 364 ThisThread::sleep_for(5U);
skyscraper 0:37dbfb036586 365 }
skyscraper 0:37dbfb036586 366 return mag_read(result);
skyscraper 0:37dbfb036586 367 }
skyscraper 0:37dbfb036586 368
skyscraper 0:37dbfb036586 369 bool mag_get_offsets(quan::three_d::vect<quan::magnetic_flux_density::uT> & result)
skyscraper 0:37dbfb036586 370 {
skyscraper 0:37dbfb036586 371 // set single measurement mode
skyscraper 0:37dbfb036586 372
skyscraper 0:37dbfb036586 373 // to prevent saturation
skyscraper 0:37dbfb036586 374 mag_set_range(5.7_gauss);
skyscraper 0:37dbfb036586 375 quan::three_d::vect<quan::magnetic_flux_density::uT> mSet;
skyscraper 0:37dbfb036586 376 // throw away first
skyscraper 0:37dbfb036586 377 mag_do_single_measurement(mSet);
skyscraper 0:37dbfb036586 378
skyscraper 0:37dbfb036586 379 mag_set_positive_bias();
skyscraper 0:37dbfb036586 380 mag_do_single_measurement(mSet);
skyscraper 0:37dbfb036586 381 std::cout << "mSet = " << mSet <<'\n';
skyscraper 0:37dbfb036586 382
skyscraper 0:37dbfb036586 383 mag_set_negative_bias();
skyscraper 0:37dbfb036586 384 quan::three_d::vect<quan::magnetic_flux_density::uT> mReset;
skyscraper 0:37dbfb036586 385 mag_do_single_measurement(mReset);
skyscraper 0:37dbfb036586 386 mag_clear_bias();
skyscraper 0:37dbfb036586 387
skyscraper 0:37dbfb036586 388 std::cout << "mReset = " << mReset <<'\n';
skyscraper 0:37dbfb036586 389
skyscraper 0:37dbfb036586 390 result = (mSet - mReset )/2;
skyscraper 0:37dbfb036586 391
skyscraper 0:37dbfb036586 392 std::cout << "result = " << result << '\n';
skyscraper 0:37dbfb036586 393
skyscraper 0:37dbfb036586 394 return true;
skyscraper 0:37dbfb036586 395 }
skyscraper 0:37dbfb036586 396
skyscraper 0:37dbfb036586 397 }// namespace
skyscraper 0:37dbfb036586 398
skyscraper 0:37dbfb036586 399 int main()
skyscraper 0:37dbfb036586 400 {
skyscraper 0:37dbfb036586 401
skyscraper 0:37dbfb036586 402 std::cout << "HMC5883 test\n";
skyscraper 0:37dbfb036586 403
skyscraper 0:37dbfb036586 404 //wait for mag to init
skyscraper 0:37dbfb036586 405 ThisThread::sleep_for(500U);
skyscraper 0:37dbfb036586 406
skyscraper 0:37dbfb036586 407 bool success = false;
skyscraper 0:37dbfb036586 408 if ( mag_detected()) {
skyscraper 0:37dbfb036586 409 success = true;
skyscraper 0:37dbfb036586 410 std::cout << "Detected a HMC5883\n";
skyscraper 0:37dbfb036586 411 } else {
skyscraper 0:37dbfb036586 412 loop_forever("Failed to detect HMC5883");
skyscraper 0:37dbfb036586 413 }
skyscraper 0:37dbfb036586 414 /*
skyscraper 0:37dbfb036586 415
skyscraper 0:37dbfb036586 416 for ( int i = 0; i < 5; ++i){
skyscraper 0:37dbfb036586 417 mag_get_offsets(offsets);
skyscraper 0:37dbfb036586 418 ThisThread::sleep_for(100U);
skyscraper 0:37dbfb036586 419 }
skyscraper 0:37dbfb036586 420 */
skyscraper 0:37dbfb036586 421 // N.b after offsets removed mag was reading around 33.6 uT, so not bad!
skyscraper 0:37dbfb036586 422 constexpr auto earth_magnetic_field_flux_density = 31.869_uT;
skyscraper 0:37dbfb036586 423
skyscraper 0:37dbfb036586 424 success =
skyscraper 0:37dbfb036586 425 mag_set_samples_average(8) &&
skyscraper 0:37dbfb036586 426 mag_set_data_rate<3,4>() &&
skyscraper 0:37dbfb036586 427 // N.B if offsets are large then may need to set larger range
skyscraper 0:37dbfb036586 428 // prob need to cycle through looking for best range
skyscraper 0:37dbfb036586 429 // so this may not work
skyscraper 0:37dbfb036586 430 mag_set_range( earth_magnetic_field_flux_density * 2U);
skyscraper 0:37dbfb036586 431
skyscraper 0:37dbfb036586 432 if ( !success) {
skyscraper 0:37dbfb036586 433 loop_forever("HMC5883 setup failed");
skyscraper 0:37dbfb036586 434 }
skyscraper 0:37dbfb036586 435
skyscraper 0:37dbfb036586 436 // calculate the offsets dynamically by averaging
skyscraper 0:37dbfb036586 437 // the min and max over time
skyscraper 0:37dbfb036586 438 quan::three_d::vect<quan::magnetic_flux_density::uT> voffsets;
skyscraper 0:37dbfb036586 439 quan::three_d::vect<quan::magnetic_flux_density::uT> vmax;
skyscraper 0:37dbfb036586 440 quan::three_d::vect<quan::magnetic_flux_density::uT> vmin;
skyscraper 0:37dbfb036586 441
skyscraper 1:e11ab941748b 442 auto prev = Kernel::get_ms_count();
skyscraper 0:37dbfb036586 443 for (;;) {
skyscraper 0:37dbfb036586 444
skyscraper 0:37dbfb036586 445 quan::three_d::vect<quan::magnetic_flux_density::uT> values;
skyscraper 0:37dbfb036586 446 if(mag_do_single_measurement(values)) {
skyscraper 0:37dbfb036586 447
skyscraper 0:37dbfb036586 448 vmax.x = quan::max(vmax.x,values.x);
skyscraper 0:37dbfb036586 449 vmax.y = quan::max(vmax.y,values.y);
skyscraper 0:37dbfb036586 450 vmax.z = quan::max(vmax.z,values.z);
skyscraper 0:37dbfb036586 451
skyscraper 0:37dbfb036586 452 vmin.x = quan::min(vmin.x,values.x);
skyscraper 0:37dbfb036586 453 vmin.y = quan::min(vmin.y,values.y);
skyscraper 0:37dbfb036586 454 vmin.z = quan::min(vmin.z,values.z);
skyscraper 0:37dbfb036586 455
skyscraper 0:37dbfb036586 456 voffsets = (vmin + vmax)/2.f;
skyscraper 0:37dbfb036586 457
skyscraper 0:37dbfb036586 458 values -= voffsets;
skyscraper 0:37dbfb036586 459
skyscraper 0:37dbfb036586 460 std::cout << "val = " << values << '\n';
skyscraper 0:37dbfb036586 461 std::cout << "off = " << voffsets << "\n\n";
skyscraper 0:37dbfb036586 462 } else {
skyscraper 0:37dbfb036586 463 std::cout << "mag read failed\n";
skyscraper 0:37dbfb036586 464 }
skyscraper 0:37dbfb036586 465 ThisThread::sleep_for(10U);
skyscraper 1:e11ab941748b 466 auto now = Kernel::get_ms_count();
skyscraper 1:e11ab941748b 467 if (( now - prev)> 500){
skyscraper 1:e11ab941748b 468 prev = now;
skyscraper 1:e11ab941748b 469 led2 = ! led2;
skyscraper 1:e11ab941748b 470 }
skyscraper 0:37dbfb036586 471 }
skyscraper 0:37dbfb036586 472 }