ROVER - SRF02 Task added (for those who want to use it)
Signed-off-by: Mustafa Ozcelikors <mozcelikors@gmail.com>
diff --git a/rover/src/tasks/srf02_task.cpp b/rover/src/tasks/srf02_task.cpp
new file mode 100644
index 0000000..7b3865c
--- /dev/null
+++ b/rover/src/tasks/srf02_task.cpp
@@ -0,0 +1,165 @@
+/*
+ * 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:
+ * SRF02 Proximity sensor Task using PThreads
+ *
+ * Contributors:
+ * M.Ozcelikors <mozcelikors@gmail.com>, created 05.10.2017
+ *
+ */
+
+#include "srf02_task.h"
+
+#include <wiringPi.h>
+#include <ctime>
+#include <unistd.h>
+#include "../timing/timing.h"
+#include "../api/basic_psys_rover.h"
+#include "../interfaces.h"
+#include <pthread.h>
+#include "../RaspberryTest.h"
+
+#include "../pthread_monitoring/collect_thread_name.h"
+
+#include <linux/i2c-dev.h> //!! Check if this header WORKS!!
+#include <fcntl.h>
+#include <string.h>
+#include <sys/ioctl.h>
+#include <errno.h>
+
+#include <sys/types.h>
+#include <sys/stat.h>
+
+
+/* Name of the I2C device we will be using */
+/* For older raspberry pi modules use "/dev/i2c-0" instead of "/dev/i2c-1" for the i2c port */
+const char *I2CDeviceName = "/dev/i2c-1";
+
+
+unsigned int readFromSRF02 (int deviceAddr)
+{
+ /* Parameters for SRF02 */
+ int fd; //File descriptor
+
+ int address = deviceAddr;
+ unsigned char buf[10]; // Buffer for data being read/ written on the i2c bus
+
+ unsigned int result;
+
+ /* Open device for R/W operations */
+ if ((fd = open(I2CDeviceName, O_RDWR)) < 0)
+ {
+ perror("This error occured");
+ printf("Failed to open i2c port\n");
+ exit(1);
+ }
+
+ /* Set the port options and set the address of the device we wish to speak to */
+ if (ioctl(fd, I2C_SLAVE, address) < 0) {
+ perror("This error occured");
+ printf("Unable to get bus access to talk to slave\n");
+ exit(1);
+ }
+
+ /* Commands for performing a ranging */
+ buf[0] = 0;
+ buf[1] = 81;
+
+ /* Write commands to the i2c port */
+ if ((write(fd, buf, 2)) != 2)
+ {
+ perror("This error occured");
+ printf("Error writing to i2c slave\n");
+ exit(1);
+ }
+
+ /* This sleep waits for the ping to come back */
+ usleep(900000); //High-precision sleep system call!
+
+ /* This is the register we wish to read from */
+ buf[0] = 0;
+
+ /* Send the register to read from */
+ if ((write(fd, buf, 1)) != 1)
+ {
+ perror("This error occured");
+ printf("Error writing to i2c slave\n");
+ exit(1);
+ }
+
+ if (read(fd, buf, 4) != 4)
+ {
+ /* Read back data into buf[] */
+ perror("This error occured");
+ printf("Unable to read from slave\n");
+ exit(1);
+ }
+ else
+ {
+ unsigned char highByte = buf[2];
+ unsigned char lowByte = buf[3];
+ result = highByte;
+
+ /* Calculate range as a word value */
+ result <<= 8;
+ result += lowByte;
+ //printf("Device#%d: Software v: %d\n", deviceAddr, buf[0]);
+ //printf("Device#%d: Range was: %u\n", deviceAddr, result);
+ }
+
+ close(fd);
+
+ return result;
+}
+
+
+void *SRF02_Task (void * arg)
+{
+ timing srf02_task_tmr;
+ srf02_task_tmr.setTaskID("SRF02");
+ srf02_task_tmr.setDeadline(1);
+ srf02_task_tmr.setPeriod(1);
+
+ CollectThreadName("SRF02_Task");
+
+ unsigned int front_sensor, rear_sensor;
+
+ while (1)
+ {
+ srf02_task_tmr.recordStartTime();
+ srf02_task_tmr.calculatePreviousSlackTime();
+
+ //Task content starts here -----------------------------------------------
+
+ front_sensor = readFromSRF02 (FRONT_SRF02_ADDR);
+ printf("FRONT SRF02: %u\n", front_sensor);
+ rear_sensor = readFromSRF02 (REAR_SRF02_ADDR);
+ printf("REAR SRF02: %u\n", rear_sensor);
+
+ //Task content ends here -------------------------------------------------
+
+ srf02_task_tmr.recordEndTime();
+ srf02_task_tmr.calculateExecutionTime();
+ srf02_task_tmr.calculateDeadlineMissPercentage();
+ srf02_task_tmr.incrementTotalCycles();
+ pthread_mutex_lock(&srf02_task_ti_l);
+ srf02_task_ti.deadline = srf02_task_tmr.getDeadline();
+ srf02_task_ti.deadline_miss_percentage = srf02_task_tmr.getDeadlineMissPercentage();
+ srf02_task_ti.execution_time = srf02_task_tmr.getExecutionTime();
+ srf02_task_ti.period = srf02_task_tmr.getPeriod();
+ srf02_task_ti.prev_slack_time = srf02_task_tmr.getPrevSlackTime();
+ srf02_task_ti.task_id = srf02_task_tmr.getTaskID();
+ srf02_task_ti.start_time = srf02_task_tmr.getStartTime();
+ srf02_task_ti.end_time = srf02_task_tmr.getEndTime();
+ pthread_mutex_unlock(&srf02_task_ti_l);
+ srf02_task_tmr.sleepToMatchPeriod();
+ }
+
+ /* the function must return something - NULL will do */
+ return NULL;
+}
diff --git a/rover/src/tasks/srf02_task.h b/rover/src/tasks/srf02_task.h
new file mode 100644
index 0000000..fe02460
--- /dev/null
+++ b/rover/src/tasks/srf02_task.h
@@ -0,0 +1,27 @@
+/*
+ * 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:
+ * SRF02 Proximity sensor Task using PThreads
+ *
+ * Contributors:
+ * M.Ozcelikors <mozcelikors@gmail.com>, created 05.10.2017
+ *
+ */
+
+#ifndef TASKS_SRF02_TASK_H_
+#define TASKS_SRF02_TASK_H_
+
+/* Defines */
+#define FRONT_SRF02_ADDR ((int) 0x70)
+#define REAR_SRF02_ADDR ((int) 0x73)
+
+/* Interfaces */
+void *SRF02_Task(void * arg);
+
+
+#endif /* TASKS_SRF02_TASK_H_ */
