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@7:5d14da0b4c95, 2020-03-26 (annotated)

Committer:
skyscraper
Date:
Thu Mar 26 12:27:43 2020 +0000
Revision:
7:5d14da0b4c95
Parent:
3:2834be4e10ef
Child:
8:c5dc1ce10722
Use "resourceManager" to keep global board resources in one place

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