ROVER - RoverSensors API added
Signed-off-by: Mustafa Ozcelikors <mozcelikors@gmail.com>
diff --git a/rover/include/roverapi/rover_sensors.hpp b/rover/include/roverapi/rover_sensors.hpp
new file mode 100644
index 0000000..8ca663d
--- /dev/null
+++ b/rover/include/roverapi/rover_sensors.hpp
@@ -0,0 +1,207 @@
+/*
+ * 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:
+ * Rover Sensors API - Interfaces for Rover sensors application development
+ * Header file
+ *
+ * Contributors:
+ * M.Ozcelikors <mozcelikors@gmail.com>, created C++ API 17.11.2017
+ * David Schmelter, Fraunhofer IEM - compass sensor initial implementation
+ * Gael Blondelle, Eclipse Foundation - initial API and parameters
+ *
+ */
+
+#ifndef API_ROVER_SENSORS_HPP_
+#define API_ROVER_SENSORS_HPP_
+
+#include <stdint.h>
+#include <stdio.h>
+#include <unistd.h>
+
+namespace rover
+{
+ /**
+ * @brief RoverSensors class contains member functions to setup and read from embedded sensors from rover's RoverSenseLayer sensor layer.
+ */
+ class RoverSensors
+ {
+ private:
+ /* Pins of ultrasonic sensors */
+ /**
+ * @brief Groove Sensor Pin for Front Sensor Socket (in wiringPi format)
+ */
+ static const int SIG0 = 0; //BCM-17 -> WiringPi 0 //Same as ECHO0 pin, if some one wants to replace front sr04 with groove sensor
+
+ /**
+ * @brief Groove Sensor Pin for Rear Sensor Socket (in wiringPi format)
+ */
+ static const int SIG1 = 2; //BCM-27 -> WiringPi 2 //Same as ECHO1 pin, if some one wants to replace back sr04 with groove sensor
+
+ /**
+ * @brief HC-SR04 Sensor Trigger Pin for Front Sensor Socket (in wiringPi format)
+ */
+ static const int TRIG0 = 7; //BCM-4 -> WiringPi 7
+ /**
+ * @brief HC-SR04 Sensor Echo Pin for Front Sensor Socket (in wiringPi format)
+ */
+ static const int ECHO0 = 0; //BCM-17 -> WiringPi 0
+
+ /**
+ * @brief HC-SR04 Sensor Trigger Pin for Rear Sensor Socket (in wiringPi format)
+ */
+ static const int TRIG1 = 1; //BCM-18 -> WiringPi 1
+ /**
+ * @brief HC-SR04 Sensor Echo Pin for Rear Sensor Socket (in wiringPi format)
+ */
+ static const int ECHO1 = 2; //BCM-27 -> WiringPi 2
+
+ /* Definitions related to DHT22 sensor */
+ /**
+ * @brief DHT22 sensor pin in wiringPi format
+ */
+ static const int DHT22_RPI_PIN = 24; //BCM19 -> wiringPi 24
+ static const int MAX_TIMINGS = 85;
+
+ /* Definitions related to compass sensor */
+ /**
+ * @brief Address for compass sensor
+ */
+ static const int HMC588L_ADDRESS = 0x1E;
+ /**
+ * @brief Calibration duration for compass sensor
+ */
+ static const 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
+
+ /**
+ * @brief Calibration variable for HMC588L
+ */
+ unsigned int calibration_start;
+
+ public:
+
+ /* Rover Sensor IDs */
+ /**
+ * @brief Static definition to indicate rover's front ultrasonic sensor socket
+ */
+ static const int ROVER_FRONT = 0;
+
+ /**
+ * @brief Static definition to indicate rover's rear ultrasonic sensor socket
+ */
+ static const int ROVER_REAR = 1;
+
+ /* Rover Infrared Sensor IDs */
+ /**
+ * @brief Static definition to indicate rover's rear-right infrared sensor
+ */
+ static const int ROVER_REAR_RIGHT = 0;
+
+ /**
+ * @brief Static definition to indicate rover's rear-left infrared sensor
+ */
+ static const int ROVER_REAR_LEFT = 1;
+
+ /**
+ * @brief Static definition to indicate rover's front-right infrared sensor
+ */
+ static const int ROVER_FRONT_RIGHT = 2;
+
+ /**
+ * @brief Static definition to indicate rover's front-left infrared sensor
+ */
+ static const int ROVER_FRONT_LEFT = 3;
+
+ /**
+ * @brief Initializes the sensors
+ * @return void
+ */
+ void initialize();
+
+ /***
+ * @brief Constructor for the RoverSensors class.
+ */
+ RoverSensors();
+
+ /**
+ * @brief Sets up the HC-SR04 ultrasonic sensor
+ * @return void
+ */
+ void setupHCSR04UltrasonicSensor(int sensor_id);
+
+ /**
+ * @brief Reads from HC-SR04 ultrasonic sensor in centimeters. sensor_id RoverSensors::ROVER_FRONT: Front ultrasonic sensor, RoverSensors::ROVER_REAR: Rear ultrasonic sensor.
+ * @param sensor_id RoverSensors::ROVER_FRONT: Front ultrasonic sensor, RoverSensors::ROVER_REAR: Rear ultrasonic sensor.
+ * @return sensor_val in centimeters
+ */
+ int readHCSR04UltrasonicSensor (int sensor_id);
+
+ /**
+ * @brief Sets up the groove ultrasonic sensor
+ * @return void
+ */
+ void setupGrooveUltrasonicSensor (void);
+
+ /**
+ * @brief Reads from groove ultrasonic sensor in centimeters. sensor_id RoverSensors::ROVER_FRONT: Front ultrasonic sensor, RoverSensors::ROVER_REAR: Rear ultrasonic sensor.
+ * @param sensor_id RoverSensors::ROVER_FRONT: Front ultrasonic sensor, RoverSensors::ROVER_REAR: Rear ultrasonic sensor.
+ * @return sensor_val in centimeters
+ */
+ int readGrooveUltrasonicSensor (int sensor_id);
+
+ /**
+ * @brief Sets up the infrared sensors and Analog to digital converter
+ * @return void
+ */
+ void setupInfraredSensors (void);
+
+ /**
+ * @brief Reads from infrared sensor in centimeters (float).
+ * @param infrared_sensor_id Indicates the channel of which on-board sensor is addressed. RoverSensors::ROVER_REAR_RIGHT: Rear-right, RoverSensors::ROVER_REAR_LEFT: Rear-left, RoverSensors::ROVER_FRONT_RIGHT: Front-right, RoverSensors::ROVER_FRONT_LEFT: Front-left.
+ * @return sensor_val in centimeters (float).
+ */
+ float readInfraredSensor (int infrared_sensor_id);
+
+ /**
+ * @brief Sets up the bearing sensor and its I2C interface
+ * @return void
+ */
+ void setupBearingSensor (void);
+
+ /**
+ * @brief Starts calibration for the bearing sensor
+ * @return void
+ */
+ void calibrateBearingSensor (void);
+
+ /**
+ * @brief Reads the bearing value from Bearing sensor (float).
+ * @return bearing_val in degrees (float).
+ */
+ float readBearing (void);
+
+ /**
+ * @brief Reads the temperature value in Celcius degrees from DHT22 temperature and humidity sensor (float).
+ * @return temperature_val in Celcius degrees (float).
+ */
+ float readTemperature (void);
+
+ /**
+ * @brief Reads the humidity value in percentage from DHT22 temperature and humidity sensor (float).
+ * @return humidity_val in percentage (float).
+ */
+ float readHumidity (void);
+ };
+}
+
+
+
+#endif /* API_ROVER_SENSORS_HPP_ */
diff --git a/rover/src/roverapi/rover_sensors.cpp b/rover/src/roverapi/rover_sensors.cpp
new file mode 100644
index 0000000..647a035
--- /dev/null
+++ b/rover/src/roverapi/rover_sensors.cpp
@@ -0,0 +1,444 @@
+/*
+ * 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:
+ * Rover Sensors API - Interfaces for Rover sensors application development
+ *
+ * Contributors:
+ * M.Ozcelikors <mozcelikors@gmail.com>, created C++ API 17.11.2017
+ * Gael Blondelle - initial API and parameters
+ *
+ */
+
+#include <roverapi/basic_psys_rover.h>
+#include <roverapi/rover_sensors.hpp>
+#include <mcp3004.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::RoverSensors::RoverSensors()
+{
+ this->calibration_start = 0;
+}
+
+void rover::RoverSensors::initialize (void)
+{
+ //wiringPiSetup();
+
+ this->setupHCSR04UltrasonicSensor(this->ROVER_FRONT);
+ this->setupHCSR04UltrasonicSensor(this->ROVER_REAR);
+ this->setupInfraredSensors();
+ this->setupBearingSensor();
+}
+
+void rover::RoverSensors::setupHCSR04UltrasonicSensor (int sensor_id)
+{
+ int trig_pin, echo_pin;
+
+ if (sensor_id == 0)
+ {
+ trig_pin = this->TRIG1;
+ echo_pin = this->ECHO1;
+ }
+ else
+ {
+ trig_pin = this->TRIG0;
+ echo_pin = this->ECHO0;
+ }
+
+ pinMode(trig_pin, OUTPUT);
+ pinMode(echo_pin, INPUT);
+
+ //TRIG pin must start LOW
+ digitalWrite(trig_pin, LOW);
+ delayMicroseconds(2);
+}
+
+int rover::RoverSensors::readHCSR04UltrasonicSensor (int sensor_id)
+{
+ int trig_pin, echo_pin;
+
+ if (sensor_id == 0)
+ {
+ trig_pin = this->TRIG1;
+ echo_pin = this->ECHO1;
+ }
+ else
+ {
+ trig_pin = this->TRIG0;
+ echo_pin = this->ECHO0;
+ }
+
+ int distance = 0;
+ //Send trig pulse
+ digitalWrite(trig_pin, HIGH);
+ delayMicroseconds(10);
+ digitalWrite(trig_pin, LOW);
+
+ //Wait for echo start
+ long startTime = micros();
+ while(digitalRead(echo_pin) == LOW && micros() < startTime + 100000);
+
+ //Wait for echo end
+ startTime = micros();
+ while(digitalRead(echo_pin) == HIGH);
+ long travelTime = micros() - startTime;
+
+ //Get distance in cm
+ distance = travelTime * 34300;
+ distance = distance / 1000000;
+ distance = distance / 2;
+ // The below protection is to ensure there is no value fluctuation due to timeout
+ if (distance > 40 )
+ distance = 40;
+
+ // printf("dist=%d\n",distance);
+ return distance;
+}
+
+void rover::RoverSensors::setupGrooveUltrasonicSensor(void) {
+ //wiringPiSetup(); //Since this can only be used once in a program, we do it in main and comment this.
+}
+
+int rover::RoverSensors::readGrooveUltrasonicSensor (int sensor_id)
+{
+ int sig_pin;
+
+ if (sensor_id == 0)
+ {
+ sig_pin = this->SIG1;
+ }
+ else
+ {
+ sig_pin = this->SIG0;
+ }
+
+ long startTime, stopTime, elapsedTime, distance = 0;
+ pinMode(sig_pin, OUTPUT);
+ digitalWrite(sig_pin, LOW);
+ delayMicroseconds(2);
+ digitalWrite(sig_pin, HIGH);
+ delayMicroseconds(5);
+ digitalWrite(sig_pin, LOW);
+ pinMode(sig_pin,INPUT);
+
+ startTime = micros();
+ while (digitalRead(sig_pin) == LOW );
+ startTime = micros();
+ // For values above 40cm, groove sensor is unable to receive signals which causes it to stuck
+ // This is resolved by adding the timeout below.
+ // However, this timeout cause values bigger than 40 to fluctuate
+ while (digitalRead(sig_pin) == HIGH && micros() < startTime + 100000);
+ stopTime = micros();
+ elapsedTime = stopTime - startTime;
+ distance = elapsedTime / 29 /2;
+ // The below protection is to ensure there is no value fluctuation
+ if (distance > 40 )
+ distance = 40;
+ return distance;
+}
+
+void rover::RoverSensors::setupInfraredSensors (void)
+{
+ /* Init the analog digital converter */
+ mcp3004Setup (BASE, SPI_CHAN); // 3004 and 3008 are the same 4/8 channels
+}
+
+float rover::RoverSensors::readInfraredSensor (int infrared_sensor_id)
+{
+ float x;
+ float y = analogRead (BASE+infrared_sensor_id);
+
+ // 1/cm to output voltage is almost linear between
+ // 80cm->0,4V->123
+ // 6cm->3,1V->961
+ // => y=5477*x+55 => x= (y-55)/5477
+ if (y<123){
+ x=100.00;
+ } else {
+ float inverse = (y-55)/5477;
+ //printf("inverse=%f\n",inverse);
+ // x is the distance in cm
+ x = 1/inverse;
+ }
+
+ return x;
+}
+
+void rover::RoverSensors::calibrateBearingSensor (void)
+{
+ this->calibration_start = millis();
+}
+
+void rover::RoverSensors::setupBearingSensor(void)
+{
+ 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->calibration_start = millis();
+}
+
+float rover::RoverSensors::readBearing(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);
+
+ 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) {
+ 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);
+
+ //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);
+
+ return headingDegrees;
+}
+
+float rover::RoverSensors::readTemperature (void)
+{
+ int data[5] = { 0, 0, 0, 0, 0 };
+
+ uint8_t laststate;
+ uint8_t counter;
+ uint8_t j;
+ uint8_t i;
+
+ int try_again = 1;
+ float f, h, c;
+
+ while (try_again == 1)
+ {
+ data[0] = data[1] = data[2] = data[3] = data[4] = 0;
+ laststate = HIGH;
+ counter = 0;
+ j = 0;
+
+ /* pull pin down for 18 milliseconds */
+ pinMode( this->DHT22_RPI_PIN, OUTPUT );
+ digitalWrite( this->DHT22_RPI_PIN, LOW );
+ delay( 18 );
+
+ /* prepare to read the pin */
+ pinMode( this->DHT22_RPI_PIN, INPUT );
+
+ /* detect change and read data */
+ for ( i = 0; i < this->MAX_TIMINGS; i++ )
+ {
+ counter = 0;
+ while ( digitalRead( this->DHT22_RPI_PIN ) == laststate )
+ {
+ counter++;
+ delayMicroseconds( 1 );
+ if ( counter == 255 )
+ {
+ break;
+ }
+ }
+ laststate = digitalRead( this->DHT22_RPI_PIN );
+
+ if ( counter == 255 )
+ break;
+
+ /* ignore first 3 transitions */
+ if ( (i >= 4) && (i % 2 == 0) )
+ {
+ /* shove each bit into the storage bytes */
+ data[j / 8] <<= 1;
+ if ( counter > 16 )
+ data[j / 8] |= 1;
+ j++;
+ }
+ }
+
+ /*
+ * check we read 40 bits (8bit x 5 ) + verify checksum in the last byte
+ * print it out if data is good
+ */
+ if ( (j >= 40) &&
+ (data[4] == ( (data[0] + data[1] + data[2] + data[3]) & 0xFF) ) )
+ {
+ h = (float)((data[0] << 8) + data[1]) / 10;
+ if ( h > 100 )
+ {
+ h = data[0]; // for DHT11
+ }
+ c = (float)(((data[2] & 0x7F) << 8) + data[3]) / 10;
+ if ( c > 125 )
+ {
+ c = data[2]; // for DHT11
+ }
+ if ( data[2] & 0x80 )
+ {
+ c = -c;
+ }
+ f = c * 1.8f + 32;
+ //printf( "Humidity = %.1f %% Temperature = %.1f *C (%.1f *F)\n", h, c, f );
+ try_again = 0;
+ }
+ else
+ {
+ /* Data not good */
+ try_again = 1;
+ //printf ("Data not good, skipping\n");
+ }
+ }
+
+ /* Return temperature */
+ return c;
+}
+
+float rover::RoverSensors::readHumidity (void)
+{
+ int data[5] = { 0, 0, 0, 0, 0 };
+
+ uint8_t laststate;
+ uint8_t counter;
+ uint8_t j;
+ uint8_t i;
+
+ int try_again = 1;
+ float f, h, c;
+
+ while (try_again == 1)
+ {
+ data[0] = data[1] = data[2] = data[3] = data[4] = 0;
+ laststate = HIGH;
+ counter = 0;
+ j = 0;
+
+ /* pull pin down for 18 milliseconds */
+ pinMode( this->DHT22_RPI_PIN, OUTPUT );
+ digitalWrite( this->DHT22_RPI_PIN, LOW );
+ delay( 18 );
+
+ /* prepare to read the pin */
+ pinMode( this->DHT22_RPI_PIN, INPUT );
+
+ /* detect change and read data */
+ for ( i = 0; i < this->MAX_TIMINGS; i++ )
+ {
+ counter = 0;
+ while ( digitalRead( this->DHT22_RPI_PIN ) == laststate )
+ {
+ counter++;
+ delayMicroseconds( 1 );
+ if ( counter == 255 )
+ {
+ break;
+ }
+ }
+ laststate = digitalRead( this->DHT22_RPI_PIN );
+
+ if ( counter == 255 )
+ break;
+
+ /* ignore first 3 transitions */
+ if ( (i >= 4) && (i % 2 == 0) )
+ {
+ /* shove each bit into the storage bytes */
+ data[j / 8] <<= 1;
+ if ( counter > 16 )
+ data[j / 8] |= 1;
+ j++;
+ }
+ }
+
+ /*
+ * check we read 40 bits (8bit x 5 ) + verify checksum in the last byte
+ * print it out if data is good
+ */
+ if ( (j >= 40) &&
+ (data[4] == ( (data[0] + data[1] + data[2] + data[3]) & 0xFF) ) )
+ {
+ h = (float)((data[0] << 8) + data[1]) / 10;
+ if ( h > 100 )
+ {
+ h = data[0]; // for DHT11
+ }
+ c = (float)(((data[2] & 0x7F) << 8) + data[3]) / 10;
+ if ( c > 125 )
+ {
+ c = data[2]; // for DHT11
+ }
+ if ( data[2] & 0x80 )
+ {
+ c = -c;
+ }
+ f = c * 1.8f + 32;
+ //printf( "Humidity = %.1f %% Temperature = %.1f *C (%.1f *F)\n", h, c, f );
+ try_again = 0;
+ }
+ else
+ {
+ /* Data not good */
+ try_again = 1;
+ }
+ }
+
+ /* Return humidity */
+ return h;
+}