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);
+}
