rc2007-rescue/Srf10.cpp

#include "Srf10.h"

#include "i2c.h"
#include "timer.h"


Srf10::Srf10(uint8_t id)
{
	this->id = id;
	
	gain = 16;
	range = 255;
	
	unit = Inches;
	
	distance = 0;
	has_distance = false;
	
	firmware = readFirmware();
}

int Srf10::readFirmware() {
	uint8_t byte;
	
	if(!I2CSendByte(id, 0)) return -1;
	if(!I2CRecvByte(id, &byte)) return -1;
	
	return byte;
}

bool Srf10::setGain(uint8_t gain) {
	uint8_t data[2] = {1, gain};
	
	if(!I2CSend(id, data, 2)) return false;
	
	this->gain = gain;
	return true;
}

bool Srf10::setRange(uint8_t range) {
	uint8_t data[2] = {2, range};
	
	if(!I2CSend(id, data, 2)) return false;
	
	this->range = range;
	return true;
}

long Srf10::updateDistance() {
	uint8_t data[2] = {0, unit};
	uint16_t d;
	
	
	if(!I2CSend(id, data, 2)) return -1;
	
	do {
		sleep(1);
	} while(readFirmware() == 0xFF);
	
	if(!I2CSendByte(id, 2)) return -1;
	if(I2CRecv(id, data, 2) < 2) return -1;
	
	d = (((uint16_t)data[0]) << 8) | data[1];
	
	if(d == 0) has_distance = false;
	else {
		has_distance = true;
		distance = d;
	}
	
	return d;
}