| /* |
| * 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 ""; |
| } |
| |