mirror of
https://github.com/eclipse/upm.git
synced 2025-07-03 02:11:15 +03:00
394 lines
9.8 KiB
C
394 lines
9.8 KiB
C
/*
|
|
* 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(int a_pin, int reset_pin,
|
|
int trigger_pin, float a_ref,
|
|
int uart_bus, bool analog_mode)
|
|
{
|
|
// make sure MRAA is initialized
|
|
int mraa_rv;
|
|
if ((mraa_rv = mraa_init()) != MRAA_SUCCESS)
|
|
{
|
|
printf("%s: mraa_init() failed (%d).\n", __FUNCTION__, mraa_rv);
|
|
return NULL;
|
|
}
|
|
|
|
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;
|
|
}
|