Compass sensor library
Dependents: compassDemo weather_station_proj weather_station_project weather_station_proj_v1_2
HMC5983.cpp@3:c2cd6cc71ee2, 2018-07-05 (annotated)
- Committer:
- acracan
- Date:
- Thu Jul 05 17:59:23 2018 +0000
- Revision:
- 3:c2cd6cc71ee2
- Parent:
- 2:4debef04091d
Swap Z and Y in internal vector
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
acracan | 0:cffff4c45a14 | 1 | /* |
acracan | 0:cffff4c45a14 | 2 | * HMC5983.cpp - library header |
acracan | 0:cffff4c45a14 | 3 | * |
acracan | 0:cffff4c45a14 | 4 | * simple library for the HMC5983 sensor from Honeywell |
acracan | 0:cffff4c45a14 | 5 | * adaptation form MBED from the Arduino library |
acracan | 0:cffff4c45a14 | 6 | * |
acracan | 0:cffff4c45a14 | 7 | * (c) 2014 Korneliusz Jarzebski, www.jarzebski.pl |
acracan | 0:cffff4c45a14 | 8 | * (c) 2014 David Cuartielles, Arduino LLC |
acracan | 0:cffff4c45a14 | 9 | * (c) 2016 Abel Romero, www.abelromero.com |
acracan | 0:cffff4c45a14 | 10 | * (c) 2018 Arcadie Cracan |
acracan | 0:cffff4c45a14 | 11 | */ |
acracan | 0:cffff4c45a14 | 12 | |
acracan | 0:cffff4c45a14 | 13 | // MISSING: EARTH DECLINATION ANGLE |
acracan | 0:cffff4c45a14 | 14 | // In other words, we are not making any compensation for the earth's north pole location vs the magnetic measurement |
acracan | 0:cffff4c45a14 | 15 | |
acracan | 0:cffff4c45a14 | 16 | #include "HMC5983.h" |
acracan | 0:cffff4c45a14 | 17 | #include <new> |
acracan | 0:cffff4c45a14 | 18 | |
acracan | 0:cffff4c45a14 | 19 | HMC5983::HMC5983(PinName sda, PinName scl) : i2c_(*reinterpret_cast<I2C*>(i2cRaw)) |
acracan | 0:cffff4c45a14 | 20 | { |
acracan | 0:cffff4c45a14 | 21 | // Placement new to avoid additional heap memory allocation. |
acracan | 0:cffff4c45a14 | 22 | new(i2cRaw) I2C(sda, scl); |
acracan | 2:4debef04091d | 23 | |
acracan | 0:cffff4c45a14 | 24 | init(); |
acracan | 0:cffff4c45a14 | 25 | } |
acracan | 0:cffff4c45a14 | 26 | |
acracan | 2:4debef04091d | 27 | HMC5983::HMC5983(I2C &i2c): i2c_(i2c) |
acracan | 2:4debef04091d | 28 | { |
acracan | 2:4debef04091d | 29 | init(); |
acracan | 0:cffff4c45a14 | 30 | } |
acracan | 0:cffff4c45a14 | 31 | |
acracan | 0:cffff4c45a14 | 32 | bool HMC5983::init() |
acracan | 0:cffff4c45a14 | 33 | { |
acracan | 2:4debef04091d | 34 | const float initial_m[9] = {1.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 1.0f}; |
acracan | 2:4debef04091d | 35 | const float initial_b[3] = {0.0f, 0.0f, 0.0f}; |
acracan | 2:4debef04091d | 36 | if ((fastRegister8(HMC5983_REG_IDENT_A) != 0x48) |
acracan | 2:4debef04091d | 37 | || (fastRegister8(HMC5983_REG_IDENT_B) != 0x34) |
acracan | 2:4debef04091d | 38 | || (fastRegister8(HMC5983_REG_IDENT_C) != 0x33)) { |
acracan | 2:4debef04091d | 39 | return false; |
acracan | 2:4debef04091d | 40 | } |
acracan | 0:cffff4c45a14 | 41 | |
acracan | 2:4debef04091d | 42 | // Set Gain Range |
acracan | 2:4debef04091d | 43 | setRange(HMC5983_RANGE_8_1GA); |
acracan | 2:4debef04091d | 44 | // Set DataRate 220Hz ~4.5ms |
acracan | 2:4debef04091d | 45 | setDataRate(HMC5983_DATARATE_220HZ); |
acracan | 2:4debef04091d | 46 | // Set number of samples to average |
acracan | 2:4debef04091d | 47 | setSampleAverages(HMC5983_SAMPLEAVERAGE_2); |
acracan | 2:4debef04091d | 48 | // Set Mode |
acracan | 2:4debef04091d | 49 | setMeasurementMode(HMC5983_CONTINOUS); |
acracan | 2:4debef04091d | 50 | setCalibrationMatrices(initial_m, initial_b); |
acracan | 2:4debef04091d | 51 | H[0] = 0; |
acracan | 2:4debef04091d | 52 | H[1] = 0; |
acracan | 2:4debef04091d | 53 | H[2] = 0; |
acracan | 2:4debef04091d | 54 | // Setup DRDY int |
acracan | 0:cffff4c45a14 | 55 | // if (ISR_callback != NULL) { |
acracan | 0:cffff4c45a14 | 56 | // pinMode(3, INPUT_PULLUP); |
acracan | 0:cffff4c45a14 | 57 | // attachInterrupt(digitalPinToInterrupt(3), ISR_callback, FALLING); |
acracan | 0:cffff4c45a14 | 58 | // } |
acracan | 2:4debef04091d | 59 | return true; |
acracan | 0:cffff4c45a14 | 60 | } |
acracan | 0:cffff4c45a14 | 61 | |
acracan | 0:cffff4c45a14 | 62 | /* |
acracan | 0:cffff4c45a14 | 63 | From datasheet for the HMC5983 |
acracan | 0:cffff4c45a14 | 64 | Below is an example of a (power-on) initialization process for “continuous-measurement mode” via I2C interface: |
acracan | 0:cffff4c45a14 | 65 | 1. Write CRA (00) – send 0x3C 0x00 0x70 (8-average, 15 Hz default or any other rate, normal measurement) |
acracan | 0:cffff4c45a14 | 66 | 2. Write CRB (01) – send 0x3C 0x01 0xA0 (Gain=5, or any other desired gain) |
acracan | 0:cffff4c45a14 | 67 | 3. For each measurement query: |
acracan | 0:cffff4c45a14 | 68 | Write Mode (02) – send 0x3C 0x02 0x01 (Single-measurement mode) |
acracan | 0:cffff4c45a14 | 69 | Wait 6 ms or monitor status register or DRDY hardware interrupt pin |
acracan | 0:cffff4c45a14 | 70 | Send 0x3D 0x06 (Read all 6 bytes. If gain is changed then this data set is using previous gain) |
acracan | 0:cffff4c45a14 | 71 | Convert three 16-bit 2’s compliment hex values to decimal values and assign to X, Z, Y, respectively. |
acracan | 0:cffff4c45a14 | 72 | (Self addition:) |
acracan | 0:cffff4c45a14 | 73 | 4. Convert the magnetic information into a compass value |
acracan | 0:cffff4c45a14 | 74 | REGARDING THE CALCULATION OF THE ACTUAL HEADING VALUE |
acracan | 0:cffff4c45a14 | 75 | From AN-203 http://www51.honeywell.com/aero/common/documents/myaerospacecatalog-documents/Defense_Brochures-documents/Magnetic__Literature_Application_notes-documents/AN203_Compass_Heading_Using_Magnetometers.pdf |
acracan | 0:cffff4c45a14 | 76 | The magnetic compass heading can be determined (in degrees) from the magnetometer's x and y readings by using the |
acracan | 0:cffff4c45a14 | 77 | following set of equations: |
acracan | 0:cffff4c45a14 | 78 | Direction (y>0) = 90 - [arcTAN(x/y)]*180/PI |
acracan | 0:cffff4c45a14 | 79 | Direction (y<0) = 270 - [arcTAN(x/y)]*180/PI |
acracan | 0:cffff4c45a14 | 80 | Direction (y=0, x<0) = 180.0 |
acracan | 0:cffff4c45a14 | 81 | Direction (y=0, x>0) = 0.0 |
acracan | 0:cffff4c45a14 | 82 | */ |
acracan | 0:cffff4c45a14 | 83 | |
acracan | 2:4debef04091d | 84 | void HMC5983::setRange(hmc5983_range_t range) |
acracan | 2:4debef04091d | 85 | { |
acracan | 0:cffff4c45a14 | 86 | |
acracan | 0:cffff4c45a14 | 87 | writeRegister8(HMC5983_REG_CONFIG_B, range << 5); |
acracan | 0:cffff4c45a14 | 88 | } |
acracan | 0:cffff4c45a14 | 89 | |
acracan | 0:cffff4c45a14 | 90 | hmc5983_range_t HMC5983::getRange(void) |
acracan | 0:cffff4c45a14 | 91 | { |
acracan | 0:cffff4c45a14 | 92 | return (hmc5983_range_t)((readRegister8(HMC5983_REG_CONFIG_B) >> 5)); |
acracan | 0:cffff4c45a14 | 93 | } |
acracan | 0:cffff4c45a14 | 94 | |
acracan | 2:4debef04091d | 95 | void HMC5983::setMeasurementMode(hmc5983_mode_t mode) |
acracan | 2:4debef04091d | 96 | { |
acracan | 0:cffff4c45a14 | 97 | uint8_t value; |
acracan | 0:cffff4c45a14 | 98 | |
acracan | 0:cffff4c45a14 | 99 | value = readRegister8(HMC5983_REG_MODE); |
acracan | 0:cffff4c45a14 | 100 | value &= 0b11111100; |
acracan | 0:cffff4c45a14 | 101 | value |= mode; |
acracan | 0:cffff4c45a14 | 102 | |
acracan | 0:cffff4c45a14 | 103 | writeRegister8(HMC5983_REG_MODE, value); |
acracan | 0:cffff4c45a14 | 104 | } |
acracan | 0:cffff4c45a14 | 105 | |
acracan | 2:4debef04091d | 106 | hmc5983_mode_t HMC5983::getMeasurementMode(void) |
acracan | 2:4debef04091d | 107 | { |
acracan | 0:cffff4c45a14 | 108 | uint8_t value; |
acracan | 0:cffff4c45a14 | 109 | |
acracan | 0:cffff4c45a14 | 110 | value = readRegister8(HMC5983_REG_MODE); |
acracan | 0:cffff4c45a14 | 111 | value &= 0b00000011; |
acracan | 0:cffff4c45a14 | 112 | |
acracan | 0:cffff4c45a14 | 113 | return (hmc5983_mode_t)value; |
acracan | 0:cffff4c45a14 | 114 | } |
acracan | 0:cffff4c45a14 | 115 | |
acracan | 2:4debef04091d | 116 | void HMC5983::setDataRate(hmc5983_dataRate_t dataRate) |
acracan | 2:4debef04091d | 117 | { |
acracan | 0:cffff4c45a14 | 118 | uint8_t value; |
acracan | 0:cffff4c45a14 | 119 | |
acracan | 0:cffff4c45a14 | 120 | value = readRegister8(HMC5983_REG_CONFIG_A); |
acracan | 0:cffff4c45a14 | 121 | value &= 0b11100011; |
acracan | 0:cffff4c45a14 | 122 | value |= (dataRate << 2); |
acracan | 0:cffff4c45a14 | 123 | |
acracan | 0:cffff4c45a14 | 124 | writeRegister8(HMC5983_REG_CONFIG_A, value); |
acracan | 0:cffff4c45a14 | 125 | } |
acracan | 0:cffff4c45a14 | 126 | |
acracan | 2:4debef04091d | 127 | hmc5983_dataRate_t HMC5983::getDataRate(void) |
acracan | 2:4debef04091d | 128 | { |
acracan | 0:cffff4c45a14 | 129 | uint8_t value; |
acracan | 0:cffff4c45a14 | 130 | |
acracan | 0:cffff4c45a14 | 131 | value = readRegister8(HMC5983_REG_CONFIG_A); |
acracan | 0:cffff4c45a14 | 132 | value &= 0b00011100; |
acracan | 0:cffff4c45a14 | 133 | value >>= 2; |
acracan | 0:cffff4c45a14 | 134 | |
acracan | 0:cffff4c45a14 | 135 | return (hmc5983_dataRate_t)value; |
acracan | 0:cffff4c45a14 | 136 | } |
acracan | 0:cffff4c45a14 | 137 | |
acracan | 2:4debef04091d | 138 | void HMC5983::setSampleAverages(hmc5983_sampleAverages_t sampleAverages) |
acracan | 2:4debef04091d | 139 | { |
acracan | 0:cffff4c45a14 | 140 | uint8_t value; |
acracan | 0:cffff4c45a14 | 141 | |
acracan | 0:cffff4c45a14 | 142 | value = readRegister8(HMC5983_REG_CONFIG_A); |
acracan | 0:cffff4c45a14 | 143 | value &= 0b10011111; |
acracan | 0:cffff4c45a14 | 144 | value |= (sampleAverages << 5); |
acracan | 0:cffff4c45a14 | 145 | |
acracan | 0:cffff4c45a14 | 146 | writeRegister8(HMC5983_REG_CONFIG_A, value); |
acracan | 0:cffff4c45a14 | 147 | } |
acracan | 0:cffff4c45a14 | 148 | |
acracan | 2:4debef04091d | 149 | hmc5983_sampleAverages_t HMC5983::getSampleAverages(void) |
acracan | 2:4debef04091d | 150 | { |
acracan | 0:cffff4c45a14 | 151 | uint8_t value; |
acracan | 0:cffff4c45a14 | 152 | |
acracan | 0:cffff4c45a14 | 153 | value = readRegister8(HMC5983_REG_CONFIG_A); |
acracan | 0:cffff4c45a14 | 154 | value &= 0b01100000; |
acracan | 0:cffff4c45a14 | 155 | value >>= 5; |
acracan | 0:cffff4c45a14 | 156 | |
acracan | 0:cffff4c45a14 | 157 | return (hmc5983_sampleAverages_t)value; |
acracan | 0:cffff4c45a14 | 158 | } |
acracan | 0:cffff4c45a14 | 159 | |
acracan | 0:cffff4c45a14 | 160 | // Write byte to register |
acracan | 2:4debef04091d | 161 | void HMC5983::writeRegister8(uint8_t reg, uint8_t value) |
acracan | 2:4debef04091d | 162 | { |
acracan | 2:4debef04091d | 163 | char cmd[2]; |
acracan | 2:4debef04091d | 164 | |
acracan | 2:4debef04091d | 165 | cmd[0] = reg; |
acracan | 2:4debef04091d | 166 | cmd[1] = value; |
acracan | 2:4debef04091d | 167 | i2c_.write(HMC5983_ADDRESS, cmd, 2); |
acracan | 0:cffff4c45a14 | 168 | } |
acracan | 0:cffff4c45a14 | 169 | |
acracan | 0:cffff4c45a14 | 170 | // Read byte to register |
acracan | 2:4debef04091d | 171 | uint8_t HMC5983::fastRegister8(uint8_t reg) |
acracan | 2:4debef04091d | 172 | { |
acracan | 2:4debef04091d | 173 | uint8_t value; |
acracan | 0:cffff4c45a14 | 174 | |
acracan | 2:4debef04091d | 175 | i2c_.write(HMC5983_ADDRESS, (char *)®, 1); |
acracan | 2:4debef04091d | 176 | i2c_.read(HMC5983_ADDRESS, (char *)&value, 1); |
acracan | 0:cffff4c45a14 | 177 | |
acracan | 2:4debef04091d | 178 | return value; |
acracan | 0:cffff4c45a14 | 179 | } |
acracan | 0:cffff4c45a14 | 180 | |
acracan | 0:cffff4c45a14 | 181 | // Read byte from register |
acracan | 2:4debef04091d | 182 | uint8_t HMC5983::readRegister8(uint8_t reg) |
acracan | 2:4debef04091d | 183 | { |
acracan | 2:4debef04091d | 184 | uint8_t value; |
acracan | 0:cffff4c45a14 | 185 | |
acracan | 2:4debef04091d | 186 | i2c_.write(HMC5983_ADDRESS, (char *)®, 1); |
acracan | 2:4debef04091d | 187 | i2c_.read(HMC5983_ADDRESS, (char *)&value, 1); |
acracan | 2:4debef04091d | 188 | |
acracan | 2:4debef04091d | 189 | return value; |
acracan | 0:cffff4c45a14 | 190 | |
acracan | 0:cffff4c45a14 | 191 | // Wire.requestFrom(HMC5983_ADDRESS, 1); |
acracan | 0:cffff4c45a14 | 192 | // while(!Wire.available()) {}; |
acracan | 0:cffff4c45a14 | 193 | // value = Wire.read(); |
acracan | 0:cffff4c45a14 | 194 | } |
acracan | 0:cffff4c45a14 | 195 | |
acracan | 0:cffff4c45a14 | 196 | // Read word from register |
acracan | 2:4debef04091d | 197 | int16_t HMC5983::readRegister16(uint8_t reg) |
acracan | 2:4debef04091d | 198 | { |
acracan | 2:4debef04091d | 199 | int16_t value; |
acracan | 2:4debef04091d | 200 | char resp[2]; |
acracan | 0:cffff4c45a14 | 201 | |
acracan | 2:4debef04091d | 202 | i2c_.write(HMC5983_ADDRESS, (char *)®, 1); |
acracan | 2:4debef04091d | 203 | i2c_.read(HMC5983_ADDRESS, resp, 2); |
acracan | 0:cffff4c45a14 | 204 | |
acracan | 0:cffff4c45a14 | 205 | // Wire.requestFrom(HMC5983_ADDRESS, 2); |
acracan | 0:cffff4c45a14 | 206 | // while(!Wire.available()) {}; |
acracan | 0:cffff4c45a14 | 207 | |
acracan | 2:4debef04091d | 208 | value = (uint8_t)resp[0] << 8 | (uint8_t)resp[1]; |
acracan | 0:cffff4c45a14 | 209 | |
acracan | 2:4debef04091d | 210 | return value; |
acracan | 0:cffff4c45a14 | 211 | } |
acracan | 0:cffff4c45a14 | 212 | |
acracan | 2:4debef04091d | 213 | void HMC5983::getHeadingRegs() |
acracan | 2:4debef04091d | 214 | { |
acracan | 2:4debef04091d | 215 | // the values for X, Y & Z must be read in X, Z & Y order. |
acracan | 2:4debef04091d | 216 | char data[6]; |
acracan | 2:4debef04091d | 217 | writeRegister8(HMC5983_OUT_X_MSB, 0); // Select MSB X register |
acracan | 2:4debef04091d | 218 | i2c_.read(HMC5983_ADDRESS, data, 6); |
acracan | 2:4debef04091d | 219 | uint8_t X_MSB = data[0]; |
acracan | 2:4debef04091d | 220 | uint8_t X_LSB = data[1]; |
acracan | 2:4debef04091d | 221 | uint8_t Z_MSB = data[2]; |
acracan | 2:4debef04091d | 222 | uint8_t Z_LSB = data[3]; |
acracan | 2:4debef04091d | 223 | uint8_t Y_MSB = data[4]; |
acracan | 2:4debef04091d | 224 | uint8_t Y_LSB = data[5]; |
acracan | 2:4debef04091d | 225 | |
acracan | 2:4debef04091d | 226 | // compose byte for X, Y, Z's LSB & MSB 8bit registers |
acracan | 2:4debef04091d | 227 | H[0] = (uint16_t(X_MSB) << 8) | X_LSB; |
acracan | 3:c2cd6cc71ee2 | 228 | H[1] = (uint16_t(Y_MSB) << 8) | Y_LSB; |
acracan | 3:c2cd6cc71ee2 | 229 | H[2] = (uint16_t(Z_MSB) << 8) | Z_LSB; |
acracan | 2:4debef04091d | 230 | } |
acracan | 0:cffff4c45a14 | 231 | |
acracan | 2:4debef04091d | 232 | void HMC5983::computeCalibratedHeadingVector(float *v) |
acracan | 2:4debef04091d | 233 | { |
acracan | 2:4debef04091d | 234 | float Hub[3]; |
acracan | 2:4debef04091d | 235 | |
acracan | 2:4debef04091d | 236 | int i, j; |
acracan | 2:4debef04091d | 237 | |
acracan | 2:4debef04091d | 238 | // remove bias |
acracan | 2:4debef04091d | 239 | for (i = 0; i < 3; i++) { |
acracan | 2:4debef04091d | 240 | Hub[i] = H[i] - _b[i]; |
acracan | 2:4debef04091d | 241 | v[i] = 0.0f; |
acracan | 2:4debef04091d | 242 | } |
acracan | 2:4debef04091d | 243 | |
acracan | 2:4debef04091d | 244 | // apply matrix transformation |
acracan | 2:4debef04091d | 245 | for (i = 0; i < 3; i++) { |
acracan | 2:4debef04091d | 246 | for (j = 0; j < 3; j++) { |
acracan | 2:4debef04091d | 247 | v[i] += _m[3*i+j]*Hub[j]; |
acracan | 2:4debef04091d | 248 | } |
acracan | 2:4debef04091d | 249 | } |
acracan | 1:fb6804e865fd | 250 | } |
acracan | 0:cffff4c45a14 | 251 | |
acracan | 2:4debef04091d | 252 | void HMC5983::setCalibrationMatrices(const float *m, const float *b) |
acracan | 2:4debef04091d | 253 | { |
acracan | 2:4debef04091d | 254 | int i, j; |
acracan | 0:cffff4c45a14 | 255 | |
acracan | 2:4debef04091d | 256 | for (i = 0; i < 3; i++) { |
acracan | 2:4debef04091d | 257 | for (j = 0; j < 3; j++) { |
acracan | 2:4debef04091d | 258 | _m[3*i+j] = m[3*i+j]; |
acracan | 2:4debef04091d | 259 | } |
acracan | 2:4debef04091d | 260 | _b[i] = b[i]; |
acracan | 2:4debef04091d | 261 | } |
acracan | 1:fb6804e865fd | 262 | } |
acracan | 1:fb6804e865fd | 263 | |
acracan | 2:4debef04091d | 264 | float HMC5983::readHeading(bool calibrate) |
acracan | 2:4debef04091d | 265 | { |
acracan | 2:4debef04091d | 266 | // declare the heading variable we'll be returning |
acracan | 2:4debef04091d | 267 | float result = 0; |
acracan | 2:4debef04091d | 268 | float cH[3]; |
acracan | 2:4debef04091d | 269 | int i; |
acracan | 2:4debef04091d | 270 | |
acracan | 1:fb6804e865fd | 271 | getHeadingRegs(); |
acracan | 2:4debef04091d | 272 | for (i = 0; i < 3; i++) { |
acracan | 2:4debef04091d | 273 | cH[i] = H[i]; |
acracan | 2:4debef04091d | 274 | } |
acracan | 2:4debef04091d | 275 | |
acracan | 2:4debef04091d | 276 | if (calibrate) { |
acracan | 2:4debef04091d | 277 | computeCalibratedHeadingVector(cH); |
acracan | 2:4debef04091d | 278 | } |
acracan | 2:4debef04091d | 279 | // this is the correct way, fixed from original David's work. |
acracan | 2:4debef04091d | 280 | // corrected following datasheet and his own comments...xD |
acracan | 2:4debef04091d | 281 | // even corrected from datasheet, the 90-270 angle is a bit confusing, but the 360º are captured. |
acracan | 2:4debef04091d | 282 | if (cH[1] > 0.0f) result = 90.0f - (float)atan(cH[0] / cH[1]) * 180.0f / M_PI; |
acracan | 2:4debef04091d | 283 | if (cH[1] < 0.0f) result = 270.0f - (float)atan(cH[0] / cH[1]) * 180.0f / M_PI; |
acracan | 2:4debef04091d | 284 | if (cH[1] == 0.0f && cH[0] < 0.0f) result = 180.0f; |
acracan | 2:4debef04091d | 285 | if (cH[1] == 0.0f && cH[0] > 0.0f) result = 0.0f; |
acracan | 2:4debef04091d | 286 | |
acracan | 2:4debef04091d | 287 | return result; |
acracan | 2:4debef04091d | 288 | } |
acracan | 2:4debef04091d | 289 | |
acracan | 2:4debef04091d | 290 | void HMC5983::readHeadingVector(float *v, bool calibrate) |
acracan | 2:4debef04091d | 291 | { |
acracan | 2:4debef04091d | 292 | getHeadingRegs(); |
acracan | 2:4debef04091d | 293 | if (calibrate) { |
acracan | 2:4debef04091d | 294 | computeCalibratedHeadingVector(v); |
acracan | 2:4debef04091d | 295 | } |
acracan | 2:4debef04091d | 296 | else { |
acracan | 2:4debef04091d | 297 | v[0] = H[0]; |
acracan | 2:4debef04091d | 298 | v[1] = H[1]; |
acracan | 2:4debef04091d | 299 | v[2] = H[2]; |
acracan | 2:4debef04091d | 300 | } |
acracan | 0:cffff4c45a14 | 301 | } |