| /* |
| * 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 <tasks/srf02_task.h> |
| |
| #include <wiringPi.h> |
| #include <ctime> |
| #include <unistd.h> |
| #include <libraries/timing/timing.h> |
| #include <interfaces.h> |
| #include <pthread.h> |
| |
| #include <linux/i2c-dev.h> |
| #include <fcntl.h> |
| #include <string.h> |
| #include <sys/ioctl.h> |
| #include <errno.h> |
| |
| #include <sys/types.h> |
| #include <sys/stat.h> |
| |
| #include <roverapp.h> |
| #include <roverapi/basic_psys_rover.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); |
| |
| 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; |
| } |