cansat-d_2018 / Mbed 2 deprecated cansat_main_new_3

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?

UserRevisionLine numberNew 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 }