Changes to RoverSensor API
Signed-off-by: Mustafa Ozcelikors <mozcelikors@gmail.com>
diff --git a/rover/include/roverapi/rover_sensor.hpp b/rover/include/roverapi/rover_sensor.hpp
new file mode 100644
index 0000000..85173c7
--- /dev/null
+++ b/rover/include/roverapi/rover_sensor.hpp
@@ -0,0 +1,73 @@
+/*
+ * 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 Sensor API - Interfaces for Rover sensors application development
+ * Header file
+ *
+ * Contributors:
+ * M.Ozcelikors <mozcelikors@gmail.com>, modified C++ API 04.12.2017
+ * 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
+ *
+ */
+
+#ifndef API_ROVER_SENSORS_HPP_
+#define API_ROVER_SENSORS_HPP_
+
+#include <stdint.h>
+#include <stdio.h>
+#include <unistd.h>
+
+
+//To debug the application, uncomment the following
+//#define DEBUG 1
+
+
+namespace rover
+{
+ /**
+ * @brief RoverSensorID_t defines the sensor location ID of the proximity sensors on the rover
+ */
+ enum RoverSensorID_t { ROVER_REAR_RIGHT = 0,
+ ROVER_REAR_LEFT = 1,
+ ROVER_FRONT_RIGHT = 2,
+ ROVER_FRONT_LEFT = 3,
+ ROVER_FRONT = 4,
+ ROVER_REAR = 5};
+
+ /**
+ * @brief RoverSensor class is an abstract class to define interface between common sensors used in the rover.
+ */
+ class RoverSensor
+ {
+ public:
+ /**
+ * @brief Virtual function (interface) for setting up sensors
+ * @return void
+ */
+ virtual void initialize (void) = 0;
+
+ /**
+ * @brief Virtual function (interface) for reading from sensors
+ * @return sensor_val Sensor value read from sensor
+ */
+ virtual float read (void) = 0;
+
+ /**
+ * @brief Destructor for the RoverSensor class
+ */
+ virtual ~RoverSensor() {};
+
+ };
+}
+
+
+
+#endif /* API_ROVER_SENSORS_HPP_ */
diff --git a/rover/include/roverapi/rover_sensors.hpp b/rover/include/roverapi/rover_sensors.hpp
deleted file mode 100644
index 4eddcee..0000000
--- a/rover/include/roverapi/rover_sensors.hpp
+++ /dev/null
@@ -1,243 +0,0 @@
-/*
- * 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
- * initial QMC5883L driver implemented 30.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>
-
-
-//To debug the application, uncomment the following
-//#define DEBUG 1
-
-
-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
- */
- int HMC588L_ADDRESS = 0x1E; //0x1E;
- /**
- * @brief Calibration duration for compass sensor
- */
- int CALIBRATION_DURATION = 10000; //compass calibration has a duration of 5 seconds
- /**
- * @brief Declination angle / correction factor for compass sensor
- */
- 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.
- */
- explicit 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 Sunfounder HMC5883L and its I2C interface
- * @return void
- */
- void setupBearingHMC5883L (void);
-
- /**
- * @brief Starts calibration for the bearing sensor
- * @return void
- */
- void calibrateBearingSensor (void);
-
- /**
- * @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 readBearingHMC5883L (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);
-
- /**
- * @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);
- };
-}
-
-
-
-#endif /* API_ROVER_SENSORS_HPP_ */
diff --git a/rover/src/roverapi/rover_sensors.cpp b/rover/src/roverapi/rover_sensors.cpp
deleted file mode 100644
index 0e96497..0000000
--- a/rover/src/roverapi/rover_sensors.cpp
+++ /dev/null
@@ -1,607 +0,0 @@
-/*
- * 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
- * initial QMC5883L driver implemented 30.11.2017
- * David Schmelter, Fraunhofer IEM - compass sensor initial implementation
- * 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->setupBearingHMC5883L();
- this->setupBearingQMC5883L();
-}
-
-void rover::RoverSensors::setupHCSR04UltrasonicSensor (int sensor_id)
-{
- int trig_pin, echo_pin;
-
- if (sensor_id == this->ROVER_FRONT)
- {
- trig_pin = this->TRIG1;
- echo_pin = this->ECHO1;
- }
- 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);
-
- //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 == this->ROVER_FRONT)
- {
- trig_pin = this->TRIG1;
- echo_pin = this->ECHO1;
- }
- 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
- 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 == this->ROVER_FRONT)
- {
- sig_pin = this->SIG1;
- }
- 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);
- 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);
-
- 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
- // => 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::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");
- }
-
- 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::readBearingHMC5883L(void)
-{
- 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) {
- 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;
-}
-
-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;
-#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 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;
-#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;
-}
