otamesi
Dependencies: mbed
HMC5883L.cpp@5:f630dbbae44e, 2018-10-20 (annotated)
- Committer:
- seangshim
- Date:
- Sat Oct 20 03:10:04 2018 +0000
- Revision:
- 5:f630dbbae44e
- Child:
- 10:280a25bcc8bb
new
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
seangshim | 5:f630dbbae44e | 1 | /*This program is uncompleted.*/ |
seangshim | 5:f630dbbae44e | 2 | #include "HMC5883L.h" |
seangshim | 5:f630dbbae44e | 3 | #include <new> |
seangshim | 5:f630dbbae44e | 4 | |
seangshim | 5:f630dbbae44e | 5 | HMC5883L::HMC5883L(PinName p28, PinName p27) : i2c_(*reinterpret_cast<I2C*>(i2cRaw)) |
seangshim | 5:f630dbbae44e | 6 | { |
seangshim | 5:f630dbbae44e | 7 | new(i2cRaw) I2C(p28, p27); |
seangshim | 5:f630dbbae44e | 8 | init(); |
seangshim | 5:f630dbbae44e | 9 | } |
seangshim | 5:f630dbbae44e | 10 | |
seangshim | 5:f630dbbae44e | 11 | HMC5883L::~HMC5883L() |
seangshim | 5:f630dbbae44e | 12 | { |
seangshim | 5:f630dbbae44e | 13 | if(&i2c_ == reinterpret_cast<I2C*>(&i2cRaw)) |
seangshim | 5:f630dbbae44e | 14 | reinterpret_cast<I2C*>(&i2cRaw)->~I2C(); |
seangshim | 5:f630dbbae44e | 15 | } |
seangshim | 5:f630dbbae44e | 16 | void HMC5883L::init() |
seangshim | 5:f630dbbae44e | 17 | { |
seangshim | 5:f630dbbae44e | 18 | // init - configure your setup here |
seangshim | 5:f630dbbae44e | 19 | setConfigurationA(AVG8_SAMPLES | OUTPUT_RATE_15); // 8 sample average, 15Hz, normal mode |
seangshim | 5:f630dbbae44e | 20 | setConfigurationB(0x20); // default |
seangshim | 5:f630dbbae44e | 21 | setMode(CONTINUOUS_MODE); // continuous sample mode |
seangshim | 5:f630dbbae44e | 22 | } |
seangshim | 5:f630dbbae44e | 23 | |
seangshim | 5:f630dbbae44e | 24 | void HMC5883L::setConfigurationA(char config) |
seangshim | 5:f630dbbae44e | 25 | { |
seangshim | 5:f630dbbae44e | 26 | char cmd[2]; |
seangshim | 5:f630dbbae44e | 27 | cmd[0] = CONFIG_A_REG; // register a address |
seangshim | 5:f630dbbae44e | 28 | cmd[1] = config; |
seangshim | 5:f630dbbae44e | 29 | i2c_.write(I2C_ADDRESS, cmd, 2); |
seangshim | 5:f630dbbae44e | 30 | } |
seangshim | 5:f630dbbae44e | 31 | |
seangshim | 5:f630dbbae44e | 32 | void HMC5883L::setConfigurationB(char config) |
seangshim | 5:f630dbbae44e | 33 | { |
seangshim | 5:f630dbbae44e | 34 | char cmd[2]; |
seangshim | 5:f630dbbae44e | 35 | cmd[0] = CONFIG_B_REG; // register b address |
seangshim | 5:f630dbbae44e | 36 | cmd[1] = config; |
seangshim | 5:f630dbbae44e | 37 | i2c_.write(I2C_ADDRESS, cmd, 2); |
seangshim | 5:f630dbbae44e | 38 | } |
seangshim | 5:f630dbbae44e | 39 | |
seangshim | 5:f630dbbae44e | 40 | char HMC5883L::getConfigurationA() |
seangshim | 5:f630dbbae44e | 41 | { |
seangshim | 5:f630dbbae44e | 42 | char cmd[2]; |
seangshim | 5:f630dbbae44e | 43 | cmd[0] = CONFIG_A_REG; // register a address |
seangshim | 5:f630dbbae44e | 44 | i2c_.write(I2C_ADDRESS, cmd, 1, true); |
seangshim | 5:f630dbbae44e | 45 | i2c_.read(I2C_ADDRESS, &cmd[1], 1, false); |
seangshim | 5:f630dbbae44e | 46 | return cmd[1]; |
seangshim | 5:f630dbbae44e | 47 | } |
seangshim | 5:f630dbbae44e | 48 | |
seangshim | 5:f630dbbae44e | 49 | char HMC5883L::getConfigurationB() |
seangshim | 5:f630dbbae44e | 50 | { |
seangshim | 5:f630dbbae44e | 51 | char cmd[2]; |
seangshim | 5:f630dbbae44e | 52 | cmd[0] = CONFIG_B_REG; // register b address |
seangshim | 5:f630dbbae44e | 53 | i2c_.write(I2C_ADDRESS, cmd, 1, true); |
seangshim | 5:f630dbbae44e | 54 | i2c_.read(I2C_ADDRESS, &cmd[1], 1, false); |
seangshim | 5:f630dbbae44e | 55 | return cmd[1]; |
seangshim | 5:f630dbbae44e | 56 | } |
seangshim | 5:f630dbbae44e | 57 | |
seangshim | 5:f630dbbae44e | 58 | void HMC5883L::setMode(char mode = SINGLE_MODE) |
seangshim | 5:f630dbbae44e | 59 | { |
seangshim | 5:f630dbbae44e | 60 | char cmd[2]; |
seangshim | 5:f630dbbae44e | 61 | cmd[0] = MODE_REG; // mode register address |
seangshim | 5:f630dbbae44e | 62 | cmd[1] = mode; |
seangshim | 5:f630dbbae44e | 63 | i2c_.write(I2C_ADDRESS,cmd,2); |
seangshim | 5:f630dbbae44e | 64 | } |
seangshim | 5:f630dbbae44e | 65 | |
seangshim | 5:f630dbbae44e | 66 | char HMC5883L::getMode() |
seangshim | 5:f630dbbae44e | 67 | { |
seangshim | 5:f630dbbae44e | 68 | char cmd[2]; |
seangshim | 5:f630dbbae44e | 69 | cmd[0] = MODE_REG; // mode register |
seangshim | 5:f630dbbae44e | 70 | i2c_.write(I2C_ADDRESS, cmd, 1, true); |
seangshim | 5:f630dbbae44e | 71 | i2c_.read(I2C_ADDRESS, &cmd[1], 1, false); |
seangshim | 5:f630dbbae44e | 72 | return cmd[1]; |
seangshim | 5:f630dbbae44e | 73 | } |
seangshim | 5:f630dbbae44e | 74 | |
seangshim | 5:f630dbbae44e | 75 | char HMC5883L::getStatus() |
seangshim | 5:f630dbbae44e | 76 | { |
seangshim | 5:f630dbbae44e | 77 | char cmd[2]; |
seangshim | 5:f630dbbae44e | 78 | cmd[0] = STATUS_REG; // status register |
seangshim | 5:f630dbbae44e | 79 | i2c_.write(I2C_ADDRESS, cmd, 1, true); |
seangshim | 5:f630dbbae44e | 80 | i2c_.read(I2C_ADDRESS, &cmd[1], 1, false); |
seangshim | 5:f630dbbae44e | 81 | return cmd[1]; |
seangshim | 5:f630dbbae44e | 82 | } |
seangshim | 5:f630dbbae44e | 83 | |
seangshim | 5:f630dbbae44e | 84 | void HMC5883L::getXYZ(int16_t output[3])//データの取得 |
seangshim | 5:f630dbbae44e | 85 | { |
seangshim | 5:f630dbbae44e | 86 | char cmd[2]; |
seangshim | 5:f630dbbae44e | 87 | char data[6]; |
seangshim | 5:f630dbbae44e | 88 | cmd[0] = 0x03; // starting point for reading |
seangshim | 5:f630dbbae44e | 89 | i2c_.write(I2C_ADDRESS, cmd, 1, true); // set the pointer to the start of x |
seangshim | 5:f630dbbae44e | 90 | i2c_.read(I2C_ADDRESS, data, 6, false); |
seangshim | 5:f630dbbae44e | 91 | for(int i = 0; i < 3; i++) // fill the output variables |
seangshim | 5:f630dbbae44e | 92 | output[i] = int16_t(((unsigned char)data[i*2] << 8) | (unsigned char)data[i*2+1]); |
seangshim | 5:f630dbbae44e | 93 | } |
seangshim | 5:f630dbbae44e | 94 | |
seangshim | 5:f630dbbae44e | 95 | double HMC5883L::getHeadingXY() |
seangshim | 5:f630dbbae44e | 96 | { |
seangshim | 5:f630dbbae44e | 97 | int16_t raw_data[3]; |
seangshim | 5:f630dbbae44e | 98 | getXYZ(raw_data); |
seangshim | 5:f630dbbae44e | 99 | double heading = atan2(static_cast<double>(raw_data[2]), static_cast<double>(raw_data[0])); // heading = arctan(Y/X) (Y/X)の逆正接 |
seangshim | 5:f630dbbae44e | 100 | // 角度範囲の補正 |
seangshim | 5:f630dbbae44e | 101 | if(heading < 0.0) // fix sign |
seangshim | 5:f630dbbae44e | 102 | heading += PI2; |
seangshim | 5:f630dbbae44e | 103 | if(heading > PI2) // fix overflow |
seangshim | 5:f630dbbae44e | 104 | heading -= PI2; |
seangshim | 5:f630dbbae44e | 105 | return heading; |
seangshim | 5:f630dbbae44e | 106 | } |