/**
* \par Function
* getHeadingZ
* \par Description
* Get the sensor value of Z-axis.
* \param[in]
* None
* \par Output
* None
* \return
* The sensor value of Z-axis. If error, will return a error code.
* \par Others
* The sensor value is a 16 bits signed integer.
*/
int16_t MeCompass::getHeadingZ(void)
{
int8_t return_value = 0;
deviceCalibration();
return_value = readData(COMPASS_RA_DATAZ_H, buffer, 2);
if(return_value != 0)
{
return return_value;
}
if(Measurement_Mode == COMPASS_MODE_SINGLE)
{
writeReg(COMPASS_RA_MODE, COMPASS_MODE_SINGLE);
}
return ( ( (int16_t)buffer[0] ) << 8) | buffer[1];
}
/**
* \par Function
* getAngle
* \par Description
* Calculate the yaw angle by the calibrated sensor value.
* \param[in]
* None
* \par Output
* None
* \return
* The angle value from 0 to 360 degrees. If not success, return an error code.
* \par Others
* Will return a correct angle when you keep the MeCompass working in the plane which have calibrated.
*/
double MeCompass::getAngle(void)
{
int16_t cx,cy,cz;
double compass_angle;
int8_t return_value = 0;
deviceCalibration();
return_value = getHeading(&cx, &cy, &cz);
if(return_value != 0)
{
return (double)return_value;
}
if(Calibration_Flag == true)
{
cx = (cx + Cal_parameter.X_excursion) * Cal_parameter.X_gain;
cy = (cy + Cal_parameter.Y_excursion) * Cal_parameter.Y_gain;
cz = (cz + Cal_parameter.Z_excursion) * Cal_parameter.Z_gain;
if(Cal_parameter.Rotation_Axis == 1) //X_Axis
{
compass_angle = atan2( (double)cy, (double)cz );
}
else if(Cal_parameter.Rotation_Axis == 2) //Y_Axis
{
compass_angle = atan2( (double)cx, (double)cz );
}
else if(Cal_parameter.Rotation_Axis == 3) //Z_Axis
{
compass_angle = atan2( (double)cy, (double)cx );
}
}
else
{
compass_angle = atan2( (double)cy, (double)cx );
}
if(compass_angle < 0)
{
compass_angle = (compass_angle + 2 * COMPASS_PI) * 180 / COMPASS_PI;
}
else
{
compass_angle = compass_angle * 180 / COMPASS_PI;
}
return compass_angle;
}
/**
* \par Function
* getHeading
* \par Description
* Get the sensor value of 3 axes including X-axis, Y-axis and Z-axis.
* \param[in]
* x - the address of the variable you want to store the value in.
* \param[in]
* y - the address of the variable you want to store the value in.
* \param[in]
* z - the address of the variable you want to store the value in.
* \par Output
* None
* \return
* If error, will return a error code, else return 0.
* \par Others
* The sequence of the sensor data registors of HMC5883 is X, Z, Y.
*/
int16_t MeCompass::getHeading(int16_t *x, int16_t *y, int16_t *z)
{
int8_t return_value = 0;
deviceCalibration();
return_value = readData(COMPASS_RA_DATAX_H, buffer, 6);
if(return_value != 0)
{
return return_value;
}
if(Measurement_Mode == COMPASS_MODE_SINGLE)
{
writeReg(COMPASS_RA_MODE, COMPASS_MODE_SINGLE);
}
*x = ( ( (int16_t)buffer[0] ) << 8) | buffer[1];
*y = ( ( (int16_t)buffer[4] ) << 8) | buffer[5];
*z = ( ( (int16_t)buffer[2] ) << 8) | buffer[3];
return return_value;
}
/**
* \par Function
* begin
* \par Description
* Initialize the MeCompass.
* \param[in]
* None
* \par Output
* None
* \return
* None
* \par Others
* You can check the HMC5883 datasheet for the macro definition.
*/
void MeCompass::begin(void)
{
Wire.begin();
#ifdef ME_PORT_DEFINED
dWrite2(HIGH);//LED
#else // ME_PORT_DEFINED
pinMode(_ledPin, OUTPUT);
digitalWrite(_ledPin, HIGH);
#endif
// write CONFIG_A register
writeReg(COMPASS_RA_CONFIG_A, COMPASS_AVERAGING_8 | COMPASS_RATE_15 | COMPASS_BIAS_NORMAL);
// write CONFIG_B register
writeReg(COMPASS_RA_CONFIG_B, COMPASS_GAIN_1090);
// write MODE register
Measurement_Mode = COMPASS_MODE_SINGLE;
writeReg(COMPASS_RA_MODE, Measurement_Mode);
read_EEPROM_Buffer();
deviceCalibration();
}
/**
* \par Function
* begin
* \par Description
* Initialize the MeGyro.
* \param[in]
* None
* \par Output
* None
* \return
* None
* \par Others
* You can check the MPU6050 datasheet for the registor address.
*/
void MeGyro::begin(void)
{
gSensitivity = 65.5; //for 500 deg/s, check data sheet
gx = 0;
gy = 0;
gz = 0;
gyrX = 0;
gyrY = 0;
gyrZ = 0;
accX = 0;
accY = 0;
accZ = 0;
gyrXoffs = 0;
gyrYoffs = 0;
gyrZoffs = 0;
Wire.begin();
delay(800);
writeReg(0x6b, 0x00);//close the sleep mode
writeReg(0x1a, 0x01);//configurate the digital low pass filter
writeReg(0x1b, 0x08);//set the gyro scale to 500 deg/s
deviceCalibration();
}
