Merge leads_vec_power/Pedal and leads_vec_wsc/Accelerometer into LEADS-Arduino

arduino/leads_vec_wsc/Adafruit_BNO08x_RVC.cpp

@@ -0,0 +1,129 @@
+/*!
+*  @file Adafruit_BNO08x_RVC.cpp
+*
+*  @mainpage Adafruit BNO08x RVC A simple library to use the UART-RVC mode of
+* the BNO08x sensors from Hillcrest Laboratories
+*
+*  @section intro_sec Introduction
+*
+* 	I2C Driver for the Library for the BNO08x_RVC A simple library to use
+* the UART-RVC mode of the BNO08x sensors from Hillcrest Laboratories
+*
+* 	This is a library for the Adafruit BNO08x_RVC breakout:
+* 	https://www.adafruit.com/product/4754
+*
+* 	Adafruit invests time and resources providing this open source code,
+*  please support Adafruit and open-source hardware by purchasing products from
+* 	Adafruit!
+*
+*  @section dependencies Dependencies
+*  This library depends on the Adafruit BusIO library
+*
+*  This library depends on the Adafruit Unified Sensor library
+*
+*  @section author Author
+*
+*  Bryan Siepert for Adafruit Industries
+*
+* 	@section license License
+*
+* 	BSD (see license.txt)
+*
+* 	@section  HISTORY
+*
+*     v1.0 - First release
+*/
+
+#include "Arduino.h"
+#include <Wire.h>
+
+#include "Adafruit_BNO08x_RVC.h"
+
+/**
+* @brief Construct a new Adafruit_BNO08x_RVC::Adafruit_BNO08x_RVC object
+*
+*/
+Adafruit_BNO08x_RVC::Adafruit_BNO08x_RVC(void) {}
+
+/**
+* @brief Destroy the Adafruit_BNO08x_RVC::Adafruit_BNO08x_RVC object
+*
+*/
+Adafruit_BNO08x_RVC::~Adafruit_BNO08x_RVC(void) {}
+
+/*!
+*  @brief  Setups the hardware
+*  @param  theSerial
+*          Pointer to Stream (HardwareSerial/SoftwareSerial) interface
+*  @return True
+*/
+bool Adafruit_BNO08x_RVC::begin(Stream *theSerial) {
+   serial_dev = theSerial;
+   return true;
+}
+
+/**
+* @brief Get the next available heading and acceleration data from the sensor
+*
+* @param heading pointer to a BNO08x_RVC_Data struct to hold the measurements
+* @return true: success false: failure
+*/
+bool Adafruit_BNO08x_RVC::read(BNO08x_RVC_Data *heading) {
+   if (!heading) {
+       return false;
+   }
+
+   if (!serial_dev->available()) {
+       return false;
+   }
+   if (serial_dev->peek() != 0xAA) {
+       serial_dev->read();
+       return false;
+   }
+   // Now read all 19 bytes
+
+   if (serial_dev->available() < 19) {
+       return false;
+   }
+   // at this point we know there's at least 19 bytes available and the first
+   if (serial_dev->read() != 0xAA) {
+       // shouldn't happen baecause peek said it was 0xAA
+       return false;
+   }
+   // make sure the next byte is the second 0xAA
+   if (serial_dev->read() != 0xAA) {
+       return false;
+   }
+   uint8_t buffer[19];
+   // ok, we've got our header, read the actual data+crc
+   if (!serial_dev->readBytes(buffer, 17)) {
+       return false;
+   };
+
+   uint8_t sum = 0;
+   // get checksum ready
+   for (uint8_t i = 0; i < 16; i++) {
+       sum += buffer[i];
+   }
+   if (sum != buffer[16]) {
+       return false;
+   }
+
+   // The data comes in endian'd, this solves it so it works on all platforms
+   int16_t buffer_16[6];
+
+   for (uint8_t i = 0; i < 6; i++) {
+
+       buffer_16[i] = (buffer[1 + (i * 2)]);
+       buffer_16[i] += (buffer[1 + (i * 2) + 1] << 8);
+   }
+   heading->yaw = (float)buffer_16[0] * DEGREE_SCALE;
+   heading->pitch = (float)buffer_16[1] * DEGREE_SCALE;
+   heading->roll = (float)buffer_16[2] * DEGREE_SCALE;
+
+   heading->x_accel = (float)buffer_16[3] * MILLI_G_TO_MS2;
+   heading->y_accel = (float)buffer_16[4] * MILLI_G_TO_MS2;
+   heading->z_accel = (float)buffer_16[5] * MILLI_G_TO_MS2;
+
+   return true;
+}

arduino/leads_vec_wsc/Adafruit_BNO08x_RVC.h

