UltrasonicSensor.cpp

#include "ultrasonic/UltrasonicSensor.h"
#include "wiringPiI2C.h"

#include <iostream>

#include <unistd.h>

constexpr int UltrasonicSensor::SRF02_SDA;
constexpr int UltrasonicSensor::SRF02_SCL;
constexpr int UltrasonicSensor::DEVICE_ADDRESS1;
constexpr int UltrasonicSensor::DEVICE_ADDRESS2;
constexpr int UltrasonicSensor::DEVICE_ADDRESS3;
constexpr char UltrasonicSensor::DEVICE[];
constexpr int UltrasonicSensor::COMMAND_REGISTER;
constexpr int UltrasonicSensor::RESULT_HIGH_BYTE;
constexpr int UltrasonicSensor::RESULT_LOW_BYTE;
constexpr int UltrasonicSensor::RANGING_MODE_CM;
constexpr int UltrasonicSensor::DELAY;
constexpr int UltrasonicSensor::MAX_DISTANCE;

UltrasonicSensor::UltrasonicSensor(int devId)
    : devId(devId)
      , fd(-1)
{}

void UltrasonicSensor::init()
{
    fd = wiringPiI2CSetupInterface(DEVICE, devId);
    if (fd == -1)
        throw std::runtime_error{"Sensor not found!\n"};
}

int UltrasonicSensor::getDistance()
{
    if (devId == -1)
        throw std::runtime_error{"Sensor not initialized!\n"};

    wiringPiI2CWriteReg8(fd, COMMAND_REGISTER, RANGING_MODE_CM);
    int distance = wiringPiI2CReadReg16(fd, RESULT_LOW_BYTE);
    if (distance == 0)  // 0 means distance too far
        distance = 255;
    return distance;
}