├── examples ├── more_reports │ ├── .uno.test.skip │ ├── .leonardo.test.skip │ └── more_reports.ino ├── rotation_vector │ ├── .uno.test.skip │ ├── .leonardo.test.skip │ └── rotation_vector.ino └── quaternion_yaw_pitch_roll │ ├── .uno.test.skip │ ├── .leonardo.test.skip │ └── quaternion_yaw_pitch_roll.ino ├── library.properties ├── src ├── NOTICE.txt ├── README.md ├── sh2_err.h ├── sh2_util.h ├── Adafruit_BNO08x.h ├── sh2_util.c ├── shtp.h ├── sh2_hal.h ├── sh2_SensorValue.h ├── Adafruit_BNO08x.cpp ├── sh2_SensorValue.c ├── sh2.h └── shtp.c ├── .github ├── workflows │ └── githubci.yml ├── PULL_REQUEST_TEMPLATE.md └── ISSUE_TEMPLATE.md ├── license.txt ├── README.md └── code-of-conduct.md /examples/more_reports/.uno.test.skip: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /examples/more_reports/.leonardo.test.skip: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /examples/rotation_vector/.uno.test.skip: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /examples/rotation_vector/.leonardo.test.skip: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /examples/quaternion_yaw_pitch_roll/.uno.test.skip: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /examples/quaternion_yaw_pitch_roll/.leonardo.test.skip: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /library.properties: -------------------------------------------------------------------------------- 1 | name=Adafruit BNO08x 2 | version=1.2.5 3 | author=Adafruit 4 | maintainer=Adafruit 5 | sentence=Arduino library for the BNO08x sensors in the Adafruit shop 6 | paragraph=Arduino library for the BNO08x sensors in the Adafruit shop 7 | category=Sensors 8 | url=https://github.com/adafruit/Adafruit_BNO08x 9 | architectures=* 10 | depends=Adafruit Unified Sensor, Adafruit BusIO 11 | -------------------------------------------------------------------------------- /src/NOTICE.txt: -------------------------------------------------------------------------------- 1 | This software is licensed from Hillcrest Laboratories, Inc. 2 | Copyright (c) Hillcrest Laboratories, Inc. and its licensors. 3 | All rights reserved. 4 | 5 | Hillcrest Laboratories and the Hillcrest logo are trademarks of 6 | Hillcrest Laboratories, Inc. 7 | 8 | You can contact Hillcrest Laboratories on the web at 9 | http://hillcrestlabs.com/company/contact-us/ or at Hillcrest Laboratories, 10 | 15245 Shady Grove Road, Suite 400, Rockville, MD 20850 USA 11 | 12 | -------------------------------------------------------------------------------- /src/README.md: -------------------------------------------------------------------------------- 1 | # SH2 Sensorhub driver for MCU application (No RTOS version) 2 | 3 | The files in this repository provide application-level SH2 sensor hub functionality. 4 | 5 | To use this code, an application developer will need to: 6 | * Incorporate this code into a project. 7 | * Provide platform-level functions, as specified in sh2_hal.h 8 | * Develop application logic to call the functions in sh2.h 9 | 10 | More complete instruction can be found in the User's Guide: 11 | * [SH2 Library User's Guide](UserGuide.pdf) 12 | 13 | An example project based on this driver can be found here: 14 | * [sh2-demo-nucleo](https://github.com/hcrest/sh2-demo-nucleo) 15 | -------------------------------------------------------------------------------- /.github/workflows/githubci.yml: -------------------------------------------------------------------------------- 1 | name: Arduino Library CI 2 | 3 | on: [pull_request, push, repository_dispatch] 4 | 5 | jobs: 6 | build: 7 | runs-on: ubuntu-latest 8 | 9 | steps: 10 | - uses: actions/setup-python@v4 11 | with: 12 | python-version: '3.x' 13 | - uses: actions/checkout@v3 14 | - uses: actions/checkout@v3 15 | with: 16 | repository: adafruit/ci-arduino 17 | path: ci 18 | 19 | - name: pre-install 20 | run: bash ci/actions_install.sh 21 | 22 | - name: test platforms 23 | run: python3 ci/build_platform.py main_platforms 24 | 25 | - name: clang 26 | run: python3 ci/run-clang-format.py -e "ci/*" -e "bin/*" src/Adafruit_* 27 | 28 | - name: doxygen 29 | env: 30 | GH_REPO_TOKEN: ${{ secrets.GH_REPO_TOKEN }} 31 | PRETTYNAME : "Adafruit BNO08x Library" 32 | run: bash ci/doxy_gen_and_deploy.sh 33 | -------------------------------------------------------------------------------- /.github/PULL_REQUEST_TEMPLATE.md: -------------------------------------------------------------------------------- 1 | Thank you for creating a pull request to contribute to Adafruit's GitHub code! 2 | Before you open the request please review the following guidelines and tips to 3 | help it be more easily integrated: 4 | 5 | - **Describe the scope of your change--i.e. what the change does and what parts 6 | of the code were modified.** This will help us understand any risks of integrating 7 | the code. 8 | 9 | - **Describe any known limitations with your change.** For example if the change 10 | doesn't apply to a supported platform of the library please mention it. 11 | 12 | - **Please run any tests or examples that can exercise your modified code.** We 13 | strive to not break users of the code and running tests/examples helps with this 14 | process. 15 | 16 | Thank you again for contributing! We will try to test and integrate the change 17 | as soon as we can, but be aware we have many GitHub repositories to manage and 18 | can't immediately respond to every request. There is no need to bump or check in 19 | on a pull request (it will clutter the discussion of the request). 20 | 21 | Also don't be worried if the request is closed or not integrated--sometimes the 22 | priorities of Adafruit's GitHub code (education, ease of use) might not match the 23 | priorities of the pull request. Don't fret, the open source community thrives on 24 | forks and GitHub makes it easy to keep your changes in a forked repo. 25 | 26 | After reviewing the guidelines above you can delete this text from the pull request. 27 | -------------------------------------------------------------------------------- /src/sh2_err.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2015-16 Hillcrest Laboratories, Inc. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License and 6 | * any applicable agreements you may have with Hillcrest Laboratories, Inc. 7 | * You may obtain a copy of the License at 8 | * 9 | * http://www.apache.org/licenses/LICENSE-2.0 10 | * 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | */ 17 | 18 | /** 19 | * @file sh2_err.h 20 | * @author David Wheeler 21 | * @date 22 May 2015 22 | * @brief Type definitions for Hillcrest SH-2 API. 23 | * 24 | * Struct and type definitions supporting the Hillcrest SH-2 SensorHub API. 25 | * 26 | */ 27 | 28 | 29 | #ifndef SH2_ERR_H 30 | #define SH2_ERR_H 31 | 32 | 33 | #define SH2_OK (0) /**< Success */ 34 | #define SH2_ERR (-1) /**< General Error */ 35 | #define SH2_ERR_BAD_PARAM (-2) /**< Bad parameter to an API call */ 36 | #define SH2_ERR_OP_IN_PROGRESS (-3) /**< Operation in progress */ 37 | #define SH2_ERR_IO (-4) /**< Error communicating with hub */ 38 | #define SH2_ERR_HUB (-5) /**< Error reported by hub */ 39 | #define SH2_ERR_TIMEOUT (-6) /**< Operation timed out */ 40 | 41 | 42 | #endif 43 | -------------------------------------------------------------------------------- /license.txt: -------------------------------------------------------------------------------- 1 | Software License Agreement (BSD License) 2 | 3 | Copyright (c) 2019 Bryan Siepert for Adafruit Industries 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 1. Redistributions of source code must retain the above copyright 9 | notice, this list of conditions and the following disclaimer. 10 | 2. Redistributions in binary form must reproduce the above copyright 11 | notice, this list of conditions and the following disclaimer in the 12 | documentation and/or other materials provided with the distribution. 13 | 3. Neither the name of the copyright holders nor the 14 | names of its contributors may be used to endorse or promote products 15 | derived from this software without specific prior written permission. 16 | 17 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY 18 | EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY 21 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | -------------------------------------------------------------------------------- /src/sh2_util.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2015-16 Hillcrest Laboratories, Inc. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License and 6 | * any applicable agreements you may have with Hillcrest Laboratories, Inc. 7 | * You may obtain a copy of the License at 8 | * 9 | * http://www.apache.org/licenses/LICENSE-2.0 10 | * 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | */ 17 | 18 | /* 19 | * Simple Utility functions common to several SH2 files. 20 | */ 21 | 22 | #ifndef SH2_UTIL_H 23 | #define SH2_UTIL_H 24 | 25 | #include 26 | 27 | #ifndef ARRAY_LEN 28 | #define ARRAY_LEN(a) (sizeof(a)/sizeof(a[0])) 29 | #endif 30 | 31 | uint8_t readu8(const uint8_t * buffer); 32 | void writeu8(uint8_t * buffer, uint8_t value); 33 | uint16_t readu16(const uint8_t * buffer); 34 | void writeu16(uint8_t * buffer, uint16_t value); 35 | uint32_t readu32(const uint8_t * buffer); 36 | void writeu32(uint8_t * buffer, uint32_t value); 37 | 38 | int8_t read8(const uint8_t * buffer); 39 | void write8(uint8_t * buffer, int8_t value); 40 | int16_t read16(const uint8_t * buffer); 41 | void write16(uint8_t * buffer, int16_t value); 42 | int32_t read32(const uint8_t * buffer); 43 | void write32(uint8_t * buffer, int32_t value); 44 | 45 | #endif 46 | -------------------------------------------------------------------------------- /src/Adafruit_BNO08x.h: -------------------------------------------------------------------------------- 1 | /*! 2 | * @file Adafruit_BNO08x.h 3 | * 4 | * I2C Driver for the Adafruit BNO08x 9-DOF Orientation IMU Fusion Breakout 5 | * 6 | * This is a library for the Adafruit BNO08x breakout: 7 | * https://www.adafruit.com/products/4754 8 | * 9 | * Adafruit invests time and resources providing this open source code, 10 | * please support Adafruit and open-source hardware by purchasing products from 11 | * Adafruit! 12 | * 13 | * 14 | * BSD license (see license.txt) 15 | */ 16 | 17 | #ifndef _ADAFRUIT_BNO08x_H 18 | #define _ADAFRUIT_BNO08x_H 19 | 20 | #include "Arduino.h" 21 | #include "sh2.h" 22 | #include "sh2_SensorValue.h" 23 | #include "sh2_err.h" 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | #define BNO08x_I2CADDR_DEFAULT 0x4A ///< The default I2C address 30 | 31 | /* Additional Activities not listed in SH-2 lib */ 32 | #define PAC_ON_STAIRS 8 ///< Activity code for being on stairs 33 | #define PAC_OPTION_COUNT \ 34 | 9 ///< The number of current options for the activity classifier 35 | 36 | /*! 37 | * @brief Class that stores state and functions for interacting with 38 | * the BNO08x 9-DOF Orientation IMU Fusion Breakout 39 | */ 40 | class Adafruit_BNO08x { 41 | public: 42 | Adafruit_BNO08x(int8_t reset_pin = -1); 43 | ~Adafruit_BNO08x(); 44 | 45 | bool begin_I2C(uint8_t i2c_addr = BNO08x_I2CADDR_DEFAULT, 46 | TwoWire *wire = &Wire, int32_t sensor_id = 0); 47 | bool begin_UART(HardwareSerial *serial, int32_t sensor_id = 0); 48 | 49 | bool begin_SPI(uint8_t cs_pin, uint8_t int_pin, SPIClass *theSPI = &SPI, 50 | int32_t sensor_id = 0); 51 | 52 | void hardwareReset(void); 53 | bool wasReset(void); 54 | 55 | bool enableReport(sh2_SensorId_t sensor, uint32_t interval_us = 10000); 56 | bool getSensorEvent(sh2_SensorValue_t *value); 57 | 58 | sh2_ProductIds_t prodIds; ///< The product IDs returned by the sensor 59 | 60 | protected: 61 | virtual bool _init(int32_t sensor_id); 62 | 63 | sh2_Hal_t 64 | _HAL; ///< The struct representing the SH2 Hardware Abstraction Layer 65 | }; 66 | 67 | #endif 68 | -------------------------------------------------------------------------------- /examples/rotation_vector/rotation_vector.ino: -------------------------------------------------------------------------------- 1 | // Basic demo for readings from Adafruit BNO08x 2 | #include 3 | 4 | // For SPI mode, we need a CS pin 5 | #define BNO08X_CS 10 6 | #define BNO08X_INT 9 7 | 8 | // For SPI mode, we also need a RESET 9 | //#define BNO08X_RESET 5 10 | // but not for I2C or UART 11 | #define BNO08X_RESET -1 12 | 13 | Adafruit_BNO08x bno08x(BNO08X_RESET); 14 | sh2_SensorValue_t sensorValue; 15 | 16 | void setup(void) { 17 | Serial.begin(115200); 18 | while (!Serial) delay(10); // will pause Zero, Leonardo, etc until serial console opens 19 | 20 | Serial.println("Adafruit BNO08x test!"); 21 | 22 | // Try to initialize! 23 | if (!bno08x.begin_I2C()) { 24 | //if (!bno08x.begin_UART(&Serial1)) { // Requires a device with > 300 byte UART buffer! 25 | //if (!bno08x.begin_SPI(BNO08X_CS, BNO08X_INT)) { 26 | Serial.println("Failed to find BNO08x chip"); 27 | while (1) { delay(10); } 28 | } 29 | Serial.println("BNO08x Found!"); 30 | 31 | for (int n = 0; n < bno08x.prodIds.numEntries; n++) { 32 | Serial.print("Part "); 33 | Serial.print(bno08x.prodIds.entry[n].swPartNumber); 34 | Serial.print(": Version :"); 35 | Serial.print(bno08x.prodIds.entry[n].swVersionMajor); 36 | Serial.print("."); 37 | Serial.print(bno08x.prodIds.entry[n].swVersionMinor); 38 | Serial.print("."); 39 | Serial.print(bno08x.prodIds.entry[n].swVersionPatch); 40 | Serial.print(" Build "); 41 | Serial.println(bno08x.prodIds.entry[n].swBuildNumber); 42 | } 43 | 44 | setReports(); 45 | 46 | Serial.println("Reading events"); 47 | delay(100); 48 | } 49 | 50 | // Here is where you define the sensor outputs you want to receive 51 | void setReports(void) { 52 | Serial.println("Setting desired reports"); 53 | if (! bno08x.enableReport(SH2_GAME_ROTATION_VECTOR)) { 54 | Serial.println("Could not enable game vector"); 55 | } 56 | } 57 | 58 | 59 | void loop() { 60 | delay(10); 61 | 62 | if (bno08x.wasReset()) { 63 | Serial.print("sensor was reset "); 64 | setReports(); 65 | } 66 | 67 | if (! bno08x.getSensorEvent(&sensorValue)) { 68 | return; 69 | } 70 | 71 | switch (sensorValue.sensorId) { 72 | 73 | case SH2_GAME_ROTATION_VECTOR: 74 | Serial.print("Game Rotation Vector - r: "); 75 | Serial.print(sensorValue.un.gameRotationVector.real); 76 | Serial.print(" i: "); 77 | Serial.print(sensorValue.un.gameRotationVector.i); 78 | Serial.print(" j: "); 79 | Serial.print(sensorValue.un.gameRotationVector.j); 80 | Serial.print(" k: "); 81 | Serial.println(sensorValue.un.gameRotationVector.k); 82 | break; 83 | } 84 | 85 | } -------------------------------------------------------------------------------- /.github/ISSUE_TEMPLATE.md: -------------------------------------------------------------------------------- 1 | Thank you for opening an issue on an Adafruit Arduino library repository. To 2 | improve the speed of resolution please review the following guidelines and 3 | common troubleshooting steps below before creating the issue: 4 | 5 | - **Do not use GitHub issues for troubleshooting projects and issues.** Instead use 6 | the forums at http://forums.adafruit.com to ask questions and troubleshoot why 7 | something isn't working as expected. In many cases the problem is a common issue 8 | that you will more quickly receive help from the forum community. GitHub issues 9 | are meant for known defects in the code. If you don't know if there is a defect 10 | in the code then start with troubleshooting on the forum first. 11 | 12 | - **If following a tutorial or guide be sure you didn't miss a step.** Carefully 13 | check all of the steps and commands to run have been followed. Consult the 14 | forum if you're unsure or have questions about steps in a guide/tutorial. 15 | 16 | - **For Arduino projects check these very common issues to ensure they don't apply**: 17 | 18 | - For uploading sketches or communicating with the board make sure you're using 19 | a **USB data cable** and **not** a **USB charge-only cable**. It is sometimes 20 | very hard to tell the difference between a data and charge cable! Try using the 21 | cable with other devices or swapping to another cable to confirm it is not 22 | the problem. 23 | 24 | - **Be sure you are supplying adequate power to the board.** Check the specs of 25 | your board and plug in an external power supply. In many cases just 26 | plugging a board into your computer is not enough to power it and other 27 | peripherals. 28 | 29 | - **Double check all soldering joints and connections.** Flakey connections 30 | cause many mysterious problems. See the [guide to excellent soldering](https://learn.adafruit.com/adafruit-guide-excellent-soldering/tools) for examples of good solder joints. 31 | 32 | - **Ensure you are using an official Arduino or Adafruit board.** We can't 33 | guarantee a clone board will have the same functionality and work as expected 34 | with this code and don't support them. 35 | 36 | If you're sure this issue is a defect in the code and checked the steps above 37 | please fill in the following fields to provide enough troubleshooting information. 38 | You may delete the guideline and text above to just leave the following details: 39 | 40 | - Arduino board: **INSERT ARDUINO BOARD NAME/TYPE HERE** 41 | 42 | - Arduino IDE version (found in Arduino -> About Arduino menu): **INSERT ARDUINO 43 | VERSION HERE** 44 | 45 | - List the steps to reproduce the problem below (if possible attach a sketch or 46 | copy the sketch code in too): **LIST REPRO STEPS BELOW** 47 | -------------------------------------------------------------------------------- /src/sh2_util.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2015-16 Hillcrest Laboratories, Inc. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License and 6 | * any applicable agreements you may have with Hillcrest Laboratories, Inc. 7 | * You may obtain a copy of the License at 8 | * 9 | * http://www.apache.org/licenses/LICENSE-2.0 10 | * 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | */ 17 | 18 | /* 19 | * Simple Utility functions common to several SH2 files. 20 | */ 21 | 22 | #include "sh2_util.h" 23 | 24 | uint8_t readu8(const uint8_t *p) 25 | { 26 | uint8_t retval = p[0]; 27 | return retval; 28 | } 29 | 30 | void writeu8(uint8_t * p, uint8_t value) 31 | { 32 | *p = (uint8_t)(value & 0xFF); 33 | } 34 | 35 | uint16_t readu16(const uint8_t *p) 36 | { 37 | uint16_t retval = p[0] | (p[1] << 8); 38 | return retval; 39 | } 40 | 41 | void writeu16(uint8_t * p, uint16_t value) 42 | { 43 | *p++ = (uint8_t)(value & 0xFF); 44 | value >>= 8; 45 | *p = (uint8_t)(value & 0xFF); 46 | } 47 | 48 | uint32_t readu32(const uint8_t *p) 49 | { 50 | uint32_t retval = p[0] | (p[1] << 8) | (p[2] << 16) | (p[3] << 24); 51 | return retval; 52 | } 53 | 54 | void writeu32(uint8_t * p, uint32_t value) 55 | { 56 | *p++ = (uint8_t)(value & 0xFF); 57 | value >>= 8; 58 | *p++ = (uint8_t)(value & 0xFF); 59 | value >>= 8; 60 | *p++ = (uint8_t)(value & 0xFF); 61 | value >>= 8; 62 | *p = (uint8_t)(value & 0xFF); 63 | } 64 | 65 | int8_t read8(const uint8_t *p) 66 | { 67 | int8_t retval = p[0]; 68 | return retval; 69 | } 70 | 71 | void write8(uint8_t * p, int8_t value) 72 | { 73 | *p = (uint8_t)(value & 0xFF); 74 | } 75 | 76 | int16_t read16(const uint8_t *p) 77 | { 78 | int16_t retval = p[0] | (p[1] << 8); 79 | return retval; 80 | } 81 | 82 | void write16(uint8_t * p, int16_t value) 83 | { 84 | *p++ = (uint8_t)(value & 0xFF); 85 | value >>= 8; 86 | *p = (uint8_t)(value & 0xFF); 87 | } 88 | 89 | int32_t read32(const uint8_t *p) 90 | { 91 | int32_t retval = p[0] | (p[1] << 8) | (p[2] << 16) | (p[3] << 24); 92 | return retval; 93 | } 94 | 95 | void write32(uint8_t * p, int32_t value) 96 | { 97 | *p++ = (uint8_t)(value & 0xFF); 98 | value >>= 8; 99 | *p++ = (uint8_t)(value & 0xFF); 100 | value >>= 8; 101 | *p++ = (uint8_t)(value & 0xFF); 102 | value >>= 8; 103 | *p = (uint8_t)(value & 0xFF); 104 | } 105 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Adafruit BNO08x [![Build Status](https://github.com/adafruit/Adafruit_BNO08x/workflows/Arduino%20Library%20CI/badge.svg)](https://github.com/adafruit/Adafruit_BNO08x/actions)[![Documentation](https://github.com/adafruit/ci-arduino/blob/master/assets/doxygen_badge.svg)](http://adafruit.github.io/Adafruit_BNO08x/html/index.html) 2 | 3 | 4 | This is the Adafruit BNO08x 9-DOF Orientation IMU Fusion Breakout for Arduino 5 | Tested and works great with the Adafruit BNO08x Breakout Board 6 | [](https://www.adafruit.com/products/4754) 7 | Adafruit invests time and resources providing this open source code, please support Adafruit and open-source hardware by purchasing products from Adafruit! 8 | 9 | # Installation 10 | To install, use the Arduino Library Manager and search for "Adafruit BNO08x" and install the library. 11 | 12 | ## Dependencies 13 | * [Adafruit BusIO](https://github.com/adafruit/Adafruit_BusIO) 14 | * [Adafruit Unified Sensor Driver](https://github.com/adafruit/Adafruit_Sensor) 15 | 16 | # Contributing 17 | 18 | Contributions are welcome! Please read our [Code of Conduct](https://github.com/adafruit/Adafruit_BNO08x/blob/master/CODE_OF_CONDUCT.md>) 19 | before contributing to help this project stay welcoming. 20 | 21 | ## Documentation and doxygen 22 | Documentation is produced by doxygen. Contributions should include documentation for any new code added. 23 | 24 | Some examples of how to use doxygen can be found in these guide pages: 25 | 26 | https://learn.adafruit.com/the-well-automated-arduino-library/doxygen 27 | 28 | https://learn.adafruit.com/the-well-automated-arduino-library/doxygen-tips 29 | 30 | ## Formatting and clang-format 31 | This library uses [`clang-format`](https://releases.llvm.org/download.html) to standardize the formatting of `.cpp` and `.h` files. 32 | Contributions should be formatted using `clang-format`: 33 | 34 | The `-i` flag will make the changes to the file. 35 | ```bash 36 | clang-format -i *.cpp *.h 37 | ``` 38 | If you prefer to make the changes yourself, running `clang-format` without the `-i` flag will print out a formatted version of the file. You can save this to a file and diff it against the original to see the changes. 39 | 40 | Note that the formatting output by `clang-format` is what the automated formatting checker will expect. Any diffs from this formatting will result in a failed build until they are addressed. Using the `-i` flag is highly recommended. 41 | 42 | ### clang-format resources 43 | * [Binary builds and source available on the LLVM downloads page](https://releases.llvm.org/download.html) 44 | * [Documentation and IDE integration](https://clang.llvm.org/docs/ClangFormat.html) 45 | 46 | ## About this Driver 47 | Written by Bryan Siepert for Adafruit Industries. 48 | MIT license, check license.txt for more information 49 | All text above must be included in any redistribution 50 | -------------------------------------------------------------------------------- /src/shtp.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2015-18 Hillcrest Laboratories, Inc. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License and 6 | * any applicable agreements you may have with Hillcrest Laboratories, Inc. 7 | * You may obtain a copy of the License at 8 | * 9 | * http://www.apache.org/licenses/LICENSE-2.0 10 | * 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | */ 17 | 18 | /* 19 | * Hillcrest Sensor Hub Transport Protocol (SHTP) API 20 | */ 21 | 22 | #ifndef SHTP_H 23 | #define SHTP_H 24 | 25 | #include 26 | #include 27 | 28 | #include "sh2_hal.h" 29 | 30 | // Advertisement TLV tags 31 | #define TAG_NULL 0 32 | #define TAG_GUID 1 33 | #define TAG_MAX_CARGO_PLUS_HEADER_WRITE 2 34 | #define TAG_MAX_CARGO_PLUS_HEADER_READ 3 35 | #define TAG_MAX_TRANSFER_WRITE 4 36 | #define TAG_MAX_TRANSFER_READ 5 37 | #define TAG_NORMAL_CHANNEL 6 38 | #define TAG_WAKE_CHANNEL 7 39 | #define TAG_APP_NAME 8 40 | #define TAG_CHANNEL_NAME 9 41 | #define TAG_ADV_COUNT 10 42 | #define TAG_APP_SPECIFIC 0x80 43 | 44 | typedef enum shtp_Event_e { 45 | SHTP_TX_DISCARD = 0, 46 | SHTP_SHORT_FRAGMENT = 1, 47 | SHTP_TOO_LARGE_PAYLOADS = 2, 48 | SHTP_BAD_RX_CHAN = 3, 49 | SHTP_BAD_TX_CHAN = 4, 50 | } shtp_Event_t; 51 | 52 | typedef void shtp_Callback_t(void * cookie, uint8_t *payload, uint16_t len, uint32_t timestamp); 53 | typedef void shtp_AdvertCallback_t(void * cookie, uint8_t tag, uint8_t len, uint8_t *value); 54 | typedef void shtp_SendCallback_t(void *cookie); 55 | typedef void shtp_EventCallback_t(void *cookie, shtp_Event_t shtpEvent); 56 | 57 | // Takes HAL pointer, returns shtp ID for use in future calls. 58 | // HAL will be opened by this call. 59 | void * shtp_open(sh2_Hal_t *pHal); 60 | 61 | // Releases resources associated with this SHTP instance. 62 | // HAL will not be closed. 63 | void shtp_close(void *pShtp); 64 | 65 | // Provide the point of the callback function for reporting SHTP asynchronous events 66 | void shtp_setEventCallback(void *pInstance, 67 | shtp_EventCallback_t * eventCallback, 68 | void *eventCookie); 69 | 70 | // Register a listener for an SHTP channel 71 | int shtp_listenChan(void *pShtp, 72 | uint16_t guid, const char * chan, 73 | shtp_Callback_t *callback, void * cookie); 74 | 75 | // Register a listener for SHTP advertisements 76 | int shtp_listenAdvert(void *pShtp, 77 | uint16_t guid, 78 | shtp_AdvertCallback_t *advertCallback, void * cookie); 79 | 80 | // Look up the channel number for a particular app, channel. 81 | uint8_t shtp_chanNo(void *pShtp, 82 | const char * appName, const char * chanName); 83 | 84 | // Send an SHTP payload on a particular channel 85 | int shtp_send(void *pShtp, 86 | uint8_t channel, const uint8_t *payload, uint16_t len); 87 | 88 | // Check for received data and process it. 89 | void shtp_service(void *pShtp); 90 | 91 | // #ifdef SHTP_H 92 | #endif 93 | -------------------------------------------------------------------------------- /examples/quaternion_yaw_pitch_roll/quaternion_yaw_pitch_roll.ino: -------------------------------------------------------------------------------- 1 | #include 2 | // This demo explores two reports (SH2_ARVR_STABILIZED_RV and SH2_GYRO_INTEGRATED_RV) both can be used to give 3 | // quartenion and euler (yaw, pitch roll) angles. Toggle the FAST_MODE define to see other report. 4 | // Note sensorValue.status gives calibration accuracy (which improves over time) 5 | #include 6 | 7 | // For SPI mode, we need a CS pin 8 | #define BNO08X_CS 10 9 | #define BNO08X_INT 9 10 | 11 | 12 | // #define FAST_MODE 13 | 14 | // For SPI mode, we also need a RESET 15 | //#define BNO08X_RESET 5 16 | // but not for I2C or UART 17 | #define BNO08X_RESET -1 18 | 19 | struct euler_t { 20 | float yaw; 21 | float pitch; 22 | float roll; 23 | } ypr; 24 | 25 | Adafruit_BNO08x bno08x(BNO08X_RESET); 26 | sh2_SensorValue_t sensorValue; 27 | 28 | #ifdef FAST_MODE 29 | // Top frequency is reported to be 1000Hz (but freq is somewhat variable) 30 | sh2_SensorId_t reportType = SH2_GYRO_INTEGRATED_RV; 31 | long reportIntervalUs = 2000; 32 | #else 33 | // Top frequency is about 250Hz but this report is more accurate 34 | sh2_SensorId_t reportType = SH2_ARVR_STABILIZED_RV; 35 | long reportIntervalUs = 5000; 36 | #endif 37 | void setReports(sh2_SensorId_t reportType, long report_interval) { 38 | Serial.println("Setting desired reports"); 39 | if (! bno08x.enableReport(reportType, report_interval)) { 40 | Serial.println("Could not enable stabilized remote vector"); 41 | } 42 | } 43 | 44 | void setup(void) { 45 | 46 | Serial.begin(115200); 47 | while (!Serial) delay(10); // will pause Zero, Leonardo, etc until serial console opens 48 | 49 | Serial.println("Adafruit BNO08x test!"); 50 | 51 | // Try to initialize! 52 | if (!bno08x.begin_I2C()) { 53 | //if (!bno08x.begin_UART(&Serial1)) { // Requires a device with > 300 byte UART buffer! 54 | //if (!bno08x.begin_SPI(BNO08X_CS, BNO08X_INT)) { 55 | Serial.println("Failed to find BNO08x chip"); 56 | while (1) { delay(10); } 57 | } 58 | Serial.println("BNO08x Found!"); 59 | 60 | 61 | setReports(reportType, reportIntervalUs); 62 | 63 | Serial.println("Reading events"); 64 | delay(100); 65 | } 66 | 67 | void quaternionToEuler(float qr, float qi, float qj, float qk, euler_t* ypr, bool degrees = false) { 68 | 69 | float sqr = sq(qr); 70 | float sqi = sq(qi); 71 | float sqj = sq(qj); 72 | float sqk = sq(qk); 73 | 74 | ypr->yaw = atan2(2.0 * (qi * qj + qk * qr), (sqi - sqj - sqk + sqr)); 75 | ypr->pitch = asin(-2.0 * (qi * qk - qj * qr) / (sqi + sqj + sqk + sqr)); 76 | ypr->roll = atan2(2.0 * (qj * qk + qi * qr), (-sqi - sqj + sqk + sqr)); 77 | 78 | if (degrees) { 79 | ypr->yaw *= RAD_TO_DEG; 80 | ypr->pitch *= RAD_TO_DEG; 81 | ypr->roll *= RAD_TO_DEG; 82 | } 83 | } 84 | 85 | void quaternionToEulerRV(sh2_RotationVectorWAcc_t* rotational_vector, euler_t* ypr, bool degrees = false) { 86 | quaternionToEuler(rotational_vector->real, rotational_vector->i, rotational_vector->j, rotational_vector->k, ypr, degrees); 87 | } 88 | 89 | void quaternionToEulerGI(sh2_GyroIntegratedRV_t* rotational_vector, euler_t* ypr, bool degrees = false) { 90 | quaternionToEuler(rotational_vector->real, rotational_vector->i, rotational_vector->j, rotational_vector->k, ypr, degrees); 91 | } 92 | 93 | void loop() { 94 | 95 | if (bno08x.wasReset()) { 96 | Serial.print("sensor was reset "); 97 | setReports(reportType, reportIntervalUs); 98 | } 99 | 100 | if (bno08x.getSensorEvent(&sensorValue)) { 101 | // in this demo only one report type will be received depending on FAST_MODE define (above) 102 | switch (sensorValue.sensorId) { 103 | case SH2_ARVR_STABILIZED_RV: 104 | quaternionToEulerRV(&sensorValue.un.arvrStabilizedRV, &ypr, true); 105 | case SH2_GYRO_INTEGRATED_RV: 106 | // faster (more noise?) 107 | quaternionToEulerGI(&sensorValue.un.gyroIntegratedRV, &ypr, true); 108 | break; 109 | } 110 | static long last = 0; 111 | long now = micros(); 112 | Serial.print(now - last); Serial.print("\t"); 113 | last = now; 114 | Serial.print(sensorValue.status); Serial.print("\t"); // This is accuracy in the range of 0 to 3 115 | Serial.print(ypr.yaw); Serial.print("\t"); 116 | Serial.print(ypr.pitch); Serial.print("\t"); 117 | Serial.println(ypr.roll); 118 | } 119 | 120 | } 121 | -------------------------------------------------------------------------------- /src/sh2_hal.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018 Hillcrest Laboratories, Inc. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License and 6 | * any applicable agreements you may have with Hillcrest Laboratories, Inc. 7 | * You may obtain a copy of the License at 8 | * 9 | * http://www.apache.org/licenses/LICENSE-2.0 10 | * 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | */ 17 | 18 | /* 19 | * SH2 HAL Interface for Non-RTOS Applications. 20 | */ 21 | 22 | // Include guard 23 | #ifndef SH2_HAL_H 24 | #define SH2_HAL_H 25 | 26 | #include 27 | 28 | // SH2 Implementations generally have a max out transfer len of 256 29 | #define SH2_HAL_MAX_TRANSFER_OUT (256) 30 | #define SH2_HAL_MAX_PAYLOAD_OUT (256) 31 | 32 | // Although some implementations adversize a max in transfer of 32K, 33 | // in practice, the largest transfer performed is the advertisements 34 | // which is 272 bytes at time of writing. 35 | #define SH2_HAL_MAX_TRANSFER_IN (384) 36 | #define SH2_HAL_MAX_PAYLOAD_IN (384) 37 | 38 | // This needs to be a power of 2, greater than max of the above. 39 | #define SH2_HAL_DMA_SIZE (512) 40 | 41 | typedef struct sh2_Hal_s sh2_Hal_t; 42 | 43 | // The SH2 interface uses these functions to access the underlying 44 | // communications device, so the system integrator will need to 45 | // implement these. At system intialization time, an sh2_Hal_t 46 | // structure should be initialized with pointers to all the hardware 47 | // access layer functions. A pointer to this structure must then be 48 | // passed to sh2_open() to initialize the SH2 interface. 49 | // 50 | // If the DFU (download firmware update) capability is needed, the 51 | // example DFU code also uses this interface but each function has 52 | // somewhat different requirements. So a separate instance of an 53 | // sh2_Hal_t structure, pointing to different functions, is 54 | // recommended for supporting DFU. 55 | 56 | struct sh2_Hal_s { 57 | // This function initializes communications with the device. It 58 | // can initialize any GPIO pins and peripheral devices used to 59 | // interface with the sensor hub. 60 | // It should also perform a reset cycle on the sensor hub to 61 | // ensure communications start from a known state. 62 | int (*open)(sh2_Hal_t *self); 63 | 64 | // This function completes communications with the sensor hub. 65 | // It should put the device in reset then de-initialize any 66 | // peripherals or hardware resources that were used. 67 | void (*close)(sh2_Hal_t *self); 68 | 69 | // This function supports reading data from the sensor hub. 70 | // It will be called frequently to sevice the device. 71 | // 72 | // If the HAL has received a full SHTP transfer, this function 73 | // should load the data into pBuffer, set the timestamp to the 74 | // time the interrupt was detected and return the non-zero length 75 | // of data in this transfer. 76 | // 77 | // If the HAL has not recevied a full SHTP transfer, this function 78 | // should return 0. 79 | // 80 | // Because this function is called regularly, it can be used to 81 | // perform other housekeeping operations. (In the case of UART 82 | // interfacing, bytes transmitted are staggered in time and this 83 | // function can be used to keep the transmission flowing.) 84 | int (*read)(sh2_Hal_t *self, uint8_t *pBuffer, unsigned len, uint32_t *t_us); 85 | 86 | // This function supports writing data to the sensor hub. 87 | // It is called each time the application has a block of data to 88 | // transfer to the device. 89 | // 90 | // If the device isn't ready to receive data this function can 91 | // return 0 without performing the transmit function. 92 | // 93 | // If the transmission can be started, this function needs to 94 | // copy the data from pBuffer and return the number of bytes 95 | // accepted. It need not block. The actual transmission of 96 | // the data can continue after this function returns. 97 | int (*write)(sh2_Hal_t *self, uint8_t *pBuffer, unsigned len); 98 | 99 | // This function should return a 32-bit value representing a 100 | // microsecond counter. The count may roll over after 2^32 101 | // microseconds. 102 | uint32_t (*getTimeUs)(sh2_Hal_t *self); 103 | }; 104 | 105 | // End of include guard 106 | #endif 107 | -------------------------------------------------------------------------------- /code-of-conduct.md: -------------------------------------------------------------------------------- 1 | # Adafruit Community Code of Conduct 2 | 3 | ## Our Pledge 4 | 5 | In the interest of fostering an open and welcoming environment, we as 6 | contributors and leaders pledge to making participation in our project and 7 | our community a harassment-free experience for everyone, regardless of age, body 8 | size, disability, ethnicity, gender identity and expression, level or type of 9 | experience, education, socio-economic status, nationality, personal appearance, 10 | race, religion, or sexual identity and orientation. 11 | 12 | ## Our Standards 13 | 14 | We are committed to providing a friendly, safe and welcoming environment for 15 | all. 16 | 17 | Examples of behavior that contributes to creating a positive environment 18 | include: 19 | 20 | * Be kind and courteous to others 21 | * Using welcoming and inclusive language 22 | * Being respectful of differing viewpoints and experiences 23 | * Collaborating with other community members 24 | * Gracefully accepting constructive criticism 25 | * Focusing on what is best for the community 26 | * Showing empathy towards other community members 27 | 28 | Examples of unacceptable behavior by participants include: 29 | 30 | * The use of sexualized language or imagery and sexual attention or advances 31 | * The use of inappropriate images, including in a community member's avatar 32 | * The use of inappropriate language, including in a community member's nickname 33 | * Any spamming, flaming, baiting or other attention-stealing behavior 34 | * Excessive or unwelcome helping; answering outside the scope of the question 35 | asked 36 | * Trolling, insulting/derogatory comments, and personal or political attacks 37 | * Public or private harassment 38 | * Publishing others' private information, such as a physical or electronic 39 | address, without explicit permission 40 | * Other conduct which could reasonably be considered inappropriate 41 | 42 | The goal of the standards and moderation guidelines outlined here is to build 43 | and maintain a respectful community. We ask that you don’t just aim to be 44 | "technically unimpeachable", but rather try to be your best self. 45 | 46 | We value many things beyond technical expertise, including collaboration and 47 | supporting others within our community. Providing a positive experience for 48 | other community members can have a much more significant impact than simply 49 | providing the correct answer. 50 | 51 | ## Our Responsibilities 52 | 53 | Project leaders are responsible for clarifying the standards of acceptable 54 | behavior and are expected to take appropriate and fair corrective action in 55 | response to any instances of unacceptable behavior. 56 | 57 | Project leaders have the right and responsibility to remove, edit, or 58 | reject messages, comments, commits, code, issues, and other contributions 59 | that are not aligned to this Code of Conduct, or to ban temporarily or 60 | permanently any community member for other behaviors that they deem 61 | inappropriate, threatening, offensive, or harmful. 62 | 63 | ## Moderation 64 | 65 | Instances of behaviors that violate the Adafruit Community Code of Conduct 66 | may be reported by any member of the community. Community members are 67 | encouraged to report these situations, including situations they witness 68 | involving other community members. 69 | 70 | You may report in the following ways: 71 | 72 | In any situation, you may send an email to . 73 | 74 | On the Adafruit Discord, you may send an open message from any channel 75 | to all Community Helpers by tagging @community helpers. You may also send an 76 | open message from any channel, or a direct message to @kattni#1507, 77 | @tannewt#4653, @Dan Halbert#1614, @cater#2442, @sommersoft#0222, or 78 | @Andon#8175. 79 | 80 | Email and direct message reports will be kept confidential. 81 | 82 | In situations on Discord where the issue is particularly egregious, possibly 83 | illegal, requires immediate action, or violates the Discord terms of service, 84 | you should also report the message directly to Discord. 85 | 86 | These are the steps for upholding our community’s standards of conduct. 87 | 88 | 1. Any member of the community may report any situation that violates the 89 | Adafruit Community Code of Conduct. All reports will be reviewed and 90 | investigated. 91 | 2. If the behavior is an egregious violation, the community member who 92 | committed the violation may be banned immediately, without warning. 93 | 3. Otherwise, moderators will first respond to such behavior with a warning. 94 | 4. Moderators follow a soft "three strikes" policy - the community member may 95 | be given another chance, if they are receptive to the warning and change their 96 | behavior. 97 | 5. If the community member is unreceptive or unreasonable when warned by a 98 | moderator, or the warning goes unheeded, they may be banned for a first or 99 | second offense. Repeated offenses will result in the community member being 100 | banned. 101 | 102 | ## Scope 103 | 104 | This Code of Conduct and the enforcement policies listed above apply to all 105 | Adafruit Community venues. This includes but is not limited to any community 106 | spaces (both public and private), the entire Adafruit Discord server, and 107 | Adafruit GitHub repositories. Examples of Adafruit Community spaces include 108 | but are not limited to meet-ups, audio chats on the Adafruit Discord, or 109 | interaction at a conference. 110 | 111 | This Code of Conduct applies both within project spaces and in public spaces 112 | when an individual is representing the project or its community. As a community 113 | member, you are representing our community, and are expected to behave 114 | accordingly. 115 | 116 | ## Attribution 117 | 118 | This Code of Conduct is adapted from the [Contributor Covenant][homepage], 119 | version 1.4, available at 120 | , 121 | and the [Rust Code of Conduct](https://www.rust-lang.org/en-US/conduct.html). 122 | 123 | For other projects adopting the Adafruit Community Code of 124 | Conduct, please contact the maintainers of those projects for enforcement. 125 | If you wish to use this code of conduct for your own project, consider 126 | explicitly mentioning your moderation policy or making a copy with your 127 | own moderation policy so as to avoid confusion. 128 | -------------------------------------------------------------------------------- /examples/more_reports/more_reports.ino: -------------------------------------------------------------------------------- 1 | // Basic demo for readings from Adafruit BNO08x 2 | #include 3 | 4 | // For SPI mode, we need a CS pin 5 | #define BNO08X_CS 10 6 | #define BNO08X_INT 9 7 | 8 | // For SPI mode, we also need a RESET 9 | //#define BNO08X_RESET 5 10 | // but not for I2C or UART 11 | #define BNO08X_RESET -1 12 | 13 | Adafruit_BNO08x bno08x(BNO08X_RESET); 14 | sh2_SensorValue_t sensorValue; 15 | 16 | void setup(void) { 17 | Serial.begin(115200); 18 | while (!Serial) 19 | delay(10); // will pause Zero, Leonardo, etc until serial console opens 20 | 21 | Serial.println("Adafruit BNO08x test!"); 22 | 23 | // Try to initialize! 24 | if (!bno08x.begin_I2C()) { 25 | // if (!bno08x.begin_UART(&Serial1)) { // Requires a device with > 300 byte 26 | // UART buffer! if (!bno08x.begin_SPI(BNO08X_CS, BNO08X_INT)) { 27 | Serial.println("Failed to find BNO08x chip"); 28 | while (1) { 29 | delay(10); 30 | } 31 | } 32 | Serial.println("BNO08x Found!"); 33 | 34 | for (int n = 0; n < bno08x.prodIds.numEntries; n++) { 35 | Serial.print("Part "); 36 | Serial.print(bno08x.prodIds.entry[n].swPartNumber); 37 | Serial.print(": Version :"); 38 | Serial.print(bno08x.prodIds.entry[n].swVersionMajor); 39 | Serial.print("."); 40 | Serial.print(bno08x.prodIds.entry[n].swVersionMinor); 41 | Serial.print("."); 42 | Serial.print(bno08x.prodIds.entry[n].swVersionPatch); 43 | Serial.print(" Build "); 44 | Serial.println(bno08x.prodIds.entry[n].swBuildNumber); 45 | } 46 | 47 | setReports(); 48 | 49 | Serial.println("Reading events"); 50 | delay(100); 51 | } 52 | 53 | // Here is where you define the sensor outputs you want to receive 54 | void setReports(void) { 55 | Serial.println("Setting desired reports"); 56 | if (!bno08x.enableReport(SH2_ACCELEROMETER)) { 57 | Serial.println("Could not enable accelerometer"); 58 | } 59 | if (!bno08x.enableReport(SH2_GYROSCOPE_CALIBRATED)) { 60 | Serial.println("Could not enable gyroscope"); 61 | } 62 | if (!bno08x.enableReport(SH2_MAGNETIC_FIELD_CALIBRATED)) { 63 | Serial.println("Could not enable magnetic field calibrated"); 64 | } 65 | if (!bno08x.enableReport(SH2_LINEAR_ACCELERATION)) { 66 | Serial.println("Could not enable linear acceleration"); 67 | } 68 | if (!bno08x.enableReport(SH2_GRAVITY)) { 69 | Serial.println("Could not enable gravity vector"); 70 | } 71 | if (!bno08x.enableReport(SH2_ROTATION_VECTOR)) { 72 | Serial.println("Could not enable rotation vector"); 73 | } 74 | if (!bno08x.enableReport(SH2_GEOMAGNETIC_ROTATION_VECTOR)) { 75 | Serial.println("Could not enable geomagnetic rotation vector"); 76 | } 77 | if (!bno08x.enableReport(SH2_GAME_ROTATION_VECTOR)) { 78 | Serial.println("Could not enable game rotation vector"); 79 | } 80 | if (!bno08x.enableReport(SH2_STEP_COUNTER)) { 81 | Serial.println("Could not enable step counter"); 82 | } 83 | if (!bno08x.enableReport(SH2_STABILITY_CLASSIFIER)) { 84 | Serial.println("Could not enable stability classifier"); 85 | } 86 | if (!bno08x.enableReport(SH2_RAW_ACCELEROMETER)) { 87 | Serial.println("Could not enable raw accelerometer"); 88 | } 89 | if (!bno08x.enableReport(SH2_RAW_GYROSCOPE)) { 90 | Serial.println("Could not enable raw gyroscope"); 91 | } 92 | if (!bno08x.enableReport(SH2_RAW_MAGNETOMETER)) { 93 | Serial.println("Could not enable raw magnetometer"); 94 | } 95 | if (!bno08x.enableReport(SH2_SHAKE_DETECTOR)) { 96 | Serial.println("Could not enable shake detector"); 97 | } 98 | if (!bno08x.enableReport(SH2_PERSONAL_ACTIVITY_CLASSIFIER)) { 99 | Serial.println("Could not enable personal activity classifier"); 100 | } 101 | } 102 | void printActivity(uint8_t activity_id) { 103 | switch (activity_id) { 104 | case PAC_UNKNOWN: 105 | Serial.print("Unknown"); 106 | break; 107 | case PAC_IN_VEHICLE: 108 | Serial.print("In Vehicle"); 109 | break; 110 | case PAC_ON_BICYCLE: 111 | Serial.print("On Bicycle"); 112 | break; 113 | case PAC_ON_FOOT: 114 | Serial.print("On Foot"); 115 | break; 116 | case PAC_STILL: 117 | Serial.print("Still"); 118 | break; 119 | case PAC_TILTING: 120 | Serial.print("Tilting"); 121 | break; 122 | case PAC_WALKING: 123 | Serial.print("Walking"); 124 | break; 125 | case PAC_RUNNING: 126 | Serial.print("Running"); 127 | break; 128 | case PAC_ON_STAIRS: 129 | Serial.print("On Stairs"); 130 | break; 131 | default: 132 | Serial.print("NOT LISTED"); 133 | } 134 | Serial.print(" ("); 135 | Serial.print(activity_id); 136 | Serial.print(")"); 137 | } 138 | void loop() { 139 | delay(10); 140 | 141 | if (bno08x.wasReset()) { 142 | Serial.print("sensor was reset "); 143 | setReports(); 144 | } 145 | 146 | if (!bno08x.getSensorEvent(&sensorValue)) { 147 | return; 148 | } 149 | 150 | switch (sensorValue.sensorId) { 151 | 152 | case SH2_ACCELEROMETER: 153 | Serial.print("Accelerometer - x: "); 154 | Serial.print(sensorValue.un.accelerometer.x); 155 | Serial.print(" y: "); 156 | Serial.print(sensorValue.un.accelerometer.y); 157 | Serial.print(" z: "); 158 | Serial.println(sensorValue.un.accelerometer.z); 159 | break; 160 | case SH2_GYROSCOPE_CALIBRATED: 161 | Serial.print("Gyro - x: "); 162 | Serial.print(sensorValue.un.gyroscope.x); 163 | Serial.print(" y: "); 164 | Serial.print(sensorValue.un.gyroscope.y); 165 | Serial.print(" z: "); 166 | Serial.println(sensorValue.un.gyroscope.z); 167 | break; 168 | case SH2_MAGNETIC_FIELD_CALIBRATED: 169 | Serial.print("Magnetic Field - x: "); 170 | Serial.print(sensorValue.un.magneticField.x); 171 | Serial.print(" y: "); 172 | Serial.print(sensorValue.un.magneticField.y); 173 | Serial.print(" z: "); 174 | Serial.println(sensorValue.un.magneticField.z); 175 | break; 176 | case SH2_LINEAR_ACCELERATION: 177 | Serial.print("Linear Acceration - x: "); 178 | Serial.print(sensorValue.un.linearAcceleration.x); 179 | Serial.print(" y: "); 180 | Serial.print(sensorValue.un.linearAcceleration.y); 181 | Serial.print(" z: "); 182 | Serial.println(sensorValue.un.linearAcceleration.z); 183 | break; 184 | case SH2_GRAVITY: 185 | Serial.print("Gravity - x: "); 186 | Serial.print(sensorValue.un.gravity.x); 187 | Serial.print(" y: "); 188 | Serial.print(sensorValue.un.gravity.y); 189 | Serial.print(" z: "); 190 | Serial.println(sensorValue.un.gravity.z); 191 | break; 192 | case SH2_ROTATION_VECTOR: 193 | Serial.print("Rotation Vector - r: "); 194 | Serial.print(sensorValue.un.rotationVector.real); 195 | Serial.print(" i: "); 196 | Serial.print(sensorValue.un.rotationVector.i); 197 | Serial.print(" j: "); 198 | Serial.print(sensorValue.un.rotationVector.j); 199 | Serial.print(" k: "); 200 | Serial.println(sensorValue.un.rotationVector.k); 201 | break; 202 | case SH2_GEOMAGNETIC_ROTATION_VECTOR: 203 | Serial.print("Geo-Magnetic Rotation Vector - r: "); 204 | Serial.print(sensorValue.un.geoMagRotationVector.real); 205 | Serial.print(" i: "); 206 | Serial.print(sensorValue.un.geoMagRotationVector.i); 207 | Serial.print(" j: "); 208 | Serial.print(sensorValue.un.geoMagRotationVector.j); 209 | Serial.print(" k: "); 210 | Serial.println(sensorValue.un.geoMagRotationVector.k); 211 | break; 212 | 213 | case SH2_GAME_ROTATION_VECTOR: 214 | Serial.print("Game Rotation Vector - r: "); 215 | Serial.print(sensorValue.un.gameRotationVector.real); 216 | Serial.print(" i: "); 217 | Serial.print(sensorValue.un.gameRotationVector.i); 218 | Serial.print(" j: "); 219 | Serial.print(sensorValue.un.gameRotationVector.j); 220 | Serial.print(" k: "); 221 | Serial.println(sensorValue.un.gameRotationVector.k); 222 | break; 223 | 224 | case SH2_STEP_COUNTER: 225 | Serial.print("Step Counter - steps: "); 226 | Serial.print(sensorValue.un.stepCounter.steps); 227 | Serial.print(" latency: "); 228 | Serial.println(sensorValue.un.stepCounter.latency); 229 | break; 230 | 231 | case SH2_STABILITY_CLASSIFIER: { 232 | Serial.print("Stability Classification: "); 233 | sh2_StabilityClassifier_t stability = sensorValue.un.stabilityClassifier; 234 | switch (stability.classification) { 235 | case STABILITY_CLASSIFIER_UNKNOWN: 236 | Serial.println("Unknown"); 237 | break; 238 | case STABILITY_CLASSIFIER_ON_TABLE: 239 | Serial.println("On Table"); 240 | break; 241 | case STABILITY_CLASSIFIER_STATIONARY: 242 | Serial.println("Stationary"); 243 | break; 244 | case STABILITY_CLASSIFIER_STABLE: 245 | Serial.println("Stable"); 246 | break; 247 | case STABILITY_CLASSIFIER_MOTION: 248 | Serial.println("In Motion"); 249 | break; 250 | } 251 | break; 252 | } 253 | 254 | case SH2_RAW_ACCELEROMETER: 255 | Serial.print("Raw Accelerometer - x: "); 256 | Serial.print(sensorValue.un.rawAccelerometer.x); 257 | Serial.print(" y: "); 258 | Serial.print(sensorValue.un.rawAccelerometer.y); 259 | Serial.print(" z: "); 260 | Serial.println(sensorValue.un.rawAccelerometer.z); 261 | break; 262 | case SH2_RAW_GYROSCOPE: 263 | Serial.print("Raw Gyro - x: "); 264 | Serial.print(sensorValue.un.rawGyroscope.x); 265 | Serial.print(" y: "); 266 | Serial.print(sensorValue.un.rawGyroscope.y); 267 | Serial.print(" z: "); 268 | Serial.println(sensorValue.un.rawGyroscope.z); 269 | break; 270 | case SH2_RAW_MAGNETOMETER: 271 | Serial.print("Raw Magnetic Field - x: "); 272 | Serial.print(sensorValue.un.rawMagnetometer.x); 273 | Serial.print(" y: "); 274 | Serial.print(sensorValue.un.rawMagnetometer.y); 275 | Serial.print(" z: "); 276 | Serial.println(sensorValue.un.rawMagnetometer.z); 277 | break; 278 | 279 | case SH2_SHAKE_DETECTOR: { 280 | Serial.print("Shake Detector - shake detected on axis: "); 281 | sh2_ShakeDetector_t detection = sensorValue.un.shakeDetector; 282 | switch (detection.shake) { 283 | case SHAKE_X: 284 | Serial.println("X"); 285 | break; 286 | case SHAKE_Y: 287 | Serial.println("Y"); 288 | break; 289 | case SHAKE_Z: 290 | Serial.println("Z"); 291 | break; 292 | default: 293 | Serial.println("None"); 294 | break; 295 | } 296 | } 297 | 298 | case SH2_PERSONAL_ACTIVITY_CLASSIFIER: { 299 | 300 | sh2_PersonalActivityClassifier_t activity = 301 | sensorValue.un.personalActivityClassifier; 302 | Serial.print("Activity classification - Most likely: "); 303 | printActivity(activity.mostLikelyState); 304 | Serial.println(""); 305 | 306 | Serial.println("Confidences:"); 307 | // if PAC_OPTION_COUNT is ever > 10, we'll need to 308 | // care about page 309 | for (uint8_t i = 0; i < PAC_OPTION_COUNT; i++) { 310 | Serial.print("\t"); 311 | printActivity(i); 312 | Serial.print(": "); 313 | Serial.println(activity.confidence[i]); 314 | } 315 | } 316 | } 317 | } 318 | -------------------------------------------------------------------------------- /src/sh2_SensorValue.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2015-16 Hillcrest Laboratories, Inc. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License and 6 | * any applicable agreements you may have with Hillcrest Laboratories, Inc. 7 | * You may obtain a copy of the License at 8 | * 9 | * http://www.apache.org/licenses/LICENSE-2.0 10 | * 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | */ 17 | 18 | /** 19 | * @file sh2_SensorValue.h 20 | * @author David Wheeler 21 | * @date 10 Nov 2015 22 | * @brief Support for converting sensor events (messages) into natural data structures. 23 | * 24 | */ 25 | 26 | #ifndef SH2_SENSORVALUE_H 27 | #define SH2_SENSORVALUE_H 28 | 29 | #ifdef __cplusplus 30 | extern "C" { 31 | #endif 32 | 33 | #include 34 | 35 | #include "sh2.h" 36 | 37 | /* Note on quaternion naming conventions: 38 | * Quaternions are values with four real components that are usually 39 | * interpreted as coefficients in the complex quantity, Q. 40 | * 41 | * As in, Q = W + Xi + Yj + Zk 42 | * 43 | * Where i, j and k represent the three imaginary dimensions. 44 | * 45 | * So W represents the Real components and X, Y and Z the Imaginary ones. 46 | * 47 | * In the Hillcrest datasheets and in this code, however, the four components 48 | * are named real, i, j and k, to make it explicit which is which. If you 49 | * need to translate these names into the "wxyz" or "xyzw" convention, then, the 50 | * appropriate mapping is this: 51 | * w = real 52 | * x = i 53 | * y = j 54 | * z = k 55 | */ 56 | 57 | /** 58 | * @brief Raw Accelerometer 59 | * 60 | * See the SH-2 Reference Manual for more detail. 61 | */ 62 | typedef struct sh2_RawAccelerometer { 63 | /* Units are ADC counts */ 64 | int16_t x; /**< @brief [ADC counts] */ 65 | int16_t y; /**< @brief [ADC counts] */ 66 | int16_t z; /**< @brief [ADC counts] */ 67 | 68 | /* Microseconds */ 69 | uint32_t timestamp; /**< @brief [uS] */ 70 | } sh2_RawAccelerometer_t; 71 | 72 | /** 73 | * @brief Accelerometer 74 | * 75 | * See the SH-2 Reference Manual for more detail. 76 | */ 77 | typedef struct sh2_Accelerometer { 78 | float x; 79 | float y; 80 | float z; 81 | } sh2_Accelerometer_t; 82 | 83 | /** 84 | * @brief Raw gyroscope 85 | * 86 | * See the SH-2 Reference Manual for more detail. 87 | */ 88 | typedef struct sh2_RawGyroscope { 89 | /* Units are ADC counts */ 90 | int16_t x; /**< @brief [ADC Counts] */ 91 | int16_t y; /**< @brief [ADC Counts] */ 92 | int16_t z; /**< @brief [ADC Counts] */ 93 | int16_t temperature; /**< @brief [ADC Counts] */ 94 | 95 | /* Microseconds */ 96 | uint32_t timestamp; /**< @brief [uS] */ 97 | } sh2_RawGyroscope_t; 98 | 99 | /** 100 | * @brief Gyroscope 101 | * 102 | * See the SH-2 Reference Manual for more detail. 103 | */ 104 | typedef struct sh2_Gyroscope { 105 | /* Units are rad/s */ 106 | float x; 107 | float y; 108 | float z; 109 | } sh2_Gyroscope_t; 110 | 111 | /** 112 | * @brief Uncalibrated gyroscope 113 | * 114 | * See the SH-2 Reference Manual for more detail. 115 | */ 116 | typedef struct sh2_GyroscopeUncalibrated { 117 | /* Units are rad/s */ 118 | float x; /**< @brief [rad/s] */ 119 | float y; /**< @brief [rad/s] */ 120 | float z; /**< @brief [rad/s] */ 121 | float biasX; /**< @brief [rad/s] */ 122 | float biasY; /**< @brief [rad/s] */ 123 | float biasZ; /**< @brief [rad/s] */ 124 | } sh2_GyroscopeUncalibrated_t; 125 | 126 | /** 127 | * @brief Raw Magnetometer 128 | * 129 | * See the SH-2 Reference Manual for more detail. 130 | */ 131 | typedef struct sh2_RawMagnetometer { 132 | /* Units are ADC counts */ 133 | int16_t x; /**< @brief [ADC Counts] */ 134 | int16_t y; /**< @brief [ADC Counts] */ 135 | int16_t z; /**< @brief [ADC Counts] */ 136 | 137 | /* Microseconds */ 138 | uint32_t timestamp; /**< @brief [uS] */ 139 | } sh2_RawMagnetometer_t; 140 | 141 | /** 142 | * @brief Magnetic field 143 | * 144 | * See the SH-2 Reference Manual for more detail. 145 | */ 146 | typedef struct sh2_MagneticField { 147 | /* Units are uTesla */ 148 | float x; /**< @brief [uTesla] */ 149 | float y; /**< @brief [uTesla] */ 150 | float z; /**< @brief [uTesla] */ 151 | } sh2_MagneticField_t; 152 | 153 | /** 154 | * @brief Uncalibrated magnetic field 155 | * 156 | * See the SH-2 Reference Manual for more detail. 157 | */ 158 | typedef struct sh2_MagneticFieldUncalibrated { 159 | /* Units are uTesla */ 160 | float x; /**< @brief [uTesla] */ 161 | float y; /**< @brief [uTesla] */ 162 | float z; /**< @brief [uTesla] */ 163 | float biasX; /**< @brief [uTesla] */ 164 | float biasY; /**< @brief [uTesla] */ 165 | float biasZ; /**< @brief [uTesla] */ 166 | } sh2_MagneticFieldUncalibrated_t; 167 | 168 | /** 169 | * @brief Rotation Vector with Accuracy 170 | * 171 | * See the SH-2 Reference Manual for more detail. 172 | */ 173 | typedef struct sh2_RotationVectorWAcc { 174 | float i; /**< @brief Quaternion component i */ 175 | float j; /**< @brief Quaternion component j */ 176 | float k; /**< @brief Quaternion component k */ 177 | float real; /**< @brief Quaternion component, real */ 178 | float accuracy; /**< @brief Accuracy estimate [radians] */ 179 | } sh2_RotationVectorWAcc_t; 180 | 181 | /** 182 | * @brief Rotation Vector 183 | * 184 | * See the SH-2 Reference Manual for more detail. 185 | */ 186 | typedef struct sh2_RotationVector { 187 | float i; /**< @brief Quaternion component i */ 188 | float j; /**< @brief Quaternion component j */ 189 | float k; /**< @brief Quaternion component k */ 190 | float real; /**< @brief Quaternion component real */ 191 | } sh2_RotationVector_t; 192 | 193 | /** 194 | * @brief Atmospheric Pressure 195 | * 196 | * See the SH-2 Reference Manual for more detail. 197 | */ 198 | typedef struct sh2_Pressure { 199 | float value; /**< @brief Atmospheric Pressure. [hectopascals] */ 200 | } sh2_Pressure_t; 201 | 202 | /** 203 | * @brief Ambient Light 204 | * 205 | * See the SH-2 Reference Manual for more detail. 206 | */ 207 | typedef struct sh2_AmbientLight { 208 | float value; /**< @brief Ambient Light. [lux] */ 209 | } sh2_AmbientLight_t; 210 | 211 | /** 212 | * @brief Humidity 213 | * 214 | * See the SH-2 Reference Manual for more detail. 215 | */ 216 | typedef struct sh2_Humidity { 217 | float value; /**< @brief Relative Humidity. [percent] */ 218 | } sh2_Humidity_t; 219 | 220 | /** 221 | * @brief Proximity 222 | * 223 | * See the SH-2 Reference Manual for more detail. 224 | */ 225 | typedef struct sh2_Proximity { 226 | float value; /**< @brief Proximity. [cm] */ 227 | } sh2_Proximity_t; 228 | 229 | /** 230 | * @brief Temperature 231 | * 232 | * See the SH-2 Reference Manual for more detail. 233 | */ 234 | typedef struct sh2_Temperature { 235 | float value; /**< @brief Temperature. [C] */ 236 | } sh2_Temperature_t; 237 | 238 | /** 239 | * @brief Reserved 240 | * 241 | * See the SH-2 Reference Manual for more detail. 242 | */ 243 | typedef struct sh2_Reserved { 244 | float tbd; /**< @brief Reserved */ 245 | } sh2_Reserved_t; 246 | 247 | /** 248 | * @brief TapDetector 249 | * 250 | * See the SH-2 Reference Manual for more detail. 251 | */ 252 | #define TAPDET_X (1) 253 | #define TAPDET_X_POS (2) 254 | #define TAPDET_Y (4) 255 | #define TAPDET_Y_POS (8) 256 | #define TAPDET_Z (16) 257 | #define TAPDET_Z_POS (32) 258 | #define TAPDET_DOUBLE (64) 259 | typedef struct sh2_TapDetector { 260 | uint8_t flags; /**< @brief TapDetector. */ 261 | } sh2_TapDetector_t; 262 | 263 | /** 264 | * @brief StepDetector 265 | * 266 | * See the SH-2 Reference Manual for more detail. 267 | */ 268 | typedef struct sh2_StepDetector { 269 | uint32_t latency; /**< @brief Step detect latency [uS]. */ 270 | } sh2_StepDetector_t; 271 | 272 | /** 273 | * @brief StepCounter 274 | * 275 | * See the SH-2 Reference Manual for more detail. 276 | */ 277 | typedef struct sh2_StepCounter { 278 | uint32_t latency; /**< @brief Step counter latency [uS]. */ 279 | uint16_t steps; /**< @brief Steps counted. */ 280 | } sh2_StepCounter_t; 281 | 282 | /** 283 | * @brief SigMotion 284 | * 285 | * See the SH-2 Reference Manual for more detail. 286 | */ 287 | typedef struct sh2_SigMotion { 288 | uint16_t motion; 289 | } sh2_SigMotion_t; 290 | 291 | /** 292 | * @brief StabilityClassifier 293 | * 294 | * See the SH-2 Reference Manual for more detail. 295 | */ 296 | #define STABILITY_CLASSIFIER_UNKNOWN (0) 297 | #define STABILITY_CLASSIFIER_ON_TABLE (1) 298 | #define STABILITY_CLASSIFIER_STATIONARY (2) 299 | #define STABILITY_CLASSIFIER_STABLE (3) 300 | #define STABILITY_CLASSIFIER_MOTION (4) 301 | typedef struct sh2_StabilityClassifier { 302 | uint8_t classification; 303 | } sh2_StabilityClassifier_t; 304 | 305 | /** 306 | * @brief ShakeDetector 307 | * 308 | * See the SH-2 Reference Manual for more detail. 309 | */ 310 | #define SHAKE_X (1) 311 | #define SHAKE_Y (2) 312 | #define SHAKE_Z (4) 313 | typedef struct sh2_ShakeDetector { 314 | uint16_t shake; 315 | } sh2_ShakeDetector_t; 316 | 317 | /** 318 | * @brief flipDetector 319 | * 320 | * See the SH-2 Reference Manual for more detail. 321 | */ 322 | typedef struct sh2_FlipDetector { 323 | uint16_t flip; 324 | } sh2_FlipDetector_t; 325 | 326 | /** 327 | * @brief pickupDetector 328 | * 329 | * See the SH-2 Reference Manual for more detail. 330 | */ 331 | #define PICKUP_LEVEL_TO_NOT_LEVEL (1) 332 | #define PICKUP_STOP_WITHIN_REGION (2) 333 | typedef struct sh2_PickupDetector { 334 | uint16_t pickup; /**< flag field with bits defined above. */ 335 | } sh2_PickupDetector_t; 336 | 337 | /** 338 | * @brief stabilityDetector 339 | * 340 | * See the SH-2 Reference Manual for more detail. 341 | */ 342 | #define STABILITY_ENTERED (1) 343 | #define STABILITY_EXITED (2) 344 | typedef struct sh2_StabilityDetector { 345 | uint16_t stability; /**< flag field with bits defined above. */ 346 | } sh2_StabilityDetector_t; 347 | 348 | /** 349 | * @brief Personal Activity Classifier 350 | * 351 | * See the SH-2 Reference Manual for more detail. 352 | */ 353 | #define PAC_UNKNOWN (0) 354 | #define PAC_IN_VEHICLE (1) 355 | #define PAC_ON_BICYCLE (2) 356 | #define PAC_ON_FOOT (3) 357 | #define PAC_STILL (4) 358 | #define PAC_TILTING (5) 359 | #define PAC_WALKING (6) 360 | #define PAC_RUNNING (7) 361 | typedef struct sh2_PersonalActivityClassifier { 362 | uint8_t page; 363 | bool lastPage; 364 | uint8_t mostLikelyState; 365 | uint8_t confidence[10]; 366 | } sh2_PersonalActivityClassifier_t; 367 | 368 | /** 369 | * @brief sleepDetector 370 | * 371 | * See the SH-2 Reference Manual for more detail. 372 | */ 373 | typedef struct sh2_SleepDetector { 374 | uint8_t sleepState; 375 | } sh2_SleepDetector_t; 376 | 377 | /** 378 | * @brief tiltDetector 379 | * 380 | * See the SH-2 Reference Manual for more detail. 381 | */ 382 | typedef struct sh2_TiltDetector { 383 | uint16_t tilt; 384 | } sh2_TiltDetector_t; 385 | 386 | /** 387 | * @brief pocketDetector 388 | * 389 | * See the SH-2 Reference Manual for more detail. 390 | */ 391 | typedef struct sh2_PocketDetector { 392 | uint16_t pocket; 393 | } sh2_PocketDetector_t; 394 | 395 | /** 396 | * @brief circleDetector 397 | * 398 | * See the SH-2 Reference Manual for more detail. 399 | */ 400 | typedef struct sh2_CircleDetector { 401 | uint16_t circle; 402 | } sh2_CircleDetector_t; 403 | 404 | /** 405 | * @brief heartRateMonitor 406 | * 407 | * See SH-2 Reference Manual for details. 408 | */ 409 | typedef struct sh2_HeartRateMonitor { 410 | uint16_t heartRate; /**< heart rate in beats per minute. */ 411 | } sh2_HeartRateMonitor_t; 412 | 413 | /** 414 | * @brief Gyro Integrated Rotation Vector 415 | * 416 | * See SH-2 Reference Manual for details. 417 | */ 418 | typedef struct sh2_GyroIntegratedRV { 419 | float i; /**< @brief Quaternion component i */ 420 | float j; /**< @brief Quaternion component j */ 421 | float k; /**< @brief Quaternion component k */ 422 | float real; /**< @brief Quaternion component real */ 423 | float angVelX; /**< @brief Angular velocity about x [rad/s] */ 424 | float angVelY; /**< @brief Angular velocity about y [rad/s] */ 425 | float angVelZ; /**< @brief Angular velocity about z [rad/s] */ 426 | } sh2_GyroIntegratedRV_t; 427 | 428 | typedef struct sh2_IZroRequest { 429 | sh2_IZroMotionIntent_t intent; 430 | sh2_IZroMotionRequest_t request; 431 | } sh2_IZroRequest_t; 432 | 433 | typedef struct sh2_SensorValue { 434 | 435 | /** Which sensor produced this event. */ 436 | uint8_t sensorId; 437 | 438 | /** @brief 8-bit unsigned integer used to track reports. 439 | * 440 | * The sequence number increments once for each report sent. Gaps 441 | * in the sequence numbers indicate missing or dropped reports. 442 | */ 443 | uint8_t sequence; 444 | 445 | /* Status of a sensor 446 | * 0 - Unreliable 447 | * 1 - Accuracy low 448 | * 2 - Accuracy medium 449 | * 3 - Accuracy high 450 | */ 451 | uint8_t status; /**< @brief bits 7-5: reserved, 4-2: exponent delay, 1-0: Accuracy */ 452 | 453 | uint64_t timestamp; /**< [uS] */ 454 | 455 | uint32_t delay; /**< @brief [uS] value is delay * 2^exponent (see status) */ 456 | 457 | /** @brief Sensor Data 458 | * 459 | * Use the structure based on the value of the sensor 460 | * field. 461 | */ 462 | union { 463 | sh2_RawAccelerometer_t rawAccelerometer; 464 | sh2_Accelerometer_t accelerometer; 465 | sh2_Accelerometer_t linearAcceleration; 466 | sh2_Accelerometer_t gravity; 467 | sh2_RawGyroscope_t rawGyroscope; 468 | sh2_Gyroscope_t gyroscope; 469 | sh2_GyroscopeUncalibrated_t gyroscopeUncal; 470 | sh2_RawMagnetometer_t rawMagnetometer; 471 | sh2_MagneticField_t magneticField; 472 | sh2_MagneticFieldUncalibrated_t magneticFieldUncal; 473 | sh2_RotationVectorWAcc_t rotationVector; 474 | sh2_RotationVector_t gameRotationVector; 475 | sh2_RotationVectorWAcc_t geoMagRotationVector; 476 | sh2_Pressure_t pressure; 477 | sh2_AmbientLight_t ambientLight; 478 | sh2_Humidity_t humidity; 479 | sh2_Proximity_t proximity; 480 | sh2_Temperature_t temperature; 481 | sh2_Reserved_t reserved; 482 | sh2_TapDetector_t tapDetector; 483 | sh2_StepDetector_t stepDetector; 484 | sh2_StepCounter_t stepCounter; 485 | sh2_SigMotion_t sigMotion; 486 | sh2_StabilityClassifier_t stabilityClassifier; 487 | sh2_ShakeDetector_t shakeDetector; 488 | sh2_FlipDetector_t flipDetector; 489 | sh2_PickupDetector_t pickupDetector; 490 | sh2_StabilityDetector_t stabilityDetector; 491 | sh2_PersonalActivityClassifier_t personalActivityClassifier; 492 | sh2_SleepDetector_t sleepDetector; 493 | sh2_TiltDetector_t tiltDetector; 494 | sh2_PocketDetector_t pocketDetector; 495 | sh2_CircleDetector_t circleDetector; 496 | sh2_HeartRateMonitor_t heartRateMonitor; 497 | sh2_RotationVectorWAcc_t arvrStabilizedRV; 498 | sh2_RotationVector_t arvrStabilizedGRV; 499 | sh2_GyroIntegratedRV_t gyroIntegratedRV; 500 | sh2_IZroRequest_t izroRequest; 501 | } un; 502 | } sh2_SensorValue_t; 503 | 504 | int sh2_decodeSensorEvent(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 505 | 506 | 507 | #ifdef __cplusplus 508 | } // extern "C" 509 | #endif 510 | 511 | #endif 512 | -------------------------------------------------------------------------------- /src/Adafruit_BNO08x.cpp: -------------------------------------------------------------------------------- 1 | /*! 2 | * @file Adafruit_BNO08x.cpp 3 | * 4 | * @mainpage Adafruit BNO08x 9-DOF Orientation IMU Fusion Breakout 5 | * 6 | * @section intro_sec Introduction 7 | * 8 | * I2C Driver for the Library for the BNO08x 9-DOF Orientation IMU Fusion 9 | * Breakout 10 | * 11 | * This is a library for the Adafruit BNO08x breakout: 12 | * https://www.adafruit.com/product/4754 13 | * 14 | * Adafruit invests time and resources providing this open source code, 15 | * please support Adafruit and open-source hardware by purchasing products from 16 | * Adafruit! 17 | * 18 | * @section dependencies Dependencies 19 | * This library depends on the Adafruit BusIO library 20 | * 21 | * This library depends on the Adafruit Unified Sensor library 22 | * 23 | * @section author Author 24 | * 25 | * Bryan Siepert for Adafruit Industries 26 | * 27 | * @section license License 28 | * 29 | * BSD (see license.txt) 30 | * 31 | * @section HISTORY 32 | * 33 | * v1.0 - First release 34 | */ 35 | 36 | #include "Arduino.h" 37 | #include 38 | 39 | #include "Adafruit_BNO08x.h" 40 | 41 | static Adafruit_SPIDevice *spi_dev = NULL; ///< Pointer to SPI bus interface 42 | static int8_t _int_pin, _reset_pin; 43 | 44 | static Adafruit_I2CDevice *i2c_dev = NULL; ///< Pointer to I2C bus interface 45 | static HardwareSerial *uart_dev = NULL; 46 | 47 | static sh2_SensorValue_t *_sensor_value = NULL; 48 | static bool _reset_occurred = false; 49 | 50 | static int i2chal_write(sh2_Hal_t *self, uint8_t *pBuffer, unsigned len); 51 | static int i2chal_read(sh2_Hal_t *self, uint8_t *pBuffer, unsigned len, 52 | uint32_t *t_us); 53 | static void i2chal_close(sh2_Hal_t *self); 54 | static int i2chal_open(sh2_Hal_t *self); 55 | 56 | static int uarthal_write(sh2_Hal_t *self, uint8_t *pBuffer, unsigned len); 57 | static int uarthal_read(sh2_Hal_t *self, uint8_t *pBuffer, unsigned len, 58 | uint32_t *t_us); 59 | static void uarthal_close(sh2_Hal_t *self); 60 | static int uarthal_open(sh2_Hal_t *self); 61 | 62 | static bool spihal_wait_for_int(void); 63 | static int spihal_write(sh2_Hal_t *self, uint8_t *pBuffer, unsigned len); 64 | static int spihal_read(sh2_Hal_t *self, uint8_t *pBuffer, unsigned len, 65 | uint32_t *t_us); 66 | static void spihal_close(sh2_Hal_t *self); 67 | static int spihal_open(sh2_Hal_t *self); 68 | 69 | static uint32_t hal_getTimeUs(sh2_Hal_t *self); 70 | static void hal_callback(void *cookie, sh2_AsyncEvent_t *pEvent); 71 | static void sensorHandler(void *cookie, sh2_SensorEvent_t *pEvent); 72 | static void hal_hardwareReset(void); 73 | 74 | /** 75 | * @brief Construct a new Adafruit_BNO08x::Adafruit_BNO08x object 76 | * 77 | */ 78 | 79 | /** 80 | * @brief Construct a new Adafruit_BNO08x::Adafruit_BNO08x object 81 | * 82 | * @param reset_pin The arduino pin # connected to the BNO Reset pin 83 | */ 84 | Adafruit_BNO08x::Adafruit_BNO08x(int8_t reset_pin) { _reset_pin = reset_pin; } 85 | 86 | /** 87 | * @brief Destroy the Adafruit_BNO08x::Adafruit_BNO08x object 88 | * 89 | */ 90 | Adafruit_BNO08x::~Adafruit_BNO08x(void) { 91 | // if (temp_sensor) 92 | // delete temp_sensor; 93 | } 94 | 95 | /*! 96 | * @brief Sets up the hardware and initializes I2C 97 | * @param i2c_address 98 | * The I2C address to be used. 99 | * @param wire 100 | * The Wire object to be used for I2C connections. 101 | * @param sensor_id 102 | * The unique ID to differentiate the sensors from others 103 | * @return True if initialization was successful, otherwise false. 104 | */ 105 | bool Adafruit_BNO08x::begin_I2C(uint8_t i2c_address, TwoWire *wire, 106 | int32_t sensor_id) { 107 | if (i2c_dev) { 108 | delete i2c_dev; // remove old interface 109 | } 110 | 111 | i2c_dev = new Adafruit_I2CDevice(i2c_address, wire); 112 | 113 | if (!i2c_dev->begin()) { 114 | Serial.println(F("I2C address not found")); 115 | return false; 116 | } 117 | 118 | _HAL.open = i2chal_open; 119 | _HAL.close = i2chal_close; 120 | _HAL.read = i2chal_read; 121 | _HAL.write = i2chal_write; 122 | _HAL.getTimeUs = hal_getTimeUs; 123 | 124 | return _init(sensor_id); 125 | } 126 | 127 | /** 128 | * @brief Sets up the hardware and initializes UART 129 | * 130 | * @param serial Pointer to Stream (HardwareSerial/SoftwareSerial) interface 131 | * @param sensor_id 132 | * The user-defined ID to differentiate different sensors 133 | * @return true if initialization was successful, otherwise false. 134 | */ 135 | bool Adafruit_BNO08x::begin_UART(HardwareSerial *serial, int32_t sensor_id) { 136 | uart_dev = serial; 137 | 138 | _HAL.open = uarthal_open; 139 | _HAL.close = uarthal_close; 140 | _HAL.read = uarthal_read; 141 | _HAL.write = uarthal_write; 142 | _HAL.getTimeUs = hal_getTimeUs; 143 | 144 | return _init(sensor_id); 145 | } 146 | 147 | /*! 148 | * @brief Sets up the hardware and initializes hardware SPI 149 | * @param cs_pin The arduino pin # connected to chip select 150 | * @param int_pin The arduino pin # connected to BNO08x INT 151 | * @param theSPI The SPI object to be used for SPI connections. 152 | * @param sensor_id 153 | * The user-defined ID to differentiate different sensors 154 | * @return true if initialization was successful, otherwise false. 155 | */ 156 | bool Adafruit_BNO08x::begin_SPI(uint8_t cs_pin, uint8_t int_pin, 157 | SPIClass *theSPI, int32_t sensor_id) { 158 | i2c_dev = NULL; 159 | 160 | _int_pin = int_pin; 161 | pinMode(_int_pin, INPUT_PULLUP); 162 | 163 | if (spi_dev) { 164 | delete spi_dev; // remove old interface 165 | } 166 | spi_dev = new Adafruit_SPIDevice(cs_pin, 167 | 1000000, // frequency 168 | SPI_BITORDER_MSBFIRST, // bit order 169 | SPI_MODE3, // data mode 170 | theSPI); 171 | if (!spi_dev->begin()) { 172 | return false; 173 | } 174 | 175 | _HAL.open = spihal_open; 176 | _HAL.close = spihal_close; 177 | _HAL.read = spihal_read; 178 | _HAL.write = spihal_write; 179 | _HAL.getTimeUs = hal_getTimeUs; 180 | 181 | return _init(sensor_id); 182 | } 183 | 184 | /*! @brief Initializer for post i2c/spi init 185 | * @param sensor_id Optional unique ID for the sensor set 186 | * @returns True if chip identified and initialized 187 | */ 188 | bool Adafruit_BNO08x::_init(int32_t sensor_id) { 189 | int status; 190 | 191 | hardwareReset(); 192 | 193 | // Open SH2 interface (also registers non-sensor event handler.) 194 | status = sh2_open(&_HAL, hal_callback, NULL); 195 | if (status != SH2_OK) { 196 | return false; 197 | } 198 | 199 | // Check connection partially by getting the product id's 200 | memset(&prodIds, 0, sizeof(prodIds)); 201 | status = sh2_getProdIds(&prodIds); 202 | if (status != SH2_OK) { 203 | return false; 204 | } 205 | 206 | // Register sensor listener 207 | sh2_setSensorCallback(sensorHandler, NULL); 208 | 209 | return true; 210 | } 211 | 212 | /** 213 | * @brief Reset the device using the Reset pin 214 | * 215 | */ 216 | void Adafruit_BNO08x::hardwareReset(void) { hal_hardwareReset(); } 217 | 218 | /** 219 | * @brief Check if a reset has occured 220 | * 221 | * @return true: a reset has occured false: no reset has occoured 222 | */ 223 | bool Adafruit_BNO08x::wasReset(void) { 224 | bool x = _reset_occurred; 225 | _reset_occurred = false; 226 | 227 | return x; 228 | } 229 | 230 | /** 231 | * @brief Fill the given sensor value object with a new report 232 | * 233 | * @param value Pointer to an sh2_SensorValue_t struct to fil 234 | * @return true: The report object was filled with a new report 235 | * @return false: No new report available to fill 236 | */ 237 | bool Adafruit_BNO08x::getSensorEvent(sh2_SensorValue_t *value) { 238 | _sensor_value = value; 239 | 240 | value->timestamp = 0; 241 | 242 | sh2_service(); 243 | 244 | if (value->timestamp == 0 && value->sensorId != SH2_GYRO_INTEGRATED_RV) { 245 | // no new events 246 | return false; 247 | } 248 | 249 | return true; 250 | } 251 | 252 | /** 253 | * @brief Enable the given report type 254 | * 255 | * @param sensorId The report ID to enable 256 | * @param interval_us The update interval for reports to be generated, in 257 | * microseconds 258 | * @return true: success false: failure 259 | */ 260 | bool Adafruit_BNO08x::enableReport(sh2_SensorId_t sensorId, 261 | uint32_t interval_us) { 262 | static sh2_SensorConfig_t config; 263 | 264 | // These sensor options are disabled or not used in most cases 265 | config.changeSensitivityEnabled = false; 266 | config.wakeupEnabled = false; 267 | config.changeSensitivityRelative = false; 268 | config.alwaysOnEnabled = false; 269 | config.changeSensitivity = 0; 270 | config.batchInterval_us = 0; 271 | config.sensorSpecific = 0; 272 | 273 | config.reportInterval_us = interval_us; 274 | int status = sh2_setSensorConfig(sensorId, &config); 275 | 276 | if (status != SH2_OK) { 277 | return false; 278 | } 279 | 280 | return true; 281 | } 282 | 283 | /**************************************** I2C interface 284 | * ***********************************************************/ 285 | 286 | static int i2chal_open(sh2_Hal_t *self) { 287 | // Serial.println("I2C HAL open"); 288 | uint8_t softreset_pkt[] = {5, 0, 1, 0, 1}; 289 | bool success = false; 290 | for (uint8_t attempts = 0; attempts < 5; attempts++) { 291 | if (i2c_dev->write(softreset_pkt, 5)) { 292 | success = true; 293 | break; 294 | } 295 | delay(30); 296 | } 297 | if (!success) 298 | return -1; 299 | delay(300); 300 | return 0; 301 | } 302 | 303 | static void i2chal_close(sh2_Hal_t *self) { 304 | // Serial.println("I2C HAL close"); 305 | } 306 | 307 | static int i2chal_read(sh2_Hal_t *self, uint8_t *pBuffer, unsigned len, 308 | uint32_t *t_us) { 309 | // Serial.println("I2C HAL read"); 310 | 311 | // uint8_t *pBufferOrig = pBuffer; 312 | 313 | uint8_t header[4]; 314 | if (!i2c_dev->read(header, 4)) { 315 | return 0; 316 | } 317 | 318 | // Determine amount to read 319 | uint16_t packet_size = (uint16_t)header[0] | (uint16_t)header[1] << 8; 320 | // Unset the "continue" bit 321 | packet_size &= ~0x8000; 322 | 323 | /* 324 | Serial.print("Read SHTP header. "); 325 | Serial.print("Packet size: "); 326 | Serial.print(packet_size); 327 | Serial.print(" & buffer size: "); 328 | Serial.println(len); 329 | */ 330 | 331 | size_t i2c_buffer_max = i2c_dev->maxBufferSize(); 332 | 333 | if (packet_size > len) { 334 | // packet wouldn't fit in our buffer 335 | return 0; 336 | } 337 | // the number of non-header bytes to read 338 | uint16_t cargo_remaining = packet_size; 339 | uint8_t i2c_buffer[i2c_buffer_max]; 340 | uint16_t read_size; 341 | uint16_t cargo_read_amount = 0; 342 | bool first_read = true; 343 | 344 | while (cargo_remaining > 0) { 345 | if (first_read) { 346 | read_size = min(i2c_buffer_max, (size_t)cargo_remaining); 347 | } else { 348 | read_size = min(i2c_buffer_max, (size_t)cargo_remaining + 4); 349 | } 350 | 351 | // Serial.print("Reading from I2C: "); Serial.println(read_size); 352 | // Serial.print("Remaining to read: "); Serial.println(cargo_remaining); 353 | 354 | if (!i2c_dev->read(i2c_buffer, read_size)) { 355 | return 0; 356 | } 357 | 358 | if (first_read) { 359 | // The first time we're saving the "original" header, so include it in the 360 | // cargo count 361 | cargo_read_amount = read_size; 362 | memcpy(pBuffer, i2c_buffer, cargo_read_amount); 363 | first_read = false; 364 | } else { 365 | // this is not the first read, so copy from 4 bytes after the beginning of 366 | // the i2c buffer to skip the header included with every new i2c read and 367 | // don't include the header in the amount of cargo read 368 | cargo_read_amount = read_size - 4; 369 | memcpy(pBuffer, i2c_buffer + 4, cargo_read_amount); 370 | } 371 | // advance our pointer by the amount of cargo read 372 | pBuffer += cargo_read_amount; 373 | // mark the cargo as received 374 | cargo_remaining -= cargo_read_amount; 375 | } 376 | 377 | /* 378 | for (int i=0; imaxBufferSize(); 391 | 392 | /* 393 | Serial.print("I2C HAL write packet size: "); 394 | Serial.print(len); 395 | Serial.print(" & max buffer size: "); 396 | Serial.println(i2c_buffer_max); 397 | */ 398 | 399 | uint16_t write_size = min(i2c_buffer_max, len); 400 | if (!i2c_dev->write(pBuffer, write_size)) { 401 | return 0; 402 | } 403 | 404 | return write_size; 405 | } 406 | 407 | /**************************************** UART interface 408 | * ***********************************************************/ 409 | 410 | static int uarthal_open(sh2_Hal_t *self) { 411 | // Serial.println("UART HAL open"); 412 | uart_dev->begin(3000000); 413 | 414 | // flush input 415 | while (uart_dev->available()) { 416 | uart_dev->read(); 417 | yield(); 418 | } 419 | 420 | // send a software reset 421 | uint8_t softreset_pkt[] = {0x7E, 1, 5, 0, 1, 0, 1, 0x7E}; 422 | for (int i = 0; i < sizeof(softreset_pkt); i++) { 423 | uart_dev->write(softreset_pkt[i]); 424 | delay(1); 425 | } 426 | 427 | return 0; 428 | } 429 | 430 | static void uarthal_close(sh2_Hal_t *self) { 431 | // Serial.println("UART HAL close"); 432 | uart_dev->end(); 433 | } 434 | 435 | static int uarthal_read(sh2_Hal_t *self, uint8_t *pBuffer, unsigned len, 436 | uint32_t *t_us) { 437 | uint8_t c; 438 | uint16_t packet_size = 0; 439 | 440 | // Serial.println("UART HAL read"); 441 | 442 | // read packet start 443 | while (1) { 444 | yield(); 445 | 446 | if (!uart_dev->available()) { 447 | continue; 448 | } 449 | c = uart_dev->read(); 450 | // Serial.print(c, HEX); Serial.print(", "); 451 | if (c == 0x7E) { 452 | break; 453 | } 454 | } 455 | 456 | // read protocol id 457 | while (uart_dev->available() < 2) { 458 | yield(); 459 | } 460 | c = uart_dev->read(); 461 | // Serial.print(c, HEX); Serial.print(", "); 462 | if (c == 0x7E) { 463 | c = uart_dev->read(); 464 | // Serial.print(c, HEX); Serial.print(", "); 465 | if (c != 0x01) { 466 | return 0; 467 | } 468 | } else if (c != 0x01) { 469 | return 0; 470 | } 471 | 472 | while (true) { 473 | yield(); 474 | 475 | if (!uart_dev->available()) { 476 | continue; 477 | } 478 | c = uart_dev->read(); 479 | // Serial.print(c, HEX); Serial.print(", "); 480 | if (c == 0x7E) { 481 | break; 482 | } 483 | if (c == 0x7D) { 484 | // escape! 485 | while (!uart_dev->available()) { 486 | continue; 487 | } 488 | c = uart_dev->read(); 489 | c ^= 0x20; 490 | } 491 | pBuffer[packet_size] = c; 492 | packet_size++; 493 | } 494 | 495 | /* 496 | Serial.print("Read UART packet size: "); 497 | Serial.println(packet_size); 498 | for (int i=0; iwrite(0x7E); 517 | delay(1); 518 | // protocol id 519 | uart_dev->write(0x01); 520 | delay(1); 521 | 522 | for (int i = 0; i < len; i++) { 523 | c = pBuffer[i]; 524 | if ((c == 0x7E) || (c == 0x7D)) { 525 | uart_dev->write(0x7D); // control 526 | delay(1); 527 | c ^= 0x20; 528 | } 529 | uart_dev->write(c); 530 | delay(1); 531 | } 532 | // end byte 533 | uart_dev->write(0x7E); 534 | 535 | return len; 536 | } 537 | 538 | /**************************************** UART interface 539 | * ***********************************************************/ 540 | 541 | static int spihal_open(sh2_Hal_t *self) { 542 | // Serial.println("SPI HAL open"); 543 | 544 | spihal_wait_for_int(); 545 | 546 | return 0; 547 | } 548 | 549 | static bool spihal_wait_for_int(void) { 550 | for (int i = 0; i < 500; i++) { 551 | if (!digitalRead(_int_pin)) 552 | return true; 553 | // Serial.print("."); 554 | delay(1); 555 | } 556 | // Serial.println("Timed out!"); 557 | hal_hardwareReset(); 558 | 559 | return false; 560 | } 561 | 562 | static void spihal_close(sh2_Hal_t *self) { 563 | // Serial.println("SPI HAL close"); 564 | } 565 | 566 | static int spihal_read(sh2_Hal_t *self, uint8_t *pBuffer, unsigned len, 567 | uint32_t *t_us) { 568 | // Serial.println("SPI HAL read"); 569 | 570 | uint16_t packet_size = 0; 571 | 572 | if (!spihal_wait_for_int()) { 573 | return 0; 574 | } 575 | 576 | if (!spi_dev->read(pBuffer, 4, 0x00)) { 577 | return 0; 578 | } 579 | 580 | // Determine amount to read 581 | packet_size = (uint16_t)pBuffer[0] | (uint16_t)pBuffer[1] << 8; 582 | // Unset the "continue" bit 583 | packet_size &= ~0x8000; 584 | 585 | /* 586 | Serial.print("Read SHTP header. "); 587 | Serial.print("Packet size: "); 588 | Serial.print(packet_size); 589 | Serial.print(" & buffer size: "); 590 | Serial.println(len); 591 | */ 592 | 593 | if (packet_size > len) { 594 | return 0; 595 | } 596 | 597 | if (!spihal_wait_for_int()) { 598 | return 0; 599 | } 600 | 601 | if (!spi_dev->read(pBuffer, packet_size, 0x00)) { 602 | return 0; 603 | } 604 | 605 | return packet_size; 606 | } 607 | 608 | static int spihal_write(sh2_Hal_t *self, uint8_t *pBuffer, unsigned len) { 609 | // Serial.print("SPI HAL write packet size: "); 610 | // Serial.println(len); 611 | 612 | if (!spihal_wait_for_int()) { 613 | return 0; 614 | } 615 | 616 | spi_dev->write(pBuffer, len); 617 | 618 | return len; 619 | } 620 | 621 | /**************************************** HAL interface 622 | * ***********************************************************/ 623 | 624 | static void hal_hardwareReset(void) { 625 | if (_reset_pin != -1) { 626 | // Serial.println("BNO08x Hardware reset"); 627 | 628 | pinMode(_reset_pin, OUTPUT); 629 | digitalWrite(_reset_pin, HIGH); 630 | delay(10); 631 | digitalWrite(_reset_pin, LOW); 632 | delay(10); 633 | digitalWrite(_reset_pin, HIGH); 634 | delay(10); 635 | } 636 | } 637 | 638 | static uint32_t hal_getTimeUs(sh2_Hal_t *self) { 639 | uint32_t t = millis() * 1000; 640 | // Serial.printf("I2C HAL get time: %d\n", t); 641 | return t; 642 | } 643 | 644 | static void hal_callback(void *cookie, sh2_AsyncEvent_t *pEvent) { 645 | // If we see a reset, set a flag so that sensors will be reconfigured. 646 | if (pEvent->eventId == SH2_RESET) { 647 | // Serial.println("Reset!"); 648 | _reset_occurred = true; 649 | } 650 | } 651 | 652 | // Handle sensor events. 653 | static void sensorHandler(void *cookie, sh2_SensorEvent_t *event) { 654 | int rc; 655 | 656 | // Serial.println("Got an event!"); 657 | 658 | rc = sh2_decodeSensorEvent(_sensor_value, event); 659 | if (rc != SH2_OK) { 660 | Serial.println("BNO08x - Error decoding sensor event"); 661 | _sensor_value->timestamp = 0; 662 | return; 663 | } 664 | } 665 | -------------------------------------------------------------------------------- /src/sh2_SensorValue.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2015-16 Hillcrest Laboratories, Inc. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License and 6 | * any applicable agreements you may have with Hillcrest Laboratories, Inc. 7 | * You may obtain a copy of the License at 8 | * 9 | * http://www.apache.org/licenses/LICENSE-2.0 10 | * 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | */ 17 | 18 | /* 19 | * BNO080 Sensor Event decoding 20 | */ 21 | 22 | #include "sh2_SensorValue.h" 23 | #include "sh2_err.h" 24 | #include "sh2_util.h" 25 | 26 | #define SCALE_Q(n) (1.0f / (1 << n)) 27 | 28 | const float scaleRadToDeg = 180.0 / 3.14159265358; 29 | 30 | // ------------------------------------------------------------------------ 31 | // Forward declarations 32 | 33 | static int decodeRawAccelerometer(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 34 | static int decodeAccelerometer(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 35 | static int decodeLinearAcceleration(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 36 | static int decodeGravity(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 37 | static int decodeRawGyroscope(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 38 | static int decodeGyroscopeCalibrated(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 39 | static int decodeGyroscopeUncal(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 40 | static int decodeRawMagnetometer(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 41 | static int decodeMagneticFieldCalibrated(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 42 | static int decodeMagneticFieldUncal(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 43 | static int decodeRotationVector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 44 | static int decodeGameRotationVector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 45 | static int decodeGeomagneticRotationVector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 46 | static int decodePressure(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 47 | static int decodeAmbientLight(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 48 | static int decodeHumidity(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 49 | static int decodeProximity(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 50 | static int decodeTemperature(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 51 | static int decodeReserved(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 52 | static int decodeTapDetector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 53 | static int decodeStepDetector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 54 | static int decodeStepCounter(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 55 | static int decodeSignificantMotion(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 56 | static int decodeStabilityClassifier(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 57 | static int decodeShakeDetector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 58 | static int decodeFlipDetector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 59 | static int decodePickupDetector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 60 | static int decodeStabilityDetector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 61 | static int decodePersonalActivityClassifier(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 62 | static int decodeSleepDetector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 63 | static int decodeTiltDetector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 64 | static int decodePocketDetector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 65 | static int decodeCircleDetector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 66 | static int decodeHeartRateMonitor(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 67 | static int decodeArvrStabilizedRV(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 68 | static int decodeArvrStabilizedGRV(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 69 | static int decodeGyroIntegratedRV(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 70 | static int decodeIZroRequest(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 71 | 72 | // ------------------------------------------------------------------------ 73 | // Public API 74 | 75 | int sh2_decodeSensorEvent(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 76 | { 77 | // Fill out fields of *value based on *event, converting data from message representation 78 | // to natural representation. 79 | 80 | int rc = SH2_OK; 81 | 82 | value->sensorId = event->reportId; 83 | value->timestamp = event->timestamp_uS; 84 | 85 | if (value->sensorId != SH2_GYRO_INTEGRATED_RV) { 86 | value->sequence = event->report[1]; 87 | value->status = event->report[2] & 0x03; 88 | } 89 | else { 90 | value->sequence = 0; 91 | value->status = 0; 92 | } 93 | 94 | // extract delay field (100uS units) 95 | 96 | 97 | switch (value->sensorId) { 98 | case SH2_RAW_ACCELEROMETER: 99 | rc = decodeRawAccelerometer(value, event); 100 | break; 101 | case SH2_ACCELEROMETER: 102 | rc = decodeAccelerometer(value, event); 103 | break; 104 | case SH2_LINEAR_ACCELERATION: 105 | rc = decodeLinearAcceleration(value, event); 106 | break; 107 | case SH2_GRAVITY: 108 | rc = decodeGravity(value, event); 109 | break; 110 | case SH2_RAW_GYROSCOPE: 111 | rc = decodeRawGyroscope(value, event); 112 | break; 113 | case SH2_GYROSCOPE_CALIBRATED: 114 | rc = decodeGyroscopeCalibrated(value, event); 115 | break; 116 | case SH2_GYROSCOPE_UNCALIBRATED: 117 | rc = decodeGyroscopeUncal(value, event); 118 | break; 119 | case SH2_RAW_MAGNETOMETER: 120 | rc = decodeRawMagnetometer(value, event); 121 | break; 122 | case SH2_MAGNETIC_FIELD_CALIBRATED: 123 | rc = decodeMagneticFieldCalibrated(value, event); 124 | break; 125 | case SH2_MAGNETIC_FIELD_UNCALIBRATED: 126 | rc = decodeMagneticFieldUncal(value, event); 127 | break; 128 | case SH2_ROTATION_VECTOR: 129 | rc = decodeRotationVector(value, event); 130 | break; 131 | case SH2_GAME_ROTATION_VECTOR: 132 | rc = decodeGameRotationVector(value, event); 133 | break; 134 | case SH2_GEOMAGNETIC_ROTATION_VECTOR: 135 | rc = decodeGeomagneticRotationVector(value, event); 136 | break; 137 | case SH2_PRESSURE: 138 | rc = decodePressure(value, event); 139 | break; 140 | case SH2_AMBIENT_LIGHT: 141 | rc = decodeAmbientLight(value, event); 142 | break; 143 | case SH2_HUMIDITY: 144 | rc = decodeHumidity(value, event); 145 | break; 146 | case SH2_PROXIMITY: 147 | rc = decodeProximity(value, event); 148 | break; 149 | case SH2_TEMPERATURE: 150 | rc = decodeTemperature(value, event); 151 | break; 152 | case SH2_RESERVED: 153 | rc = decodeReserved(value, event); 154 | break; 155 | case SH2_TAP_DETECTOR: 156 | rc = decodeTapDetector(value, event); 157 | break; 158 | case SH2_STEP_DETECTOR: 159 | rc = decodeStepDetector(value, event); 160 | break; 161 | case SH2_STEP_COUNTER: 162 | rc = decodeStepCounter(value, event); 163 | break; 164 | case SH2_SIGNIFICANT_MOTION: 165 | rc = decodeSignificantMotion(value, event); 166 | break; 167 | case SH2_STABILITY_CLASSIFIER: 168 | rc = decodeStabilityClassifier(value, event); 169 | break; 170 | case SH2_SHAKE_DETECTOR: 171 | rc = decodeShakeDetector(value, event); 172 | break; 173 | case SH2_FLIP_DETECTOR: 174 | rc = decodeFlipDetector(value, event); 175 | break; 176 | case SH2_PICKUP_DETECTOR: 177 | rc = decodePickupDetector(value, event); 178 | break; 179 | case SH2_STABILITY_DETECTOR: 180 | rc = decodeStabilityDetector(value, event); 181 | break; 182 | case SH2_PERSONAL_ACTIVITY_CLASSIFIER: 183 | rc = decodePersonalActivityClassifier(value, event); 184 | break; 185 | case SH2_SLEEP_DETECTOR: 186 | rc = decodeSleepDetector(value, event); 187 | break; 188 | case SH2_TILT_DETECTOR: 189 | rc = decodeTiltDetector(value, event); 190 | break; 191 | case SH2_POCKET_DETECTOR: 192 | rc = decodePocketDetector(value, event); 193 | break; 194 | case SH2_CIRCLE_DETECTOR: 195 | rc = decodeCircleDetector(value, event); 196 | break; 197 | case SH2_HEART_RATE_MONITOR: 198 | rc = decodeHeartRateMonitor(value, event); 199 | break; 200 | case SH2_ARVR_STABILIZED_RV: 201 | rc = decodeArvrStabilizedRV(value, event); 202 | break; 203 | case SH2_ARVR_STABILIZED_GRV: 204 | rc = decodeArvrStabilizedGRV(value, event); 205 | break; 206 | case SH2_GYRO_INTEGRATED_RV: 207 | rc = decodeGyroIntegratedRV(value, event); 208 | break; 209 | case SH2_IZRO_MOTION_REQUEST: 210 | rc = decodeIZroRequest(value, event); 211 | break; 212 | default: 213 | // Unknown report id 214 | rc = SH2_ERR; 215 | break; 216 | } 217 | 218 | return rc; 219 | } 220 | 221 | // ------------------------------------------------------------------------ 222 | // Private utility functions 223 | 224 | static int decodeRawAccelerometer(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 225 | { 226 | value->un.rawAccelerometer.x = read16(&event->report[4]); 227 | value->un.rawAccelerometer.y = read16(&event->report[6]); 228 | value->un.rawAccelerometer.z = read16(&event->report[8]); 229 | value->un.rawAccelerometer.timestamp = read32(&event->report[12]); 230 | 231 | return SH2_OK; 232 | } 233 | 234 | static int decodeAccelerometer(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 235 | { 236 | value->un.accelerometer.x = read16(&event->report[4]) * SCALE_Q(8); 237 | value->un.accelerometer.y = read16(&event->report[6]) * SCALE_Q(8); 238 | value->un.accelerometer.z = read16(&event->report[8]) * SCALE_Q(8); 239 | 240 | return SH2_OK; 241 | } 242 | 243 | static int decodeLinearAcceleration(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 244 | { 245 | value->un.linearAcceleration.x = read16(&event->report[4]) * SCALE_Q(8); 246 | value->un.linearAcceleration.y = read16(&event->report[6]) * SCALE_Q(8); 247 | value->un.linearAcceleration.z = read16(&event->report[8]) * SCALE_Q(8); 248 | 249 | return SH2_OK; 250 | } 251 | 252 | static int decodeGravity(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 253 | { 254 | value->un.gravity.x = read16(&event->report[4]) * SCALE_Q(8); 255 | value->un.gravity.y = read16(&event->report[6]) * SCALE_Q(8); 256 | value->un.gravity.z = read16(&event->report[8]) * SCALE_Q(8); 257 | 258 | return SH2_OK; 259 | } 260 | 261 | static int decodeRawGyroscope(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 262 | { 263 | value->un.rawGyroscope.x = read16(&event->report[4]); 264 | value->un.rawGyroscope.y = read16(&event->report[6]); 265 | value->un.rawGyroscope.z = read16(&event->report[8]); 266 | value->un.rawGyroscope.temperature = read16(&event->report[10]); 267 | value->un.rawGyroscope.timestamp = read32(&event->report[12]); 268 | 269 | return SH2_OK; 270 | } 271 | 272 | static int decodeGyroscopeCalibrated(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 273 | { 274 | value->un.gyroscope.x = read16(&event->report[4]) * SCALE_Q(9); 275 | value->un.gyroscope.y = read16(&event->report[6]) * SCALE_Q(9); 276 | value->un.gyroscope.z = read16(&event->report[8]) * SCALE_Q(9); 277 | 278 | return SH2_OK; 279 | } 280 | 281 | static int decodeGyroscopeUncal(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 282 | { 283 | value->un.gyroscopeUncal.x = read16(&event->report[4]) * SCALE_Q(9); 284 | value->un.gyroscopeUncal.y = read16(&event->report[6]) * SCALE_Q(9); 285 | value->un.gyroscopeUncal.z = read16(&event->report[8]) * SCALE_Q(9); 286 | 287 | value->un.gyroscopeUncal.biasX = read16(&event->report[10]) * SCALE_Q(9); 288 | value->un.gyroscopeUncal.biasY = read16(&event->report[12]) * SCALE_Q(9); 289 | value->un.gyroscopeUncal.biasZ = read16(&event->report[14]) * SCALE_Q(9); 290 | 291 | return SH2_OK; 292 | } 293 | 294 | static int decodeRawMagnetometer(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 295 | { 296 | value->un.rawMagnetometer.x = read16(&event->report[4]); 297 | value->un.rawMagnetometer.y = read16(&event->report[6]); 298 | value->un.rawMagnetometer.z = read16(&event->report[8]); 299 | value->un.rawMagnetometer.timestamp = read32(&event->report[12]); 300 | 301 | return SH2_OK; 302 | } 303 | 304 | static int decodeMagneticFieldCalibrated(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 305 | { 306 | value->un.magneticField.x = read16(&event->report[4]) * SCALE_Q(4); 307 | value->un.magneticField.y = read16(&event->report[6]) * SCALE_Q(4); 308 | value->un.magneticField.z = read16(&event->report[8]) * SCALE_Q(4); 309 | 310 | return SH2_OK; 311 | } 312 | 313 | static int decodeMagneticFieldUncal(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 314 | { 315 | value->un.magneticFieldUncal.x = read16(&event->report[4]) * SCALE_Q(4); 316 | value->un.magneticFieldUncal.y = read16(&event->report[6]) * SCALE_Q(4); 317 | value->un.magneticFieldUncal.z = read16(&event->report[8]) * SCALE_Q(4); 318 | 319 | value->un.magneticFieldUncal.biasX = read16(&event->report[10]) * SCALE_Q(4); 320 | value->un.magneticFieldUncal.biasY = read16(&event->report[12]) * SCALE_Q(4); 321 | value->un.magneticFieldUncal.biasZ = read16(&event->report[14]) * SCALE_Q(4); 322 | 323 | return SH2_OK; 324 | } 325 | 326 | static int decodeRotationVector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 327 | { 328 | value->un.rotationVector.i = read16(&event->report[4]) * SCALE_Q(14); 329 | value->un.rotationVector.j = read16(&event->report[6]) * SCALE_Q(14); 330 | value->un.rotationVector.k = read16(&event->report[8]) * SCALE_Q(14); 331 | value->un.rotationVector.real = read16(&event->report[10]) * SCALE_Q(14); 332 | value->un.rotationVector.accuracy = read16(&event->report[12]) * SCALE_Q(12); 333 | 334 | return SH2_OK; 335 | } 336 | 337 | static int decodeGameRotationVector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 338 | { 339 | value->un.gameRotationVector.i = read16(&event->report[4]) * SCALE_Q(14); 340 | value->un.gameRotationVector.j = read16(&event->report[6]) * SCALE_Q(14); 341 | value->un.gameRotationVector.k = read16(&event->report[8]) * SCALE_Q(14); 342 | value->un.gameRotationVector.real = read16(&event->report[10]) * SCALE_Q(14); 343 | 344 | return SH2_OK; 345 | } 346 | 347 | static int decodeGeomagneticRotationVector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 348 | { 349 | value->un.geoMagRotationVector.i = read16(&event->report[4]) * SCALE_Q(14); 350 | value->un.geoMagRotationVector.j = read16(&event->report[6]) * SCALE_Q(14); 351 | value->un.geoMagRotationVector.k = read16(&event->report[8]) * SCALE_Q(14); 352 | value->un.geoMagRotationVector.real = read16(&event->report[10]) * SCALE_Q(14); 353 | value->un.geoMagRotationVector.accuracy = read16(&event->report[12]) * SCALE_Q(12); 354 | 355 | return SH2_OK; 356 | } 357 | 358 | static int decodePressure(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 359 | { 360 | value->un.pressure.value = read32(&event->report[4]) * SCALE_Q(20); 361 | 362 | return SH2_OK; 363 | } 364 | 365 | static int decodeAmbientLight(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 366 | { 367 | value->un.ambientLight.value = read32(&event->report[4]) * SCALE_Q(8); 368 | 369 | return SH2_OK; 370 | } 371 | 372 | static int decodeHumidity(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 373 | { 374 | value->un.humidity.value = read16(&event->report[4]) * SCALE_Q(8); 375 | 376 | return SH2_OK; 377 | } 378 | 379 | static int decodeProximity(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 380 | { 381 | value->un.proximity.value = read16(&event->report[4]) * SCALE_Q(4); 382 | 383 | return SH2_OK; 384 | } 385 | 386 | static int decodeTemperature(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 387 | { 388 | value->un.temperature.value = read16(&event->report[4]) * SCALE_Q(7); 389 | 390 | return SH2_OK; 391 | } 392 | 393 | static int decodeReserved(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 394 | { 395 | value->un.reserved.tbd = read16(&event->report[4]) * SCALE_Q(7); 396 | 397 | return SH2_OK; 398 | } 399 | 400 | static int decodeTapDetector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 401 | { 402 | value->un.tapDetector.flags = event->report[4]; 403 | 404 | return SH2_OK; 405 | } 406 | 407 | static int decodeStepDetector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 408 | { 409 | value->un.stepDetector.latency = readu32(&event->report[4]); 410 | 411 | return SH2_OK; 412 | } 413 | 414 | static int decodeStepCounter(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 415 | { 416 | value->un.stepCounter.latency = readu32(&event->report[4]); 417 | value->un.stepCounter.steps = readu32(&event->report[8]); 418 | 419 | return SH2_OK; 420 | } 421 | 422 | static int decodeSignificantMotion(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 423 | { 424 | value->un.sigMotion.motion = readu16(&event->report[4]); 425 | 426 | return SH2_OK; 427 | } 428 | 429 | static int decodeStabilityClassifier(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 430 | { 431 | value->un.stabilityClassifier.classification = event->report[4]; 432 | 433 | return SH2_OK; 434 | } 435 | 436 | static int decodeShakeDetector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 437 | { 438 | value->un.shakeDetector.shake = readu16(&event->report[4]); 439 | 440 | return SH2_OK; 441 | } 442 | 443 | static int decodeFlipDetector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 444 | { 445 | value->un.flipDetector.flip = readu16(&event->report[4]); 446 | 447 | return SH2_OK; 448 | } 449 | 450 | static int decodePickupDetector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 451 | { 452 | value->un.pickupDetector.pickup = readu16(&event->report[4]); 453 | 454 | return SH2_OK; 455 | } 456 | 457 | static int decodeStabilityDetector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 458 | { 459 | value->un.stabilityDetector.stability = readu16(&event->report[4]); 460 | 461 | return SH2_OK; 462 | } 463 | 464 | static int decodePersonalActivityClassifier(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 465 | { 466 | value->un.personalActivityClassifier.page = event->report[4] & 0x7F; 467 | value->un.personalActivityClassifier.lastPage = ((event->report[4] & 0x80) != 0); 468 | value->un.personalActivityClassifier.mostLikelyState = event->report[5]; 469 | for (int n = 0; n < 10; n++) { 470 | value->un.personalActivityClassifier.confidence[n] = event->report[6+n]; 471 | } 472 | 473 | return SH2_OK; 474 | } 475 | 476 | static int decodeSleepDetector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 477 | { 478 | value->un.sleepDetector.sleepState = event->report[4]; 479 | 480 | return SH2_OK; 481 | } 482 | 483 | static int decodeTiltDetector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 484 | { 485 | value->un.tiltDetector.tilt = readu16(&event->report[4]); 486 | 487 | return SH2_OK; 488 | } 489 | 490 | static int decodePocketDetector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 491 | { 492 | value->un.pocketDetector.pocket = readu16(&event->report[4]); 493 | 494 | return SH2_OK; 495 | } 496 | 497 | static int decodeCircleDetector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 498 | { 499 | value->un.circleDetector.circle = readu16(&event->report[4]); 500 | 501 | return SH2_OK; 502 | } 503 | 504 | static int decodeHeartRateMonitor(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 505 | { 506 | value->un.heartRateMonitor.heartRate = readu16(&event->report[4]); 507 | 508 | return SH2_OK; 509 | } 510 | 511 | static int decodeArvrStabilizedRV(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 512 | { 513 | value->un.arvrStabilizedRV.i = read16(&event->report[4]) * SCALE_Q(14); 514 | value->un.arvrStabilizedRV.j = read16(&event->report[6]) * SCALE_Q(14); 515 | value->un.arvrStabilizedRV.k = read16(&event->report[8]) * SCALE_Q(14); 516 | value->un.arvrStabilizedRV.real = read16(&event->report[10]) * SCALE_Q(14); 517 | value->un.arvrStabilizedRV.accuracy = read16(&event->report[12]) * SCALE_Q(12); 518 | 519 | return SH2_OK; 520 | } 521 | 522 | static int decodeArvrStabilizedGRV(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 523 | { 524 | value->un.arvrStabilizedGRV.i = read16(&event->report[4]) * SCALE_Q(14); 525 | value->un.arvrStabilizedGRV.j = read16(&event->report[6]) * SCALE_Q(14); 526 | value->un.arvrStabilizedGRV.k = read16(&event->report[8]) * SCALE_Q(14); 527 | value->un.arvrStabilizedGRV.real = read16(&event->report[10]) * SCALE_Q(14); 528 | 529 | return SH2_OK; 530 | } 531 | 532 | static int decodeGyroIntegratedRV(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 533 | { 534 | value->un.gyroIntegratedRV.i = read16(&event->report[0]) * SCALE_Q(14); 535 | value->un.gyroIntegratedRV.j = read16(&event->report[2]) * SCALE_Q(14); 536 | value->un.gyroIntegratedRV.k = read16(&event->report[4]) * SCALE_Q(14); 537 | value->un.gyroIntegratedRV.real = read16(&event->report[6]) * SCALE_Q(14); 538 | value->un.gyroIntegratedRV.angVelX = read16(&event->report[8]) * SCALE_Q(10); 539 | value->un.gyroIntegratedRV.angVelY = read16(&event->report[10]) * SCALE_Q(10); 540 | value->un.gyroIntegratedRV.angVelZ = read16(&event->report[12]) * SCALE_Q(10); 541 | 542 | return SH2_OK; 543 | } 544 | 545 | static int decodeIZroRequest(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 546 | { 547 | value->un.izroRequest.intent = (sh2_IZroMotionIntent_t)event->report[4]; 548 | value->un.izroRequest.request = (sh2_IZroMotionRequest_t)event->report[5]; 549 | 550 | return SH2_OK; 551 | } 552 | -------------------------------------------------------------------------------- /src/sh2.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2015-2018 Hillcrest Laboratories, Inc. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License and 6 | * any applicable agreements you may have with Hillcrest Laboratories, Inc. 7 | * You may obtain a copy of the License at 8 | * 9 | * http://www.apache.org/licenses/LICENSE-2.0 10 | * 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | */ 17 | 18 | /** 19 | * @file sh2.h 20 | * @author David Wheeler 21 | * @date 22 Sept 2015 22 | * @brief API Definition for Hillcrest SH-2 Sensor Hub. 23 | * 24 | * The sh2 API provides an interface to the Hillcrest Labs sensor hub devices. 25 | */ 26 | 27 | #ifndef SH2_H 28 | #define SH2_H 29 | 30 | #ifdef __cplusplus 31 | extern "C" { 32 | #endif 33 | 34 | #include 35 | #include 36 | 37 | #include "sh2_hal.h" 38 | 39 | /*************************************************************************************** 40 | * Public type definitions 41 | ***************************************************************************************/ 42 | 43 | /** 44 | * @brief Sensor Event 45 | * 46 | * See the SH-2 Reference Manual for more detail. 47 | */ 48 | #define SH2_MAX_SENSOR_EVENT_LEN (16) 49 | typedef struct sh2_SensorEvent { 50 | uint64_t timestamp_uS; 51 | uint8_t len; 52 | uint8_t reportId; 53 | uint8_t report[SH2_MAX_SENSOR_EVENT_LEN]; 54 | } sh2_SensorEvent_t; 55 | 56 | typedef void (sh2_SensorCallback_t)(void * cookie, sh2_SensorEvent_t *pEvent); 57 | 58 | /** 59 | * @brief Product Id value 60 | * 61 | * See the SH-2 Reference Manual for more detail. 62 | */ 63 | typedef struct sh2_ProductId_s { 64 | uint8_t resetCause; 65 | uint8_t swVersionMajor; 66 | uint8_t swVersionMinor; 67 | uint32_t swPartNumber; 68 | uint32_t swBuildNumber; 69 | uint16_t swVersionPatch; 70 | uint8_t reserved0; 71 | uint8_t reserved1; 72 | } sh2_ProductId_t; 73 | 74 | #define SH2_MAX_PROD_ID_ENTRIES (5) 75 | typedef struct sh2_ProductIds_s { 76 | sh2_ProductId_t entry[SH2_MAX_PROD_ID_ENTRIES]; 77 | uint8_t numEntries; 78 | } sh2_ProductIds_t; 79 | 80 | /** 81 | * @brief List of sensor types supported by the hub 82 | * 83 | * See the SH-2 Reference Manual for more information on each type. 84 | */ 85 | enum sh2_SensorId_e { 86 | SH2_RAW_ACCELEROMETER = 0x14, 87 | SH2_ACCELEROMETER = 0x01, 88 | SH2_LINEAR_ACCELERATION = 0x04, 89 | SH2_GRAVITY = 0x06, 90 | SH2_RAW_GYROSCOPE = 0x15, 91 | SH2_GYROSCOPE_CALIBRATED = 0x02, 92 | SH2_GYROSCOPE_UNCALIBRATED = 0x07, 93 | SH2_RAW_MAGNETOMETER = 0x16, 94 | SH2_MAGNETIC_FIELD_CALIBRATED = 0x03, 95 | SH2_MAGNETIC_FIELD_UNCALIBRATED = 0x0f, 96 | SH2_ROTATION_VECTOR = 0x05, 97 | SH2_GAME_ROTATION_VECTOR = 0x08, 98 | SH2_GEOMAGNETIC_ROTATION_VECTOR = 0x09, 99 | SH2_PRESSURE = 0x0a, 100 | SH2_AMBIENT_LIGHT = 0x0b, 101 | SH2_HUMIDITY = 0x0c, 102 | SH2_PROXIMITY = 0x0d, 103 | SH2_TEMPERATURE = 0x0e, 104 | SH2_RESERVED = 0x17, 105 | SH2_TAP_DETECTOR = 0x10, 106 | SH2_STEP_DETECTOR = 0x18, 107 | SH2_STEP_COUNTER = 0x11, 108 | SH2_SIGNIFICANT_MOTION = 0x12, 109 | SH2_STABILITY_CLASSIFIER = 0x13, 110 | SH2_SHAKE_DETECTOR = 0x19, 111 | SH2_FLIP_DETECTOR = 0x1a, 112 | SH2_PICKUP_DETECTOR = 0x1b, 113 | SH2_STABILITY_DETECTOR = 0x1c, 114 | SH2_PERSONAL_ACTIVITY_CLASSIFIER = 0x1e, 115 | SH2_SLEEP_DETECTOR = 0x1f, 116 | SH2_TILT_DETECTOR = 0x20, 117 | SH2_POCKET_DETECTOR = 0x21, 118 | SH2_CIRCLE_DETECTOR = 0x22, 119 | SH2_HEART_RATE_MONITOR = 0x23, 120 | SH2_ARVR_STABILIZED_RV = 0x28, 121 | SH2_ARVR_STABILIZED_GRV = 0x29, 122 | SH2_GYRO_INTEGRATED_RV = 0x2A, 123 | SH2_IZRO_MOTION_REQUEST = 0x2B, 124 | 125 | // UPDATE to reflect greatest sensor id 126 | SH2_MAX_SENSOR_ID = 0x2B, 127 | }; 128 | typedef uint8_t sh2_SensorId_t; 129 | 130 | /** 131 | * @brief Sensor Configuration settings 132 | * 133 | * See the SH-2 Reference Manual for more detail. 134 | */ 135 | typedef struct sh2_SensorConfig { 136 | /* Change sensitivity enabled */ 137 | bool changeSensitivityEnabled; /**< @brief Enable reports on change */ 138 | 139 | /* Change sensitivity - true if relative; false if absolute */ 140 | bool changeSensitivityRelative; /**< @brief Change reports relative (vs absolute) */ 141 | 142 | /* Wake-up enabled */ 143 | bool wakeupEnabled; /**< @brief Wake host on event */ 144 | 145 | /* Always on enabled */ 146 | bool alwaysOnEnabled; /**< @brief Sensor remains on in sleep state */ 147 | /* 16-bit signed fixed point integer representing the value a 148 | * sensor output must exceed in order to trigger another input 149 | * report. A setting of 0 causes all reports to be sent. 150 | */ 151 | uint16_t changeSensitivity; /**< @brief Report-on-change threshold */ 152 | 153 | /* Interval in microseconds between asynchronous input reports. */ 154 | uint32_t reportInterval_us; /**< @brief [uS] Report interval */ 155 | 156 | /* Reserved field, not used. */ 157 | uint32_t batchInterval_us; /**< @brief [uS] Batch interval */ 158 | 159 | /* Meaning is sensor specific */ 160 | uint32_t sensorSpecific; /**< @brief See SH-2 Reference Manual for details. */ 161 | } sh2_SensorConfig_t; 162 | 163 | /** 164 | * @brief Sensor Metadata Record 165 | * 166 | * See the SH-2 Reference Manual for more detail. 167 | */ 168 | typedef struct sh2_SensorMetadata { 169 | uint8_t meVersion; /**< @brief Motion Engine Version */ 170 | uint8_t mhVersion; /**< @brief Motion Hub Version */ 171 | uint8_t shVersion; /**< @brief SensorHub Version */ 172 | uint32_t range; /**< @brief Same units as sensor reports */ 173 | uint32_t resolution; /**< @brief Same units as sensor reports */ 174 | uint16_t revision; /**< @brief Metadata record format revision */ 175 | uint16_t power_mA; /**< @brief [mA] Fixed point 16Q10 format */ 176 | uint32_t minPeriod_uS; /**< @brief [uS] */ 177 | uint32_t maxPeriod_uS; /**< @brief [uS] */ 178 | uint32_t fifoReserved; /**< @brief (Unused) */ 179 | uint32_t fifoMax; /**< @brief (Unused) */ 180 | uint32_t batchBufferBytes; /**< @brief (Unused) */ 181 | uint16_t qPoint1; /**< @brief q point for sensor values */ 182 | uint16_t qPoint2; /**< @brief q point for accuracy or bias fields */ 183 | uint16_t qPoint3; /**< @brief q point for sensor data change sensitivity */ 184 | uint32_t vendorIdLen; /**< @brief [bytes] */ 185 | char vendorId[48]; /**< @brief Vendor name and part number */ 186 | uint32_t sensorSpecificLen; /**< @brief [bytes] */ 187 | uint8_t sensorSpecific[48]; /**< @brief See SH-2 Reference Manual */ 188 | } sh2_SensorMetadata_t; 189 | 190 | /** 191 | * @brief SensorHub Error Record 192 | * 193 | * See the SH-2 Reference Manual for more detail. 194 | */ 195 | typedef struct sh2_ErrorRecord { 196 | uint8_t severity; /**< @brief Error severity, 0: most severe. */ 197 | uint8_t sequence; /**< @brief Sequence number (by severity) */ 198 | uint8_t source; /**< @brief 1-MotionEngine, 2-MotionHub, 3-SensorHub, 4-Chip */ 199 | uint8_t error; /**< @brief See SH-2 Reference Manual */ 200 | uint8_t module; /**< @brief See SH-2 Reference Manual */ 201 | uint8_t code; /**< @brief See SH-2 Reference Manual */ 202 | } sh2_ErrorRecord_t; 203 | 204 | /** 205 | * @brief SensorHub Counter Record 206 | * 207 | * See the SH-2 Reference Manual for more detail. 208 | */ 209 | typedef struct sh2_Counts { 210 | uint32_t offered; /**< @brief [events] */ 211 | uint32_t accepted; /**< @brief [events] */ 212 | uint32_t on; /**< @brief [events] */ 213 | uint32_t attempted; /**< @brief [events] */ 214 | } sh2_Counts_t; 215 | 216 | /** 217 | * @brief Values for specifying tare basis 218 | * 219 | * See the SH-2 Reference Manual for more detail. 220 | */ 221 | typedef enum sh2_TareBasis { 222 | SH2_TARE_BASIS_ROTATION_VECTOR = 0, /**< @brief Use Rotation Vector */ 223 | SH2_TARE_BASIS_GAMING_ROTATION_VECTOR = 1, /**< @brief Use Game Rotation Vector */ 224 | SH2_TARE_BASIS_GEOMAGNETIC_ROTATION_VECTOR = 2, /**< @brief Use Geomagnetic R.V. */ 225 | } sh2_TareBasis_t; 226 | 227 | /** 228 | * @brief Bit Fields for specifying tare axes. 229 | * 230 | * See the SH-2 Reference Manual for more detail. 231 | */ 232 | typedef enum sh2_TareAxis { 233 | SH2_TARE_X = 1, /**< @brief sh2_tareNow() axes bit field */ 234 | SH2_TARE_Y = 2, /**< @brief sh2_tareNow() axes bit field */ 235 | SH2_TARE_Z = 4, /**< @brief sh2_tareNow() axes bit field */ 236 | } sh2_TareAxis_t; 237 | 238 | /** 239 | * @brief Quaternion (double precision floating point representation.) 240 | * 241 | * See the SH-2 Reference Manual for more detail. 242 | */ 243 | typedef struct sh2_Quaternion { 244 | double x; 245 | double y; 246 | double z; 247 | double w; 248 | } sh2_Quaternion_t; 249 | 250 | /** 251 | * @brief Oscillator type: Internal or External 252 | * 253 | * See the SH-2 Reference Manual for more detail. 254 | */ 255 | typedef enum { 256 | SH2_OSC_INTERNAL = 0, 257 | SH2_OSC_EXT_CRYSTAL = 1, 258 | SH2_OSC_EXT_CLOCK = 2, 259 | } sh2_OscType_t; 260 | 261 | /** 262 | * @brief Calibration result 263 | * 264 | * See the SH-2 Reference Manual, Finish Calibration Response. 265 | */ 266 | typedef enum { 267 | SH2_CAL_SUCCESS = 0, 268 | SH2_CAL_NO_ZRO, 269 | SH2_CAL_NO_STATIONARY_DETECTION, 270 | SH2_CAL_ROTATION_OUTSIDE_SPEC, 271 | SH2_CAL_ZRO_OUTSIDE_SPEC, 272 | SH2_CAL_ZGO_OUTSIDE_SPEC, 273 | SH2_CAL_GYRO_GAIN_OUTSIDE_SPEC, 274 | SH2_CAL_GYRO_PERIOD_OUTSIDE_SPEC, 275 | SH2_CAL_GYRO_DROPS_OUTSIDE_SPEC, 276 | } sh2_CalStatus_t; 277 | 278 | // FRS Record Ids 279 | #define STATIC_CALIBRATION_AGM (0x7979) 280 | #define NOMINAL_CALIBRATION (0x4D4D) 281 | #define STATIC_CALIBRATION_SRA (0x8A8A) 282 | #define NOMINAL_CALIBRATION_SRA (0x4E4E) 283 | #define DYNAMIC_CALIBRATION (0x1F1F) 284 | #define ME_POWER_MGMT (0xD3E2) 285 | #define SYSTEM_ORIENTATION (0x2D3E) 286 | #define ACCEL_ORIENTATION (0x2D41) 287 | #define SCREEN_ACCEL_ORIENTATION (0x2D43) 288 | #define GYROSCOPE_ORIENTATION (0x2D46) 289 | #define MAGNETOMETER_ORIENTATION (0x2D4C) 290 | #define ARVR_STABILIZATION_RV (0x3E2D) 291 | #define ARVR_STABILIZATION_GRV (0x3E2E) 292 | #define TAP_DETECT_CONFIG (0xC269) 293 | #define SIG_MOTION_DETECT_CONFIG (0xC274) 294 | #define SHAKE_DETECT_CONFIG (0x7D7D) 295 | #define MAX_FUSION_PERIOD (0xD7D7) 296 | #define SERIAL_NUMBER (0x4B4B) 297 | #define ES_PRESSURE_CAL (0x39AF) 298 | #define ES_TEMPERATURE_CAL (0x4D20) 299 | #define ES_HUMIDITY_CAL (0x1AC9) 300 | #define ES_AMBIENT_LIGHT_CAL (0x39B1) 301 | #define ES_PROXIMITY_CAL (0x4DA2) 302 | #define ALS_CAL (0xD401) 303 | #define PROXIMITY_SENSOR_CAL (0xD402) 304 | #define PICKUP_DETECTOR_CONFIG (0x1B2A) 305 | #define FLIP_DETECTOR_CONFIG (0xFC94) 306 | #define STABILITY_DETECTOR_CONFIG (0xED85) 307 | #define ACTIVITY_TRACKER_CONFIG (0xED88) 308 | #define SLEEP_DETECTOR_CONFIG (0xED87) 309 | #define TILT_DETECTOR_CONFIG (0xED89) 310 | #define POCKET_DETECTOR_CONFIG (0xEF27) 311 | #define CIRCLE_DETECTOR_CONFIG (0xEE51) 312 | #define USER_RECORD (0x74B4) 313 | #define ME_TIME_SOURCE_SELECT (0xD403) 314 | #define UART_FORMAT (0xA1A1) 315 | #define GYRO_INTEGRATED_RV_CONFIG (0xA1A2) 316 | #define FRS_ID_META_RAW_ACCELEROMETER (0xE301) 317 | #define FRS_ID_META_ACCELEROMETER (0xE302) 318 | #define FRS_ID_META_LINEAR_ACCELERATION (0xE303) 319 | #define FRS_ID_META_GRAVITY (0xE304) 320 | #define FRS_ID_META_RAW_GYROSCOPE (0xE305) 321 | #define FRS_ID_META_GYROSCOPE_CALIBRATED (0xE306) 322 | #define FRS_ID_META_GYROSCOPE_UNCALIBRATED (0xE307) 323 | #define FRS_ID_META_RAW_MAGNETOMETER (0xE308) 324 | #define FRS_ID_META_MAGNETIC_FIELD_CALIBRATED (0xE309) 325 | #define FRS_ID_META_MAGNETIC_FIELD_UNCALIBRATED (0xE30A) 326 | #define FRS_ID_META_ROTATION_VECTOR (0xE30B) 327 | #define FRS_ID_META_GAME_ROTATION_VECTOR (0xE30C) 328 | #define FRS_ID_META_GEOMAGNETIC_ROTATION_VECTOR (0xE30D) 329 | #define FRS_ID_META_PRESSURE (0xE30E) 330 | #define FRS_ID_META_AMBIENT_LIGHT (0xE30F) 331 | #define FRS_ID_META_HUMIDITY (0xE310) 332 | #define FRS_ID_META_PROXIMITY (0xE311) 333 | #define FRS_ID_META_TEMPERATURE (0xE312) 334 | #define FRS_ID_META_TAP_DETECTOR (0xE313) 335 | #define FRS_ID_META_STEP_DETECTOR (0xE314) 336 | #define FRS_ID_META_STEP_COUNTER (0xE315) 337 | #define FRS_ID_META_SIGNIFICANT_MOTION (0xE316) 338 | #define FRS_ID_META_STABILITY_CLASSIFIER (0xE317) 339 | #define FRS_ID_META_SHAKE_DETECTOR (0xE318) 340 | #define FRS_ID_META_FLIP_DETECTOR (0xE319) 341 | #define FRS_ID_META_PICKUP_DETECTOR (0xE31A) 342 | #define FRS_ID_META_STABILITY_DETECTOR (0xE31B) 343 | #define FRS_ID_META_PERSONAL_ACTIVITY_CLASSIFIER (0xE31C) 344 | #define FRS_ID_META_SLEEP_DETECTOR (0xE31D) 345 | #define FRS_ID_META_TILT_DETECTOR (0xE31E) 346 | #define FRS_ID_META_POCKET_DETECTOR (0xE31F) 347 | #define FRS_ID_META_CIRCLE_DETECTOR (0xE320) 348 | #define FRS_ID_META_HEART_RATE_MONITOR (0xE321) 349 | #define FRS_ID_META_ARVR_STABILIZED_RV (0xE322) 350 | #define FRS_ID_META_ARVR_STABILIZED_GRV (0xE323) 351 | #define FRS_ID_META_GYRO_INTEGRATED_RV (0xE324) 352 | 353 | /** 354 | * @brief Interactive ZRO Motion Intent 355 | * 356 | * See the SH-2 Reference Manual, 6.4.13 357 | */ 358 | typedef enum { 359 | SH2_IZRO_MI_UNKNOWN = 0, 360 | SH2_IZRO_MI_STATIONARY_NO_VIBRATION, 361 | SH2_IZRO_MI_STATIONARY_WITH_VIBRATION, 362 | SH2_IZRO_MI_IN_MOTION, 363 | } sh2_IZroMotionIntent_t; 364 | 365 | /** 366 | * @brief Interactive ZRO Motion Intent 367 | * 368 | * See the SH-2 Reference Manual, 6.4.13 369 | */ 370 | typedef enum { 371 | SH2_IZRO_MR_NO_REQUEST = 0, 372 | SH2_IZRO_MR_STAY_STATIONARY, 373 | SH2_IZRO_MR_STATIONARY_NON_URGENT, 374 | SH2_IZRO_MR_STATIONARY_URGENT, 375 | } sh2_IZroMotionRequest_t; 376 | 377 | 378 | /** 379 | * @brief Asynchronous Event 380 | * 381 | * Represents reset events and other non-sensor events received from SH-2 sensor hub. 382 | */ 383 | 384 | enum sh2_AsyncEventId_e { 385 | SH2_RESET, 386 | SH2_SHTP_EVENT, 387 | SH2_GET_FEATURE_RESP, 388 | }; 389 | typedef enum sh2_AsyncEventId_e sh2_AsyncEventId_t; 390 | 391 | enum sh2_ShtpEvent_e { 392 | SH2_SHTP_TX_DISCARD = 0, 393 | SH2_SHTP_SHORT_FRAGMENT = 1, 394 | SH2_SHTP_TOO_LARGE_PAYLOADS = 2, 395 | SH2_SHTP_BAD_RX_CHAN = 3, 396 | SH2_SHTP_BAD_TX_CHAN = 4, 397 | }; 398 | typedef uint8_t sh2_ShtpEvent_t; 399 | 400 | typedef struct sh2_SensorConfigResp_e { 401 | sh2_SensorId_t sensorId; 402 | sh2_SensorConfig_t sensorConfig; 403 | } sh2_SensorConfigResp_t; 404 | 405 | typedef struct sh2_AsyncEvent { 406 | uint32_t eventId; 407 | union { 408 | sh2_ShtpEvent_t shtpEvent; 409 | sh2_SensorConfigResp_t sh2SensorConfigResp; 410 | }; 411 | } sh2_AsyncEvent_t; 412 | 413 | typedef void (sh2_EventCallback_t)(void * cookie, sh2_AsyncEvent_t *pEvent); 414 | 415 | 416 | /*************************************************************************************** 417 | * Public API 418 | **************************************************************************************/ 419 | 420 | /** 421 | * @brief Open a session with a sensor hub. 422 | * 423 | * This function should be called before others in this API. 424 | * An instance of an SH2 HAL should be passed in. 425 | * This call will result in the open() function of the HAL being called. 426 | * 427 | * As part of the initialization process, a callback function is registered that will 428 | * be invoked when the device generates certain events. (See sh2_AsyncEventId) 429 | * 430 | * @param pHal Pointer to an SH2 HAL instance, provided by the target system. 431 | * @param eventCallback Will be called when events, such as reset complete, occur. 432 | * @param eventCookie Will be passed to eventCallback. 433 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 434 | */ 435 | int sh2_open(sh2_Hal_t *pHal, 436 | sh2_EventCallback_t *eventCallback, void *eventCookie); 437 | 438 | /** 439 | * @brief Close a session with a sensor hub. 440 | * 441 | * This should be called at the end of a sensor hub session. 442 | * The underlying SHTP and HAL instances will be closed. 443 | * 444 | */ 445 | void sh2_close(void); 446 | 447 | /** 448 | * @brief Service the SH2 device, reading any data that is available and dispatching callbacks. 449 | * 450 | * This function should be called periodically by the host system to service an open sensor hub. 451 | * 452 | */ 453 | void sh2_service(void); 454 | 455 | /** 456 | * @brief Register a function to receive sensor events. 457 | * 458 | * @param callback A function that will be called each time a sensor event is received. 459 | * @param cookie A value that will be passed to the sensor callback function. 460 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 461 | */ 462 | int sh2_setSensorCallback(sh2_SensorCallback_t *callback, void *cookie); 463 | 464 | /** 465 | * @brief Reset the sensor hub device by sending RESET (1) command on "device" channel. 466 | * 467 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 468 | */ 469 | int sh2_devReset(void); 470 | 471 | /** 472 | * @brief Turn sensor hub on by sending ON (2) command on "device" channel. 473 | * 474 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 475 | */ 476 | int sh2_devOn(void); 477 | 478 | /** 479 | * @brief Put sensor hub in sleep state by sending SLEEP (3) command on "device" channel. 480 | * 481 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 482 | */ 483 | int sh2_devSleep(void); 484 | 485 | /** 486 | * @brief Get Product ID information from Sensorhub. 487 | * 488 | * @param prodIds Pointer to structure that will receive results. 489 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 490 | */ 491 | int sh2_getProdIds(sh2_ProductIds_t *prodIds); 492 | 493 | /** 494 | * @brief Get sensor configuration. 495 | * 496 | * @param sensorId Which sensor to query. 497 | * @param config SensorConfig structure to store results. 498 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 499 | */ 500 | int sh2_getSensorConfig(sh2_SensorId_t sensorId, sh2_SensorConfig_t *config); 501 | 502 | /** 503 | * @brief Set sensor configuration. (e.g enable a sensor at a particular rate.) 504 | * 505 | * @param sensorId Which sensor to configure. 506 | * @param pConfig Pointer to structure holding sensor configuration. 507 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 508 | */ 509 | int sh2_setSensorConfig(sh2_SensorId_t sensorId, const sh2_SensorConfig_t *pConfig); 510 | 511 | /** 512 | * @brief Get metadata related to a sensor. 513 | * 514 | * @param sensorId Which sensor to query. 515 | * @param pData Pointer to structure to receive the results. 516 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 517 | */ 518 | int sh2_getMetadata(sh2_SensorId_t sensorId, sh2_SensorMetadata_t *pData); 519 | 520 | /** 521 | * @brief Get an FRS record. 522 | * 523 | * @param recordId Which FRS Record to retrieve. 524 | * @param pData pointer to buffer to receive the results 525 | * @param[in] words Size of pData buffer, in 32-bit words. 526 | * @param[out] words Number of 32-bit words retrieved. 527 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 528 | */ 529 | int sh2_getFrs(uint16_t recordId, uint32_t *pData, uint16_t *words); 530 | 531 | /** 532 | * @brief Set an FRS record 533 | * 534 | * @param recordId Which FRS Record to set. 535 | * @param pData pointer to buffer containing the new data. 536 | * @param words number of 32-bit words to write. (0 to delete record.) 537 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 538 | */ 539 | int sh2_setFrs(uint16_t recordId, uint32_t *pData, uint16_t words); 540 | 541 | /** 542 | * @brief Get error counts. 543 | * 544 | * @param severity Only errors of this severity or greater are returned. 545 | * @param pErrors Buffer to receive error codes. 546 | * @param numErrors size of pErrors array 547 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 548 | */ 549 | int sh2_getErrors(uint8_t severity, sh2_ErrorRecord_t *pErrors, uint16_t *numErrors); 550 | 551 | /** 552 | * @brief Read counters related to a sensor. 553 | * 554 | * @param sensorId Which sensor to operate on. 555 | * @param pCounts Pointer to Counts structure that will receive data. 556 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 557 | */ 558 | int sh2_getCounts(sh2_SensorId_t sensorId, sh2_Counts_t *pCounts); 559 | 560 | /** 561 | * @brief Clear counters related to a sensor. 562 | * 563 | * @param sensorId which sensor to operate on. 564 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 565 | */ 566 | int sh2_clearCounts(sh2_SensorId_t sensorId); 567 | 568 | /** 569 | * @brief Perform a tare operation on one or more axes. 570 | * 571 | * @param axes Bit mask specifying which axes should be tared. 572 | * @param basis Which rotation vector to use as the basis for Tare adjustment. 573 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 574 | */ 575 | int sh2_setTareNow(uint8_t axes, // SH2_TARE_X | SH2_TARE_Y | SH2_TARE_Z 576 | sh2_TareBasis_t basis); 577 | 578 | /** 579 | * @brief Clears the previously applied tare operation. 580 | * 581 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 582 | */ 583 | int sh2_clearTare(void); 584 | 585 | /** 586 | * @brief Persist the results of last tare operation to flash. 587 | * 588 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 589 | */ 590 | int sh2_persistTare(void); 591 | 592 | /** 593 | * @brief Set the current run-time sensor reorientation. (Set to zero to clear tare.) 594 | * 595 | * @param orientation Quaternion rotation vector to apply as new tare. 596 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 597 | */ 598 | int sh2_setReorientation(sh2_Quaternion_t *orientation); 599 | 600 | /** 601 | * @brief Command the sensorhub to reset. 602 | * 603 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 604 | */ 605 | int sh2_reinitialize(void); 606 | 607 | /** 608 | * @brief Save Dynamic Calibration Data to flash. 609 | * 610 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 611 | */ 612 | int sh2_saveDcdNow(void); 613 | 614 | /** 615 | * @brief Get Oscillator type. 616 | * 617 | * @param pOscType pointer to data structure to receive results. 618 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 619 | */ 620 | int sh2_getOscType(sh2_OscType_t *pOscType); 621 | 622 | // Flags for sensors field of sh_calConfig 623 | #define SH2_CAL_ACCEL (0x01) 624 | #define SH2_CAL_GYRO (0x02) 625 | #define SH2_CAL_MAG (0x04) 626 | #define SH2_CAL_PLANAR (0x08) 627 | 628 | /** 629 | * @brief Enable/Disable dynamic calibration for certain sensors 630 | * 631 | * @param sensors Bit mask to configure which sensors are affected. 632 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 633 | */ 634 | int sh2_setCalConfig(uint8_t sensors); 635 | 636 | /** 637 | * @brief Get dynamic calibration configuration settings. 638 | * 639 | * @param pSensors pointer to Bit mask, set on return. 640 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 641 | */ 642 | int sh2_getCalConfig(uint8_t *pSensors); 643 | 644 | /** 645 | * @brief Configure automatic saving of dynamic calibration data. 646 | * 647 | * @param enabled Enable or Disable DCD auto-save. 648 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 649 | */ 650 | int sh2_setDcdAutoSave(bool enabled); 651 | 652 | /** 653 | * @brief Immediately issue all buffered sensor reports from a given sensor. 654 | * 655 | * @param sensorId Which sensor reports to flush. 656 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 657 | */ 658 | int sh2_flush(sh2_SensorId_t sensorId); 659 | 660 | /** 661 | * @brief Command clear DCD in RAM, then reset sensor hub. 662 | * 663 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 664 | */ 665 | int sh2_clearDcdAndReset(void); 666 | 667 | /** 668 | * @brief Start simple self-calibration procedure. 669 | * 670 | * @parameter interval_us sensor report interval, uS. 671 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 672 | */ 673 | int sh2_startCal(uint32_t interval_us); 674 | 675 | /** 676 | * @brief Finish simple self-calibration procedure. 677 | * 678 | * @parameter status contains calibration status code on return. 679 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 680 | */ 681 | int sh2_finishCal(sh2_CalStatus_t *status); 682 | 683 | /** 684 | * @brief send Interactive ZRO Request. 685 | * 686 | * @parameter intent Inform the sensor hub what sort of motion should be in progress. 687 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 688 | */ 689 | int sh2_setIZro(sh2_IZroMotionIntent_t intent); 690 | 691 | #ifdef __cplusplus 692 | } // extern "C" 693 | #endif 694 | 695 | #endif 696 | -------------------------------------------------------------------------------- /src/shtp.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2015-18 Hillcrest Laboratories, Inc. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License and 6 | * any applicable agreements you may have with Hillcrest Laboratories, Inc. 7 | * You may obtain a copy of the License at 8 | * 9 | * http://www.apache.org/licenses/LICENSE-2.0 10 | * 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | */ 17 | 18 | /* 19 | * Hillcrest Sensor Hub Transport Protocol (SHTP) API 20 | */ 21 | 22 | #include "shtp.h" 23 | #include "sh2_err.h" 24 | #include "sh2_util.h" 25 | 26 | #include 27 | 28 | // ------------------------------------------------------------------------ 29 | // Private types 30 | 31 | #define SH2_MAX_APPS (5) 32 | #define SHTP_APP_NAME_LEN (32) 33 | #define SH2_MAX_CHANS (8) 34 | #define SHTP_CHAN_NAME_LEN (32) 35 | 36 | // Defined Globally Unique Identifiers 37 | #define GUID_SHTP (0) 38 | 39 | // Command Channel commands and parameters 40 | #define SHTP_CHAN_COMMAND (0) 41 | #define CMD_ADVERTISE (0) 42 | #define CMD_ADVERTISE_SHTP (0) 43 | #define CMD_ADVERTISE_ALL (1) 44 | #define RESP_ADVERTISE (0) 45 | 46 | #define SHTP_HDR_LEN (4) 47 | 48 | #define TAG_SHTP_VERSION 0x80 49 | 50 | typedef struct shtp_App_s { 51 | uint32_t guid; 52 | char appName[SHTP_APP_NAME_LEN]; 53 | } shtp_App_t; 54 | 55 | typedef struct shtp_AppListener_s { 56 | uint16_t guid; 57 | shtp_AdvertCallback_t *callback; 58 | void *cookie; 59 | } shtp_AppListener_t; 60 | 61 | typedef struct shtp_ChanListener_s { 62 | uint16_t guid; 63 | char chanName[SHTP_CHAN_NAME_LEN]; 64 | shtp_Callback_t *callback; 65 | void *cookie; 66 | } shtp_ChanListener_t; 67 | 68 | typedef struct shtp_Channel_s { 69 | uint8_t nextOutSeq; 70 | uint8_t nextInSeq; 71 | uint32_t guid; // app id 72 | char chanName[SHTP_CHAN_NAME_LEN]; 73 | bool wake; 74 | shtp_Callback_t *callback; 75 | void *cookie; 76 | } shtp_Channel_t; 77 | 78 | typedef enum { 79 | ADVERT_NEEDED, 80 | ADVERT_REQUESTED, 81 | ADVERT_IDLE, 82 | } advert_phase_t; 83 | 84 | // Per-instance data for SHTP 85 | typedef struct shtp_s { 86 | // Associated SHTP HAL 87 | // If 0, this indicates the SHTP instance is available for new opens 88 | sh2_Hal_t *pHal; 89 | 90 | // Asynchronous Event callback and it's cookie 91 | shtp_EventCallback_t *eventCallback; 92 | void * eventCookie; 93 | 94 | // Data from adverts 95 | char shtpVersion[8]; 96 | uint16_t outMaxPayload; 97 | uint16_t outMaxTransfer; 98 | 99 | // Transmit support 100 | uint8_t outTransfer[SH2_HAL_MAX_TRANSFER_OUT]; 101 | 102 | // Receive support 103 | uint16_t inMaxTransfer; 104 | uint16_t inRemaining; 105 | uint8_t inChan; 106 | uint8_t inPayload[SH2_HAL_MAX_PAYLOAD_IN]; 107 | uint16_t inCursor; 108 | uint32_t inTimestamp; 109 | uint8_t inTransfer[SH2_HAL_MAX_TRANSFER_IN]; 110 | 111 | // What stage of advertisement processing are we in. 112 | advert_phase_t advertPhase; 113 | 114 | // Applications 115 | shtp_App_t app[SH2_MAX_APPS]; 116 | uint8_t nextApp; 117 | 118 | // Advert registrations 119 | uint8_t nextAppListener; 120 | shtp_AppListener_t appListener[SH2_MAX_APPS]; 121 | 122 | // SHTP Channels 123 | shtp_Channel_t chan[SH2_MAX_CHANS]; 124 | 125 | // Channel listeners 126 | shtp_ChanListener_t chanListener[SH2_MAX_CHANS]; 127 | uint8_t nextChanListener; 128 | 129 | // Stats 130 | uint32_t txDiscards; 131 | uint32_t shortFragments; 132 | uint32_t tooLargePayloads; 133 | uint32_t badRxChan; 134 | uint32_t badTxChan; 135 | 136 | } shtp_t; 137 | 138 | 139 | // ------------------------------------------------------------------------ 140 | // Private data 141 | 142 | // Advertisement request 143 | static const uint8_t advertise[] = { 144 | CMD_ADVERTISE, 145 | CMD_ADVERTISE_ALL 146 | }; 147 | 148 | #define MAX_INSTANCES (1) 149 | static shtp_t instances[MAX_INSTANCES]; 150 | 151 | static bool shtp_initialized = false; 152 | 153 | // ------------------------------------------------------------------------ 154 | // Private functions 155 | 156 | static void shtp_init(void) 157 | { 158 | // clear instance memory. 159 | // In particular, this clears the pHal pointers which are used 160 | // to determine if an instance is open and in-use. 161 | memset(instances, 0, sizeof(instances)); 162 | 163 | // Set the initialized flag so this doesn't happen again. 164 | shtp_initialized = true; 165 | } 166 | 167 | static shtp_t *getInstance(void) 168 | { 169 | for (int n = 0; n < MAX_INSTANCES; n++) { 170 | if (instances[n].pHal == 0) { 171 | // This instance is free 172 | return &instances[n]; 173 | } 174 | } 175 | 176 | // Can't give an instance, none are free 177 | return 0; 178 | } 179 | 180 | // Register a listener for an app (advertisement listener) 181 | static void addAdvertListener(shtp_t *pShtp, uint16_t guid, 182 | shtp_AdvertCallback_t *callback, void * cookie) 183 | { 184 | shtp_AppListener_t *pAppListener = 0; 185 | 186 | // Bail out if no space for more apps 187 | if (pShtp->nextAppListener >= SH2_MAX_APPS) return; 188 | 189 | // Register this app 190 | pAppListener = &pShtp->appListener[pShtp->nextAppListener]; 191 | pShtp->nextAppListener++; 192 | pAppListener->guid = guid; 193 | pAppListener->callback = callback; 194 | pAppListener->cookie = cookie; 195 | } 196 | 197 | // Try to match registered listeners with their channels. 198 | // This is performed every time the underlying Channel, App, Listener data structures are updated. 199 | // As a result, channel number to callback association is fast when receiving packets 200 | static void updateCallbacks(shtp_t *pShtp) 201 | { 202 | // Figure out which callback is associated with each channel. 203 | // Channel -> (GUID, Chan name). 204 | // GUID -> App name. 205 | // (App name, Chan name) -> Callback 206 | 207 | uint32_t guid; 208 | const char * chanName = 0; 209 | 210 | for (int chanNo = 0; chanNo < SH2_MAX_CHANS; chanNo++) { 211 | // Reset callback for this channel until we find the right one. 212 | pShtp->chan[chanNo].callback = 0; 213 | 214 | if (pShtp->chan[chanNo].guid == 0xFFFFFFFF) { 215 | // This channel entry not used. 216 | continue; 217 | } 218 | 219 | // Get GUID and Channel Name for this channel 220 | guid = pShtp->chan[chanNo].guid; 221 | chanName = pShtp->chan[chanNo].chanName; 222 | 223 | // Look for a listener registered with this guid, channel name 224 | for (int listenerNo = 0; listenerNo < SH2_MAX_CHANS; listenerNo++) 225 | { 226 | if ((pShtp->chanListener[listenerNo].callback != 0) && 227 | (pShtp->chanListener[listenerNo].guid == guid) && 228 | (strcmp(chanName, pShtp->chanListener[listenerNo].chanName) == 0)) 229 | { 230 | 231 | // This listener is the one for this channel 232 | pShtp->chan[chanNo].callback = pShtp->chanListener[listenerNo].callback; 233 | pShtp->chan[chanNo].cookie = pShtp->chanListener[listenerNo].cookie; 234 | break; 235 | } 236 | } 237 | } 238 | } 239 | 240 | // Register a new channel listener 241 | static int addChanListener(shtp_t *pShtp, 242 | uint16_t guid, const char * chanName, 243 | shtp_Callback_t *callback, void *cookie) 244 | { 245 | shtp_ChanListener_t *pListener = 0; 246 | 247 | // Bail out if there are too many listeners registered 248 | if (pShtp->nextChanListener >= SH2_MAX_CHANS) return SH2_ERR; 249 | 250 | // Register channel listener 251 | pListener = &pShtp->chanListener[pShtp->nextChanListener]; 252 | pShtp->nextChanListener++; 253 | pListener->guid = guid; 254 | strcpy(pListener->chanName, chanName); 255 | pListener->callback = callback; 256 | pListener->cookie = cookie; 257 | 258 | // re-evaluate channel callbacks 259 | updateCallbacks(pShtp); 260 | 261 | return SH2_OK; 262 | } 263 | 264 | static inline uint16_t min_u16(uint16_t a, uint16_t b) 265 | { 266 | if (a < b) { 267 | return a; 268 | } 269 | else { 270 | return b; 271 | } 272 | } 273 | 274 | // Send a cargo as a sequence of transports 275 | static int txProcess(shtp_t *pShtp, uint8_t chan, const uint8_t* pData, uint32_t len) 276 | { 277 | int status = SH2_OK; 278 | 279 | bool continuation = false; 280 | uint16_t cursor = 0; 281 | uint16_t remaining; 282 | uint16_t transferLen; // length of transfer, minus the header 283 | uint16_t lenField; 284 | 285 | cursor = 0; 286 | remaining = len; 287 | while (remaining > 0) { 288 | // How much data (not header) can we send in next transfer 289 | transferLen = min_u16(remaining, pShtp->outMaxTransfer-SHTP_HDR_LEN); 290 | 291 | // Length field will be transferLen + SHTP_HDR_LEN 292 | lenField = transferLen + SHTP_HDR_LEN; 293 | 294 | // Put the header in the out buffer 295 | pShtp->outTransfer[0] = lenField & 0xFF; 296 | pShtp->outTransfer[1] = (lenField >> 8) & 0xFF; 297 | if (continuation) { 298 | pShtp->outTransfer[1] |= 0x80; 299 | } 300 | pShtp->outTransfer[2] = chan; 301 | pShtp->outTransfer[3] = pShtp->chan[chan].nextOutSeq++; 302 | 303 | // Stage one tranfer in the out buffer 304 | memcpy(pShtp->outTransfer+SHTP_HDR_LEN, pData+cursor, transferLen); 305 | remaining -= transferLen; 306 | cursor += transferLen; 307 | 308 | // Transmit (try repeatedly while HAL write returns 0) 309 | status = pShtp->pHal->write(pShtp->pHal, pShtp->outTransfer, lenField); 310 | while (status == 0) 311 | { 312 | shtp_service(pShtp); 313 | status = pShtp->pHal->write(pShtp->pHal, pShtp->outTransfer, lenField); 314 | } 315 | 316 | if (status < 0) 317 | { 318 | // Error, throw away this cargo 319 | pShtp->txDiscards++; 320 | return status; 321 | } 322 | 323 | // For the rest of this transmission, packets are continuations. 324 | continuation = true; 325 | } 326 | 327 | return SH2_OK; 328 | } 329 | 330 | // Callback for SHTP app-specific advertisement tags 331 | static void shtpAdvertHdlr(void *cookie, uint8_t tag, uint8_t len, uint8_t *val) 332 | { 333 | shtp_t *pShtp = (shtp_t *)cookie; 334 | 335 | switch (tag) { 336 | case TAG_SHTP_VERSION: 337 | if (strlen((const char *)val) < sizeof(pShtp->shtpVersion)) { 338 | strcpy(pShtp->shtpVersion, (const char *)val); 339 | } 340 | break; 341 | default: 342 | break; 343 | } 344 | } 345 | 346 | // Add one to the set of known Apps 347 | static void addApp(shtp_t *pShtp, uint32_t guid) 348 | { 349 | shtp_App_t *pApp = 0; 350 | 351 | // Bail out if this GUID is already registered 352 | for (int n = 0; n < pShtp->nextApp; n++) { 353 | if (pShtp->app[n].guid == guid) return; 354 | } 355 | 356 | // Bail out if no space for more apps 357 | if (pShtp->nextApp >= SH2_MAX_APPS) return; 358 | 359 | // Register this app 360 | pApp = &pShtp->app[pShtp->nextApp]; 361 | pShtp->nextApp++; 362 | pApp->guid = guid; 363 | strcpy(pApp->appName, ""); 364 | 365 | // Re-evaluate channel callbacks 366 | updateCallbacks(pShtp); 367 | } 368 | 369 | static void setAppName(shtp_t *pShtp, uint32_t guid, const char * appName) 370 | { 371 | shtp_App_t *pApp = 0; 372 | 373 | // Find the app entry with this GUID 374 | for (unsigned n = 0; n < pShtp->nextApp; n++) { 375 | if (pShtp->app[n].guid == guid) { 376 | pApp = &pShtp->app[n]; 377 | strcpy(pApp->appName, appName); 378 | return; 379 | } 380 | } 381 | } 382 | 383 | // Add one to the set of known channels 384 | static void addChannel(shtp_t *pShtp, uint8_t chanNo, uint32_t guid, const char * chanName, bool wake) 385 | { 386 | if (chanNo >= SH2_MAX_CHANS) return; 387 | 388 | shtp_Channel_t * pChan = &pShtp->chan[chanNo]; 389 | 390 | // Store channel definition 391 | pChan->guid = guid; 392 | strcpy(pChan->chanName, chanName); 393 | pChan->wake = wake; 394 | 395 | // Init channel-associated data 396 | pChan->nextOutSeq = 0; 397 | pChan->nextInSeq = 0; 398 | pChan->callback = 0; 399 | pChan->cookie = 0; 400 | 401 | // Re-evaluate channel callbacks 402 | updateCallbacks(pShtp); 403 | } 404 | 405 | static void callAdvertHandler(shtp_t *pShtp, uint32_t guid, 406 | uint8_t tag, uint8_t len, uint8_t *val) 407 | { 408 | // Find listener for this app 409 | for (int n = 0; n < SH2_MAX_APPS; n++) 410 | { 411 | if (pShtp->appListener[n].guid == guid) { 412 | // Found matching App entry 413 | if (pShtp->appListener[n].callback != 0) { 414 | pShtp->appListener[n].callback(pShtp->appListener[n].cookie, tag, len, val); 415 | return; 416 | } 417 | } 418 | } 419 | } 420 | 421 | static void processAdvertisement(shtp_t *pShtp, uint8_t *payload, uint16_t payloadLen) 422 | { 423 | uint16_t x; 424 | uint8_t tag; 425 | uint8_t len; 426 | uint8_t *val; 427 | uint16_t cursor = 1; 428 | uint32_t guid = 0; 429 | char appName[SHTP_APP_NAME_LEN]; 430 | char chanName[SHTP_CHAN_NAME_LEN]; 431 | uint8_t chanNo = 0; 432 | bool wake = false; 433 | 434 | strcpy(appName, ""); 435 | strcpy(chanName, ""); 436 | 437 | pShtp->advertPhase = ADVERT_IDLE; 438 | 439 | while (cursor < payloadLen) { 440 | tag = payload[cursor++]; 441 | len = payload[cursor++]; 442 | val = payload+cursor; 443 | cursor += len; 444 | 445 | // Process tag 446 | switch (tag) { 447 | case TAG_NULL: 448 | // Reserved value, not a valid tag. 449 | break; 450 | case TAG_GUID: 451 | // A new GUID is being established so terminate advertisement process with earlier app, if any. 452 | callAdvertHandler(pShtp, guid, TAG_NULL, 0, 0); 453 | 454 | guid = readu32(val); 455 | addApp(pShtp, guid); 456 | 457 | strcpy(appName, ""); 458 | strcpy(chanName, ""); 459 | break; 460 | case TAG_MAX_CARGO_PLUS_HEADER_WRITE: 461 | x = readu16(val) - SHTP_HDR_LEN; 462 | 463 | if (x < SH2_HAL_MAX_PAYLOAD_OUT) { 464 | pShtp->outMaxPayload = x; 465 | } 466 | break; 467 | case TAG_MAX_CARGO_PLUS_HEADER_READ: 468 | x = readu16(val) - SHTP_HDR_LEN; 469 | // No need to store this! 470 | break; 471 | case TAG_MAX_TRANSFER_WRITE: 472 | x = readu16(val) - SHTP_HDR_LEN; 473 | if (x < SH2_HAL_MAX_TRANSFER_OUT) { 474 | pShtp->outMaxTransfer = x; 475 | } else { 476 | pShtp->outMaxTransfer = SH2_HAL_MAX_TRANSFER_OUT; 477 | } 478 | break; 479 | case TAG_MAX_TRANSFER_READ: 480 | x = readu16(val) - SHTP_HDR_LEN; 481 | if (x < SH2_HAL_MAX_TRANSFER_IN) { 482 | pShtp->inMaxTransfer = x; 483 | } 484 | break; 485 | case TAG_NORMAL_CHANNEL: 486 | chanNo = readu8(val); 487 | wake = false; 488 | break; 489 | case TAG_WAKE_CHANNEL: 490 | chanNo = readu8(val); 491 | wake = true; 492 | break; 493 | case TAG_APP_NAME: 494 | strcpy(appName, (const char *)val); 495 | setAppName(pShtp, guid, appName); 496 | 497 | break; 498 | case TAG_CHANNEL_NAME: 499 | strcpy(chanName, (const char *)val); 500 | addChannel(pShtp, chanNo, guid, (const char *)val, wake); 501 | 502 | // Store channel metadata 503 | if (chanNo < SH2_MAX_CHANS) { 504 | pShtp->chan[chanNo].guid = guid; 505 | strcpy(pShtp->chan[chanNo].chanName, chanName); 506 | pShtp->chan[chanNo].wake = wake; 507 | } 508 | break; 509 | case TAG_ADV_COUNT: 510 | // Not yet supported. 511 | break; 512 | default: 513 | // Nothing special needs to be done with this tag. 514 | break; 515 | } 516 | 517 | // Deliver a TLV entry to the app's handler 518 | callAdvertHandler(pShtp, guid, tag, len, val); 519 | } 520 | 521 | // terminate advertisement process with last app 522 | callAdvertHandler(pShtp, guid, TAG_NULL, 0, 0); 523 | } 524 | 525 | // Callback for SHTP command channel 526 | static void shtpCmdListener(void *cookie, uint8_t *payload, uint16_t len, uint32_t timestamp) 527 | { 528 | shtp_t *pShtp = (shtp_t *)cookie; 529 | 530 | if ((payload == 0) || (len == 0)) return; 531 | 532 | uint8_t response = payload[0]; 533 | 534 | switch (response) { 535 | case RESP_ADVERTISE: 536 | processAdvertisement(pShtp, payload, len); 537 | break; 538 | default: 539 | // unknown response 540 | break; 541 | } 542 | } 543 | 544 | static void rxAssemble(shtp_t *pShtp, uint8_t *in, uint16_t len, uint32_t t_us) 545 | { 546 | uint16_t payloadLen; 547 | bool continuation; 548 | uint8_t chan = 0; 549 | uint8_t seq = 0; 550 | 551 | // discard invalid short fragments 552 | if (len < SHTP_HDR_LEN) { 553 | pShtp->shortFragments++; 554 | return; 555 | } 556 | 557 | // Interpret header fields 558 | payloadLen = (in[0] + (in[1] << 8)) & (~0x8000); 559 | continuation = ((in[1] & 0x80) != 0); 560 | chan = in[2]; 561 | seq = in[3]; 562 | 563 | if (payloadLen < SHTP_HDR_LEN) { 564 | pShtp->shortFragments++; 565 | 566 | if (pShtp->eventCallback) { 567 | pShtp->eventCallback(pShtp->eventCookie, SHTP_SHORT_FRAGMENT); 568 | } 569 | return; 570 | } 571 | 572 | if ((chan >= SH2_MAX_CHANS) || 573 | (chan >= pShtp->nextChanListener)) { 574 | // Invalid channel id. 575 | pShtp->badRxChan++; 576 | 577 | if (pShtp->eventCallback) { 578 | pShtp->eventCallback(pShtp->eventCookie, SHTP_BAD_RX_CHAN); 579 | } 580 | return; 581 | } 582 | 583 | // Discard earlier assembly in progress if the received data doesn't match it. 584 | if (pShtp->inRemaining) { 585 | // Check this against previously received data. 586 | if (!continuation || 587 | (chan != pShtp->inChan) || 588 | (seq != pShtp->chan[chan].nextInSeq)) { 589 | // This fragment doesn't fit with previous one, discard earlier data 590 | pShtp->inRemaining = 0; 591 | } 592 | } 593 | 594 | if (pShtp->inRemaining == 0) { 595 | if (payloadLen > sizeof(pShtp->inPayload)) { 596 | // Error: This payload won't fit! Discard it. 597 | pShtp->tooLargePayloads++; 598 | 599 | if (pShtp->eventCallback) { 600 | pShtp->eventCallback(pShtp->eventCookie, SHTP_TOO_LARGE_PAYLOADS); 601 | } 602 | return; 603 | } 604 | 605 | // This represents a new payload 606 | 607 | // Store timestamp 608 | pShtp->inTimestamp = t_us; 609 | 610 | // Start a new assembly. 611 | pShtp->inCursor = 0; 612 | pShtp->inChan = chan; 613 | } 614 | 615 | // Append the new fragment to the payload under construction. 616 | if (len > payloadLen) { 617 | // Only use the valid portion of the transfer 618 | len = payloadLen; 619 | } 620 | memcpy(pShtp->inPayload + pShtp->inCursor, in+SHTP_HDR_LEN, len-SHTP_HDR_LEN); 621 | pShtp->inCursor += len-SHTP_HDR_LEN; 622 | pShtp->inRemaining = payloadLen - len; 623 | 624 | // If whole payload received, deliver it to channel listener. 625 | if (pShtp->inRemaining == 0) { 626 | 627 | // Call callback if there is one. 628 | if (pShtp->chan[chan].callback != 0) { 629 | pShtp->chan[chan].callback(pShtp->chan[chan].cookie, 630 | pShtp->inPayload, pShtp->inCursor, 631 | pShtp->inTimestamp); 632 | } 633 | } 634 | 635 | // Remember next sequence number we expect for this channel. 636 | pShtp->chan[chan].nextInSeq = seq + 1; 637 | } 638 | 639 | // ------------------------------------------------------------------------ 640 | // Public functions 641 | 642 | // Takes HAL pointer, returns shtp ID for use in future calls. 643 | // HAL will be opened by this call. 644 | void *shtp_open(sh2_Hal_t *pHal) 645 | { 646 | if (!shtp_initialized) { 647 | // Perform one-time module initialization 648 | shtp_init(); 649 | } 650 | 651 | // Validate params 652 | if (pHal == 0) { 653 | // Error 654 | return 0; 655 | } 656 | 657 | // Find an available instance for this open 658 | shtp_t *pShtp = getInstance(); 659 | if (pShtp == 0) { 660 | // No instances available, return error 661 | return 0; 662 | } 663 | 664 | // Clear the SHTP instance as a shortcut to initializing all fields 665 | memset(pShtp, 0, sizeof(shtp_t)); 666 | 667 | // Store reference to the HAL 668 | pShtp->pHal = pHal; 669 | 670 | // Clear the asynchronous event callback point 671 | pShtp->eventCallback = 0; 672 | pShtp->eventCookie = 0; 673 | 674 | // Initialize state vars (be prepared for adverts) 675 | pShtp->outMaxPayload = SH2_HAL_MAX_PAYLOAD_OUT; 676 | pShtp->outMaxTransfer = SH2_HAL_MAX_TRANSFER_OUT; 677 | 678 | // Establish SHTP App and command channel a priori 679 | addApp(pShtp, GUID_SHTP); 680 | addChannel(pShtp, 0, GUID_SHTP, "command", false); 681 | 682 | // Register SHTP advert listener and command channel listener 683 | shtp_listenAdvert(pShtp, GUID_SHTP, shtpAdvertHdlr, pShtp); 684 | shtp_listenChan(pShtp, GUID_SHTP, "command", shtpCmdListener, pShtp); 685 | 686 | // When we open the HAL, it resets the device and adverts are sent automatically. 687 | // So we go to ADVERT_REQUESTED state. They are on the way. 688 | pShtp->advertPhase = ADVERT_REQUESTED; 689 | 690 | // Open HAL 691 | pHal->open(pHal); 692 | 693 | return pShtp; 694 | } 695 | 696 | // Releases resources associated with this SHTP instance. 697 | // HAL will not be closed. 698 | void shtp_close(void *pInstance) 699 | { 700 | shtp_t *pShtp = (shtp_t *)pInstance; 701 | 702 | pShtp->pHal->close(pShtp->pHal); 703 | 704 | // Clear pShtp 705 | // (Resetting pShtp->pHal to 0, returns this instance to the free pool) 706 | memset(pShtp, 0, sizeof(shtp_t)); 707 | } 708 | 709 | // Register the pointer of the callback function for reporting asynchronous events 710 | void shtp_setEventCallback(void *pInstance, 711 | shtp_EventCallback_t * eventCallback, 712 | void *eventCookie) { 713 | shtp_t *pShtp = (shtp_t *)pInstance; 714 | 715 | pShtp->eventCallback = eventCallback; 716 | pShtp->eventCookie = eventCookie; 717 | } 718 | 719 | // Register a listener for an SHTP channel 720 | int shtp_listenChan(void *pInstance, 721 | uint16_t guid, const char * chan, 722 | shtp_Callback_t *callback, void * cookie) 723 | { 724 | shtp_t *pShtp = (shtp_t *)pInstance; 725 | 726 | // Balk if channel name isn't valid 727 | if ((chan == 0) || (strlen(chan) == 0)) return SH2_ERR_BAD_PARAM; 728 | 729 | return addChanListener(pShtp, guid, chan, callback, cookie); 730 | } 731 | 732 | // Register a listener for SHTP advertisements 733 | int shtp_listenAdvert(void *pInstance, 734 | uint16_t guid, 735 | shtp_AdvertCallback_t *advertCallback, void * cookie) 736 | { 737 | shtp_t *pShtp = (shtp_t *)pInstance; 738 | 739 | // Register the advert listener 740 | addAdvertListener(pShtp, guid, advertCallback, cookie); 741 | 742 | // Arrange for a new set of advertisements, for this listener 743 | if (pShtp->advertPhase == ADVERT_IDLE) { 744 | pShtp->advertPhase = ADVERT_NEEDED; 745 | } 746 | 747 | return SH2_OK; 748 | } 749 | 750 | // Look up the channel number for a particular app, channel. 751 | uint8_t shtp_chanNo(void *pInstance, 752 | const char * appName, const char * chanName) 753 | { 754 | shtp_t *pShtp = (shtp_t *)pInstance; 755 | 756 | int chan = 0; 757 | uint32_t guid = 0xFFFFFFFF; 758 | 759 | // Determine GUID for this appname 760 | for (int n = 0; n < SH2_MAX_APPS; n++) { 761 | if (strcmp(pShtp->app[n].appName, appName) == 0) { 762 | guid = pShtp->app[n].guid; 763 | break; 764 | } 765 | } 766 | if (guid == 0xFFFFFFFF) return -1; 767 | 768 | for (chan = 0; chan < SH2_MAX_CHANS; chan++) { 769 | if ((strcmp(pShtp->chan[chan].chanName, chanName) == 0) && 770 | pShtp->chan[chan].guid == guid) { 771 | // Found match 772 | return chan; 773 | } 774 | } 775 | 776 | // Not found 777 | return 0xFF; 778 | } 779 | 780 | // Send an SHTP payload on a particular channel 781 | int shtp_send(void *pInstance, 782 | uint8_t channel, const uint8_t *payload, uint16_t len) 783 | { 784 | shtp_t *pShtp = (shtp_t *)pInstance; 785 | int ret = SH2_OK; 786 | 787 | if (len > pShtp->outMaxPayload) { 788 | return SH2_ERR_BAD_PARAM; 789 | } 790 | if (channel >= SH2_MAX_CHANS) { 791 | pShtp->badTxChan++; 792 | return SH2_ERR_BAD_PARAM; 793 | } 794 | 795 | ret = txProcess(pShtp, channel, payload, len); 796 | 797 | return ret; 798 | } 799 | 800 | // Check for received data and process it. 801 | void shtp_service(void *pInstance) 802 | { 803 | shtp_t *pShtp = (shtp_t *)pInstance; 804 | uint32_t t_us = 0; 805 | 806 | if (pShtp->advertPhase == ADVERT_NEEDED) { 807 | pShtp->advertPhase = ADVERT_REQUESTED; // do this before send, to avoid recursion. 808 | int status = shtp_send(pShtp, SHTP_CHAN_COMMAND, advertise, sizeof(advertise)); 809 | if (status != SH2_OK) { 810 | // Oops, advert request failed. Go back to needing one. 811 | pShtp->advertPhase = ADVERT_NEEDED; 812 | } 813 | } 814 | 815 | int len = pShtp->pHal->read(pShtp->pHal, pShtp->inTransfer, sizeof(pShtp->inTransfer), &t_us); 816 | if (len) { 817 | rxAssemble(pShtp, pShtp->inTransfer, len, t_us); 818 | } 819 | } 820 | --------------------------------------------------------------------------------