RoverQMC5883L and RoverHMC5883L classes added. Signed-off-by: Mustafa Ozcelikors <mozcelikors@gmail.com>
diff --git a/rover/include/roverapi/rover_hmc5883l.hpp b/rover/include/roverapi/rover_hmc5883l.hpp new file mode 100644 index 0000000..55fb394 --- /dev/null +++ b/rover/include/roverapi/rover_hmc5883l.hpp
@@ -0,0 +1,153 @@ +/* + * Copyright (c) 2017 FH Dortmund and others + * All rights reserved. This program and the accompanying materials + * are made available under the terms of the Eclipse Public License v1.0 + * which accompanies this distribution, and is available at + * http://www.eclipse.org/legal/epl-v10.html + * + * Description: + * RoverHMC5883L API - Interfaces for Rover HMC5883L bearing sensor application development + * Header file + * + * Contributors: + * M.Ozcelikors <mozcelikors@gmail.com>, created RoverHMC5883L class 04.12.2017 + * David Schmelter, Fraunhofer IEM - compass sensor initial implementation + * + */ + +#ifndef ROVERAPI_ROVER_HMC5883L_HPP_ +#define ROVERAPI_ROVER_HMC5883L_HPP_ + +#include <roverapi/rover_sensor.hpp> +#include <stdint.h> +#include <stdio.h> +#include <unistd.h> + +namespace rover +{ + /** + * @brief RoverHMC5883L is a class that is inherited from RoverSensor abstract class. RoverHMC5883L class contains member functions and variables to set up and read from HMC5883L bearing sensor that is embedded on the rover. + */ + class RoverHMC5883L : public RoverSensor + { + private: + /* Definitions related to compass sensor */ + /** + * @brief Address for compass sensor + */ + int HMC588L_ADDRESS; + + /** + * @brief Calibration duration for compass sensor + */ + int CALIBRATION_DURATION; + + /** + * @brief Declination angle / correction factor for compass sensor + */ + float DECLINATION_ANGLE; + + /** + * @brief Calibration variable for HMC5883L + */ + mutable unsigned int calibration_start; + + /** + * @brief Flag to hold if HMC5883L is set up + */ + int ROVERHMC5883L_SETUP_; + + public: + /** + * @brief Constructor for the RoverHMC5883L class + */ + explicit RoverHMC5883L (); + + /** + * @brief Destructor for the RoverHMC5883L class + */ + ~RoverHMC5883L(); + + /** + * @brief Function to setup HMC5883L sensor + * @return void + */ + void initialize (void); + + /** + * @brief Reads the bearing value from Bearing sensor Sunfounder HMC5883L (float). + * @return bearing_val in degrees (float). + */ + float read (void); + + /** + * @brief Starts calibration for the bearing sensor + * @return void + */ + void calibrate (void) const; + + /** + * @brief Sets the HMC588L Address + * @param address Address to set + * @return void + */ + void setHMC588LAddress (const int address); + + /** + * @brief Sets the HMC588L Calibration Duration + * @param period Period to set + * @return void + */ + void setHMC588LCalibrationPeriod(const int period); + + /** + * @brief Sets the HMC588L Declination angle + * @param angle Declination angle to set + * @return void + */ + void setHMC588LDeclinationAngle (const float angle); + + /** + * @brief Returns the private attribute HMC588L_ADDRESS + * @return address Address to return + */ + int getHMC588LAddress (void); + + /** + * @brief Returns the HMC588L Calibration Duration + * @return period Period to return + */ + int getHMC588LCalibrationPeriod (void); + + /** + * @brief Returns the HMC588L Declination angle + * @return angle Declination angle to return + */ + float getHMC588LDeclinationAngle (void); + + private: + + /** + * @brief Inline macro to find minimum of two values regardles of their types (template). + * @param a Parameter 1 + * @param b Parameter 2 + * @return minimum value + */ + template<typename T> + inline T MINIMUM_ (const T& a, const T& b); + + /** + * @brief Inline macro to find maximum of two values regardles of their types (template). + * @param a Parameter 1 + * @param b Parameter 2 + * @return maximum value + */ + template<typename T> + inline T MAXIMUM_ (const T& a, const T& b); + + }; +} + + + +#endif /* ROVERAPI_ROVER_HMC5883L_HPP_ */
diff --git a/rover/include/roverapi/rover_qmc5883l.hpp b/rover/include/roverapi/rover_qmc5883l.hpp new file mode 100644 index 0000000..81b1c00 --- /dev/null +++ b/rover/include/roverapi/rover_qmc5883l.hpp
@@ -0,0 +1,149 @@ +/* + * Copyright (c) 2017 FH Dortmund and others + * All rights reserved. This program and the accompanying materials + * are made available under the terms of the Eclipse Public License v1.0 + * which accompanies this distribution, and is available at + * http://www.eclipse.org/legal/epl-v10.html + * + * Description: + * RoverQMC5883L API - Interfaces for Rover QMC5883L bearing sensor application development + * Header file + * + * Contributors: + * M.Ozcelikors <mozcelikors@gmail.com>, created RoverQMC5883L class 04.12.2017 + * + */ + +#ifndef ROVERAPI_ROVER_QMC5883L_HPP_ +#define ROVERAPI_ROVER_QMC5883L_HPP_ + +#include <roverapi/rover_sensor.hpp> + +namespace rover +{ + /** + * @brief RoverQMC5883L is a class that is inherited from RoverSensor abstract class. RoverQMC5883L class contains member functions and variables to set up and read from QMC5883L bearing sensor that is embedded on the rover. + */ + class RoverQMC5883L : public RoverSensor + { + private: + /* Definitions related to compass sensor */ + /** + * @brief Address for compass sensor + */ + int QMC5883L_ADDRESS; + + /** + * @brief Calibration duration for compass sensor + */ + int CALIBRATION_DURATION; + + /** + * @brief Declination angle / correction factor for compass sensor + */ + float DECLINATION_ANGLE; + + /** + * @brief Calibration variable for QMC5883L + */ + mutable unsigned int calibration_start; + + /** + * @brief Flag to hold if RoverQMC5883L is set up + */ + int ROVERQMC5883L_SETUP_; + + public: + /** + * @brief Constructor for the RoverQMC5883L class + */ + explicit RoverQMC5883L (); + + /** + * @brief Destructor for the RoverQMC5883L class + */ + ~RoverQMC5883L(); + + /** + * @brief Function to setup QMC5883L sensor + * @return void + */ + void initialize (void); + + /** + * @brief Reads the bearing value from Bearing sensor QMC5883L (float). + * @return bearing_val in degrees (float). + */ + float read (void); + + /** + * @brief Starts calibration for the bearing sensor + * @return void + */ + void calibrate (void) const; + + /** + * @brief Sets the QMC588L Address + * @param address Address to set + * @return void + */ + void setQMC5883LAddress (const int address); + + /** + * @brief Sets the QMC588L Calibration Duration + * @param period Period to set + * @return void + */ + void setQMC5883LCalibrationPeriod(const int period); + + /** + * @brief Sets the QMC588L Declination angle + * @param angle Declination angle to set + * @return void + */ + void setQMC5883LDeclinationAngle (const float angle); + + /** + * @brief Returns the private attribute QMC588L_ADDRESS + * @return address Address to return + */ + int getQMC5883LAddress (void); + + /** + * @brief Returns the QMC588L Calibration Duration + * @return period Period to return + */ + int getQMC5883LCalibrationPeriod (void); + + /** + * @brief Returns the QMC588L Declination angle + * @return angle Declination angle to return + */ + float getQMC5883LDeclinationAngle (void); + + private: + + /** + * @brief Inline macro to find minimum of two values regardles of their types (template). + * @param a Parameter 1 + * @param b Parameter 2 + * @return minimum value + */ + template<typename T> + inline T MINIMUM_ (const T& a, const T& b); + + /** + * @brief Inline macro to find maximum of two values regardles of their types (template). + * @param a Parameter 1 + * @param b Parameter 2 + * @return maximum value + */ + template<typename T> + inline T MAXIMUM_ (const T& a, const T& b); + + }; +} + + + +#endif /* ROVERAPI_ROVER_QMC5883L_HPP_ */
diff --git a/rover/src/roverapi/rover_hmc5883l.cpp b/rover/src/roverapi/rover_hmc5883l.cpp new file mode 100644 index 0000000..e36a428 --- /dev/null +++ b/rover/src/roverapi/rover_hmc5883l.cpp
@@ -0,0 +1,176 @@ +/* + * Copyright (c) 2017 FH Dortmund and others + * All rights reserved. This program and the accompanying materials + * are made available under the terms of the Eclipse Public License v1.0 + * which accompanies this distribution, and is available at + * http://www.eclipse.org/legal/epl-v10.html + * + * Description: + * RoverHMC5883L API - Interfaces for Rover HMC5883L bearing sensor application development + * + * Contributors: + * M.Ozcelikors <mozcelikors@gmail.com>, created RoverHMC5883L class 04.12.2017 + * David Schmelter, Fraunhofer IEM - compass sensor initial implementation + * + */ + +#include <roverapi/rover_hmc5883l.hpp> +#include <wiringPi.h> +#include <wiringPiI2C.h> +#include <math.h> +#include <stdint.h> +#include <stdio.h> +#include <unistd.h> + +static int i2c_hmc588l_fd = -1; + +static int16_t xMinRaw = 0; +static int16_t xMaxRaw = 0; +static int16_t yMaxRaw = 0; +static int16_t yMinRaw = 0; + +rover::RoverHMC5883L::RoverHMC5883L() +:HMC588L_ADDRESS(0x1E), //default: 0x1E + CALIBRATION_DURATION(10000), //compass calibration has a duration of 5 seconds + DECLINATION_ANGLE(0.0413), //correction factor for location Paderborn + calibration_start(0), + ROVERHMC5883L_SETUP_(0) +{ + +} + +rover::RoverHMC5883L::~RoverHMC5883L(){} + +void rover::RoverHMC5883L::initialize (void) +{ +#ifdef DEBUG + printf ("HMC588L Address is: %x\n", this->HMC588L_ADDRESS); + printf ("HMC588L Calibration period is %d\n", this->CALIBRATION_DURATION); + printf ("HMC588L Declination angle is %f\n", this->DECLINATION_ANGLE); +#endif + + if ((i2c_hmc588l_fd = wiringPiI2CSetup(this->HMC588L_ADDRESS)) < 0) { + printf("Failed to initialize HMC588L compass sensor"); + } + + if (i2c_hmc588l_fd >= 0) { + int8_t gain = 5; + + wiringPiI2CWriteReg8(i2c_hmc588l_fd, 0x00, 0x70); // 8-average, 15 Hz default, positive self test measurement + wiringPiI2CWriteReg8(i2c_hmc588l_fd, 0x01, gain << 5); // Gain + wiringPiI2CWriteReg8(i2c_hmc588l_fd, 0x02, 0x00); // Continuous-measurement mode + } + + this->ROVERHMC5883L_SETUP_ = 1; + + this->calibration_start = millis(); +} + +float rover::RoverHMC5883L::read (void) +{ + if (this->ROVERHMC5883L_SETUP_ != 1) + { + fprintf(stderr,"You havent initialized RoverHMC5883L. Use RoverHMC5883L()::initialize() !\n"); + } + else + { + int8_t buffer[6]; + + //potential optimization: wiringPiI2CReadReg16 + buffer[0] = wiringPiI2CReadReg8(i2c_hmc588l_fd, 0x03); //MSB x + buffer[1] = wiringPiI2CReadReg8(i2c_hmc588l_fd, 0x04); //LSB x + buffer[2] = wiringPiI2CReadReg8(i2c_hmc588l_fd, 0x05); //MSB z + buffer[3] = wiringPiI2CReadReg8(i2c_hmc588l_fd, 0x06); //LSB z + buffer[4] = wiringPiI2CReadReg8(i2c_hmc588l_fd, 0x07); //MSB y + buffer[5] = wiringPiI2CReadReg8(i2c_hmc588l_fd, 0x08); //LSB y + + int16_t xRaw = (((int16_t) buffer[0] << 8) & 0xff00) | buffer[1]; + //int16_t zRaw = (((int16_t) buffer[2] << 8) & 0xff00) | buffer[3]; + int16_t yRaw = (((int16_t) buffer[4] << 8) & 0xff00) | buffer[5]; + + //if calibration is active calculate minimum and maximum x/y values for calibration + if (millis() <= this->calibration_start + this->CALIBRATION_DURATION) + { + xMinRaw = MINIMUM_(xRaw, xMinRaw); + + xMaxRaw = MAXIMUM_(xRaw, xMaxRaw); + + yMinRaw = MINIMUM_(yRaw, yMinRaw); + + yMaxRaw = MAXIMUM_(yRaw, yMaxRaw); + } + + //calibration: move and scale x coordinates based on minimum and maximum values to get a unit circle + float xf = xRaw - (float) (xMinRaw + xMaxRaw) / 2.0f; + xf = xf / (xMinRaw + xMaxRaw) * 2.0f; + + //calibration: move and scale y coordinates based on minimum and maximum values to get a unit circle + float yf = yRaw - (float) (yMinRaw + yMaxRaw) / 2.0f; + yf = yf / (yMinRaw + yMaxRaw) * 2.0f; + + float bearing = atan2(yf, xf); + #ifdef DEBUG + printf("%f, bearing\n", bearing); + #endif + + //location specific magnetic field correction + bearing += this->DECLINATION_ANGLE; + + if (bearing < 0) { + bearing += 2 * M_PI; + } + + if (bearing > 2 * M_PI) { + bearing -= 2 * M_PI; + } + + float headingDegrees = bearing * (180.0 / M_PI); + #ifdef DEBUG + printf("%lf, headingDegrees\n", headingDegrees); + #endif + return headingDegrees; + } +} + +void rover::RoverHMC5883L::setHMC588LAddress (const int address) +{ + this->HMC588L_ADDRESS = address; +} + +void rover::RoverHMC5883L::setHMC588LDeclinationAngle (const float angle) +{ + this->DECLINATION_ANGLE = angle; +} + +void rover::RoverHMC5883L::setHMC588LCalibrationPeriod(const int period) +{ + this->CALIBRATION_DURATION = period; +} + +int rover::RoverHMC5883L::getHMC588LAddress (void) +{ + return this->HMC588L_ADDRESS; +} + +float rover::RoverHMC5883L::getHMC588LDeclinationAngle (void) +{ + return this->DECLINATION_ANGLE; +} + +int rover::RoverHMC5883L::getHMC588LCalibrationPeriod (void) +{ + return this->CALIBRATION_DURATION; +} + +template<typename T> +inline T rover::RoverHMC5883L::MINIMUM_(const T& a, const T& b) +{ + return (a < b ? a : b); +} + +template<typename T> +inline T rover::RoverHMC5883L::MAXIMUM_(const T& a, const T& b) +{ + return (a > b ? a : b); +} +
diff --git a/rover/src/roverapi/rover_qmc5883l.cpp b/rover/src/roverapi/rover_qmc5883l.cpp new file mode 100644 index 0000000..a043ba7 --- /dev/null +++ b/rover/src/roverapi/rover_qmc5883l.cpp
@@ -0,0 +1,186 @@ +/* + * Copyright (c) 2017 FH Dortmund and others + * All rights reserved. This program and the accompanying materials + * are made available under the terms of the Eclipse Public License v1.0 + * which accompanies this distribution, and is available at + * http://www.eclipse.org/legal/epl-v10.html + * + * Description: + * RoverQMC5883L API - Interfaces for Rover QMC5883L bearing sensor application development + * Header file + * + * Contributors: + * M.Ozcelikors <mozcelikors@gmail.com>, created RoverQMC5883L class 04.12.2017 + * + */ + + +#include <roverapi/rover_qmc5883l.hpp> +#include <wiringPi.h> +#include <wiringPiI2C.h> +#include <math.h> +#include <stdint.h> +#include <stdio.h> +#include <unistd.h> + +static int i2c_qmc588l_fd = -1; + +static int16_t xMinRaw_ = 0; +static int16_t xMaxRaw_ = 0; +static int16_t yMaxRaw_ = 0; +static int16_t yMinRaw_ = 0; + +rover::RoverQMC5883L::RoverQMC5883L() +:QMC5883L_ADDRESS(0x0D), //default: 0x0D + CALIBRATION_DURATION(10000), //compass calibration has a duration of 5 seconds + DECLINATION_ANGLE(0.0413), //correction factor for location Paderborn + calibration_start(0), + ROVERQMC5883L_SETUP_(0) +{ + +} + +rover::RoverQMC5883L::~RoverQMC5883L(){} + +void rover::RoverQMC5883L::initialize (void) +{ + if ((i2c_qmc588l_fd = wiringPiI2CSetup(this->QMC5883L_ADDRESS)) < 0) + { + printf("Failed to initialize QMC588L compass sensor"); + } + + if (i2c_qmc588l_fd >= 0) + { + wiringPiI2CWriteReg8 (i2c_qmc588l_fd, 0x0B, 0x01); //init SET/PERIOD register + + /* + Define + OSR = 512 + Full Scale Range = 8G(Gauss) + ODR = 200HZ + set continuous measurement mode + */ + wiringPiI2CWriteReg8 (i2c_qmc588l_fd, 0x09, 0x1D); + } + + this->ROVERQMC5883L_SETUP_ = 1; + + this->calibration_start = millis(); + + // + // To do a software reset + // wiringPiI2CWriteReg8 (i2c_hmc588l_fd, 0x0A, 0x80); + // +} + +float rover::RoverQMC5883L::read (void) +{ + if (this->ROVERQMC5883L_SETUP_ != 1) + { + fprintf(stderr,"You havent set up RoverQMC5883L. Use RoverQMC5883L()::initialize() !\n"); + } + else + { + int8_t buffer[6]; + + //wiringPiI2CWrite (i2c_hmc588l_fd, 0x00); //Start with register 3 + //delay(25); + + buffer[0] = wiringPiI2CReadReg8(i2c_qmc588l_fd, 0x00); //LSB x + buffer[1] = wiringPiI2CReadReg8(i2c_qmc588l_fd, 0x01); //MSB x + buffer[2] = wiringPiI2CReadReg8(i2c_qmc588l_fd, 0x02); //LSB y + buffer[3] = wiringPiI2CReadReg8(i2c_qmc588l_fd, 0x03); //MSB y + buffer[4] = wiringPiI2CReadReg8(i2c_qmc588l_fd, 0x04); //LSB z + buffer[5] = wiringPiI2CReadReg8(i2c_qmc588l_fd, 0x05); //MSB z + + int16_t xRaw = (((int16_t) buffer[1] << 8) & 0xff00) | buffer[0]; + int16_t yRaw = (((int16_t) buffer[3] << 8) & 0xff00) | buffer[2]; + #ifdef DEBUG + printf ("%d %d\n",xRaw,yRaw); + #endif + + //if calibration is active calculate minimum and maximum x/y values for calibration + if (millis() <= this->calibration_start + this->CALIBRATION_DURATION) + { + xMinRaw_ = MINIMUM_(xRaw, xMinRaw_); + + xMaxRaw_ = MAXIMUM_(xRaw, xMaxRaw_); + + yMinRaw_ = MINIMUM_(yRaw, yMinRaw_); + + yMaxRaw_ = MAXIMUM_(yRaw, yMaxRaw_); + } + + //calibration: move and scale x coordinates based on minimum and maximum values to get a unit circle + float xf = xRaw - (float) (xMinRaw_ + xMaxRaw_) / 2.0f; + xf = xf / (xMinRaw_ + xMaxRaw_) * 2.0f; + + //calibration: move and scale y coordinates based on minimum and maximum values to get a unit circle + float yf = yRaw - (float) (yMinRaw_ + yMaxRaw_) / 2.0f; + yf = yf / (yMinRaw_ + yMaxRaw_) * 2.0f; + + float bearing = atan2(yf, xf); + #ifdef DEBUG + printf("%f, bearing\n", bearing); + #endif + + //location specific magnetic field correction + bearing += this->DECLINATION_ANGLE; + + if (bearing < 0) { + bearing += 2 * M_PI; + } + + if (bearing > 2 * M_PI) { + bearing -= 2 * M_PI; + } + + float headingDegrees = bearing * (180.0 / M_PI); + #ifdef DEBUG + printf("%lf, headingDegrees\n", headingDegrees); + #endif + return headingDegrees; + } +} + +void rover::RoverQMC5883L::setQMC5883LAddress (const int address) +{ + this->QMC5883L_ADDRESS = address; +} + +void rover::RoverQMC5883L::setQMC5883LDeclinationAngle (const float angle) +{ + this->DECLINATION_ANGLE = angle; +} + +void rover::RoverQMC5883L::setQMC5883LCalibrationPeriod(const int period) +{ + this->CALIBRATION_DURATION = period; +} + +int rover::RoverQMC5883L::getQMC5883LAddress (void) +{ + return this->QMC5883L_ADDRESS; +} + +float rover::RoverQMC5883L::getQMC5883LDeclinationAngle (void) +{ + return this->DECLINATION_ANGLE; +} + +int rover::RoverQMC5883L::getQMC5883LCalibrationPeriod (void) +{ + return this->CALIBRATION_DURATION; +} + +template<typename T> +inline T rover::RoverQMC5883L::MINIMUM_(const T& a, const T& b) +{ + return (a < b ? a : b); +} + +template<typename T> +inline T rover::RoverQMC5883L::MAXIMUM_(const T& a, const T& b) +{ + return (a > b ? a : b); +}