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

hmc5883.cpp@2:9ffb2f18756b, 2020-03-24 (annotated)

Committer:
skyscraper
Date:
Tue Mar 24 15:21:34 2020 +0000
Revision:
2:9ffb2f18756b
Child:
3:2834be4e10ef
move hmc5883 code to separate source and header file

Who changed what in which revision?

UserRevisionLine numberNew contents of line
skyscraper 2:9ffb2f18756b 1
skyscraper 2:9ffb2f18756b 2
skyscraper 2:9ffb2f18756b 3 #include "mbed.h"
skyscraper 2:9ffb2f18756b 4 #include <iostream>
skyscraper 2:9ffb2f18756b 5 #include <quan/out/magnetic_flux_density.hpp>
skyscraper 2:9ffb2f18756b 6 #include <quan/three_d/out/vect.hpp>
skyscraper 2:9ffb2f18756b 7
skyscraper 2:9ffb2f18756b 8 namespace {
skyscraper 2:9ffb2f18756b 9 /*
skyscraper 2:9ffb2f18756b 10 00 Configuration Register A R/W
skyscraper 2:9ffb2f18756b 11 01 Configuration Register B R/W
skyscraper 2:9ffb2f18756b 12 02 Mode Register R/W
skyscraper 2:9ffb2f18756b 13 03 Data Output X MSB Register R
skyscraper 2:9ffb2f18756b 14 04 Data Output X LSB Register R
skyscraper 2:9ffb2f18756b 15 05 Data Output Z MSB Register R
skyscraper 2:9ffb2f18756b 16 06 Data Output Z LSB Register R
skyscraper 2:9ffb2f18756b 17 07 Data Output Y MSB Register R
skyscraper 2:9ffb2f18756b 18 08 Data Output Y LSB Register R
skyscraper 2:9ffb2f18756b 19 09 Status Register R
skyscraper 2:9ffb2f18756b 20 10 Identification Register A R
skyscraper 2:9ffb2f18756b 21 11 Identification Register B R
skyscraper 2:9ffb2f18756b 22 12 Identification Register C R
skyscraper 2:9ffb2f18756b 23 */
skyscraper 2:9ffb2f18756b 24
skyscraper 2:9ffb2f18756b 25 I2C i2c(I2C_SDA, I2C_SCL );
skyscraper 2:9ffb2f18756b 26 constexpr uint8_t i2c_addr = 0x3D;
skyscraper 2:9ffb2f18756b 27 constexpr char cfg_regA = 0;
skyscraper 2:9ffb2f18756b 28 constexpr char cfg_regB = 1;
skyscraper 2:9ffb2f18756b 29 constexpr char mode_reg = 2;
skyscraper 2:9ffb2f18756b 30 constexpr char dout_reg = 3;
skyscraper 2:9ffb2f18756b 31 constexpr char status_reg = 9;
skyscraper 2:9ffb2f18756b 32 constexpr char id_regA = 10U;
skyscraper 2:9ffb2f18756b 33
skyscraper 2:9ffb2f18756b 34 // Set reg index to idx_in
skyscraper 2:9ffb2f18756b 35 // return true if successful
skyscraper 2:9ffb2f18756b 36 bool mag_set_reg_idx(uint8_t idx_in)
skyscraper 2:9ffb2f18756b 37 {
skyscraper 2:9ffb2f18756b 38 char const idx = static_cast<char>(idx_in);
skyscraper 2:9ffb2f18756b 39 bool const result = (i2c.write(i2c_addr,&idx,1) == 0);
skyscraper 2:9ffb2f18756b 40 if(result) {
skyscraper 2:9ffb2f18756b 41 return true;
skyscraper 2:9ffb2f18756b 42 } else {
skyscraper 2:9ffb2f18756b 43 std::cout << "mag_set_reg_idx failed\n";
skyscraper 2:9ffb2f18756b 44 return false;
skyscraper 2:9ffb2f18756b 45 }
skyscraper 2:9ffb2f18756b 46 }
skyscraper 2:9ffb2f18756b 47
skyscraper 2:9ffb2f18756b 48 // Write reg at idx with val
skyscraper 2:9ffb2f18756b 49 // return true if successful
skyscraper 2:9ffb2f18756b 50 bool mag_write_reg(uint8_t idx, uint8_t val)
skyscraper 2:9ffb2f18756b 51 {
skyscraper 2:9ffb2f18756b 52 char ar[2] = {idx,val};
skyscraper 2:9ffb2f18756b 53 bool const result = (i2c.write(i2c_addr,ar,2) == 0);
skyscraper 2:9ffb2f18756b 54 if(result) {
skyscraper 2:9ffb2f18756b 55 return true;
skyscraper 2:9ffb2f18756b 56 } else {
skyscraper 2:9ffb2f18756b 57 std::cout << " mag_write_reg failed\n";
skyscraper 2:9ffb2f18756b 58 return false;
skyscraper 2:9ffb2f18756b 59 }
skyscraper 2:9ffb2f18756b 60 }
skyscraper 2:9ffb2f18756b 61
skyscraper 2:9ffb2f18756b 62 // Read reg at idx to result
skyscraper 2:9ffb2f18756b 63 // return true if successfull
skyscraper 2:9ffb2f18756b 64 bool mag_get_reg(uint8_t idx_in, uint8_t& result)
skyscraper 2:9ffb2f18756b 65 {
skyscraper 2:9ffb2f18756b 66 if ( mag_set_reg_idx(idx_in)) {
skyscraper 2:9ffb2f18756b 67 char result1 = 0;
skyscraper 2:9ffb2f18756b 68 if (i2c.read(i2c_addr,&result1,1) == 0) {
skyscraper 2:9ffb2f18756b 69 result = result1;
skyscraper 2:9ffb2f18756b 70 return true;
skyscraper 2:9ffb2f18756b 71 } else {
skyscraper 2:9ffb2f18756b 72 std::cout << "mag_get_reg read failed\n";
skyscraper 2:9ffb2f18756b 73 return false;
skyscraper 2:9ffb2f18756b 74 }
skyscraper 2:9ffb2f18756b 75 } else {
skyscraper 2:9ffb2f18756b 76 return false;
skyscraper 2:9ffb2f18756b 77 }
skyscraper 2:9ffb2f18756b 78 }
skyscraper 2:9ffb2f18756b 79
skyscraper 2:9ffb2f18756b 80 // Update value in reg using and and or masks
skyscraper 2:9ffb2f18756b 81 // reg <-- (reg & and_val) | or_val
skyscraper 2:9ffb2f18756b 82 // return true if successfull
skyscraper 2:9ffb2f18756b 83 bool mag_modify_reg(uint8_t idx, uint8_t and_val, uint8_t or_val)
skyscraper 2:9ffb2f18756b 84 {
skyscraper 2:9ffb2f18756b 85 uint8_t cur_val = 0;
skyscraper 2:9ffb2f18756b 86 if(mag_get_reg(idx,cur_val)) {
skyscraper 2:9ffb2f18756b 87 uint8_t const new_val = (cur_val & and_val ) | or_val;
skyscraper 2:9ffb2f18756b 88 return mag_write_reg(idx,new_val);
skyscraper 2:9ffb2f18756b 89 } else {
skyscraper 2:9ffb2f18756b 90 return false;
skyscraper 2:9ffb2f18756b 91 }
skyscraper 2:9ffb2f18756b 92 }
skyscraper 2:9ffb2f18756b 93
skyscraper 2:9ffb2f18756b 94 } // namespace
skyscraper 2:9ffb2f18756b 95
skyscraper 2:9ffb2f18756b 96 // probe for the HMC5883 on I2C
skyscraper 2:9ffb2f18756b 97 // return true if found
skyscraper 2:9ffb2f18756b 98 bool mag_detected()
skyscraper 2:9ffb2f18756b 99 {
skyscraper 2:9ffb2f18756b 100 if ( mag_set_reg_idx(id_regA) ) {
skyscraper 2:9ffb2f18756b 101 char id_input[4];
skyscraper 2:9ffb2f18756b 102 if(i2c.read(i2c_addr,id_input,3) == 0) {
skyscraper 2:9ffb2f18756b 103 id_input[3] = '\0';
skyscraper 2:9ffb2f18756b 104 bool const is_hmc = (strcmp(id_input,"H43") == 0);
skyscraper 2:9ffb2f18756b 105 if (is_hmc) {
skyscraper 2:9ffb2f18756b 106 return true;
skyscraper 2:9ffb2f18756b 107 } else {
skyscraper 2:9ffb2f18756b 108 std::cout << "hmc5883 ID string didnt match\n";
skyscraper 2:9ffb2f18756b 109 return false;
skyscraper 2:9ffb2f18756b 110 }
skyscraper 2:9ffb2f18756b 111 } else {
skyscraper 2:9ffb2f18756b 112 std::cout << "id mag read failed\n";
skyscraper 2:9ffb2f18756b 113 return false;
skyscraper 2:9ffb2f18756b 114 }
skyscraper 2:9ffb2f18756b 115 } else {
skyscraper 2:9ffb2f18756b 116 return false;
skyscraper 2:9ffb2f18756b 117 }
skyscraper 2:9ffb2f18756b 118 }
skyscraper 2:9ffb2f18756b 119
skyscraper 2:9ffb2f18756b 120 // only 1,2,4,8 available
skyscraper 2:9ffb2f18756b 121 bool mag_set_samples_average(int n_samples)
skyscraper 2:9ffb2f18756b 122 {
skyscraper 2:9ffb2f18756b 123 uint8_t or_val = 0;
skyscraper 2:9ffb2f18756b 124 switch (n_samples) {
skyscraper 2:9ffb2f18756b 125 case 1 :
skyscraper 2:9ffb2f18756b 126 or_val = 0b00U << 5U;
skyscraper 2:9ffb2f18756b 127 break;
skyscraper 2:9ffb2f18756b 128 case 2 :
skyscraper 2:9ffb2f18756b 129 or_val = 0b01U << 5U;
skyscraper 2:9ffb2f18756b 130 break;
skyscraper 2:9ffb2f18756b 131 case 4 :
skyscraper 2:9ffb2f18756b 132 or_val = 0b10U << 5U;
skyscraper 2:9ffb2f18756b 133 break;
skyscraper 2:9ffb2f18756b 134 case 8 :
skyscraper 2:9ffb2f18756b 135 or_val = 0b11U << 5U;
skyscraper 2:9ffb2f18756b 136 break;
skyscraper 2:9ffb2f18756b 137 default:
skyscraper 2:9ffb2f18756b 138 std::cout << "mag_set_samples_average : invalid n_samples (" << n_samples << ")\n";
skyscraper 2:9ffb2f18756b 139 return false;
skyscraper 2:9ffb2f18756b 140 }
skyscraper 2:9ffb2f18756b 141 uint8_t constexpr and_val = ~(0b11 << 5U);
skyscraper 2:9ffb2f18756b 142 return mag_modify_reg(cfg_regA,and_val,or_val);
skyscraper 2:9ffb2f18756b 143 }
skyscraper 2:9ffb2f18756b 144
skyscraper 2:9ffb2f18756b 145 /*
skyscraper 2:9ffb2f18756b 146 data rate 0.75, 1.5, 3 ,7.5, 15 (Default) , 30, 75
skyscraper 2:9ffb2f18756b 147 */
skyscraper 2:9ffb2f18756b 148 namespace {
skyscraper 2:9ffb2f18756b 149
skyscraper 2:9ffb2f18756b 150 template <int N, int D=1> struct mag_data_rate_id;
skyscraper 2:9ffb2f18756b 151 // values are mag settings for each data rate
skyscraper 2:9ffb2f18756b 152 template <> struct mag_data_rate_id<3,4> : std::integral_constant<uint8_t,(0b000U << 2U)> {};
skyscraper 2:9ffb2f18756b 153 template <> struct mag_data_rate_id<3,2> : std::integral_constant<uint8_t,(0b001U << 2U)> {};
skyscraper 2:9ffb2f18756b 154 template <> struct mag_data_rate_id<3> : std::integral_constant<uint8_t,(0b010U << 2U)> {};
skyscraper 2:9ffb2f18756b 155 template <> struct mag_data_rate_id<15,2> : std::integral_constant<uint8_t,(0b011U << 2U)> {};
skyscraper 2:9ffb2f18756b 156 template <> struct mag_data_rate_id<15> : std::integral_constant<uint8_t,(0b100U << 2U)> {};
skyscraper 2:9ffb2f18756b 157 template <> struct mag_data_rate_id<30> : std::integral_constant<uint8_t,(0b101U << 2U)> {};
skyscraper 2:9ffb2f18756b 158 template <> struct mag_data_rate_id<75> : std::integral_constant<uint8_t,(0b110U << 2U)> {};
skyscraper 2:9ffb2f18756b 159
skyscraper 2:9ffb2f18756b 160 }//namespace
skyscraper 2:9ffb2f18756b 161
skyscraper 2:9ffb2f18756b 162 template <int N, int D>
skyscraper 2:9ffb2f18756b 163 bool mag_set_data_rate()
skyscraper 2:9ffb2f18756b 164 {
skyscraper 2:9ffb2f18756b 165 uint8_t constexpr and_val = static_cast<uint8_t>(~(0b111U << 2U));
skyscraper 2:9ffb2f18756b 166 uint8_t constexpr or_val = mag_data_rate_id<N,D>::value;
skyscraper 2:9ffb2f18756b 167 return mag_modify_reg(cfg_regA,and_val,or_val);
skyscraper 2:9ffb2f18756b 168 }
skyscraper 2:9ffb2f18756b 169
skyscraper 2:9ffb2f18756b 170 template bool mag_set_data_rate<3,4>();
skyscraper 2:9ffb2f18756b 171 template bool mag_set_data_rate<3,2>();
skyscraper 2:9ffb2f18756b 172 template bool mag_set_data_rate<3,1>();
skyscraper 2:9ffb2f18756b 173 template bool mag_set_data_rate<15,2>();
skyscraper 2:9ffb2f18756b 174 template bool mag_set_data_rate<15,1>();
skyscraper 2:9ffb2f18756b 175 template bool mag_set_data_rate<30,1>();
skyscraper 2:9ffb2f18756b 176 template bool mag_set_data_rate<75,1>();
skyscraper 2:9ffb2f18756b 177
skyscraper 2:9ffb2f18756b 178 namespace {
skyscraper 2:9ffb2f18756b 179
skyscraper 2:9ffb2f18756b 180 bool mag_set_positive_bias()
skyscraper 2:9ffb2f18756b 181 {
skyscraper 2:9ffb2f18756b 182 uint8_t constexpr and_val = static_cast<uint8_t>(~(0b11U ));
skyscraper 2:9ffb2f18756b 183 uint8_t constexpr or_val = 0b01U;
skyscraper 2:9ffb2f18756b 184 return mag_modify_reg(cfg_regA,and_val,or_val);
skyscraper 2:9ffb2f18756b 185 }
skyscraper 2:9ffb2f18756b 186
skyscraper 2:9ffb2f18756b 187 bool mag_set_negative_bias()
skyscraper 2:9ffb2f18756b 188 {
skyscraper 2:9ffb2f18756b 189 uint8_t constexpr and_val = static_cast<uint8_t>(~(0b11U ));
skyscraper 2:9ffb2f18756b 190 uint8_t constexpr or_val = 0b10U;
skyscraper 2:9ffb2f18756b 191 return mag_modify_reg(cfg_regA,and_val,or_val);
skyscraper 2:9ffb2f18756b 192 }
skyscraper 2:9ffb2f18756b 193
skyscraper 2:9ffb2f18756b 194 bool mag_clear_bias()
skyscraper 2:9ffb2f18756b 195 {
skyscraper 2:9ffb2f18756b 196 uint8_t constexpr and_val = static_cast<uint8_t>(~(0b11U ));
skyscraper 2:9ffb2f18756b 197 uint8_t constexpr or_val = 0b00U;
skyscraper 2:9ffb2f18756b 198 return mag_modify_reg(cfg_regA,and_val,or_val);
skyscraper 2:9ffb2f18756b 199 }
skyscraper 2:9ffb2f18756b 200
skyscraper 2:9ffb2f18756b 201 QUAN_QUANTITY_LITERAL(magnetic_flux_density,gauss);
skyscraper 2:9ffb2f18756b 202 QUAN_QUANTITY_LITERAL(magnetic_flux_density,milli_gauss);
skyscraper 2:9ffb2f18756b 203 QUAN_QUANTITY_LITERAL(magnetic_flux_density,uT);
skyscraper 2:9ffb2f18756b 204
skyscraper 2:9ffb2f18756b 205 // per lsb defualt resolution
skyscraper 2:9ffb2f18756b 206 quan::magnetic_flux_density::uT mag_resolution = 0.92_milli_gauss;
skyscraper 2:9ffb2f18756b 207 // range before saturation
skyscraper 2:9ffb2f18756b 208 quan::magnetic_flux_density::uT mag_range = 1.3_gauss;
skyscraper 2:9ffb2f18756b 209 }
skyscraper 2:9ffb2f18756b 210
skyscraper 2:9ffb2f18756b 211 // set +- range
skyscraper 2:9ffb2f18756b 212 // sets the nearest greater equal +-range to abs(range_in)
skyscraper 2:9ffb2f18756b 213 bool mag_set_range(quan::magnetic_flux_density::uT const & range_in)
skyscraper 2:9ffb2f18756b 214 {
skyscraper 2:9ffb2f18756b 215 uint8_t or_value = 0;
skyscraper 2:9ffb2f18756b 216 auto const range = abs(range_in);
skyscraper 2:9ffb2f18756b 217
skyscraper 2:9ffb2f18756b 218 if ( range <= 0.88_gauss) {
skyscraper 2:9ffb2f18756b 219 or_value = 0b001U << 5U ;
skyscraper 2:9ffb2f18756b 220 mag_range = 0.88_gauss;
skyscraper 2:9ffb2f18756b 221 mag_resolution = 0.73_milli_gauss;
skyscraper 2:9ffb2f18756b 222 } else if (range <= 1.3_gauss) {
skyscraper 2:9ffb2f18756b 223 or_value = 0b001U << 5U ;
skyscraper 2:9ffb2f18756b 224 mag_range = 1.3_gauss;
skyscraper 2:9ffb2f18756b 225 mag_resolution = 0.92_milli_gauss;
skyscraper 2:9ffb2f18756b 226 } else if (range <= 1.9_gauss) {
skyscraper 2:9ffb2f18756b 227 or_value = 0b010U << 5U ;
skyscraper 2:9ffb2f18756b 228 mag_range = 1.9_gauss;
skyscraper 2:9ffb2f18756b 229 mag_resolution = 1.22_milli_gauss;
skyscraper 2:9ffb2f18756b 230 } else if (range <= 2.5_gauss) {
skyscraper 2:9ffb2f18756b 231 or_value = 0b011U << 5U ;
skyscraper 2:9ffb2f18756b 232 mag_range = 2.5_gauss;
skyscraper 2:9ffb2f18756b 233 mag_resolution = 1.52_milli_gauss;
skyscraper 2:9ffb2f18756b 234 } else if (range <= 4.0_gauss) {
skyscraper 2:9ffb2f18756b 235 or_value = 0b100U << 5U ;
skyscraper 2:9ffb2f18756b 236 mag_range = 4.0_gauss;
skyscraper 2:9ffb2f18756b 237 mag_resolution = 2.27_milli_gauss;
skyscraper 2:9ffb2f18756b 238 } else if (range <= 4.7_gauss) {
skyscraper 2:9ffb2f18756b 239 or_value = 0b101U << 5U ;
skyscraper 2:9ffb2f18756b 240 mag_range = 4.7_gauss;
skyscraper 2:9ffb2f18756b 241 mag_resolution = 2.56_milli_gauss;
skyscraper 2:9ffb2f18756b 242 } else if (range <=5.6_gauss) {
skyscraper 2:9ffb2f18756b 243 or_value = 0b110U << 5U ;
skyscraper 2:9ffb2f18756b 244 mag_range = 5.6_gauss;
skyscraper 2:9ffb2f18756b 245 mag_resolution = 3.03_milli_gauss;
skyscraper 2:9ffb2f18756b 246 } else if ( range <= 8.1_gauss) {
skyscraper 2:9ffb2f18756b 247 or_value = 0b111U << 5U ;
skyscraper 2:9ffb2f18756b 248 mag_range = 8.1_gauss;
skyscraper 2:9ffb2f18756b 249 mag_resolution = 4.35_milli_gauss;
skyscraper 2:9ffb2f18756b 250 } else {
skyscraper 2:9ffb2f18756b 251 quan::magnetic_flux_density::uT constexpr max_range = 8.1_gauss;
skyscraper 2:9ffb2f18756b 252 std::cout << "range too big: max +- range = " << max_range <<"\n";
skyscraper 2:9ffb2f18756b 253 return false;
skyscraper 2:9ffb2f18756b 254 }
skyscraper 2:9ffb2f18756b 255 uint8_t constexpr and_val = static_cast<uint8_t>(~(0b111U << 5U));
skyscraper 2:9ffb2f18756b 256 std::cout << "mag range set to : +- " << mag_range <<'\n';
skyscraper 2:9ffb2f18756b 257 return mag_modify_reg(cfg_regB,and_val,or_value);
skyscraper 2:9ffb2f18756b 258
skyscraper 2:9ffb2f18756b 259 }
skyscraper 2:9ffb2f18756b 260
skyscraper 2:9ffb2f18756b 261 quan::magnetic_flux_density::uT
skyscraper 2:9ffb2f18756b 262 mag_get_range()
skyscraper 2:9ffb2f18756b 263 {
skyscraper 2:9ffb2f18756b 264 return mag_range;
skyscraper 2:9ffb2f18756b 265 }
skyscraper 2:9ffb2f18756b 266
skyscraper 2:9ffb2f18756b 267 bool mag_set_continuous_measurement_mode()
skyscraper 2:9ffb2f18756b 268 {
skyscraper 2:9ffb2f18756b 269 uint8_t constexpr and_val = static_cast<uint8_t>(~(0b11U ));
skyscraper 2:9ffb2f18756b 270 uint8_t constexpr or_val = 0b00U;
skyscraper 2:9ffb2f18756b 271 return mag_modify_reg(mode_reg,and_val,or_val);
skyscraper 2:9ffb2f18756b 272 }
skyscraper 2:9ffb2f18756b 273
skyscraper 2:9ffb2f18756b 274 bool mag_set_single_measurement_mode()
skyscraper 2:9ffb2f18756b 275 {
skyscraper 2:9ffb2f18756b 276 uint8_t constexpr and_val = static_cast<uint8_t>(~(0b11U ));
skyscraper 2:9ffb2f18756b 277 uint8_t constexpr or_val = 0b01U;
skyscraper 2:9ffb2f18756b 278 return mag_modify_reg(mode_reg,and_val,or_val);
skyscraper 2:9ffb2f18756b 279 }
skyscraper 2:9ffb2f18756b 280
skyscraper 2:9ffb2f18756b 281 bool mag_set_idle_mode()
skyscraper 2:9ffb2f18756b 282 {
skyscraper 2:9ffb2f18756b 283 uint8_t constexpr and_val = static_cast<uint8_t>(~(0b11U ));
skyscraper 2:9ffb2f18756b 284 uint8_t constexpr or_val = 0b10U;
skyscraper 2:9ffb2f18756b 285 return mag_modify_reg(mode_reg,and_val,or_val);
skyscraper 2:9ffb2f18756b 286 }
skyscraper 2:9ffb2f18756b 287
skyscraper 2:9ffb2f18756b 288 bool mag_data_ready()
skyscraper 2:9ffb2f18756b 289 {
skyscraper 2:9ffb2f18756b 290 uint8_t result = 0;
skyscraper 2:9ffb2f18756b 291 if ( mag_get_reg(status_reg, result)) {
skyscraper 2:9ffb2f18756b 292 return (result & 0b1U) != 0U;
skyscraper 2:9ffb2f18756b 293 } else {
skyscraper 2:9ffb2f18756b 294 std::cout << "mag data ready failed\n";
skyscraper 2:9ffb2f18756b 295 return false;
skyscraper 2:9ffb2f18756b 296 }
skyscraper 2:9ffb2f18756b 297 }
skyscraper 2:9ffb2f18756b 298
skyscraper 2:9ffb2f18756b 299 bool mag_data_locked()
skyscraper 2:9ffb2f18756b 300 {
skyscraper 2:9ffb2f18756b 301 uint8_t result = 0;
skyscraper 2:9ffb2f18756b 302 if ( mag_get_reg(status_reg, result)) {
skyscraper 2:9ffb2f18756b 303 return (result & 0b10U) != 0U;
skyscraper 2:9ffb2f18756b 304 } else {
skyscraper 2:9ffb2f18756b 305 std::cout << "mag data locked failed\n";
skyscraper 2:9ffb2f18756b 306 return false;
skyscraper 2:9ffb2f18756b 307 }
skyscraper 2:9ffb2f18756b 308 }
skyscraper 2:9ffb2f18756b 309
skyscraper 2:9ffb2f18756b 310 // assume mag_data_ready has returned true before call
skyscraper 2:9ffb2f18756b 311 bool mag_read(quan::three_d::vect<quan::magnetic_flux_density::uT> & v)
skyscraper 2:9ffb2f18756b 312 {
skyscraper 2:9ffb2f18756b 313 if( mag_set_reg_idx(dout_reg)) {
skyscraper 2:9ffb2f18756b 314 char arr[7];
skyscraper 2:9ffb2f18756b 315 if(i2c.read(i2c_addr,arr,7) == 0) {
skyscraper 2:9ffb2f18756b 316 // TODO check status reg arr[6]
skyscraper 2:9ffb2f18756b 317 // if
skyscraper 2:9ffb2f18756b 318 quan::three_d::vect<int16_t> temp;
skyscraper 2:9ffb2f18756b 319 temp.x = static_cast<int16_t>(arr[1]) + ( static_cast<int16_t>(arr[0]) << 8U);
skyscraper 2:9ffb2f18756b 320 temp.y = static_cast<int16_t>(arr[5]) + ( static_cast<int16_t>(arr[4]) << 8U);
skyscraper 2:9ffb2f18756b 321 temp.z = static_cast<int16_t>(arr[3]) + ( static_cast<int16_t>(arr[2]) << 8U);
skyscraper 2:9ffb2f18756b 322 v = temp * mag_resolution;
skyscraper 2:9ffb2f18756b 323 return true;
skyscraper 2:9ffb2f18756b 324 } else {
skyscraper 2:9ffb2f18756b 325 std::cout << "mag_read failed\n";
skyscraper 2:9ffb2f18756b 326 return false;
skyscraper 2:9ffb2f18756b 327 }
skyscraper 2:9ffb2f18756b 328 } else {
skyscraper 2:9ffb2f18756b 329 return false;
skyscraper 2:9ffb2f18756b 330 }
skyscraper 2:9ffb2f18756b 331 }
skyscraper 2:9ffb2f18756b 332
skyscraper 2:9ffb2f18756b 333 bool mag_do_single_measurement(quan::three_d::vect<quan::magnetic_flux_density::uT>& result)
skyscraper 2:9ffb2f18756b 334 {
skyscraper 2:9ffb2f18756b 335 if ( ! mag_set_single_measurement_mode()) {
skyscraper 2:9ffb2f18756b 336 return false;
skyscraper 2:9ffb2f18756b 337 }
skyscraper 2:9ffb2f18756b 338
skyscraper 2:9ffb2f18756b 339 while (! mag_data_ready()) {
skyscraper 2:9ffb2f18756b 340 ThisThread::sleep_for(5U);
skyscraper 2:9ffb2f18756b 341 }
skyscraper 2:9ffb2f18756b 342 return mag_read(result);
skyscraper 2:9ffb2f18756b 343 }
skyscraper 2:9ffb2f18756b 344
skyscraper 2:9ffb2f18756b 345 namespace {
skyscraper 2:9ffb2f18756b 346 //TODO raname to mag_self_test
skyscraper 2:9ffb2f18756b 347 bool mag_get_offsets(quan::three_d::vect<quan::magnetic_flux_density::uT> & result)
skyscraper 2:9ffb2f18756b 348 {
skyscraper 2:9ffb2f18756b 349 // set single measurement mode
skyscraper 2:9ffb2f18756b 350
skyscraper 2:9ffb2f18756b 351 // to prevent saturation
skyscraper 2:9ffb2f18756b 352 mag_set_range(5.7_gauss);
skyscraper 2:9ffb2f18756b 353 quan::three_d::vect<quan::magnetic_flux_density::uT> mSet;
skyscraper 2:9ffb2f18756b 354 // throw away first
skyscraper 2:9ffb2f18756b 355 mag_do_single_measurement(mSet);
skyscraper 2:9ffb2f18756b 356
skyscraper 2:9ffb2f18756b 357 mag_set_positive_bias();
skyscraper 2:9ffb2f18756b 358 mag_do_single_measurement(mSet);
skyscraper 2:9ffb2f18756b 359 std::cout << "mSet = " << mSet <<'\n';
skyscraper 2:9ffb2f18756b 360
skyscraper 2:9ffb2f18756b 361 mag_set_negative_bias();
skyscraper 2:9ffb2f18756b 362 quan::three_d::vect<quan::magnetic_flux_density::uT> mReset;
skyscraper 2:9ffb2f18756b 363 mag_do_single_measurement(mReset);
skyscraper 2:9ffb2f18756b 364 mag_clear_bias();
skyscraper 2:9ffb2f18756b 365
skyscraper 2:9ffb2f18756b 366 std::cout << "mReset = " << mReset <<'\n';
skyscraper 2:9ffb2f18756b 367
skyscraper 2:9ffb2f18756b 368 result = (mSet - mReset )/2;
skyscraper 2:9ffb2f18756b 369
skyscraper 2:9ffb2f18756b 370 std::cout << "result = " << result << '\n';
skyscraper 2:9ffb2f18756b 371
skyscraper 2:9ffb2f18756b 372 return true;
skyscraper 2:9ffb2f18756b 373 }
skyscraper 2:9ffb2f18756b 374 } // namepsace