/** * \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(); }