alexey.zholtikov/upm
1
0
Fork 0
mirror of https://github.com/eclipse/upm.git synced 2025-07-06 03:41:13 +03:00
Code Issues Actions Packages Projects Releases Wiki Activity

urm37: Initial implementation

Browse Source 
This driver was tested with the DFRobot URM37 Ultrasonic Ranger, V4.
Both UART and analog access modes are supported.

Signed-off-by: Jon Trulson <jtrulson@ics.com>
Signed-off-by: Abhishek Malik <abhishek.malik@intel.com>
This commit is contained in:
Jon Trulson
2015-10-15 11:03:40 -07:00
committed by Abhishek Malik
parent 6e095826d3
commit 639f99691b
13 changed files with 898 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

318
src/urm37/urm37.cxx Normal file
View File

@ -0,0 +1,318 @@
/*
* Author: Jon Trulson <jtrulson@ics.com>
* Copyright (c) 2015 Intel Corporation.
*
* Permission is hereby granted, free of charge, to any person obtaining
* a copy of this software and associated documentation files (the
* "Software"), to deal in the Software without restriction, including
* without limitation the rights to use, copy, modify, merge, publish,
* distribute, sublicense, and/or sell copies of the Software, and to
* permit persons to whom the Software is furnished to do so, subject to
* the following conditions:
*
* The above copyright notice and this permission notice shall be
* included in all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
* LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
* OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
* WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
#include <iostream>
#include "urm37.h"
using namespace upm;
using namespace std;
static const int waitTimeout = 1000;
static const int maxRetries = 10;
URM37::URM37(int aPin, int resetPin, int triggerPin, float aref) :
m_uart(0), m_aio(new mraa::Aio(aPin)), m_gpioReset(resetPin),
m_gpioTrigger(new mraa::Gpio(triggerPin))
{
m_analogMode = true;
m_aRes = (1 << m_aio->getBit());
m_aref = aref;
m_gpioTrigger->dir(mraa::DIR_OUT);
// setup trigger for mmapped access, not a big deal if this fails
m_gpioTrigger->useMmap(true);
// trigger high
m_gpioTrigger->write(1);
init();
}
URM37::URM37(int uart, int resetPin) :
m_uart(new mraa::Uart(uart)), m_aio(0), m_gpioReset(resetPin),
m_gpioTrigger(0)
{
m_analogMode = false;
m_aRes = 0;
m_aref = 0;
// 9600 baud is the only support baud rate...
if (m_uart->setBaudRate(9600))
{
throw std::runtime_error(string(__FUNCTION__) +
": setBaudRate(9600) failed");
return;
}
init();
}
URM37::~URM37()
{
if (m_uart)
delete m_uart;
if (m_aio)
delete m_aio;
if(m_gpioTrigger)
delete m_gpioTrigger;
}
void URM37::init()
{
m_gpioReset.dir(mraa::DIR_OUT);
// reset the device
reset();
}
void URM37::reset()
{
// toggle reset
m_gpioReset.write(0);
usleep(100);
m_gpioReset.write(1);
// wait for reset to complete
sleep(3);
}
bool URM37::dataAvailable(unsigned int millis)
{
return m_uart->dataAvailable(millis);
}
std::string URM37::readDataStr(int len)
{
return m_uart->readStr(len);
}
int URM37::writeDataStr(std::string data)
{
m_uart->flush();
return m_uart->writeStr(data);
}
float URM37::getDistance(int degrees)
{
// analog mode
if (m_analogMode)
{
m_gpioTrigger->write(0);
int val = m_aio->read();
m_gpioTrigger->write(1);
float mVolts = (float(val) * (m_aref / m_aRes)) * 1000.0;
// 6.8mV per CM
return (mVolts / 6.8);
}
// UART mode
// query distance cmd sequence
uint8_t deg = (uint8_t)(degrees / 6);
if (deg > 46)
throw std::out_of_range(string(__FUNCTION__) +
": degrees out of range, must be 0-270");
string cmd;
uint8_t cksum = 0x22 + deg + 0x00;
cmd.push_back(0x22);
cmd.push_back(deg);
cmd.push_back(0x00);
cmd.push_back(cksum);
string resp = sendCommand(cmd);
if (resp.empty())
{
throw std::runtime_error(string(__FUNCTION__) +
": sendCommand() failed");
return 0.0;
}
uint8_t h = (uint8_t)resp[1];
uint8_t l = (uint8_t)resp[2];
float distance = float((h << 8) | l);
return (distance);
}
float URM37::getTemperature()
{
if (m_analogMode)
{
throw std::runtime_error(string(__FUNCTION__) +
": Temperature measurement not available in analog mode");
return 0.0;
}
// query temperature cmd sequence
string cmd;
cmd.push_back(0x11);
cmd.push_back(0x00);
cmd.push_back(0x00);
cmd.push_back(0x11);
string resp = sendCommand(cmd);
if (resp.empty())
{
throw std::runtime_error(string(__FUNCTION__) +
": sendCommand() failed");
return 0.0;
}
uint8_t h = (uint8_t)resp[1];
uint8_t l = (uint8_t)resp[2];
float temp;
temp = float((h & 0x0f) * 256 + l) / 10.0;
if (h & 0xf0)
temp *= -1;
return (temp);
}
uint8_t URM37::readEEPROM(uint8_t addr)
{
if (m_analogMode)
{
throw std::runtime_error(string(__FUNCTION__) +
": readEEPROM() is not possible in analog mode");
return 0;
}
if (addr > 0x04)
throw std::out_of_range(string(__FUNCTION__) +
": addr must be between 0x00-0x04");
string cmd;
uint8_t cksum = 0x33 + addr + 0x00;
cmd.push_back(0x33);
cmd.push_back(addr);
cmd.push_back(0x00);
cmd.push_back(cksum);
string resp = sendCommand(cmd);
if (resp.empty())
{
throw std::runtime_error(string(__FUNCTION__) +
": sendCommand() failed");
return 0;
}
return resp[2];
}
void URM37::writeEEPROM(uint8_t addr, uint8_t value)
{
if (m_analogMode)
{
throw std::runtime_error(string(__FUNCTION__) +
": writeEEPROM() is not possible in analog mode");
return;
}
if (addr > 0x04)
throw std::out_of_range(string(__FUNCTION__) +
": addr must be between 0x00-0x04");
string cmd;
uint8_t cksum = 0x44 + addr + value;
cmd.push_back(0x44);
cmd.push_back(addr);
cmd.push_back(value);
cmd.push_back(cksum);
string resp = sendCommand(cmd);
if (resp.empty())
{
throw std::runtime_error(string(__FUNCTION__) +
": sendCommand() failed");
return;
}
return;
}
string URM37::sendCommand(string cmd)
{
if (m_analogMode)
{
throw std::runtime_error(string(__FUNCTION__) +
": can only be executed in UART mode");
return "";
}
int tries = 0;
string resp;
while (tries++ < maxRetries)
{
writeDataStr(cmd);
if (!dataAvailable(waitTimeout))
{
cerr << __FUNCTION__ << ": Timed out waiting for response" << endl;
continue;
}
resp = readDataStr(8);
// verify size
if (resp.size() != 4)
{
cerr << __FUNCTION__ << ": Invalid returned packet size" << endl;
continue;
}
else
{
// we have data, verify cksum, return the response if it's
// good, retry otherwise
uint8_t cksum = (uint8_t)(resp[0] + resp[1] + resp[2]);
if ((uint8_t)resp[3] != cksum)
{
cerr << __FUNCTION__ << ": cksum failure" << endl;
continue;
}
// else, we are good to go
return resp;
}
}
// :(
return "";
}