Modularization and correction across multiple files
Signed-off-by: Mustafa Ozcelikors <mozcelikors@gmail.com>
diff --git a/rover/.travis.yml b/rover/.travis.yml
index 1d0d267..ede0474 100644
--- a/rover/.travis.yml
+++ b/rover/.travis.yml
@@ -13,4 +13,4 @@
github_token: $GITHUB_TOKEN2 #Create token from Github and Set in travis-ci.org dashboard
local_dir: ./html/
on:
- branch: feature/apiapp
+ branch: master
diff --git a/rover/include/roverapi/rover_api.hpp b/rover/include/roverapi/rover_api.hpp
index 2030d15..c65fc28 100644
--- a/rover/include/roverapi/rover_api.hpp
+++ b/rover/include/roverapi/rover_api.hpp
@@ -63,7 +63,7 @@
// Initialize all components of the rover
r.initialize();
- // or use r.initializeRoverSensors(), r.initializeRoverDisplay(), r.initializeRoverDriving(), r.initializeRoverGpio() to initialize individual components
+ // or use r.initializeWiringPi(), r.initializeRoverSensors(), r.initializeRoverDisplay(), r.initializeRoverDriving(), r.initializeRoverGpio() to initialize individual components
// Set-up cloud instance and register your device
RoverCloud r_cloud = r.inRoverCloud();
@@ -103,7 +103,7 @@
r_sensors.readInfraredSensor(r_sensors.ROVER_REAR_RIGHT));
printf ("Temperature = %f\n", r_sensors.readTemperature());
printf ("Humidity = %f\n", r_sensors.readHumidity());
- printf ("Bearing = %f\n", r_sensors.readBearing());
+ printf ("Bearing = %f\n", r_sensors.readBearingHMC5883L());
// Checking if a button is pressed (LOW) and playing a tone with buzzer
if (r.inRoverGpio().readUserButton() == r.inRoverGpio().LO)
@@ -229,12 +229,17 @@
*/
int ROVER_DRIVING_INIT_;
+ /**
+ * @brief Flag for indicating whether wiringPi library initialized or not.
+ */
+ int WIRINGPI_INIT_;
+
public:
/**
* @brief Constructor for the RoverBase class
*/
- RoverBase();
+ explicit RoverBase();
/**
* @brief Destructor for the RoverBase class
@@ -242,9 +247,17 @@
virtual ~RoverBase();
/**
- * @brief Initializes the RoverBase
- * @return void
- */
+ * @brief Initializes the all classes, sensors, libraries for the Rover.
+ * @return void
+ *
+ * \warning This function can only be used once in Rover's Raspberry Pi. You can still use a second RoverBase object and configure that object. However, initializing in the low-level sense causes problems.
+ * \code{.cpp}
+ * RoverBase r;
+ * RoverBase x;
+ * r.initialize();
+ * x.initialize(); //^^^THIS IS NOT VALID AND WILL PRODUCE ERRORS
+ * \endcode
+ */
void initialize (void);
/**
@@ -255,7 +268,7 @@
/**
* @brief Sleep function to be used in rover applications
- * @param Period to sleep in milliseconds
+ * @param period_ms Period to sleep in milliseconds
* @return void
*/
void sleep (unsigned int period_ms);
@@ -319,6 +332,12 @@
* @return void
*/
void initializeRoverSensors (void);
+
+ /**
+ * @brief Initializes wiringPi library to access GPIO of Rover. This function should be called in every program run and must only be called once.
+ * @return void
+ */
+ void initializeWiringPi (void);
};
}
diff --git a/rover/include/roverapi/rover_cloud.hpp b/rover/include/roverapi/rover_cloud.hpp
index 0adc28e..8e985b4 100644
--- a/rover/include/roverapi/rover_cloud.hpp
+++ b/rover/include/roverapi/rover_cloud.hpp
@@ -51,7 +51,7 @@
/**
* @brief Constructor for the RoverCloud
*/
- RoverCloud();
+ explicit RoverCloud();
/**
* @brief Sets private attribute HOST_NAME
@@ -96,7 +96,7 @@
/**
* @brief Sets the default REGISTRATION_PORT variable
- * @param Port (int) to be set as REGISTRATION_PORT
+ * @param port (int) to be set as REGISTRATION_PORT
* @return void
*/
void setRegistrationPort (int port);
diff --git a/rover/include/roverapi/rover_display.hpp b/rover/include/roverapi/rover_display.hpp
index eaada28..6b73fed 100644
--- a/rover/include/roverapi/rover_display.hpp
+++ b/rover/include/roverapi/rover_display.hpp
@@ -134,7 +134,7 @@
/**
* @brief Sets the text and background color color to either white or black
* @param c is the text color in integer. RoverDisplay::BLACK_COLOR or RoverDisplay::WHITE_COLOR
- * @param c is the background color in integer. RoverDisplay::BLACK_COLOR or RoverDisplay::WHITE_COLOR
+ * @param b is the background color in integer. RoverDisplay::BLACK_COLOR or RoverDisplay::WHITE_COLOR
* @return void
*/
void setTextColor (uint16_t c, uint16_t b);
diff --git a/rover/include/roverapi/rover_driving.hpp b/rover/include/roverapi/rover_driving.hpp
index 4c7a8b7..0a21f9e 100644
--- a/rover/include/roverapi/rover_driving.hpp
+++ b/rover/include/roverapi/rover_driving.hpp
@@ -49,7 +49,7 @@
/**
* @brief Constructor for RoverDriving class.
*/
- RoverDriving();
+ explicit RoverDriving();
/**
* @brief Initializes wiringPi library and Analog to Digital Converter to start driving the rover.
diff --git a/rover/include/roverapi/rover_gpio.hpp b/rover/include/roverapi/rover_gpio.hpp
index cb19701..adc4577 100644
--- a/rover/include/roverapi/rover_gpio.hpp
+++ b/rover/include/roverapi/rover_gpio.hpp
@@ -99,7 +99,7 @@
* @brief Constructor function for the RoverGpio class
* @return void
*/
- RoverGpio();
+ explicit RoverGpio();
/**
* @brief Initializes the RoverGpio functionality: Buzzer and Buttons
diff --git a/rover/include/roverapi/rover_sensors.hpp b/rover/include/roverapi/rover_sensors.hpp
index 8ca663d..4eddcee 100644
--- a/rover/include/roverapi/rover_sensors.hpp
+++ b/rover/include/roverapi/rover_sensors.hpp
@@ -11,6 +11,7 @@
*
* Contributors:
* M.Ozcelikors <mozcelikors@gmail.com>, created C++ API 17.11.2017
+ * initial QMC5883L driver implemented 30.11.2017
* David Schmelter, Fraunhofer IEM - compass sensor initial implementation
* Gael Blondelle, Eclipse Foundation - initial API and parameters
*
@@ -23,6 +24,11 @@
#include <stdio.h>
#include <unistd.h>
+
+//To debug the application, uncomment the following
+//#define DEBUG 1
+
+
namespace rover
{
/**
@@ -71,15 +77,15 @@
/**
* @brief Address for compass sensor
*/
- static const int HMC588L_ADDRESS = 0x1E;
+ int HMC588L_ADDRESS = 0x1E; //0x1E;
/**
* @brief Calibration duration for compass sensor
*/
- static const int CALIBRATION_DURATION = 10000; //compass calibration has a duration of 5 seconds
+ int CALIBRATION_DURATION = 10000; //compass calibration has a duration of 5 seconds
/**
* @brief Declination angle / correction factor for compass sensor
*/
- static const float DECLINATION_ANGLE = 0.0413; //correction factor for location Paderborn
+ float DECLINATION_ANGLE = 0.0413; //correction factor for location Paderborn
/**
* @brief Calibration variable for HMC588L
@@ -129,7 +135,7 @@
/***
* @brief Constructor for the RoverSensors class.
*/
- RoverSensors();
+ explicit RoverSensors();
/**
* @brief Sets up the HC-SR04 ultrasonic sensor
@@ -171,10 +177,10 @@
float readInfraredSensor (int infrared_sensor_id);
/**
- * @brief Sets up the bearing sensor and its I2C interface
+ * @brief Sets up the bearing sensor Sunfounder HMC5883L and its I2C interface
* @return void
*/
- void setupBearingSensor (void);
+ void setupBearingHMC5883L (void);
/**
* @brief Starts calibration for the bearing sensor
@@ -183,10 +189,28 @@
void calibrateBearingSensor (void);
/**
- * @brief Reads the bearing value from Bearing sensor (float).
+ * @brief Sets the HMC588L Address
+ * @return void
+ */
+ void setHMC588LAddress (int address);
+
+ /**
+ * @brief Sets the HMC588L Caliration Duration
+ * @return void
+ */
+ void setHMC588LCalibrationPeriod(int period);
+
+ /**
+ * @brief Sets the HMC588L Declination angle
+ * @return void
+ */
+ void setHMC588LDeclinationAngle (float angle);
+
+ /**
+ * @brief Reads the bearing value from Bearing sensor Sunfounder HMC5883L (float).
* @return bearing_val in degrees (float).
*/
- float readBearing (void);
+ float readBearingHMC5883L (void);
/**
* @brief Reads the temperature value in Celcius degrees from DHT22 temperature and humidity sensor (float).
@@ -199,6 +223,18 @@
* @return humidity_val in percentage (float).
*/
float readHumidity (void);
+
+ /**
+ * @brief Sets up the bearing sensor with QMC5883L chip and its I2C interface
+ * @return void
+ */
+ void setupBearingQMC5883L (void);
+
+ /**
+ * @brief Reads the bearing value from Bearing sensor with QMC5883L chip (float).
+ * @return bearing_val in degrees (float).
+ */
+ float readBearingQMC5883L (void);
};
}
diff --git a/rover/include/roverapp.h b/rover/include/roverapp.h
index 69221d7..197a788 100644
--- a/rover/include/roverapp.h
+++ b/rover/include/roverapp.h
@@ -37,7 +37,7 @@
//#define CROSS_COMPILE_ECLIPSE 1
//When debugging without RoverSenseLayer uncomment the following to prevent OS to shutdown.
-//#define DEBUG_WO_RSL 1
+#define DEBUG_WO_RSL 1
//Please comment the line below to work with SR-04 sensor instead of GROOVE for rear proximity sensing.
//#define USE_GROOVE_SENSOR 1
diff --git a/rover/src/roverapi/rover_api.cpp b/rover/src/roverapi/rover_api.cpp
index 166f65d..8afc961 100644
--- a/rover/src/roverapi/rover_api.cpp
+++ b/rover/src/roverapi/rover_api.cpp
@@ -28,9 +28,7 @@
*/
rover::RoverBase::RoverBase()
{
- /* wiringPi can only be called once */
- wiringPiSetup();
-
+ this->WIRINGPI_INIT_ = 0;
this->ROVER_DISPLAY_INIT_ = 0;
this->ROVER_DRIVING_INIT_ = 0;
this->ROVER_GPIO_INIT_ = 0;
@@ -45,20 +43,34 @@
}
-/**
- * @brief Initializes the all classes, sensors, libraries for the Rover
- */
+
void rover::RoverBase::initialize(void)
{
+ this->initializeWiringPi();
this->initializeRoverDisplay();
this->initializeRoverDriving();
this->initializeRoverGpio();
this->initializeRoverSensors();
}
+
+void rover::RoverBase::initializeWiringPi(void)
+{
+ /* wiringPi can only be called once per program, One solution: */
+ static class Once { public: Once(){
+ wiringPiSetup();
+ printf("wiringPi Setup Done..\n");
+ }} Once_;
+ this->WIRINGPI_INIT_ = 1;
+}
+
void rover::RoverBase::shutdown (void)
{
- if (this->ROVER_DISPLAY_INIT_ != 1)
+ if (this->WIRINGPI_INIT_ != 1)
+ {
+ fprintf(stderr,"You havent initialized wiringPi library. Use RoverBase::initialize() !\n");
+ }
+ else if (this->ROVER_DISPLAY_INIT_ != 1)
{
fprintf(stderr,"You havent initialized RoverDisplay. Use RoverBase::initialize() !\n");
}
@@ -130,7 +142,11 @@
void rover::RoverBase::sleep (unsigned int period_ms)
{
- if (this->ROVER_DRIVING_INIT_ != 1)
+ if (this->WIRINGPI_INIT_ != 1)
+ {
+ fprintf(stderr,"You havent initialized wiringPi library. Use RoverBase::initialize() !\n");
+ }
+ else if (this->ROVER_DRIVING_INIT_ != 1)
{
fprintf(stderr,"You havent initialized RoverDriving. Use RoverBase::initialize() !\n");
}
@@ -152,7 +168,11 @@
rover::RoverDriving& rover::RoverBase::inRoverDriving (void)
{
- if (this->ROVER_DRIVING_INIT_ != 1)
+ if (this->WIRINGPI_INIT_ != 1)
+ {
+ fprintf(stderr,"You havent initialized wiringPi library. Use RoverBase::initialize() !\n");
+ }
+ else if (this->ROVER_DRIVING_INIT_ != 1)
{
fprintf(stderr,"You havent initialized RoverDriving. Use RoverBase::initialize() !\n");
}
@@ -164,7 +184,11 @@
rover::RoverGpio& rover::RoverBase::inRoverGpio (void)
{
- if (this->ROVER_GPIO_INIT_ != 1)
+ if (this->WIRINGPI_INIT_ != 1)
+ {
+ fprintf(stderr,"You havent initialized wiringPi library. Use RoverBase::initialize() !\n");
+ }
+ else if (this->ROVER_GPIO_INIT_ != 1)
{
fprintf(stderr,"You havent initialized RoverDisplay. Use RoverGpio::initialize() !\n");
}
@@ -176,7 +200,11 @@
rover::RoverDisplay& rover::RoverBase::inRoverDisplay (void)
{
- if (this->ROVER_DISPLAY_INIT_ != 1)
+ if (this->WIRINGPI_INIT_ != 1)
+ {
+ fprintf(stderr,"You havent initialized wiringPi library. Use RoverBase::initialize() !\n");
+ }
+ else if (this->ROVER_DISPLAY_INIT_ != 1)
{
fprintf(stderr,"You havent initialized RoverDisplay. Use RoverBase::initialize() !\n");
}
@@ -188,7 +216,11 @@
rover::RoverSensors& rover::RoverBase::inRoverSensors (void)
{
- if (this->ROVER_SENSORS_INIT_ != 1)
+ if (this->WIRINGPI_INIT_ != 1)
+ {
+ fprintf(stderr,"You havent initialized wiringPi library. Use RoverBase::initialize() !\n");
+ }
+ else if (this->ROVER_SENSORS_INIT_ != 1)
{
fprintf(stderr,"You havent initialized RoverSensors. Use RoverBase::initialize() !\n");
}
@@ -200,7 +232,11 @@
void rover::RoverBase::initializeRoverSensors (void)
{
- if (this->ROVER_SENSORS_INIT_ != 1)
+ if (this->WIRINGPI_INIT_ != 1)
+ {
+ fprintf(stderr,"You havent initialized wiringPi library. Use RoverBase::initialize() !\n");
+ }
+ else if (this->ROVER_SENSORS_INIT_ != 1)
{
this->myRoverSensors.initialize();
this->ROVER_SENSORS_INIT_ = 1;
@@ -209,7 +245,11 @@
void rover::RoverBase::initializeRoverGpio (void)
{
- if (this->ROVER_GPIO_INIT_ != 1)
+ if (this->WIRINGPI_INIT_ != 1)
+ {
+ fprintf(stderr,"You havent initialized wiringPi library. Use RoverBase::initialize() !\n");
+ }
+ else if (this->ROVER_GPIO_INIT_ != 1)
{
this->myRoverGpio.initialize();
this->ROVER_GPIO_INIT_ = 1;
@@ -218,7 +258,11 @@
void rover::RoverBase::initializeRoverDisplay (void)
{
- if (this->ROVER_DISPLAY_INIT_ != 1)
+ if (this->WIRINGPI_INIT_ != 1)
+ {
+ fprintf(stderr,"You havent initialized wiringPi library. Use RoverBase::initialize() !\n");
+ }
+ else if (this->ROVER_DISPLAY_INIT_ != 1)
{
this->myRoverDisplay.initialize();
this->ROVER_DISPLAY_INIT_ = 1;
@@ -227,7 +271,11 @@
void rover::RoverBase::initializeRoverDriving (void)
{
- if (this->ROVER_DRIVING_INIT_ != 1)
+ if (this->WIRINGPI_INIT_ != 1)
+ {
+ fprintf(stderr,"You havent initialized wiringPi library. Use RoverBase::initialize() !\n");
+ }
+ else if (this->ROVER_DRIVING_INIT_ != 1)
{
this->myRoverDriving.initialize();
this->ROVER_DRIVING_INIT_ = 1;
diff --git a/rover/src/roverapi/rover_sensors.cpp b/rover/src/roverapi/rover_sensors.cpp
index 647a035..0e96497 100644
--- a/rover/src/roverapi/rover_sensors.cpp
+++ b/rover/src/roverapi/rover_sensors.cpp
@@ -10,6 +10,8 @@
*
* Contributors:
* M.Ozcelikors <mozcelikors@gmail.com>, created C++ API 17.11.2017
+ * initial QMC5883L driver implemented 30.11.2017
+ * David Schmelter, Fraunhofer IEM - compass sensor initial implementation
* Gael Blondelle - initial API and parameters
*
*/
@@ -42,23 +44,28 @@
this->setupHCSR04UltrasonicSensor(this->ROVER_FRONT);
this->setupHCSR04UltrasonicSensor(this->ROVER_REAR);
this->setupInfraredSensors();
- this->setupBearingSensor();
+ this->setupBearingHMC5883L();
+ this->setupBearingQMC5883L();
}
void rover::RoverSensors::setupHCSR04UltrasonicSensor (int sensor_id)
{
int trig_pin, echo_pin;
- if (sensor_id == 0)
+ if (sensor_id == this->ROVER_FRONT)
{
trig_pin = this->TRIG1;
echo_pin = this->ECHO1;
}
- else
+ else if (sensor_id == this->ROVER_REAR)
{
trig_pin = this->TRIG0;
echo_pin = this->ECHO0;
}
+ else
+ {
+ printf ("Invalid Sensor ID for the Ultrasonic Sensor to function setupHCSR04UltrasonicSensor.\n");
+ }
pinMode(trig_pin, OUTPUT);
pinMode(echo_pin, INPUT);
@@ -72,16 +79,20 @@
{
int trig_pin, echo_pin;
- if (sensor_id == 0)
+ if (sensor_id == this->ROVER_FRONT)
{
trig_pin = this->TRIG1;
echo_pin = this->ECHO1;
}
- else
+ else if (sensor_id == this->ROVER_REAR)
{
trig_pin = this->TRIG0;
echo_pin = this->ECHO0;
}
+ else
+ {
+ printf ("Invalid Sensor ID for the Ultrasonic Sensor to function readHCSR04UltrasonicSensor.\n");
+ }
int distance = 0;
//Send trig pulse
@@ -118,14 +129,18 @@
{
int sig_pin;
- if (sensor_id == 0)
+ if (sensor_id == this->ROVER_FRONT)
{
sig_pin = this->SIG1;
}
- else
+ else if (sensor_id == this->ROVER_REAR)
{
sig_pin = this->SIG0;
}
+ else
+ {
+ printf ("Invalid Sensor ID for the Ultrasonic Sensor to function readGrooveUltrasonicSensor.\n");
+ }
long startTime, stopTime, elapsedTime, distance = 0;
pinMode(sig_pin, OUTPUT);
@@ -163,6 +178,14 @@
float x;
float y = analogRead (BASE+infrared_sensor_id);
+ if (infrared_sensor_id != this->ROVER_FRONT_LEFT &&
+ infrared_sensor_id != this->ROVER_FRONT_RIGHT &&
+ infrared_sensor_id != this->ROVER_REAR_LEFT &&
+ infrared_sensor_id != this->ROVER_REAR_RIGHT)
+ {
+ printf ("Invalid Sensor ID for the Infrared Sensor to function readInfraredSensor.\n");
+ }
+
// 1/cm to output voltage is almost linear between
// 80cm->0,4V->123
// 6cm->3,1V->961
@@ -184,8 +207,121 @@
this->calibration_start = millis();
}
-void rover::RoverSensors::setupBearingSensor(void)
+void rover::RoverSensors::setupBearingQMC5883L(void)
{
+ this->setHMC588LAddress(0x0D);
+ this->setHMC588LCalibrationPeriod(10000);
+ this->setHMC588LDeclinationAngle(0.0413);
+
+ if ((i2c_hmc588l_fd = wiringPiI2CSetup(this->HMC588L_ADDRESS)) < 0)
+ {
+ printf("Failed to initialize HMC588L compass sensor");
+ }
+
+ if (i2c_hmc588l_fd >= 0)
+ {
+ wiringPiI2CWriteReg8 (i2c_hmc588l_fd, 0x0B, 0x01); //init SET/PERIOD register
+
+ /*
+ Define
+ OSR = 512
+ Full Scale Range = 8G(Gauss)
+ ODR = 200HZ
+ set continuous measurement mode
+ */
+ wiringPiI2CWriteReg8 (i2c_hmc588l_fd, 0x09, 0x1D);
+ }
+
+ this->calibration_start = millis();
+
+ //
+ // To do a software reset
+ // wiringPiI2CWriteReg8 (i2c_hmc588l_fd, 0x0A, 0x80);
+ //
+
+}
+
+float rover::RoverSensors::readBearingQMC5883L(void)
+{
+ int8_t buffer[6];
+
+ //wiringPiI2CWrite (i2c_hmc588l_fd, 0x00); //Start with register 3
+ //delay(25);
+
+ buffer[0] = wiringPiI2CReadReg8(i2c_hmc588l_fd, 0x00); //LSB x
+ buffer[1] = wiringPiI2CReadReg8(i2c_hmc588l_fd, 0x01); //MSB x
+ buffer[2] = wiringPiI2CReadReg8(i2c_hmc588l_fd, 0x02); //LSB y
+ buffer[3] = wiringPiI2CReadReg8(i2c_hmc588l_fd, 0x03); //MSB y
+ buffer[4] = wiringPiI2CReadReg8(i2c_hmc588l_fd, 0x04); //LSB z
+ buffer[5] = wiringPiI2CReadReg8(i2c_hmc588l_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) {
+ if (xRaw < xMinRaw) {
+ xMinRaw = xRaw;
+ }
+ if (xRaw > xMaxRaw) {
+ xMaxRaw = xRaw;
+ }
+ if (yRaw < yMinRaw) {
+ yMinRaw = yRaw;
+ }
+ if (yRaw > yMaxRaw) {
+ yMaxRaw = yRaw;
+ }
+ }
+
+ //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::RoverSensors::setupBearingHMC5883L(void)
+{
+ this->setHMC588LAddress(0x1E);
+ this->setHMC588LCalibrationPeriod(10000);
+ this->setHMC588LDeclinationAngle(0.0413);
+
+#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");
}
@@ -201,16 +337,17 @@
this->calibration_start = millis();
}
-float rover::RoverSensors::readBearing(void)
+float rover::RoverSensors::readBearingHMC5883L(void)
{
int8_t buffer[6];
+
//potential optimization: wiringPiI2CReadReg16
- buffer[0] = wiringPiI2CReadReg8(i2c_hmc588l_fd, 0x03);
- buffer[1] = wiringPiI2CReadReg8(i2c_hmc588l_fd, 0x04);
- buffer[2] = wiringPiI2CReadReg8(i2c_hmc588l_fd, 0x05);
- buffer[3] = wiringPiI2CReadReg8(i2c_hmc588l_fd, 0x06);
- buffer[4] = wiringPiI2CReadReg8(i2c_hmc588l_fd, 0x07);
- buffer[5] = wiringPiI2CReadReg8(i2c_hmc588l_fd, 0x08);
+ 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];
@@ -241,6 +378,9 @@
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;
@@ -254,7 +394,9 @@
}
float headingDegrees = bearing * (180.0 / M_PI);
-
+#ifdef DEBUG
+ printf("%lf, headingDegrees\n", headingDegrees);
+#endif
return headingDegrees;
}
@@ -336,7 +478,9 @@
c = -c;
}
f = c * 1.8f + 32;
- //printf( "Humidity = %.1f %% Temperature = %.1f *C (%.1f *F)\n", h, c, f );
+#ifdef DEBUG
+ printf( "Humidity = %.1f %% Temperature = %.1f *C (%.1f *F)\n", h, c, f );
+#endif
try_again = 0;
}
else
@@ -344,6 +488,7 @@
/* Data not good */
try_again = 1;
//printf ("Data not good, skipping\n");
+
}
}
@@ -429,16 +574,34 @@
c = -c;
}
f = c * 1.8f + 32;
- //printf( "Humidity = %.1f %% Temperature = %.1f *C (%.1f *F)\n", h, c, f );
+#ifdef DEBUG
+ printf( "Humidity = %.1f %% Temperature = %.1f *C (%.1f *F)\n", h, c, f );
+#endif
try_again = 0;
}
else
{
/* Data not good */
try_again = 1;
+ //printf ("Data not good, skipping\n");
}
}
/* Return humidity */
return h;
}
+
+void rover::RoverSensors::setHMC588LAddress (int address)
+{
+ this->HMC588L_ADDRESS = address;
+}
+
+void rover::RoverSensors::setHMC588LDeclinationAngle (float angle)
+{
+ this->DECLINATION_ANGLE = angle;
+}
+
+void rover::RoverSensors::setHMC588LCalibrationPeriod(int period)
+{
+ this->CALIBRATION_DURATION = period;
+}
diff --git a/rover/src/tasks/compass_sensor_task.cpp b/rover/src/tasks/compass_sensor_task.cpp
index 1260dc4..f9d455d 100644
--- a/rover/src/tasks/compass_sensor_task.cpp
+++ b/rover/src/tasks/compass_sensor_task.cpp
@@ -66,7 +66,7 @@
//Asynchronous end to calibration mode --> Call EndCalibrationMode()
pthread_mutex_lock(&compass_lock);
- bearing_shared = r.inRoverSensors().readBearing();
+ bearing_shared = r.inRoverSensors().readBearingHMC5883L();
pthread_mutex_unlock(&compass_lock);
diff --git a/rover/src/tasks/parking_task.cpp b/rover/src/tasks/parking_task.cpp
index e9b2588..b4b90ec 100644
--- a/rover/src/tasks/parking_task.cpp
+++ b/rover/src/tasks/parking_task.cpp
@@ -53,6 +53,7 @@
if (driving_mode == PARKING_LEFT)
{
+ //printf ("PARKING STARTED\n");
bearing_begin = bearing_shared;
r.inRoverDriving().setSpeed(speed_shared-50);
r.inRoverDriving().turnLeft();
@@ -87,9 +88,11 @@
r.inRoverDriving().stopRover();
StopParking();
+ //printf ("PARKING ENDED \n");
}
else if (driving_mode == PARKING_RIGHT)
{
+ //printf ("PARKING STARTED\n");
bearing_begin = bearing_shared;
r.inRoverDriving().setSpeed(speed_shared-50);
r.inRoverDriving().turnRight();
@@ -107,6 +110,7 @@
r.inRoverDriving().stopRover();
StopParking();
+ //printf ("PARKING ENDED\n");
}
//Task content ends here -------------------------------------------------
