cansat-d_2018 / HMC5883L

This program is prototype for getting direction.

HMC5883L.cpp@7:3cf4fb613ebb, 2018-10-19 (annotated)

Committer:
394
Date:
Fri Oct 19 12:33:08 2018 +0000
Revision:
7:3cf4fb613ebb
Parent:
6:48004ffd01c5 
prototype

Who changed what in which revision?

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