mirror of
https://github.com/RobotechLille/cdf2018-principal
synced 2024-09-28 16:23:04 +02:00
45 lines
929 B
C
45 lines
929 B
C
|
#include <unistd.h>
|
||
|
|
||
|
#include "i2c.h"
|
||
|
#include "srf08.h"
|
||
|
|
||
|
int openSRF08(int address)
|
||
|
{
|
||
|
int fd = openI2C(address);
|
||
|
writeI2C(fd, SRF08_REGISTER_WRITE_MAX_GAIN, SRF08_DEFAULT_MAX_GAIN);
|
||
|
writeI2C(fd, SRF08_REGISTER_WRITE_RANGE, SRF08_DEFAULT_RANGE);
|
||
|
return fd;
|
||
|
}
|
||
|
|
||
|
uint8_t revSRF08(int fd)
|
||
|
{
|
||
|
return readI2C(fd, SRF08_REGISTER_READ_REVISION);
|
||
|
}
|
||
|
|
||
|
void startSRF08(int fd)
|
||
|
{
|
||
|
writeI2C(fd, SRF08_REGISTER_WRITE_COMMAND, SRF08_COMMAND_RANGING_US);
|
||
|
}
|
||
|
|
||
|
void waitSRF08(int fd)
|
||
|
{
|
||
|
while (revSRF08(fd) == 0xFF) {
|
||
|
sleep(0);
|
||
|
}
|
||
|
}
|
||
|
|
||
|
float getSRF08(int fd)
|
||
|
{
|
||
|
int8_t high = readI2C(fd, SRF08_REGISTER_READ_ECHO_1_HIGH) - SRF08_VALUES_OFFSET;
|
||
|
int8_t low = readI2C(fd, SRF08_REGISTER_READ_ECHO_1_LOW) - SRF08_VALUES_OFFSET;
|
||
|
int total = high * SRF08_VALUES_OFFSET + low;
|
||
|
return total * SOUNDSPEED_MM_P_US;
|
||
|
}
|
||
|
|
||
|
float readSRF08(int fd)
|
||
|
{
|
||
|
startSRF08(fd);
|
||
|
waitSRF08(fd);
|
||
|
return getSRF08(fd);
|
||
|
}
|