Bno085 sh2

.gitmodules

@@ -0,0 +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

src/rov/rov_bno085/CMakeLists.txt

@@ -0,0 +1,58 @@
+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(<dependency> 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_SensorValue.c
+sh2/sh2_util.c
+sh2/sh2.c
+sh2/shtp.c
+jetgpio/orin.c
+)
+target_include_directories(bno085 PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/sh2>
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/jetgpio>
+  $<INSTALL_INTERFACE:include>)
+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})
+
+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()

src/rov/rov_bno085/include/rov_bno085/bno08x_driver.hpp

@@ -0,0 +1,59 @@
+#ifndef BNO08X_DRIVER_HPP
+#define BNO08X_DRIVER_HPP
+
+#include <cstdint>
+
+extern "C" {
+#include "sh2_SensorValue.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
+
+// 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 = 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
+    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;
+
+    uint8_t reset_pin;
+    uint8_t interrupt_pin;
+
+protected:
+    // called post i2c/spi init
+    virtual bool _init(int32_t sensor_id);
+
+    sh2_Hal_t _HAL;
+};
+
+#endif

src/rov/rov_bno085/include/rov_bno085/bno08x_node.hpp

@@ -0,0 +1,41 @@
+#ifndef BNO08X_NODE_HPP
+#define BNO08X_NODE_HPP
+
+#include <cstdint>
+
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/imu.hpp>
+#include <sensor_msgs/msg/magnetic_field.hpp>
+#include <sensor_msgs/msg/temperature.hpp>
+#include <std_srvs/srv/empty.hpp>
+// 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<sensor_msgs::msg::Imu>::SharedPtr pub_accel_gyro;
+    rclcpp::Publisher<sensor_msgs::msg::MagneticField>::SharedPtr pub_mag;
+    rclcpp::Publisher<sensor_msgs::msg::Temperature>::SharedPtr pub_temp; 
+
+    rclcpp::TimerBase::SharedPtr timer_poll_bno;
+
+    BNO08X bno08x;
+
+    int spi_fd;
+};
+
+#endif

src/rov/rov_bno085/package.xml

@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>rov_bno085</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="zac71113@gmail.com">yameat</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>