main.cpp
#include <iostream>
#include "stdio.h"
#include "unistd.h"
#include <time.h>
#include "fast_gpio.h"
using namespace std;
#define HIGH 1
#define LOW 0
#define STARTDELAY 20
#define GPIO_IN_INDEX 3
#define GPIO_OUT_INDEX 2
unsigned long time_s;
unsigned long time_e;
__syscall_slong_t echotime;
int getrange(__syscall_slong_t timeus)
{
if (timeus <= 60000 && timeus >= 1)
{
return (((timeus*34)/100)) / 2;
}
}
void interrupt(void * args)
{
if (time_s == 0 && time_e == 0)
{
time_s = micros();
}
else if (time_s != 0 && time_e == 0)
{
time_e = micros();
echotime = time_e - time_s;
}
}
int main(int argc, char **argv)
{
mraa_result_t rtv = mraa_init();
timeInit();
if (rtv != MRAA_SUCCESS && rtv != MRAA_ERROR_PLATFORM_ALREADY_INITIALISED)
{
cout << "MRAA Init Failed,Return Value is ";
cout << rtv << endl;
return 0;
}
fprintf(stdout, "MRAA Version: %s\nStarting Read on IO3\n",mraa_get_version());
mraa_gpio_context gpio_out;
mraa_gpio_context gpio_in;
gpio_out = mraa_gpio_init(GPIO_OUT_INDEX);
if (gpio_out == NULL)
{
cout << "Init GPIO Out Failed" << endl;
return 0;
}
gpio_in = mraa_gpio_init(GPIO_IN_INDEX);
if (gpio_in == NULL)
{
cout << "Init GPIO In Failed" << endl;
return 0;
}
mraa_gpio_dir(gpio_out, MRAA_GPIO_OUT);
mraa_gpio_dir(gpio_in, MRAA_GPIO_IN);
mraa_gpio_use_mmaped(gpio_out, true);
mraa_gpio_use_mmaped(gpio_in, true);
//mraa_gpio_isr(gpio_in, MRAA_GPIO_EDGE_BOTH, &interrupt, NULL);
bool finishcycle = false;
for (;;)
{
time_s = 0;
time_e = 0;
mraa_gpio_write(gpio_out, HIGH);
usleep(STARTDELAY);
mraa_gpio_write(gpio_out, LOW);
while (mraa_gpio_read(gpio_in) == LOW)
{
if (time_s != 0)
{
time_e = micros2();
echotime = time_e - time_s;
break;
}
while (mraa_gpio_read(gpio_in) == HIGH)
{
if (time_s == 0)
{
time_s = micros2();
}
}
}
usleep(1000000);
cout << echotime << endl;
cout << "Echo distance is ";
cout << getrange(echotime);
cout << " mm" << endl;
}
mraa_gpio_close(gpio_out);
mraa_gpio_close(gpio_in);
return 0;
}
fast_gpio.h
#include <time.h>
#include "mraa.h"
#include <string>
#include "memory.h"
using namespace std;
static float cpufreq = 0;
static uint64_t tsc_init = 0;
static float clocks_per_ns = 0;
uint64_t rdtsc(void)
{
uint32_t lo, hi;
uint64_t returnVal;
/* We cannot use "=A", since this would use %rax on x86_64 */
__asm__ __volatile__("rdtsc" : "=a" (lo), "=d" (hi));
returnVal = hi;
returnVal <<= 32;
returnVal |= lo;
return returnVal;
}
unsigned long micros2(void)
{
struct timespec t;
t.tv_sec = t.tv_nsec = 0;
clock_gettime(CLOCK_REALTIME, &t);
return (unsigned long)(t.tv_sec) * 1000000L + t.tv_nsec / 1000L;
}
unsigned long micros(void)
{
uint64_t tsc_cur = rdtsc(), diff = 0, divisor = 0;
divisor = (cpufreq);
diff = tsc_cur - tsc_init;
return (unsigned long)(diff / divisor);
}
int timeInit(void)
{
int cpufreq_fd, ret;
char buf[0x400];
char * str = 0, *str2 = 0;
char * mhz_str = "cpu MHz\t\t: ";
/* Grab initial TSC snapshot */
tsc_init = rdtsc();
cpufreq_fd = open("/proc/cpuinfo", O_RDONLY);
if (cpufreq_fd < 0){
fprintf(stderr, "unable to open /proc/cpuinfo\n");
return -1;
}
memset(buf, 0x00, sizeof(buf));
ret = read(cpufreq_fd, buf, sizeof(buf));
if (ret < 0){
fprintf(stderr, "unable to read cpuinfo !\n");
close(cpufreq_fd);
return -1;
}
close(cpufreq_fd);
str = strstr(buf, mhz_str);
if (!str){
fprintf(stderr, "Buffer %s does not contain CPU frequency info !\n", buf);
return -1;
}
str += strlen(mhz_str);
str2 = str;
while (str2 < buf + sizeof(buf) - 1 && *str2 != '\n'){
str2++;
}
if (str2 == buf + sizeof(buf - 1) && *str2 != '\n'){
fprintf(stderr, "malformed cpufreq string %s\n", str);
return -1;
}
*str2 = '\0';
cpufreq = atof(str);
printf("cpufrequency is %f mhz\n", cpufreq);
/* Calculate nanoseconds per clock */
clocks_per_ns = 1000 / cpufreq;
printf("nanoseconds per clock %f\n", clocks_per_ns);
}