From 2694bf6c874dcd64bf5891278d5e3ca9ae142cd7 Mon Sep 17 00:00:00 2001 From: Zac Stanton Date: Thu, 27 Mar 2025 18:46:09 -0600 Subject: [PATCH 1/3] rev 1 of bno085. missing some parts. we need a gpio library for c++ --- .gitmodules | 3 + src/rov/rov_bno085/CMakeLists.txt | 43 +++ .../include/rov_bno085/bno08x_driver.hpp | 61 ++++ .../include/rov_bno085/bno08x_node.hpp | 41 +++ .../include/rov_bno085/setup_gpio_exports.hpp | 245 ++++++++++++++ src/rov/rov_bno085/package.xml | 18 + src/rov/rov_bno085/sh2 | 1 + src/rov/rov_bno085/src/bno08x_driver.cpp | 308 ++++++++++++++++++ src/rov/rov_bno085/src/bno08x_node.cpp | 26 ++ src/rov/rov_bno085/src/main.cpp | 8 + 10 files changed, 754 insertions(+) create mode 100644 .gitmodules create mode 100644 src/rov/rov_bno085/CMakeLists.txt create mode 100644 src/rov/rov_bno085/include/rov_bno085/bno08x_driver.hpp create mode 100644 src/rov/rov_bno085/include/rov_bno085/bno08x_node.hpp create mode 100644 src/rov/rov_bno085/include/rov_bno085/setup_gpio_exports.hpp create mode 100644 src/rov/rov_bno085/package.xml create mode 160000 src/rov/rov_bno085/sh2 create mode 100644 src/rov/rov_bno085/src/bno08x_driver.cpp create mode 100644 src/rov/rov_bno085/src/bno08x_node.cpp create mode 100644 src/rov/rov_bno085/src/main.cpp diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000..17a70a2 --- /dev/null +++ b/.gitmodules @@ -0,0 +1,3 @@ +[submodule "src/rov/rov_bno085/sh2"] + path = src/rov/rov_bno085/sh2 + url = https://github.com/ceva-dsp/sh2/ diff --git a/src/rov/rov_bno085/CMakeLists.txt b/src/rov/rov_bno085/CMakeLists.txt new file mode 100644 index 0000000..36f92f6 --- /dev/null +++ b/src/rov/rov_bno085/CMakeLists.txt @@ -0,0 +1,43 @@ +cmake_minimum_required(VERSION 3.16) +project(rov_bno085) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +add_executable(bno085 src/main.cpp +src/bno08x_node.cpp +src/bno08x_driver.cpp +sh2/euler.c +sh2/sh2.c +sh2/sh2_SensorValue.c +sh2/sh2_util.c +sh2/shtp.c +) +target_include_directories(bno085 PUBLIC + $ + $) +target_compile_features(bno085 PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 + +install(TARGETS bno085 + DESTINATION lib/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/src/rov/rov_bno085/include/rov_bno085/bno08x_driver.hpp b/src/rov/rov_bno085/include/rov_bno085/bno08x_driver.hpp new file mode 100644 index 0000000..ae9e54b --- /dev/null +++ b/src/rov/rov_bno085/include/rov_bno085/bno08x_driver.hpp @@ -0,0 +1,61 @@ +#ifndef BNO08X_DRIVER_HPP +#define BNO08X_DRIVER_HPP + +#include + +#include "sh2.h" +#include "sh2_SensorValue.h" +#include "sh2_err.h" + +#define I2C_DEFAULT_ADDRESS 0x4A + +#define PAC_ON_STAIRS 8 +#define PAC_OPTION_COUNT 9 + +// NOTE: how to write to gpio? -> https://www.kernel.org/doc/Documentation/gpio/sysfs.txt +// EXAMPLE: https://developer.ridgerun.com/wiki/index.php/Gpio-int-test.c +// NOTE: EXECUTABLE NEEDS ACCESS TO SYSFS (root only) https://unix.stackexchange.com/a/409780 +// MAYBE SET UDEV? https://forums.developer.nvidia.com/t/gpio-permissions-problem-udev-rules-not-working/267078/5 +// https://github.com/NVIDIA/jetson-gpio/blob/master/lib/python/Jetson/GPIO/99-gpio.rules +#include "rov_bno085/setup_gpio_exports.hpp" + +// ABOVE IS DEPRECATED?? +// https://forums.developer.nvidia.com/t/orin-nano-fast-gpio-c-library-with-direct-register-access/303681 +// https://docs.kernel.org/userspace-api/gpio/chardev.html +// looks like this works? https://github.com/Rubberazer/JETGPIO?tab=readme-ov-file +// https://github.com/Rubberazer/Jetclocks + +// see https://github.com/adafruit/Adafruit_BNO08x/blob/master/src/Adafruit_BNO08x.cpp +// for inspiration +class BNO08X { +public: + BNO08X(uint8_t reset_pin); + + bool makeI2C(uint8_t address = I2C_DEFAULT_ADDRESS, int32_t sensor_id = 0) {return false;}; // NOTE: UNIMPLEMENTED + bool makeUART(int32_t sensor_id = 0) {return false;}; // NOTE: UNIMPLEMENTED + bool makeSPI(int32_t sensor_id = 0); + + // reset the device using the reset pin + void hardwareReset(void); + // check if a reset has occured + bool wasReset(void); + + // enable the given report type + bool enableReport(sh2_SensorId_t sensor_id, uint32_t interval_us = 10000); + // fill the given sensor value object with a new report + bool getSensorEvent(sh2_SensorValue_t* value); +private: + sh2_ProductIds_t product_ids; + + int fd; + + int interrupt_pin; + +protected: + // called post i2c/spi init + virtual bool _init(int32_t sensor_id); + + sh2_Hal_t _HAL; +}; + +#endif \ No newline at end of file diff --git a/src/rov/rov_bno085/include/rov_bno085/bno08x_node.hpp b/src/rov/rov_bno085/include/rov_bno085/bno08x_node.hpp new file mode 100644 index 0000000..64101e5 --- /dev/null +++ b/src/rov/rov_bno085/include/rov_bno085/bno08x_node.hpp @@ -0,0 +1,41 @@ +#ifndef BNO08X_NODE_HPP +#define BNO08X_NODE_HPP + +#include + +#include +#include +#include +#include +#include +// TODO include quat definition or just vectord? + +#include "rov_bno085/bno08x_driver.hpp" + +class BNO08X_Node : public rclcpp::Node { +public: + BNO08X_Node(uint8_t bus = 1); + BNO08X_Node(const BNO08X_Node&) = delete; + BNO08X_Node(BNO08X_Node&&) = default; + ~BNO08X_Node(); + + void open(); + void close(); + void reset(); + +private: + void pollBNO(); + void publishBNOData(); + + rclcpp::Publisher::SharedPtr pub_accel_gyro; + rclcpp::Publisher::SharedPtr pub_mag; + rclcpp::Publisher::SharedPtr pub_temp; + + rclcpp::TimerBase::SharedPtr timer_poll_bno; + + BNO08X bno08x; + + int spi_fd; +}; + +#endif \ No newline at end of file diff --git a/src/rov/rov_bno085/include/rov_bno085/setup_gpio_exports.hpp b/src/rov/rov_bno085/include/rov_bno085/setup_gpio_exports.hpp new file mode 100644 index 0000000..9e8ed4c --- /dev/null +++ b/src/rov/rov_bno085/include/rov_bno085/setup_gpio_exports.hpp @@ -0,0 +1,245 @@ +#ifndef GPIO_EXPORTS_HPP +#define GPIO_EXPORTS_HPP + +/* Copyright (c) 2011, RidgeRun + * All rights reserved. + * + * Contributors include: + * Todd Fischer + * Brad Lu + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * 3. All advertising materials mentioning features or use of this software + * must display the following acknowledgement: + * This product includes software developed by the RidgeRun. + * 4. Neither the name of the RidgeRun nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY RIDGERUN ''AS IS'' AND ANY + * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL RIDGERUN BE LIABLE FOR ANY + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include +#include +#include +#include + +#define SYSFS_GPIO_DIR "/sys/class/gpio" +#define POLL_TIMEOUT 3000 +#define MAX_BUF 64 + +int gpio_export(unsigned int gpio) { + int fd, len; + char buf[MAX_BUF]; + + fd = open(SYSFS_GPIO_DIR "/export", O_WRONLY); + if (fd < 0) { + perror("gpio/export"); + return fd; + } + + len = snprintf(buf, sizeof(buf), "%d", gpio); + write(fd, buf, len); + close(fd); + + return 0; +} + +int gpio_unexport(unsigned int gpio) { + int fd, len; + char buf[MAX_BUF]; + + fd = open(SYSFS_GPIO_DIR "/unexport", O_WRONLY); + if (fd < 0) { + perror("gpio/export"); + return fd; + } + + len = snprintf(buf, sizeof(buf), "%d", gpio); + write(fd, buf, len); + close(fd); + return 0; +} + +int gpio_set_dir(unsigned int gpio, unsigned int out_flag) { + int fd, len; + char buf[MAX_BUF]; + + len = snprintf(buf, sizeof(buf), SYSFS_GPIO_DIR "/gpio%d/direction", gpio); + + fd = open(buf, O_WRONLY); + if (fd < 0) { + perror("gpio/direction"); + return fd; + } + + if (out_flag) + write(fd, "out", 4); + else + write(fd, "in", 3); + + close(fd); + return 0; +} + +int gpio_set_value(unsigned int gpio, unsigned int value) { + int fd, len; + char buf[MAX_BUF]; + + len = snprintf(buf, sizeof(buf), SYSFS_GPIO_DIR "/gpio%d/value", gpio); + + fd = open(buf, O_WRONLY); + if (fd < 0) { + perror("gpio/set-value"); + return fd; + } + + if (value) + write(fd, "1", 2); + else + write(fd, "0", 2); + + close(fd); + return 0; +} + +int gpio_get_value(unsigned int gpio, unsigned int *value) { + int fd, len; + char buf[MAX_BUF]; + char ch; + + len = snprintf(buf, sizeof(buf), SYSFS_GPIO_DIR "/gpio%d/value", gpio); + + fd = open(buf, O_RDONLY); + if (fd < 0) { + perror("gpio/get-value"); + return fd; + } + + read(fd, &ch, 1); + + if (ch != '0') { + *value = 1; + } else { + *value = 0; + } + + close(fd); + return 0; +} + +int gpio_set_edge(unsigned int gpio, char *edge) { + int fd, len; + char buf[MAX_BUF]; + + len = snprintf(buf, sizeof(buf), SYSFS_GPIO_DIR "/gpio%d/edge", gpio); + + fd = open(buf, O_WRONLY); + if (fd < 0) { + perror("gpio/set-edge"); + return fd; + } + + write(fd, edge, strlen(edge) + 1); + close(fd); + return 0; +} + +int gpio_fd_open(unsigned int gpio) { + int fd, len; + char buf[MAX_BUF]; + + len = snprintf(buf, sizeof(buf), SYSFS_GPIO_DIR "/gpio%d/value", gpio); + + fd = open(buf, O_RDONLY | O_NONBLOCK ); + if (fd < 0) { + perror("gpio/fd_open"); + } + return fd; +} + +int gpio_fd_close(int fd) { + return close(fd); +} + +// example main: +// int main(int argc, char **argv, char **envp) { +// struct pollfd fdset[2]; +// int nfds = 2; +// int gpio_fd, timeout, rc; +// char *buf[MAX_BUF]; +// unsigned int gpio; +// int len; + +// if (argc < 2) { +// printf("Usage: gpio-int \n\n"); +// printf("Waits for a change in the GPIO pin voltage level or input on stdin\n"); +// exit(-1); +// } + +// gpio = atoi(argv[1]); + +// gpio_export(gpio); +// gpio_set_dir(gpio, 0); +// gpio_set_edge(gpio, "rising"); +// gpio_fd = gpio_fd_open(gpio); + +// timeout = POLL_TIMEOUT; + +// while (1) { +// memset((void*)fdset, 0, sizeof(fdset)); + +// fdset[0].fd = STDIN_FILENO; +// fdset[0].events = POLLIN; + +// fdset[1].fd = gpio_fd; +// fdset[1].events = POLLPRI; + +// rc = poll(fdset, nfds, timeout); + +// if (rc < 0) { +// printf("\npoll() failed!\n"); +// return -1; +// } + +// if (rc == 0) { +// printf("."); +// } + +// if (fdset[1].revents & POLLPRI) { +// lseek(fdset[1].fd, 0, SEEK_SET); +// len = read(fdset[1].fd, buf, MAX_BUF); +// printf("\npoll() GPIO %d interrupt occurred\n", gpio); +// } + +// if (fdset[0].revents & POLLIN) { +// (void)read(fdset[0].fd, buf, 1); +// printf("\npoll() stdin read 0x%2.2X\n", (unsigned int) buf[0]); +// } + +// fflush(stdout); +// } + +// gpio_fd_close(gpio_fd); +// return 0; +// } + +#endif \ No newline at end of file diff --git a/src/rov/rov_bno085/package.xml b/src/rov/rov_bno085/package.xml new file mode 100644 index 0000000..bdad3f5 --- /dev/null +++ b/src/rov/rov_bno085/package.xml @@ -0,0 +1,18 @@ + + + + rov_bno085 + 0.0.0 + TODO: Package description + yameat + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/rov/rov_bno085/sh2 b/src/rov/rov_bno085/sh2 new file mode 160000 index 0000000..b514b1e --- /dev/null +++ b/src/rov/rov_bno085/sh2 @@ -0,0 +1 @@ +Subproject commit b514b1e2586ddc195e553dac89fc94c637b25298 diff --git a/src/rov/rov_bno085/src/bno08x_driver.cpp b/src/rov/rov_bno085/src/bno08x_driver.cpp new file mode 100644 index 0000000..b305163 --- /dev/null +++ b/src/rov/rov_bno085/src/bno08x_driver.cpp @@ -0,0 +1,308 @@ +#include "rov_bno085/bno08x_driver.hpp" + +#include +#include +#include +#include + +#define SPIDEVICE "/dev/spidev1.0" + +// HAL garbage +static sh2_SensorValue_t *_sensor_value = NULL; +static bool _reset_occurred = false; + +// TODO: implement +// static int i2chal_write(sh2_Hal_t *self, uint8_t *pBuffer, unsigned len); +// static int i2chal_read(sh2_Hal_t *self, uint8_t *pBuffer, unsigned len, uint32_t *t_us); +// static void i2chal_close(sh2_Hal_t *self); +// static int i2chal_open(sh2_Hal_t *self); + +// TODO: implement +// static int uarthal_write(sh2_Hal_t *self, uint8_t *pBuffer, unsigned len); +// static int uarthal_read(sh2_Hal_t *self, uint8_t *pBuffer, unsigned len, uint32_t *t_us); +// static void uarthal_close(sh2_Hal_t *self); +// static int uarthal_open(sh2_Hal_t *self); + +static bool spihal_wait_for_int(void); +static int spihal_write(sh2_Hal_t *self, uint8_t *pBuffer, unsigned len); +static int spihal_read(sh2_Hal_t *self, uint8_t *pBuffer, unsigned len, uint32_t *t_us); +static void spihal_close(sh2_Hal_t *self); +static int spihal_open(sh2_Hal_t *self); +static uint8_t spi_mode = SPI_MODE_3; +static uint8_t spi_bits_per_word = 8; +static uint32_t spi_speed = 1e6; // 1 MHz +static uint8_t spi_lsb_first = 0; +static uint16_t spi_delay = 0; +static std::chrono::time_point spi_start; + +static uint32_t hal_getTimeUs(sh2_Hal_t *self); +static void hal_callback(void *cookie, sh2_AsyncEvent_t *pEvent); +static void sensorHandler(void *cookie, sh2_SensorEvent_t *pEvent); +static void hal_hardwareReset(void); + +static int& _fd; + +BNO08X::BNO08X(uint8_t reset_pin) { + +} + +bool BNO08X::makeSPI(int32_t sensor_id = 0) { + // TODO: setup interrupt pin as input + // interrupt_pin; + + // if file descriptor is in use, we need to close it + if (fd != -1) { + close(fd); + // TODO: if uart and i2c have local fds, invalidate them now + } + + // open the spi device + fd = open(SPIDEVICE, O_RDWR); + if (fd < 0) { + perror("Unable to open SPI device"); + return false; + } + _fd = fd; + + // set spi mode + if (ioctl(fd, SPI_IOC_WR_MODE, &spi_mode) == -1) { + perror("Unable to set SPI mode"); + return 1; + } + + // set bits per word + if (ioctl(fd, SPI_IOC_WR_BITS_PER_WORD, &spi_bits_per_word) == -1) { + perror("Unable to set bits per word"); + return 1; + } + + // set max speed + if (ioctl(fd, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed) == -1) { + perror("Unable to set max speed"); + return 1; + } + + // set bit order + if (ioctl(fd, SPI_IOC_WR_LSB_FIRST, &spi_lsb_first) == -1) { + perror("Unable to set bitorder to msb first"); + return 1; + } + + _HAL.open = spihal_open; + _HAL.close = spihal_close; + _HAL.read = spihal_read; + _HAL.write = spihal_write; + _HAL.getTimeUs = hal_getTimeUs; + + spi_start = std::chrono::high_resolution_clock::now(); +} + +bool BNO08X::_init(int32_t sensor_id) { + int status; + + hardwareReset(); + + // open SH2 + status = sh2_open(&_HAL, hal_callback, NULL); + if (status != SH2_OK) { + return false; + } + + // check connection + memset(&product_ids, 0, sizeof(product_ids)); + status = sh2_getProdIds(&product_ids); + if (status != SH2_OK) { + return false; + } + + // register sensor listener + sh2_setSensorCallback(sensorHandler, NULL); + + return true; +} + +void BNO08X::hardwareReset(void) { + hal_hardwareReset(); +} + +bool BNO08X::wasReset(void) { + bool x = _reset_occurred; + _reset_occurred = false; + return x; +} + +bool BNO08X::enableReport(sh2_SensorId_t sensor_id, uint32_t interval_us = 10000) { + static sh2_SensorConfig_t config; + + // sane defaults + config.changeSensitivityEnabled = false; + config.wakeupEnabled = false; + config.changeSensitivityRelative = false; + config.alwaysOnEnabled = false; + config.changeSensitivity = 0; + config.batchInterval_us = 0; + config.sensorSpecific = 0; + + config.reportInterval_us = interval_us; + int status = sh2_setSensorConfig(sensor_id, &config); + + if (status != SH2_OK) { + return false; + } + return true; +} + +bool BNO08X::getSensorEvent(sh2_SensorValue_t *value) { + _sensor_value = value; + value->timestamp = 0; + + sh2_service(); + + // check if there were new events + if (value->timestamp == 0 && value->sensorId != SH2_GYRO_INTEGRATED_RV) { + return false; + } + + return true; +} + + +static int spi_transfer(int fd, uint8_t* tx_buf, uint8_t* rx_buf, size_t len) { + struct spi_ioc_transfer tr{}; + memset(&tr, 0, sizeof(tr)); + tr.tx_buf = reinterpret_cast(tx_buf); + tr.rx_buf = reinterpret_cast(rx_buf); + tr.len = len; + tr.speed_hz = spi_speed; + tr.delay_usecs = spi_delay; + tr.bits_per_word = spi_bits_per_word; + + int ret = ioctl(fd, SPI_IOC_MESSAGE(1), &tr); + if (ret < 1) { + perror("SPI_IOC_MESSAGE failed"); + return -1; + } + + return 0; +} + +static bool spihal_wait_for_int(void) { + // this is actually really stupid + // does chip interrupt within 0.5 seconds? + for (int i = 0; i < 500; i++) { + // TODO: read interrupt pin from gpio + if (true) { + return true; + } + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + + // if chip does not interrupt, reset it + hal_hardwareReset(); + + return false; +} + +static int spihal_write(sh2_Hal_t *self, uint8_t *pBuffer, unsigned len) { + static uint8_t rx[4] = {0}; + if (!spihal_wait_for_int()) { + return 0; + } + + memset(rx, 0, sizeof(rx)); + int ret = spi_transfer(_fd, pBuffer, rx, len); + + // adafruit doesnt do this? + // if (ret < 0) { + // return 0; + // } + + return len; +} + +static int spihal_read(sh2_Hal_t *self, uint8_t *pBuffer, unsigned len, uint32_t *t_us) { + static uint16_t packet_size = 0; + static uint8_t tx[4] = {0}; + if (!spihal_wait_for_int()) { + return 0; + } + + // read spi + memset(tx, 0, sizeof(tx)); + int ret = spi_transfer(_fd, tx, pBuffer, 4); + if (ret < 0) { + return 0; + } + + // determine amount to read + packet_size = static_cast(pBuffer[0]) | (static_cast(pBuffer[1]) << 8); + // unset continue bit + packet_size &= 0x8000; + + if (packet_size > len) { + return 0; + } + + if (!spihal_wait_for_int()) { + return 0; + } + + // read packet_size bytes into pBuffer + memset(tx, 0, sizeof(tx)); + ret = spi_transfer(_fd, tx, pBuffer, packet_size); + if (ret < 0) { + return 0; + } + + // return how many bytes were read + return packet_size; +} + +static void spihal_close(sh2_Hal_t *self) { + // does nothing +} + +static int spihal_open(sh2_Hal_t *self) { + spihal_wait_for_int(); + + return 0; +} + +uint32_t hal_getTimeUs(sh2_Hal_t *self) { + auto tp = std::chrono::high_resolution_clock::now(); + // ~1 hour 71 minutes before it overflows.... + int64_t time = std::chrono::duration_cast(tp - spi_start).count(); + // handle 32bit overflow by wrapping back to 0 + if (time > UINT32_MAX) { + time = time - UINT32_MAX; + } + + return static_cast(time); +} + +void hal_callback(void *cookie, sh2_AsyncEvent_t *pEvent) { + if (pEvent->eventId == SH2_RESET) { + _reset_occurred = true; + } +} + +void sensorHandler(void *cookie, sh2_SensorEvent_t *pEvent) { + int ret; + ret = sh2_decodeSensorEvent(_sensor_value, pEvent); + + if (ret != SH2_OK) { + printf("BNO08x - Error decoding sensor event\n"); + _sensor_value->timestamp = 0; + return; + } +} + +void hal_hardwareReset(void) { + // TODO: set gpio reset to output + // TODO: set reset pin high + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + // TODO: set reset pin low + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + // TODO: set reset pin high + std::this_thread::sleep_for(std::chrono::milliseconds(10)); +} \ No newline at end of file diff --git a/src/rov/rov_bno085/src/bno08x_node.cpp b/src/rov/rov_bno085/src/bno08x_node.cpp new file mode 100644 index 0000000..61c6ea8 --- /dev/null +++ b/src/rov/rov_bno085/src/bno08x_node.cpp @@ -0,0 +1,26 @@ +#include "rov_bno085/bno08x_driver.hpp" +#include "rov_bno085/bno08x_node.hpp" + +BNO08X_Node::BNO08X_Node(uint8_t bus) : Node("bno08x") { + +} + +BNO08X_Node::~BNO08X_Node() { + +} + +void BNO08X_Node::open() { + +} + +void BNO08X_Node::close() { + +} + +void BNO08X_Node::reset() { + +} + +void BNO08X_Node::publishBNOData() { + +} \ No newline at end of file diff --git a/src/rov/rov_bno085/src/main.cpp b/src/rov/rov_bno085/src/main.cpp new file mode 100644 index 0000000..4358937 --- /dev/null +++ b/src/rov/rov_bno085/src/main.cpp @@ -0,0 +1,8 @@ +#include "rov_bno085/bno08x_node.hpp" + +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file From 8f78c67007c7c2a46f61849c176d462771c8d835 Mon Sep 17 00:00:00 2001 From: Zac Stanton Date: Thu, 27 Mar 2025 23:31:54 -0600 Subject: [PATCH 2/3] jetgpio implemented. compile errors fixed --- .gitmodules | 3 + src/rov/rov_bno085/CMakeLists.txt | 17 +- .../include/rov_bno085/bno08x_driver.hpp | 26 +- .../include/rov_bno085/setup_gpio_exports.hpp | 245 ------------------ src/rov/rov_bno085/jetgpio | 1 + src/rov/rov_bno085/src/bno08x_driver.cpp | 63 +++-- 6 files changed, 79 insertions(+), 276 deletions(-) delete mode 100644 src/rov/rov_bno085/include/rov_bno085/setup_gpio_exports.hpp create mode 160000 src/rov/rov_bno085/jetgpio diff --git a/.gitmodules b/.gitmodules index 17a70a2..5213bbc 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,3 +1,6 @@ [submodule "src/rov/rov_bno085/sh2"] path = src/rov/rov_bno085/sh2 url = https://github.com/ceva-dsp/sh2/ +[submodule "src/rov/rov_bno085/jetgpio"] + path = src/rov/rov_bno085/jetgpio + url = https://github.com/Rubberazer/JETGPIO diff --git a/src/rov/rov_bno085/CMakeLists.txt b/src/rov/rov_bno085/CMakeLists.txt index 36f92f6..efe8a44 100644 --- a/src/rov/rov_bno085/CMakeLists.txt +++ b/src/rov/rov_bno085/CMakeLists.txt @@ -10,21 +10,36 @@ find_package(ament_cmake REQUIRED) # uncomment the following section in order to fill in # further dependencies manually. # find_package( REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(std_srvs REQUIRED) +find_package(sensor_msgs REQUIRED) add_executable(bno085 src/main.cpp src/bno08x_node.cpp src/bno08x_driver.cpp sh2/euler.c -sh2/sh2.c sh2/sh2_SensorValue.c sh2/sh2_util.c +sh2/sh2.c sh2/shtp.c +jetgpio/orin.c ) target_include_directories(bno085 PUBLIC $ + $ + $ $) target_compile_features(bno085 PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +ament_target_dependencies(bno085 + rclcpp + std_msgs + std_srvs + sensor_msgs +) + + install(TARGETS bno085 DESTINATION lib/${PROJECT_NAME}) diff --git a/src/rov/rov_bno085/include/rov_bno085/bno08x_driver.hpp b/src/rov/rov_bno085/include/rov_bno085/bno08x_driver.hpp index ae9e54b..f0fe776 100644 --- a/src/rov/rov_bno085/include/rov_bno085/bno08x_driver.hpp +++ b/src/rov/rov_bno085/include/rov_bno085/bno08x_driver.hpp @@ -3,33 +3,30 @@ #include -#include "sh2.h" +extern "C" { #include "sh2_SensorValue.h" -#include "sh2_err.h" +#include "sh2_hal.h" +} #define I2C_DEFAULT_ADDRESS 0x4A +#define DEFAULT_RESET_PIN 33 +#define DEFAULT_INTERRUPT_PIN 31 #define PAC_ON_STAIRS 8 #define PAC_OPTION_COUNT 9 -// NOTE: how to write to gpio? -> https://www.kernel.org/doc/Documentation/gpio/sysfs.txt -// EXAMPLE: https://developer.ridgerun.com/wiki/index.php/Gpio-int-test.c -// NOTE: EXECUTABLE NEEDS ACCESS TO SYSFS (root only) https://unix.stackexchange.com/a/409780 -// MAYBE SET UDEV? https://forums.developer.nvidia.com/t/gpio-permissions-problem-udev-rules-not-working/267078/5 -// https://github.com/NVIDIA/jetson-gpio/blob/master/lib/python/Jetson/GPIO/99-gpio.rules -#include "rov_bno085/setup_gpio_exports.hpp" - -// ABOVE IS DEPRECATED?? -// https://forums.developer.nvidia.com/t/orin-nano-fast-gpio-c-library-with-direct-register-access/303681 -// https://docs.kernel.org/userspace-api/gpio/chardev.html // looks like this works? https://github.com/Rubberazer/JETGPIO?tab=readme-ov-file // https://github.com/Rubberazer/Jetclocks +// ^^^^ TODO: NEED TO INSTALL JETCLOCKS ON ORIN // see https://github.com/adafruit/Adafruit_BNO08x/blob/master/src/Adafruit_BNO08x.cpp // for inspiration class BNO08X { public: - BNO08X(uint8_t reset_pin); + BNO08X(uint8_t reset_pin = DEFAULT_RESET_PIN, uint8_t interrupt_pin = DEFAULT_INTERRUPT_PIN); + BNO08X(BNO08X&&) = delete; + BNO08X operator=(BNO08X&) = delete; + ~BNO08X(); bool makeI2C(uint8_t address = I2C_DEFAULT_ADDRESS, int32_t sensor_id = 0) {return false;}; // NOTE: UNIMPLEMENTED bool makeUART(int32_t sensor_id = 0) {return false;}; // NOTE: UNIMPLEMENTED @@ -49,7 +46,8 @@ class BNO08X { int fd; - int interrupt_pin; + uint8_t reset_pin; + uint8_t interrupt_pin; protected: // called post i2c/spi init diff --git a/src/rov/rov_bno085/include/rov_bno085/setup_gpio_exports.hpp b/src/rov/rov_bno085/include/rov_bno085/setup_gpio_exports.hpp deleted file mode 100644 index 9e8ed4c..0000000 --- a/src/rov/rov_bno085/include/rov_bno085/setup_gpio_exports.hpp +++ /dev/null @@ -1,245 +0,0 @@ -#ifndef GPIO_EXPORTS_HPP -#define GPIO_EXPORTS_HPP - -/* Copyright (c) 2011, RidgeRun - * All rights reserved. - * - * Contributors include: - * Todd Fischer - * Brad Lu - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * 3. All advertising materials mentioning features or use of this software - * must display the following acknowledgement: - * This product includes software developed by the RidgeRun. - * 4. Neither the name of the RidgeRun nor the - * names of its contributors may be used to endorse or promote products - * derived from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY RIDGERUN ''AS IS'' AND ANY - * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL RIDGERUN BE LIABLE FOR ANY - * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES - * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND - * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT - * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS - * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - */ - -#include -#include -#include -#include -#include -#include -#include - -#define SYSFS_GPIO_DIR "/sys/class/gpio" -#define POLL_TIMEOUT 3000 -#define MAX_BUF 64 - -int gpio_export(unsigned int gpio) { - int fd, len; - char buf[MAX_BUF]; - - fd = open(SYSFS_GPIO_DIR "/export", O_WRONLY); - if (fd < 0) { - perror("gpio/export"); - return fd; - } - - len = snprintf(buf, sizeof(buf), "%d", gpio); - write(fd, buf, len); - close(fd); - - return 0; -} - -int gpio_unexport(unsigned int gpio) { - int fd, len; - char buf[MAX_BUF]; - - fd = open(SYSFS_GPIO_DIR "/unexport", O_WRONLY); - if (fd < 0) { - perror("gpio/export"); - return fd; - } - - len = snprintf(buf, sizeof(buf), "%d", gpio); - write(fd, buf, len); - close(fd); - return 0; -} - -int gpio_set_dir(unsigned int gpio, unsigned int out_flag) { - int fd, len; - char buf[MAX_BUF]; - - len = snprintf(buf, sizeof(buf), SYSFS_GPIO_DIR "/gpio%d/direction", gpio); - - fd = open(buf, O_WRONLY); - if (fd < 0) { - perror("gpio/direction"); - return fd; - } - - if (out_flag) - write(fd, "out", 4); - else - write(fd, "in", 3); - - close(fd); - return 0; -} - -int gpio_set_value(unsigned int gpio, unsigned int value) { - int fd, len; - char buf[MAX_BUF]; - - len = snprintf(buf, sizeof(buf), SYSFS_GPIO_DIR "/gpio%d/value", gpio); - - fd = open(buf, O_WRONLY); - if (fd < 0) { - perror("gpio/set-value"); - return fd; - } - - if (value) - write(fd, "1", 2); - else - write(fd, "0", 2); - - close(fd); - return 0; -} - -int gpio_get_value(unsigned int gpio, unsigned int *value) { - int fd, len; - char buf[MAX_BUF]; - char ch; - - len = snprintf(buf, sizeof(buf), SYSFS_GPIO_DIR "/gpio%d/value", gpio); - - fd = open(buf, O_RDONLY); - if (fd < 0) { - perror("gpio/get-value"); - return fd; - } - - read(fd, &ch, 1); - - if (ch != '0') { - *value = 1; - } else { - *value = 0; - } - - close(fd); - return 0; -} - -int gpio_set_edge(unsigned int gpio, char *edge) { - int fd, len; - char buf[MAX_BUF]; - - len = snprintf(buf, sizeof(buf), SYSFS_GPIO_DIR "/gpio%d/edge", gpio); - - fd = open(buf, O_WRONLY); - if (fd < 0) { - perror("gpio/set-edge"); - return fd; - } - - write(fd, edge, strlen(edge) + 1); - close(fd); - return 0; -} - -int gpio_fd_open(unsigned int gpio) { - int fd, len; - char buf[MAX_BUF]; - - len = snprintf(buf, sizeof(buf), SYSFS_GPIO_DIR "/gpio%d/value", gpio); - - fd = open(buf, O_RDONLY | O_NONBLOCK ); - if (fd < 0) { - perror("gpio/fd_open"); - } - return fd; -} - -int gpio_fd_close(int fd) { - return close(fd); -} - -// example main: -// int main(int argc, char **argv, char **envp) { -// struct pollfd fdset[2]; -// int nfds = 2; -// int gpio_fd, timeout, rc; -// char *buf[MAX_BUF]; -// unsigned int gpio; -// int len; - -// if (argc < 2) { -// printf("Usage: gpio-int \n\n"); -// printf("Waits for a change in the GPIO pin voltage level or input on stdin\n"); -// exit(-1); -// } - -// gpio = atoi(argv[1]); - -// gpio_export(gpio); -// gpio_set_dir(gpio, 0); -// gpio_set_edge(gpio, "rising"); -// gpio_fd = gpio_fd_open(gpio); - -// timeout = POLL_TIMEOUT; - -// while (1) { -// memset((void*)fdset, 0, sizeof(fdset)); - -// fdset[0].fd = STDIN_FILENO; -// fdset[0].events = POLLIN; - -// fdset[1].fd = gpio_fd; -// fdset[1].events = POLLPRI; - -// rc = poll(fdset, nfds, timeout); - -// if (rc < 0) { -// printf("\npoll() failed!\n"); -// return -1; -// } - -// if (rc == 0) { -// printf("."); -// } - -// if (fdset[1].revents & POLLPRI) { -// lseek(fdset[1].fd, 0, SEEK_SET); -// len = read(fdset[1].fd, buf, MAX_BUF); -// printf("\npoll() GPIO %d interrupt occurred\n", gpio); -// } - -// if (fdset[0].revents & POLLIN) { -// (void)read(fdset[0].fd, buf, 1); -// printf("\npoll() stdin read 0x%2.2X\n", (unsigned int) buf[0]); -// } - -// fflush(stdout); -// } - -// gpio_fd_close(gpio_fd); -// return 0; -// } - -#endif \ No newline at end of file diff --git a/src/rov/rov_bno085/jetgpio b/src/rov/rov_bno085/jetgpio new file mode 160000 index 0000000..51d7aca --- /dev/null +++ b/src/rov/rov_bno085/jetgpio @@ -0,0 +1 @@ +Subproject commit 51d7aca988c5d8cfbe1a66146ba1491cdd1a571d diff --git a/src/rov/rov_bno085/src/bno08x_driver.cpp b/src/rov/rov_bno085/src/bno08x_driver.cpp index b305163..0b84090 100644 --- a/src/rov/rov_bno085/src/bno08x_driver.cpp +++ b/src/rov/rov_bno085/src/bno08x_driver.cpp @@ -2,8 +2,19 @@ #include #include +#include +#include +#include #include #include +#include + +#include "jetgpio.h" + +extern "C" { +#include "sh2.h" +#include "sh2_err.h" +} #define SPIDEVICE "/dev/spidev1.0" @@ -40,16 +51,36 @@ static void hal_callback(void *cookie, sh2_AsyncEvent_t *pEvent); static void sensorHandler(void *cookie, sh2_SensorEvent_t *pEvent); static void hal_hardwareReset(void); -static int& _fd; +// shared stuff +static int _fd; +static unsigned int _reset_pin; +static unsigned int _interrupt_pin; -BNO08X::BNO08X(uint8_t reset_pin) { +BNO08X::BNO08X(uint8_t reset_pin, uint8_t interrupt_pin) { + int ret; + ret = gpioInitialise(); + if (ret < 0) { + printf("Jetgpio initialization failed with errco %d\n", ret); + throw std::logic_error("Jetgpio failed to initalize"); + } + + // setup reset pin as output + this->reset_pin = reset_pin; + _reset_pin = this->reset_pin; + ret = gpioSetMode(reset_pin, JET_OUTPUT); + + // setup "interrupt" pin as input + this->interrupt_pin = interrupt_pin; + _interrupt_pin = this->interrupt_pin; + ret = gpioSetMode(interrupt_pin, JET_INPUT); } -bool BNO08X::makeSPI(int32_t sensor_id = 0) { - // TODO: setup interrupt pin as input - // interrupt_pin; +BNO08X::~BNO08X() { + gpioTerminate(); +} +bool BNO08X::makeSPI(int32_t sensor_id) { // if file descriptor is in use, we need to close it if (fd != -1) { close(fd); @@ -67,25 +98,25 @@ bool BNO08X::makeSPI(int32_t sensor_id = 0) { // set spi mode if (ioctl(fd, SPI_IOC_WR_MODE, &spi_mode) == -1) { perror("Unable to set SPI mode"); - return 1; + return false; } // set bits per word if (ioctl(fd, SPI_IOC_WR_BITS_PER_WORD, &spi_bits_per_word) == -1) { perror("Unable to set bits per word"); - return 1; + return false; } // set max speed if (ioctl(fd, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed) == -1) { perror("Unable to set max speed"); - return 1; + return false; } // set bit order if (ioctl(fd, SPI_IOC_WR_LSB_FIRST, &spi_lsb_first) == -1) { perror("Unable to set bitorder to msb first"); - return 1; + return false; } _HAL.open = spihal_open; @@ -95,6 +126,8 @@ bool BNO08X::makeSPI(int32_t sensor_id = 0) { _HAL.getTimeUs = hal_getTimeUs; spi_start = std::chrono::high_resolution_clock::now(); + + return true; } bool BNO08X::_init(int32_t sensor_id) { @@ -131,7 +164,7 @@ bool BNO08X::wasReset(void) { return x; } -bool BNO08X::enableReport(sh2_SensorId_t sensor_id, uint32_t interval_us = 10000) { +bool BNO08X::enableReport(sh2_SensorId_t sensor_id, uint32_t interval_us) { static sh2_SensorConfig_t config; // sane defaults @@ -190,8 +223,7 @@ static bool spihal_wait_for_int(void) { // this is actually really stupid // does chip interrupt within 0.5 seconds? for (int i = 0; i < 500; i++) { - // TODO: read interrupt pin from gpio - if (true) { + if (gpioRead(_interrupt_pin)) { return true; } std::this_thread::sleep_for(std::chrono::milliseconds(1)); @@ -298,11 +330,10 @@ void sensorHandler(void *cookie, sh2_SensorEvent_t *pEvent) { } void hal_hardwareReset(void) { - // TODO: set gpio reset to output - // TODO: set reset pin high + gpioWrite(_reset_pin, 1); std::this_thread::sleep_for(std::chrono::milliseconds(10)); - // TODO: set reset pin low + gpioWrite(_reset_pin, 0); std::this_thread::sleep_for(std::chrono::milliseconds(10)); - // TODO: set reset pin high + gpioWrite(_reset_pin, 1); std::this_thread::sleep_for(std::chrono::milliseconds(10)); } \ No newline at end of file From 2f7c6a39a32b9bf8e9e3e051a24d58dd3f5b6fc5 Mon Sep 17 00:00:00 2001 From: Zac Stanton Date: Fri, 28 Mar 2025 21:59:43 -0600 Subject: [PATCH 3/3] spi now opens by default, add more debug output. NEEDS TO BE RUN AS SUDO CURRENTLY, THANKS /dev/mem --- src/rov/rov_bno085/src/bno08x_driver.cpp | 18 +++++++++++++----- 1 file changed, 13 insertions(+), 5 deletions(-) diff --git a/src/rov/rov_bno085/src/bno08x_driver.cpp b/src/rov/rov_bno085/src/bno08x_driver.cpp index 0b84090..aea7a3d 100644 --- a/src/rov/rov_bno085/src/bno08x_driver.cpp +++ b/src/rov/rov_bno085/src/bno08x_driver.cpp @@ -74,6 +74,14 @@ BNO08X::BNO08X(uint8_t reset_pin, uint8_t interrupt_pin) { this->interrupt_pin = interrupt_pin; _interrupt_pin = this->interrupt_pin; ret = gpioSetMode(interrupt_pin, JET_INPUT); + + // spi by default + if (!makeSPI()) { + printf("Could not initialize BNO085\n"); + exit(1); + } + + printf("Successfully initialized!\n"); } BNO08X::~BNO08X() { @@ -90,32 +98,32 @@ bool BNO08X::makeSPI(int32_t sensor_id) { // open the spi device fd = open(SPIDEVICE, O_RDWR); if (fd < 0) { - perror("Unable to open SPI device"); + printf("Unable to open SPI device"); return false; } _fd = fd; // set spi mode if (ioctl(fd, SPI_IOC_WR_MODE, &spi_mode) == -1) { - perror("Unable to set SPI mode"); + printf("Unable to set SPI mode"); return false; } // set bits per word if (ioctl(fd, SPI_IOC_WR_BITS_PER_WORD, &spi_bits_per_word) == -1) { - perror("Unable to set bits per word"); + printf("Unable to set bits per word"); return false; } // set max speed if (ioctl(fd, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed) == -1) { - perror("Unable to set max speed"); + printf("Unable to set max speed"); return false; } // set bit order if (ioctl(fd, SPI_IOC_WR_LSB_FIRST, &spi_lsb_first) == -1) { - perror("Unable to set bitorder to msb first"); + printf("Unable to set bitorder to msb first"); return false; }