mirror of
https://github.com/eclipse/upm.git
synced 2025-03-15 04:57:30 +03:00
urm37: Add C library for this sensor
Added a C implementation for this sensor. The C++ library was changed to just wrap the C library calls. Signed-off-by: Jon Trulson <jtrulson@ics.com>
This commit is contained in:
Jon Trulson
Noel Eck
parent
196654e7c6
commit
46460e20d9
@ -53,7 +53,8 @@ int main()
|
||||
{
|
||||
cout << "Detected distance (cm): " << sensor->getDistance() << endl;
|
||||
cout << "Temperature (C): " << sensor->getTemperature() << endl;
|
||||
|
||||
cout << endl;
|
||||
|
||||
usleep(500000);
|
||||
}
|
||||
|
||||
|
||||
@ -90,3 +90,5 @@ add_example (dfrph)
|
||||
add_example (vk2828u7)
|
||||
add_example (mma7361)
|
||||
add_example (bh1750)
|
||||
add_example (urm37)
|
||||
add_example (urm37-uart)
|
||||
|
||||
75
examples/c/urm37-uart.c
Normal file
75
examples/c/urm37-uart.c
Normal file
@ -0,0 +1,75 @@
|
||||
/*
|
||||
* Author: Jon Trulson <jtrulson@ics.com>
|
||||
* Copyright (c) 2016 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 <unistd.h>
|
||||
#include <signal.h>
|
||||
|
||||
#include "urm37.h"
|
||||
|
||||
bool shouldRun = true;
|
||||
|
||||
void sig_handler(int signo)
|
||||
{
|
||||
if (signo == SIGINT)
|
||||
shouldRun = false;
|
||||
}
|
||||
|
||||
int main()
|
||||
{
|
||||
signal(SIGINT, sig_handler);
|
||||
|
||||
//! [Interesting]
|
||||
|
||||
// Instantiate a URM37 sensor on UART 0, with the reset pin on D2
|
||||
urm37_context sensor = urm37_init(0, 2, 0, 0, 0, false);
|
||||
|
||||
if (!sensor)
|
||||
{
|
||||
printf("urm37_init() failed.\n");
|
||||
return(1);
|
||||
}
|
||||
|
||||
// Every half a second, sample the URM37 and output the measured
|
||||
// distance in cm.
|
||||
|
||||
while (shouldRun)
|
||||
{
|
||||
float distance, temperature;
|
||||
|
||||
urm37_get_distance(sensor, &distance, 0);
|
||||
printf("Detected distance (cm): %f\n", distance);
|
||||
|
||||
urm37_get_temperature(sensor, &temperature);
|
||||
printf("Temperature (C): %f\n\n", temperature);
|
||||
usleep(500000);
|
||||
}
|
||||
|
||||
//! [Interesting]
|
||||
|
||||
printf("Exiting\n");
|
||||
|
||||
urm37_close(sensor);
|
||||
|
||||
return 0;
|
||||
}
|
||||
73
examples/c/urm37.c
Normal file
73
examples/c/urm37.c
Normal file
@ -0,0 +1,73 @@
|
||||
/*
|
||||
* Author: Jon Trulson <jtrulson@ics.com>
|
||||
* Copyright (c) 2016 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 <unistd.h>
|
||||
#include <signal.h>
|
||||
|
||||
#include "urm37.h"
|
||||
|
||||
bool shouldRun = true;
|
||||
|
||||
void sig_handler(int signo)
|
||||
{
|
||||
if (signo == SIGINT)
|
||||
shouldRun = false;
|
||||
}
|
||||
|
||||
int main()
|
||||
{
|
||||
signal(SIGINT, sig_handler);
|
||||
|
||||
//! [Interesting]
|
||||
|
||||
// Instantiate a URM37 sensor on analog pin A0, reset pin on D2,
|
||||
// trigger pin on D3 with an analog reference voltage of 5.0
|
||||
urm37_context sensor = urm37_init(0, 2, 3, 5.0, 0, true);
|
||||
|
||||
if (!sensor)
|
||||
{
|
||||
printf("urm37_init() failed.\n");
|
||||
return(1);
|
||||
}
|
||||
|
||||
// Every half a second, sample the URM37 and output the measured
|
||||
// distance in cm.
|
||||
|
||||
while (shouldRun)
|
||||
{
|
||||
float distance;
|
||||
|
||||
urm37_get_distance(sensor, &distance, 0);
|
||||
printf("Detected distance (cm): %f\n", distance);
|
||||
usleep(500000);
|
||||
}
|
||||
|
||||
//! [Interesting]
|
||||
|
||||
printf("Exiting\n");
|
||||
|
||||
urm37_close(sensor);
|
||||
|
||||
return 0;
|
||||
}
|
||||
@ -1,5 +1,9 @@
|
||||
set (libname "urm37")
|
||||
set (libdescription "upm DFRobot URM37 Ultrasonic ranger")
|
||||
set (module_src ${libname}.cxx)
|
||||
set (module_hpp ${libname}.hpp)
|
||||
upm_module_init()
|
||||
upm_mixed_module_init (NAME urm37
|
||||
DESCRIPTION "upm URM37 Ultrasonic Ranger"
|
||||
C_HDR urm37.h
|
||||
C_SRC urm37.c
|
||||
CPP_HDR urm37.hpp
|
||||
CPP_SRC urm37.cxx
|
||||
FTI_SRC urm37_fti.c
|
||||
CPP_WRAPS_C
|
||||
REQUIRES upmc-utilities mraa)
|
||||
|
||||
386
src/urm37/urm37.c
Normal file
386
src/urm37/urm37.c
Normal file
@ -0,0 +1,386 @@
|
||||
/*
|
||||
* Author: Jon Trulson <jtrulson@ics.com>
|
||||
* Abhishek Malik <abhishek.malik@intel.com>
|
||||
* Copyright (c) 2016 Intel Corporation.
|
||||
*
|
||||
* Thanks to Adafruit for supplying a google translated version of the
|
||||
* Chinese datasheet and some clues in their code.
|
||||
*
|
||||
* 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 <string.h>
|
||||
|
||||
#include "urm37.h"
|
||||
|
||||
#include "upm_utilities.h"
|
||||
|
||||
#define URM37_MAX_DATA_LEN 4
|
||||
#define URM37_WAIT_TIMEOUT 1000
|
||||
#define URM37_MAX_RETRIES 10
|
||||
|
||||
urm37_context urm37_init(uint8_t a_pin, uint8_t reset_pin,
|
||||
uint8_t trigger_pin, float a_ref,
|
||||
uint8_t uart_bus, bool analog_mode)
|
||||
{
|
||||
|
||||
urm37_context dev = (urm37_context)malloc(sizeof(struct _urm37_context));
|
||||
|
||||
if (!dev)
|
||||
return NULL;
|
||||
|
||||
// clear out context
|
||||
memset((void *)dev, 0, sizeof(struct _urm37_context));
|
||||
|
||||
// NULL out MRAA contexts for now (redundant with memset I know, but...)
|
||||
dev->aio = NULL;
|
||||
dev->gpio_reset = NULL;
|
||||
dev->gpio_trigger = NULL;
|
||||
dev->uart = NULL;
|
||||
|
||||
dev->a_res = 0;
|
||||
dev->a_ref = a_ref;
|
||||
|
||||
// set the mode
|
||||
dev->is_analog_mode = analog_mode;
|
||||
|
||||
// initialize the MRAA contexts (only what we need)
|
||||
|
||||
// analog only
|
||||
if (dev->is_analog_mode)
|
||||
{
|
||||
if (!(dev->aio = mraa_aio_init(a_pin)))
|
||||
{
|
||||
printf("%s: mraa_aio_init() failed.\n", __FUNCTION__);
|
||||
urm37_close(dev);
|
||||
return NULL;
|
||||
}
|
||||
|
||||
// ADC resolution
|
||||
dev->a_res = (float)(1 << mraa_aio_get_bit(dev->aio)) - 1;
|
||||
|
||||
if (!(dev->gpio_trigger = mraa_gpio_init(trigger_pin)))
|
||||
{
|
||||
printf("%s: mraa_gpio_init(trigger) failed.\n", __FUNCTION__);
|
||||
urm37_close(dev);
|
||||
return NULL;
|
||||
}
|
||||
|
||||
mraa_gpio_dir(dev->gpio_trigger, MRAA_GPIO_OUT);
|
||||
mraa_gpio_write(dev->gpio_trigger, 1);
|
||||
}
|
||||
else
|
||||
{
|
||||
// UART only
|
||||
if (!(dev->uart = mraa_uart_init(uart_bus)))
|
||||
{
|
||||
printf("%s: mraa_uart_init() failed.\n", __FUNCTION__);
|
||||
urm37_close(dev);
|
||||
return NULL;
|
||||
}
|
||||
|
||||
mraa_uart_set_baudrate(dev->uart, 9600);
|
||||
mraa_uart_set_non_blocking(dev->uart, false);
|
||||
}
|
||||
|
||||
// reset - used by both analog and uart modes
|
||||
if (!(dev->gpio_reset = mraa_gpio_init(reset_pin)))
|
||||
{
|
||||
printf("%s: mraa_gpio_init(reset) failed.\n", __FUNCTION__);
|
||||
urm37_close(dev);
|
||||
return NULL;
|
||||
}
|
||||
|
||||
mraa_gpio_dir(dev->gpio_reset, MRAA_GPIO_OUT);
|
||||
|
||||
urm37_reset(dev);
|
||||
|
||||
return dev;
|
||||
}
|
||||
|
||||
void urm37_close(urm37_context dev)
|
||||
{
|
||||
if (dev->aio)
|
||||
mraa_aio_close(dev->aio);
|
||||
if (dev->gpio_reset)
|
||||
mraa_gpio_close(dev->gpio_reset);
|
||||
if (dev->gpio_trigger)
|
||||
mraa_gpio_close(dev->gpio_trigger);
|
||||
if (dev->uart)
|
||||
mraa_uart_stop(dev->uart);
|
||||
|
||||
free(dev);
|
||||
}
|
||||
|
||||
upm_result_t urm37_reset(urm37_context dev)
|
||||
{
|
||||
mraa_gpio_write(dev->gpio_reset, 0);
|
||||
upm_delay_us(100);
|
||||
mraa_gpio_write(dev->gpio_reset, 1);
|
||||
|
||||
// wait for reset to complete
|
||||
upm_delay(3);
|
||||
|
||||
return UPM_SUCCESS;
|
||||
}
|
||||
|
||||
// private
|
||||
static bool urm37_data_available(urm37_context dev, uint32_t millis)
|
||||
{
|
||||
if (mraa_uart_data_available(dev->uart, millis))
|
||||
return true;
|
||||
else
|
||||
return false;
|
||||
}
|
||||
|
||||
// private
|
||||
static int urm37_read_data(urm37_context dev, char* data)
|
||||
{
|
||||
return mraa_uart_read(dev->uart, data, (size_t)URM37_MAX_DATA_LEN);
|
||||
}
|
||||
|
||||
// private
|
||||
static int urm37_write_data(urm37_context dev, const char* data)
|
||||
{
|
||||
mraa_uart_flush(dev->uart);
|
||||
return mraa_uart_write(dev->uart, data, (size_t)URM37_MAX_DATA_LEN);
|
||||
}
|
||||
|
||||
upm_result_t urm37_send_command(urm37_context dev, char* cmd, char* response)
|
||||
{
|
||||
if (dev->is_analog_mode)
|
||||
{
|
||||
printf("%s: UART commands are not available in analog mode\n",
|
||||
__FUNCTION__);
|
||||
|
||||
return UPM_ERROR_NOT_SUPPORTED;
|
||||
}
|
||||
|
||||
int tries = 0;
|
||||
|
||||
while (tries++ < URM37_MAX_RETRIES)
|
||||
{
|
||||
if (urm37_write_data(dev, cmd) < 0)
|
||||
{
|
||||
// A write error of some kind. We don't try to continue
|
||||
// after this.
|
||||
printf("%s: write_data() failed\n", __FUNCTION__);
|
||||
|
||||
return UPM_ERROR_OPERATION_FAILED;
|
||||
}
|
||||
|
||||
if (!urm37_data_available(dev, URM37_WAIT_TIMEOUT))
|
||||
{
|
||||
// timeout, retry...
|
||||
continue;
|
||||
}
|
||||
|
||||
int rv = urm37_read_data(dev, response);
|
||||
if (rv < 0)
|
||||
{
|
||||
printf("%s: read_data() failed\n", __FUNCTION__);
|
||||
return UPM_ERROR_OPERATION_FAILED;
|
||||
}
|
||||
|
||||
if (rv != URM37_MAX_DATA_LEN)
|
||||
{
|
||||
// read wrong number of bytes...
|
||||
printf("%s: read_data() returned %d bytes, expected %d, retrying\n",
|
||||
__FUNCTION__, rv, URM37_MAX_DATA_LEN);
|
||||
continue;
|
||||
}
|
||||
else
|
||||
{
|
||||
// we have data, verify cksum, return the response if it's
|
||||
// good, retry otherwise
|
||||
uint8_t cksum = (uint8_t)(response[0] + response[1] + response[2]);
|
||||
|
||||
if ((uint8_t)response[3] != cksum)
|
||||
{
|
||||
printf("%s: checksum failure: got %d, expected %d, retrying\n",
|
||||
__FUNCTION__, (int)response[3], (int)cksum);
|
||||
continue;
|
||||
}
|
||||
|
||||
// all good
|
||||
return UPM_SUCCESS;
|
||||
}
|
||||
}
|
||||
|
||||
// If we are here, we timed out and all retries were exhausted
|
||||
return UPM_ERROR_TIMED_OUT;
|
||||
}
|
||||
|
||||
upm_result_t urm37_get_distance(urm37_context dev, float *distance,
|
||||
int degrees)
|
||||
{
|
||||
if (dev->is_analog_mode)
|
||||
{
|
||||
// analog mode
|
||||
int val;
|
||||
|
||||
// send the trigger pulse and sample
|
||||
mraa_gpio_write(dev->gpio_trigger, 0);
|
||||
val = mraa_aio_read(dev->aio);
|
||||
mraa_gpio_write(dev->gpio_trigger, 1);
|
||||
|
||||
// convert to mV
|
||||
float volts = ((float)val * (dev->a_ref / dev->a_res)) * 1000.0;
|
||||
|
||||
// 6.8 mV/cm
|
||||
*distance = volts/6.8;
|
||||
return UPM_SUCCESS;
|
||||
}
|
||||
|
||||
// UART mode
|
||||
|
||||
char cmd[URM37_MAX_DATA_LEN];
|
||||
char resp[URM37_MAX_DATA_LEN];
|
||||
|
||||
// divide degrees by 6 - this is the encoding URM37 uses.
|
||||
uint8_t deg = (uint8_t)(degrees / 6);
|
||||
if (deg > 46)
|
||||
{
|
||||
printf("%s: Degrees out of range, must be between 0-270\n",
|
||||
__FUNCTION__);
|
||||
return UPM_ERROR_OUT_OF_RANGE;
|
||||
}
|
||||
|
||||
uint8_t cksum = 0x22 + deg + 0x00;
|
||||
cmd[0] = 0x22;
|
||||
cmd[1] = deg;
|
||||
cmd[2] = 0x00;
|
||||
cmd[3] = cksum;
|
||||
|
||||
if (urm37_send_command(dev, cmd, resp) != UPM_SUCCESS)
|
||||
{
|
||||
printf("%s: urm37_send_command() failed\n", __FUNCTION__);
|
||||
return UPM_ERROR_OPERATION_FAILED;
|
||||
}
|
||||
|
||||
uint8_t h = (uint8_t) resp[1];
|
||||
uint8_t l = (uint8_t) resp[2];
|
||||
|
||||
*distance = (float)((h << 8) | l);
|
||||
|
||||
return UPM_SUCCESS;
|
||||
}
|
||||
|
||||
upm_result_t urm37_get_temperature(urm37_context dev, float* temperature)
|
||||
{
|
||||
if (dev->is_analog_mode)
|
||||
{
|
||||
printf("%s: Temperature measurement is not available in analog mode\n",
|
||||
__FUNCTION__);
|
||||
return UPM_ERROR_NOT_SUPPORTED;
|
||||
}
|
||||
|
||||
// UART mode
|
||||
char cmd[URM37_MAX_DATA_LEN];
|
||||
|
||||
// get temperature sequence
|
||||
cmd[0] = 0x11;
|
||||
cmd[1] = 0x00;
|
||||
cmd[2] = 0x00;
|
||||
cmd[3] = 0x11; // cksum
|
||||
|
||||
char resp[URM37_MAX_DATA_LEN];
|
||||
if (urm37_send_command(dev, cmd, resp) != UPM_SUCCESS)
|
||||
{
|
||||
printf("%s: urm37_send_command() failed\n", __FUNCTION__);
|
||||
return UPM_ERROR_OPERATION_FAILED;
|
||||
}
|
||||
|
||||
uint8_t h = (uint8_t) resp[1];
|
||||
uint8_t l = (uint8_t) resp[2];
|
||||
|
||||
*temperature = (float)((h & 0x0f) * 256 + l) / 10.0;
|
||||
|
||||
if (h & 0xf0)
|
||||
*temperature *= -1;
|
||||
|
||||
return UPM_SUCCESS;
|
||||
}
|
||||
|
||||
upm_result_t urm37_read_eeprom(urm37_context dev, uint8_t addr, uint8_t* value)
|
||||
{
|
||||
if (dev->is_analog_mode)
|
||||
{
|
||||
printf("%s: EEPROM is not available in analog mode\n",
|
||||
__FUNCTION__);
|
||||
return UPM_ERROR_NOT_SUPPORTED;
|
||||
}
|
||||
|
||||
if (addr > 0x04)
|
||||
{
|
||||
printf("Address must be between 0x00-0x04");
|
||||
return UPM_ERROR_OUT_OF_RANGE;
|
||||
}
|
||||
|
||||
char cmd[URM37_MAX_DATA_LEN];
|
||||
uint8_t cksum = 0x33 + addr + 0x00;
|
||||
cmd[0] = 0x33;
|
||||
cmd[1] = addr;
|
||||
cmd[2] = 0x00;
|
||||
cmd[3] = cksum;
|
||||
|
||||
char resp[URM37_MAX_DATA_LEN];
|
||||
|
||||
if (urm37_send_command(dev, cmd, resp) != UPM_SUCCESS)
|
||||
{
|
||||
printf("%s: urm37_send_command() failed\n", __FUNCTION__);
|
||||
return UPM_ERROR_OPERATION_FAILED;
|
||||
}
|
||||
|
||||
*value = resp[2];
|
||||
return UPM_SUCCESS;
|
||||
}
|
||||
|
||||
upm_result_t urm37_write_eeprom(urm37_context dev, uint8_t addr, uint8_t value)
|
||||
{
|
||||
if (dev->is_analog_mode)
|
||||
{
|
||||
printf("%s: EEPROM is not available in analog mode\n",
|
||||
__FUNCTION__);
|
||||
return UPM_ERROR_NOT_SUPPORTED;
|
||||
}
|
||||
|
||||
if (addr > 0x04)
|
||||
{
|
||||
printf("Address must be between 0x00-0x04");
|
||||
return UPM_ERROR_OUT_OF_RANGE;
|
||||
}
|
||||
|
||||
char cmd[URM37_MAX_DATA_LEN];
|
||||
uint8_t cksum = 0x44 + addr + value;
|
||||
cmd[0] = 0x44;
|
||||
cmd[1] = addr;
|
||||
cmd[2] = value;
|
||||
cmd[3] = cksum;
|
||||
|
||||
char resp[URM37_MAX_DATA_LEN]; // throw away
|
||||
if (urm37_send_command(dev, cmd, resp) != UPM_SUCCESS)
|
||||
{
|
||||
printf("%s: urm37_send_command() failed\n", __FUNCTION__);
|
||||
return UPM_ERROR_OPERATION_FAILED;
|
||||
}
|
||||
|
||||
return UPM_SUCCESS;
|
||||
}
|
||||
@ -23,296 +23,76 @@
|
||||
*/
|
||||
|
||||
#include <iostream>
|
||||
#include <stdexcept>
|
||||
|
||||
#include "urm37.hpp"
|
||||
|
||||
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_urm37(urm37_init(aPin, resetPin, triggerPin, aref, 0, true))
|
||||
{
|
||||
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();
|
||||
if (!m_urm37)
|
||||
throw std::runtime_error(string(__FUNCTION__) +
|
||||
": urm37_init() failed");
|
||||
}
|
||||
|
||||
URM37::URM37(int uart, int resetPin) :
|
||||
m_uart(new mraa::Uart(uart)), m_aio(0), m_gpioReset(resetPin),
|
||||
m_gpioTrigger(0)
|
||||
m_urm37(urm37_init(0, resetPin, 0, 0, uart, false))
|
||||
{
|
||||
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();
|
||||
if (!m_urm37)
|
||||
throw std::runtime_error(string(__FUNCTION__) +
|
||||
": urm37_init() failed");
|
||||
}
|
||||
|
||||
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();
|
||||
urm37_close(m_urm37);
|
||||
}
|
||||
|
||||
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);
|
||||
urm37_reset(m_urm37);
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
float distance;
|
||||
|
||||
// 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);
|
||||
if (urm37_get_distance(m_urm37, &distance, degrees) != UPM_SUCCESS)
|
||||
throw std::runtime_error(string(__FUNCTION__) +
|
||||
": urm37_get_distance() failed");
|
||||
|
||||
return (distance);
|
||||
}
|
||||
|
||||
float URM37::getTemperature()
|
||||
{
|
||||
if (m_analogMode)
|
||||
{
|
||||
throw std::runtime_error(string(__FUNCTION__) +
|
||||
": Temperature measurement not available in analog mode");
|
||||
float temperature;
|
||||
|
||||
return 0.0;
|
||||
}
|
||||
if (urm37_get_temperature(m_urm37, &temperature) != UPM_SUCCESS)
|
||||
throw std::runtime_error(string(__FUNCTION__) +
|
||||
": urm37_get_temperature() failed");
|
||||
|
||||
// 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);
|
||||
return temperature;
|
||||
}
|
||||
|
||||
uint8_t URM37::readEEPROM(uint8_t addr)
|
||||
{
|
||||
if (m_analogMode)
|
||||
{
|
||||
throw std::runtime_error(string(__FUNCTION__) +
|
||||
": readEEPROM() is not possible in analog mode");
|
||||
uint8_t value;
|
||||
|
||||
return 0;
|
||||
}
|
||||
if (urm37_read_eeprom(m_urm37, addr, &value) != UPM_SUCCESS)
|
||||
throw std::runtime_error(string(__FUNCTION__) +
|
||||
": urm37_read_eeprom() failed");
|
||||
|
||||
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];
|
||||
return value;
|
||||
}
|
||||
|
||||
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 "";
|
||||
if (urm37_write_eeprom(m_urm37, addr, value) != UPM_SUCCESS)
|
||||
throw std::runtime_error(string(__FUNCTION__) +
|
||||
": urm37_write_eeprom() failed");
|
||||
}
|
||||
|
||||
|
||||
173
src/urm37/urm37.h
Normal file
173
src/urm37/urm37.h
Normal file
@ -0,0 +1,173 @@
|
||||
/*
|
||||
* Author: Jon Trulson <jtrulson@ics.com>
|
||||
* Abhishek Malik <abhishek.malik@intel.com>
|
||||
* Copyright (c) 2016 Intel Corporation.
|
||||
*
|
||||
* Thanks to Adafruit for supplying a google translated version of the
|
||||
* Chinese datasheet and some clues in their code.
|
||||
*
|
||||
* 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.
|
||||
*/
|
||||
|
||||
#ifndef URM37_H_
|
||||
#define URM37_H_
|
||||
|
||||
#include <stdint.h>
|
||||
#include "upm.h"
|
||||
#include "mraa/aio.h"
|
||||
#include "mraa/gpio.h"
|
||||
#include "mraa/uart.h"
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/**
|
||||
* @brief UPM C API for the DFRobot URM37 Ultrasonic Ranger
|
||||
*
|
||||
* The driver was tested with the DFRobot URM37 Ultrasonic Ranger,
|
||||
* V4. It has a range of between 5 and 500 centimeters (cm). It
|
||||
* supports both analog distance measurement, and UART based
|
||||
* temperature and distance measurements. This driver does not
|
||||
* support PWM measurement mode.
|
||||
*
|
||||
* For UART operation, the only supported baud rate is 9600. In
|
||||
* addition, you must ensure that the UART TX/RX pins are
|
||||
* configured for TTL operation (the factory default) rather than
|
||||
* RS232 operation, or permanent damage to your URM37 and/or MCU
|
||||
* will result. On power up, the LED indicator will blink one
|
||||
* long pulse, followed by one short pulse to indicate TTL
|
||||
* operation. See the DFRobot wiki for more information:
|
||||
*
|
||||
* (https://www.dfrobot.com/wiki/index.php?title=URM37_V4.0_Ultrasonic_Sensor_%28SKU:SEN0001%29)
|
||||
*
|
||||
* An example using analog mode
|
||||
* @snippet urm37.c Interesting
|
||||
* An example using UART mode
|
||||
* @snippet urm37-uart.c Interesting
|
||||
*/
|
||||
|
||||
/**
|
||||
* device context
|
||||
*/
|
||||
typedef struct _urm37_context {
|
||||
mraa_aio_context aio;
|
||||
mraa_gpio_context gpio_reset;
|
||||
mraa_gpio_context gpio_trigger;
|
||||
mraa_uart_context uart;
|
||||
|
||||
bool is_analog_mode;
|
||||
|
||||
float a_ref;
|
||||
float a_res;
|
||||
} *urm37_context;
|
||||
|
||||
/**
|
||||
* URM37 Initializer
|
||||
*
|
||||
* @param a_pin Analog pin to use. Ignored in UART mode.
|
||||
* @param reset_pin GPIO pin to use for reset
|
||||
* @param trigger_pin GPIO pin to use for triggering a distance
|
||||
* measurement. Ignored in UART mode.
|
||||
* @param a_ref The analog reference voltage. Ignored in UART mode.
|
||||
* @param uart Default UART to use (0 or 1). Ignored in analog mode.
|
||||
* @param mode true for analog mode, false otherwise.
|
||||
*/
|
||||
urm37_context urm37_init(uint8_t a_pin, uint8_t reset_pin,
|
||||
uint8_t trigger_pin, float a_ref,
|
||||
uint8_t uart, bool analog_mode);
|
||||
|
||||
/**
|
||||
* URM37 sensor close function
|
||||
*/
|
||||
void urm37_close(urm37_context dev);
|
||||
|
||||
/**
|
||||
* Reset the device. This will take approximately 3 seconds to
|
||||
* complete.
|
||||
*
|
||||
* @param dev sensor context
|
||||
*/
|
||||
upm_result_t urm37_reset(urm37_context dev);
|
||||
|
||||
/**
|
||||
* Get the distance measurement. A return value of 65535.0
|
||||
* in UART mode indicates an invalid measurement.
|
||||
*
|
||||
* @param dev sensor context
|
||||
* @param distance A pointer to a float that will contain the distance
|
||||
* in CM if the measurement is successful.
|
||||
* @param degrees In UART mode, this specifies the degrees to turn an
|
||||
* attached PWM servo connected to the MOTO output on the URM37.
|
||||
* Valid values are 0-270. This option is ignored in analog mode. If
|
||||
* you are not using this functionality, just pass 0.
|
||||
* @return UPM status code
|
||||
*/
|
||||
upm_result_t urm37_get_distance(urm37_context dev, float *distance,
|
||||
int degrees);
|
||||
|
||||
/**
|
||||
* Get the temperature measurement. This is only valid in UART mode.
|
||||
*
|
||||
* @param dev sensor context
|
||||
* @param temperature A float pointer containing the measured
|
||||
* temperature in degrees C
|
||||
* @return UPM status code
|
||||
*
|
||||
*/
|
||||
upm_result_t urm37_get_temperature(urm37_context dev, float* temperature);
|
||||
|
||||
/**
|
||||
* In UART mode only, read a value from the EEPROM and return it.
|
||||
*
|
||||
* @param dev sensor context
|
||||
* @param addr The address in the EEPROM to read. Valid values
|
||||
* are between 0x00-0x04.
|
||||
* @param value A pointer containing the returned value.
|
||||
* @return UPM status code
|
||||
*/
|
||||
upm_result_t urm37_read_eeprom(urm37_context dev, uint8_t addr, uint8_t* value);
|
||||
|
||||
/**
|
||||
* In UART mode only, write a value into an address on the EEPROM.
|
||||
*
|
||||
* @param dev sensor context
|
||||
* @param addr The address in the EEPROM to write. Valid values
|
||||
* are between 0x00-0x04.
|
||||
* @param value The value to write
|
||||
* @return UPM status code
|
||||
*/
|
||||
upm_result_t urm37_write_eeprom(urm37_context dev, uint8_t addr, uint8_t value);
|
||||
|
||||
/**
|
||||
* In UART mode only, send a 4-byte command, and return a 4-byte response.
|
||||
*
|
||||
* @param dev sensor context
|
||||
* @param cmd A 4-byte command to transmit
|
||||
* @param response The 4-byte response
|
||||
* @return UPM response code (success, failure, or timeout)
|
||||
*/
|
||||
upm_result_t urm37_send_command(urm37_context dev, char* cmd, char* response);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif /* URM37_H_ */
|
||||
@ -1,6 +1,6 @@
|
||||
/*
|
||||
* Author: Jon Trulson <jtrulson@ics.com>
|
||||
* Copyright (c) 2015 Intel Corporation.
|
||||
* Copyright (c) 2015-2016 Intel Corporation.
|
||||
*
|
||||
* Thanks to Adafruit for supplying a google translated version of the
|
||||
* Chinese datasheet and some clues in their code.
|
||||
@ -33,12 +33,7 @@
|
||||
#include <unistd.h>
|
||||
#include <string.h>
|
||||
|
||||
#include <mraa/common.hpp>
|
||||
#include <mraa/uart.hpp>
|
||||
#include <mraa/aio.hpp>
|
||||
#include <mraa/gpio.hpp>
|
||||
|
||||
#define URM37_DEFAULT_UART 0
|
||||
#include "urm37.h"
|
||||
|
||||
namespace upm {
|
||||
/**
|
||||
@ -153,52 +148,10 @@ namespace upm {
|
||||
void writeEEPROM(uint8_t addr, uint8_t value);
|
||||
|
||||
protected:
|
||||
mraa::Uart *m_uart;
|
||||
mraa::Aio *m_aio;
|
||||
mraa::Gpio *m_gpioTrigger;
|
||||
mraa::Gpio m_gpioReset;
|
||||
|
||||
// initialize reset gpio and call reset
|
||||
void init();
|
||||
|
||||
// send a serial command and return a 4 byte response (UART mode only)
|
||||
std::string sendCommand(std::string cmd);
|
||||
// urm37 device context
|
||||
urm37_context m_urm37;
|
||||
|
||||
private:
|
||||
/**
|
||||
* Checks to see if there is data aavailable for reading
|
||||
*
|
||||
* @param millis Number of milliseconds to wait; 0 means no waiting
|
||||
* @return true if there is data available for reading
|
||||
*/
|
||||
bool dataAvailable(unsigned int millis);
|
||||
|
||||
/**
|
||||
* Reads any available data and returns it in a std::string. Note:
|
||||
* the call blocks until data is available for reading. Use
|
||||
* dataAvailable() to determine whether there is data available
|
||||
* beforehand, to avoid blocking.
|
||||
*
|
||||
* @param len Maximum length of the data to be returned
|
||||
* @return Number of bytes read
|
||||
*/
|
||||
std::string readDataStr(int len);
|
||||
|
||||
/**
|
||||
* Writes the std:string data to the device. If you are writing a
|
||||
* command, be sure to terminate it with a carriage return (\r)
|
||||
*
|
||||
* @param data Buffer to write to the device
|
||||
* @return Number of bytes written
|
||||
*/
|
||||
int writeDataStr(std::string data);
|
||||
|
||||
// analog or UART mode
|
||||
bool m_analogMode;
|
||||
|
||||
// analog reference and resolution
|
||||
float m_aref;
|
||||
int m_aRes;
|
||||
};
|
||||
}
|
||||
|
||||
|
||||
109
src/urm37/urm37_fti.c
Normal file
109
src/urm37/urm37_fti.c
Normal file
@ -0,0 +1,109 @@
|
||||
/*
|
||||
* Author: Jon Trulson <jtrulson@ics.com>
|
||||
* Abhishek Malik <abhishek.malik@intel.com>
|
||||
* Copyright (c) 2016 Intel Corporation.
|
||||
*
|
||||
* Thanks to Adafruit for supplying a google translated version of the
|
||||
* Chinese datasheet and some clues in their code.
|
||||
*
|
||||
* 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 "urm37.h"
|
||||
#include "upm_fti.h"
|
||||
|
||||
/**
|
||||
* This file implements the Function Table Interface (FTI) for this sensor
|
||||
*/
|
||||
|
||||
const char upm_light_name[] = "URM37";
|
||||
const char upm_light_description[] = "Ultrasonic Ranger";
|
||||
// problem here is it's an either/or analog vs. uart. So we will just
|
||||
// only support analog for now
|
||||
// 1st gpio is reset, 2nd is trigger
|
||||
const upm_protocol_t upm_light_protocol[] = {UPM_ANALOG, UPM_GPIO, UPM_GPIO};
|
||||
const upm_sensor_t upm_light_category[] = {UPM_DISTANCE};
|
||||
|
||||
// forward declarations
|
||||
const void* upm_urm37_get_ft(upm_sensor_t sensor_type);
|
||||
void* upm_urm37_init_name();
|
||||
void upm_urm37_close(void* dev);
|
||||
upm_result_t upm_urm37_get_distance(void* dev, float* distance,
|
||||
upm_distance_u unit);
|
||||
|
||||
static const upm_sensor_ft ft =
|
||||
{
|
||||
.upm_sensor_init_name = &upm_urm37_init_name,
|
||||
.upm_sensor_close = &upm_urm37_close,
|
||||
};
|
||||
|
||||
static const upm_distance_ft dft =
|
||||
{
|
||||
.upm_distance_get_value = &upm_urm37_get_distance
|
||||
};
|
||||
|
||||
const void* upm_urm37_get_ft(upm_sensor_t sensor_type)
|
||||
{
|
||||
switch(sensor_type)
|
||||
{
|
||||
case UPM_SENSOR:
|
||||
return &ft;
|
||||
|
||||
case UPM_DISTANCE:
|
||||
return &dft;
|
||||
|
||||
default:
|
||||
return NULL;
|
||||
}
|
||||
}
|
||||
|
||||
void* upm_urm37_init_name()
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
|
||||
|
||||
void upm_urm37_close(void* dev)
|
||||
{
|
||||
urm37_close((urm37_context)dev);
|
||||
}
|
||||
|
||||
upm_result_t upm_urm37_get_distance(void* dev, float* distance,
|
||||
upm_distance_u unit)
|
||||
{
|
||||
// only cm returned by sensor
|
||||
float dist;
|
||||
urm37_get_distance((urm37_context)dev, &dist, 0);
|
||||
|
||||
switch(unit)
|
||||
{
|
||||
case CENTIMETER:
|
||||
*distance = dist;
|
||||
return UPM_SUCCESS;
|
||||
|
||||
case INCH:
|
||||
*distance = dist / 2.54;
|
||||
return UPM_SUCCESS;
|
||||
|
||||
default:
|
||||
return UPM_ERROR_INVALID_PARAMETER;
|
||||
}
|
||||
}
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user