rpiecho.cpp

 
#include "rpiecho.h"
#include "plugin.h"
 
#include <boost/thread.hpp>
 
#ifdef RPI
#include <wiringPi.h>
#include <stdio.h>
#include <stdlib.h>
#endif
 
//
//Class RpiEcho
//
 
RpiEcho::RpiEcho()
{
}
 
RpiEcho::RpiEcho(int triggerpin, int echopin):
  mTriggerPin(triggerpin),
  mEchoPin(echopin),
  mValue(0),
  mState(false)
{
#ifdef RPI
  pinMode(mTriggerPin, OUTPUT);
  pinMode(mEchoPin, INPUT);
  digitalWrite(mTriggerPin, LOW);
 
  mState = true;
 
  /*boost::thread::attributes attrs;
  struct sched_param param;
  param.sched_priority = 50;
  pthread_attr_setschedparam(attrs.native_handle(), &param);
  */
 
  mThread = boost::thread(/*attrs,*/ boost::bind(&RpiEcho::threadLoop, this));
#endif
 
}
 
RpiEcho::~RpiEcho()
{
#ifdef RPI
  if (mState)
  {
    mState = false;
    mThread.join();
  }
  digitalWrite(mTriggerPin, LOW);
#endif
}
 
long RpiEcho::pulseIn(int timeout)
{
#ifdef RPI
  long startTime = micros();
  long stopTime = 0;
 
  while (digitalRead(mEchoPin) == LOW && mState)
  {
    if ((micros() - startTime) > timeout)
      return 0;
  }
  startTime = micros();
 
  stopTime = micros();
  while (digitalRead(mEchoPin) == HIGH && mState)
  {
    stopTime = micros();
    if ((stopTime - startTime) > timeout)
      return 0;
  }
 
  return stopTime - startTime;
#else
  return 0;
#endif
}
 
void RpiEcho::threadLoop()
{
#ifdef RPI
  int timeout = 116000; // (limit to 20/2 meter for best result)
  long avg = 0;
  unsigned int nbacc = 0;
  unsigned int spent = 0;
 
  while (mState)
  {
    avg = 0;
    nbacc = 0;
 
    for (nbacc = 0; nbacc < 10;)
    {
      digitalWrite(mTriggerPin, LOW);
      delay(5);
      digitalWrite(mTriggerPin, HIGH);
      delayMicroseconds(10);
      digitalWrite(mTriggerPin, LOW);
 
      spent = pulseIn(timeout);
      if (spent == 0)
        continue; //timeout
 
      avg += spent; //to millisecond
      nbacc++;
    }
 
    {
      boost::mutex::scoped_lock l(mMutex);
      if (nbacc > 0)
        mValue = (avg / nbacc) / 58; // concert time to cm
    }
  }
#endif
}
 
int RpiEcho::ReadValue()
{
  boost::mutex::scoped_lock l(mMutex);
  return mValue;
}