@@ -0,0 +1,59 @@
+/*!
+*  @file Adafruit_BNO08x_RVC.h
+*
+* 	I2C Driver for the Adafruit BNO08x RVC A simple library to use the
+*UART-RVC mode of the BNO08x sensors from Hillcrest Laboratories
+*
+* 	This is a library for use with thethe Adafruit BNO08x breakout:
+* 	https://www.adafruit.com/products/4754
+*
+* 	Adafruit invests time and resources providing this open source code,
+*  please support Adafruit and open-source hardware by purchasing products from
+* 	Adafruit!
+*
+*
+*	BSD license (see license.txt)
+*/
+
+#ifndef _ADAFRUIT_BNO08x_RVC_H
+#define _ADAFRUIT_BNO08x_RVC_H
+
+#define MILLI_G_TO_MS2 0.0098067 ///< Scalar to convert milli-gs to m/s^2
+#define DEGREE_SCALE 0.01        ///< To convert the degree values
+
+#include "Arduino.h"
+
+/*!
+*    @brief  Class that stores state and functions for interacting with
+*            the BNO08x_RVC A simple library to use the UART-RVC mode of the
+* BNO08x sensors from Hillcrest Laboratories
+*/
+/**! Struct to hold a UART-RVC packet **/
+typedef struct BNO08xRVCData {
+   float yaw,     ///< Yaw in Degrees
+           pitch,     ///< Pitch in Degrees
+           roll;      ///< Roll in Degrees
+   float x_accel, ///< The X acceleration value in m/s^2
+           y_accel,   ///< The Y acceleration value in m/s^2
+           z_accel;   ///< The Z acceleration value in m/s^2
+
+} BNO08x_RVC_Data;
+
+/**
+* @brief A class to interact with the BNO08x sensors from Hillcrest
+* Laboritories using the UART-RVC mode
+*
+*/
+class Adafruit_BNO08x_RVC {
+public:
+   Adafruit_BNO08x_RVC();
+   ~Adafruit_BNO08x_RVC();
+
+   bool begin(Stream *theStream);
+   bool read(BNO08x_RVC_Data *heading);
+
+private:
+   Stream *serial_dev;
+};
+
+#endif

arduino/leads_vec_wsc/Accelerometer.cpp → arduino/leads_vec_wsc/BNO08x.cpp

@@ -1,16 +1,13 @@
-#include "Accelerometer.h"
+#include "BNO08x.h"
 
-String Acceleration::toString() {
-    return String(yaw) + "," + pitch + "," + roll + "," + forwardAcceleration + "," + lateralAcceleration + "," + verticalAcceleration;
-}
-Accelerometer::Accelerometer(OnAccelerometerUpdate onUpdate) : _onUpdate(onUpdate) {}
-void Accelerometer::initialize(const ArrayList<String> &parentTags) {
-    Device<Acceleration>::initialize(parentTags);
+BNO08x::BNO08x(OnAccelerometerUpdate onUpdate) : Accelerometer(onUpdate) {}
+void BNO08x::initialize(const ArrayList<String> &parentTags) {
+    Accelerometer::initialize(parentTags);
     Serial1.begin(115200);
     while (!Serial1) delay(10);
     if (!_rvc.begin(&Serial1)) delay(10);
 }
-Acceleration Accelerometer::read() {
+Acceleration BNO08x::read() {
     BNO08x_RVC_Data heading;
     Acceleration r = Acceleration();
     if (!_rvc.read(&heading)) return r;

arduino/leads_vec_wsc/BNO08x.h

@@ -0,0 +1,18 @@
+#ifndef BNO08X_H
+#define BNO08X_H
+
+
+#include "Adafruit_BNO08x_RVC.h"
+#include "LEADS.h"
+
+class BNO08x : public Accelerometer {
+protected:
+    Adafruit_BNO08x_RVC _rvc = Adafruit_BNO08x_RVC();
+public:
+    explicit BNO08x(OnAccelerometerUpdate onUpdate);
+    void initialize(const ArrayList<String> &parentTags) override;
+    Acceleration read() override;
+};
+
+
+#endif // BNO08X_H

arduino/leads_vec_wsc/leads_vec_wsc.ino

@@ -1,4 +1,4 @@
-#include "Accelerometer.h"
+#include "BNO08x.h"
 
 const int PIN_LFWSS[] = {2};
 const int PIN_RFWSS[] = {3};
@@ -12,7 +12,7 @@ WheelSpeedSensor RFWSS{ArrayList<int>(PIN_RFWSS, 1), [](float ws) {returnFloat(P
 WheelSpeedSensor LRWSS{ArrayList<int>(PIN_LRWSS, 1), [](float ws) {returnFloat(P, LEFT_REAR_WHEEL_SPEED_SENSOR, ws);}};
 WheelSpeedSensor RRWSS{ArrayList<int>(PIN_RRWSS, 1), [](float ws) {returnFloat(P, RIGHT_REAR_WHEEL_SPEED_SENSOR, ws);}};
 WheelSpeedSensor CRWSS{ArrayList<int>(PIN_CRWSS, 1), [](float ws) {returnFloat(P, CENTER_REAR_WHEEL_SPEED_SENSOR, ws);}};
-Accelerometer ACCL{[](Acceleration acceleration) {returnString(P, ACCELEROMETER, acceleration.toString());}};
+BNO08x ACCL{[](Acceleration acceleration) {returnString(P, ACCELEROMETER, acceleration.toString());}};
 
 void setup() {
     P.initializeAsRoot();