├── .gitignore ├── NOTICE.txt ├── README.md ├── euler.c ├── euler.h ├── sh2.c ├── sh2.h ├── sh2_SensorValue.c ├── sh2_SensorValue.h ├── sh2_err.h ├── sh2_hal.h ├── sh2_util.c ├── sh2_util.h ├── shtp.c └── shtp.h /.gitignore: -------------------------------------------------------------------------------- 1 | # Emacs temp files 2 | *~ 3 | \#* 4 | .\#* 5 | 6 | # Output files 7 | /STM32F4xx-Nucleo/ 8 | /docs/ 9 | *.pyc 10 | 11 | # hcbin files and output files under util/ 12 | *.hcbin 13 | util/1000* 14 | util/firmware* 15 | 16 | -------------------------------------------------------------------------------- /NOTICE.txt: -------------------------------------------------------------------------------- 1 | This software is licensed from CEVA, Inc. 2 | Copyright (c) CEVA, Inc. and its licensors. 3 | All rights reserved. 4 | 5 | CEVA and the CEVA logo are trademarks of CEVA, Inc. 6 | 7 | You can contact CEVA, Sensor Fusion Business Unit on the web at 8 | https://www.ceva-dsp.com/app/motion-sensing/ or at 9 | 15245 Shady Grove Road, Suite 400, Rockville, MD 20850 USA 10 | 11 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # SH2 Sensorhub driver for MCU application 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 | An example project based on this driver can be found here: 11 | * [sh2-demo-nucleo](https://github.com/ceva-dsp/sh2-demo-nucleo) 12 | 13 | ## Release Notes 14 | ### Version 1.4.0 15 | * Added reset-recovery to all API operations. In the event of a device reset, SH2 API calls will return SH2_ERR. (For operations that are intended to produce a device reset, that event is handled without error.) 16 | * Added checks to all API calls to ensure sh2_open() was called prior. 17 | * New parameters added to sh2_setTareNow() 18 | * Added Euler decomposition functions to convert quaternions to yaw, pitch, roll. 19 | * Updated sh2_getProductIds to read all ids for FSP201. 20 | 21 | -------------------------------------------------------------------------------- /euler.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2023 CEVA, 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 CEVA, 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 | #define _USE_MATH_DEFINES 19 | #include 20 | 21 | #ifndef M_PI 22 | #define M_PI (3.14159265358979323846264338327950288419716939937510) 23 | #endif 24 | 25 | #include "euler.h" 26 | 27 | // Functions to convert quaternion into Roll, Pitch and Yaw values. 28 | 29 | float q_to_yaw(float r, float i, float j, float k) 30 | { 31 | // convert to Euler Angles 32 | float num = 2.0f * i * j - 2.0f * r * k; 33 | float den = 2.0f * r * r + 2.0f * j * j - 1.0f; 34 | 35 | float yaw = (float)atan2((double)num, (double)den); 36 | 37 | return yaw; 38 | } 39 | 40 | float q_to_pitch(float r, float i, float j, float k) 41 | { 42 | // convert to Euler Angles 43 | float arg = 2.0f * j * k + 2.0f * r * i; 44 | if (arg > 1.0f) arg = 1.0f; 45 | if (arg < -1.0f) arg = -1.0f; 46 | float pitch = (float)asin((double)arg); 47 | 48 | return pitch; 49 | } 50 | 51 | float q_to_roll(float r, float i, float j, float k) 52 | { 53 | // convert to Euler Angles 54 | float num = -2.0f * i * k + 2.0f * r * j; 55 | float den = 2.0f * r * r + 2.0f * k * k - 1.0f; 56 | float roll = (float)atan2((double)num, (double)den); 57 | 58 | return roll; 59 | } 60 | 61 | void q_to_ypr(float r, float i, float j, float k, float *pYaw, float *pPitch, float *pRoll) 62 | { 63 | // convert to Euler Angles 64 | float num = 2.0f * i * j - 2.0f * r * k; 65 | float den = 2.0f * r * r + 2.0f * j * j - 1.0f; 66 | *pYaw = (float)atan2((double)num, (double)den); 67 | 68 | float arg = 2.0f * j * k + 2.0f * r * i; 69 | if (arg > 1.0f) arg = 1.0f; 70 | if (arg < -1.0f) arg = -1.0f; 71 | *pPitch = (float)asin((double)arg); 72 | 73 | num = -2.0f * i * k + 2.0f * r * j; 74 | den = 2.0f * r * r + 2.0f * k * k - 1.0f; 75 | *pRoll = (float)atan2((double)num, (double)den); 76 | } 77 | 78 | -------------------------------------------------------------------------------- /euler.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2023 CEVA, 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 CEVA, 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 | #ifndef EULER_H 19 | #define EULER_H 20 | 21 | // Extract yaw value from quaternion. 22 | float q_to_yaw(float r, float i, float j, float k); 23 | 24 | // Extract pitch value from quaternion. 25 | float q_to_pitch(float r, float i, float j, float k); 26 | 27 | // Extract roll value from quaternion. 28 | float q_to_roll(float r, float i, float j, float k); 29 | 30 | // Get Yaw, Pitch and Roll from quaternion 31 | void q_to_ypr(float r, float i, float j, float k, 32 | float *pRoll, float *pPitch, float *pYaw); 33 | 34 | #endif 35 | -------------------------------------------------------------------------------- /sh2.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2015-2022 CEVA, 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 CEVA, 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.c 20 | * @author David Wheeler 21 | * @date 22 Sept 2015 22 | * @brief API Definition for SH-2 Sensor Hub. 23 | * 24 | * The sh2 API provides functions for opening a session with 25 | * the sensor hub and performing all supported operations with it. 26 | * This includes enabling sensors and reading events as well as 27 | * other housekeeping functions. 28 | * 29 | */ 30 | 31 | //Suppress warning about fopen and strcpy safety under MSVC 32 | #ifdef _MSC_VER 33 | # ifndef _CRT_SECURE_NO_WARNINGS 34 | # define _CRT_SECURE_NO_WARNINGS 35 | # endif 36 | #endif 37 | 38 | #include "sh2.h" 39 | #include "sh2_err.h" 40 | #include "shtp.h" 41 | #include "sh2_util.h" 42 | 43 | #include 44 | #include 45 | 46 | // ------------------------------------------------------------------------ 47 | // Private type definitions 48 | 49 | #define CHAN_EXECUTABLE_DEVICE (1) 50 | #define CHAN_SENSORHUB_CONTROL (2) 51 | #define CHAN_SENSORHUB_INPUT (3) 52 | #define CHAN_SENSORHUB_INPUT_WAKE (4) 53 | #define CHAN_SENSORHUB_INPUT_GIRV (5) 54 | 55 | // executable/device channel responses 56 | #define EXECUTABLE_DEVICE_CMD_RESET (1) 57 | #define EXECUTABLE_DEVICE_CMD_ON (2) 58 | #define EXECUTABLE_DEVICE_CMD_SLEEP (3) 59 | 60 | // executable/device channel responses 61 | #define EXECUTABLE_DEVICE_RESP_RESET_COMPLETE (1) 62 | 63 | // Tags for sensorhub app advertisements. 64 | #define TAG_SH2_VERSION (0x80) 65 | #define TAG_SH2_REPORT_LENGTHS (0x81) 66 | 67 | // Max length of sensorhub version string. 68 | #define MAX_VER_LEN (16) 69 | 70 | // Max number of report ids supported 71 | #define SH2_MAX_REPORT_IDS (64) 72 | 73 | #if defined(_MSC_VER) 74 | #define PACKED_STRUCT struct 75 | #pragma pack(push, 1) 76 | #elif defined(__GNUC__) 77 | #define PACKED_STRUCT struct __attribute__((packed)) 78 | #else 79 | #define PACKED_STRUCT __packed struct 80 | #endif 81 | 82 | #define ADVERT_TIMEOUT_US (200000) 83 | 84 | // Command and Subcommand values 85 | #define SH2_CMD_ERRORS 1 86 | #define SH2_CMD_COUNTS 2 87 | #define SH2_COUNTS_GET_COUNTS 0 88 | #define SH2_COUNTS_CLEAR_COUNTS 1 89 | #define SH2_CMD_TARE 3 90 | #define SH2_TARE_TARE_NOW 0 91 | #define SH2_TARE_PERSIST_TARE 1 92 | #define SH2_TARE_SET_REORIENTATION 2 93 | #define SH2_CMD_INITIALIZE 4 94 | #define SH2_INIT_SYSTEM 1 95 | #define SH2_INIT_UNSOLICITED 0x80 96 | // #define SH2_CMD_FRS 5 /* Depreciated */ 97 | #define SH2_CMD_DCD 6 98 | #define SH2_CMD_ME_CAL 7 99 | #define SH2_CMD_DCD_SAVE 9 100 | #define SH2_CMD_GET_OSC_TYPE 0x0A 101 | #define SH2_CMD_CLEAR_DCD_AND_RESET 0x0B 102 | #define SH2_CMD_CAL 0x0C 103 | #define SH2_CAL_START 0 104 | #define SH2_CAL_FINISH 1 105 | #define SH2_CMD_BOOTLOADER 0x0D /* SH-2 Reference Manual 6.4.12 */ 106 | #define SH2_BL_MODE_REQ 0 107 | #define SH2_BL_STATUS_REQ 1 108 | #define SH2_CMD_INTERACTIVE_ZRO 0x0E /* SH-2 Reference Manual 6.4.13 */ 109 | #define SH2_CMD_WHEEL_REQ 0x0F 110 | #define SH2_CMD_DR_CAL_SAVE 0x10 111 | 112 | // SENSORHUB_COMMAND_REQ 113 | #define SENSORHUB_COMMAND_REQ (0xF2) 114 | #define COMMAND_PARAMS (9) 115 | typedef PACKED_STRUCT { 116 | uint8_t reportId; 117 | uint8_t seq; 118 | uint8_t command; 119 | uint8_t p[COMMAND_PARAMS]; 120 | } CommandReq_t; 121 | 122 | // SENSORHUB_COMMAND_RESP 123 | #define SENSORHUB_COMMAND_RESP (0xF1) 124 | #define RESPONSE_VALUES (11) 125 | typedef PACKED_STRUCT { 126 | uint8_t reportId; 127 | uint8_t seq; 128 | uint8_t command; 129 | uint8_t commandSeq; 130 | uint8_t respSeq; 131 | uint8_t r[RESPONSE_VALUES]; 132 | } CommandResp_t; 133 | 134 | // SENSORHUB_PROD_ID_REQ 135 | #define SENSORHUB_PROD_ID_REQ (0xF9) 136 | typedef PACKED_STRUCT { 137 | uint8_t reportId; 138 | uint8_t reserved; 139 | } ProdIdReq_t; 140 | 141 | // SENSORHUB_PROD_ID_RESP 142 | #define SENSORHUB_PROD_ID_RESP (0xF8) 143 | typedef PACKED_STRUCT { 144 | uint8_t reportId; 145 | uint8_t resetCause; 146 | uint8_t swVerMajor; 147 | uint8_t swVerMinor; 148 | uint32_t swPartNumber; 149 | uint32_t swBuildNumber; 150 | uint16_t swVerPatch; 151 | uint8_t reserved0; 152 | uint8_t reserved1; 153 | } ProdIdResp_t; 154 | 155 | // Report definitions 156 | // Bit fields for Feature Report flags 157 | #define FEAT_CHANGE_SENSITIVITY_MODE_BIT 0 158 | #define FEAT_CHANGE_SENSITIVITY_ENABLE_BIT 1 159 | #define FEAT_WAKEUP_ENABLE_BIT 2 160 | #define FEAT_ALWAYS_ON_ENABLE_BIT 3 161 | #define FEAT_SNIFF_ENABLE_BIT 4 162 | #define FEAT_CHANGE_SENSITIVITY_RELATIVE (1 << FEAT_CHANGE_SENSITIVITY_MODE_BIT) 163 | #define FEAT_CHANGE_SENSITIVITY_ABSOLUTE (0 << FEAT_CHANGE_SENSITIVITY_MODE_BIT) 164 | #define FEAT_CHANGE_SENSITIVITY_ENABLED (1 << FEAT_CHANGE_SENSITIVITY_ENABLE_BIT) 165 | #define FEAT_CHANGE_SENSITIVITY_DISABLED (0 << FEAT_CHANGE_SENSITIVITY_ENABLE_BIT) 166 | #define FEAT_WAKE_ENABLED (1 << FEAT_WAKEUP_ENABLE_BIT) 167 | #define FEAT_WAKE_DISABLED (0 << FEAT_WAKEUP_ENABLE_BIT) 168 | #define FEAT_ALWAYS_ON_ENABLED (1 << FEAT_ALWAYS_ON_ENABLE_BIT) 169 | #define FEAT_ALWAYS_ON_DISABLED (0 << FEAT_ALWAYS_ON_ENABLE_BIT) 170 | #define FEAT_SNIFF_ENABLED (1 << FEAT_SNIFF_ENABLE_BIT) 171 | #define FEAT_SNIFF_DISABLED (0 << FEAT_SNIFF_ENABLE_BIT) 172 | 173 | 174 | // GET_FEATURE_REQ 175 | #define SENSORHUB_GET_FEATURE_REQ (0xFE) 176 | typedef PACKED_STRUCT{ 177 | uint8_t reportId; 178 | uint8_t featureReportId; 179 | } GetFeatureReq_t; 180 | 181 | // SENSORHUB_GET_FEATURE_RESP 182 | #define SENSORHUB_GET_FEATURE_RESP (0xFC) 183 | typedef PACKED_STRUCT{ 184 | uint8_t reportId; 185 | uint8_t featureReportId; // sensor id 186 | uint8_t flags; // FEAT_... values 187 | uint16_t changeSensitivity; 188 | uint32_t reportInterval_uS; 189 | uint32_t batchInterval_uS; 190 | uint32_t sensorSpecific; 191 | } GetFeatureResp_t; 192 | 193 | 194 | typedef struct sh2_s sh2_t; 195 | 196 | typedef int (sh2_OpStart_t)(sh2_t *pSh2); 197 | typedef void (sh2_OpRx_t)(sh2_t *pSh2, const uint8_t *payload, uint16_t len); 198 | typedef void (sh2_OpReset_t)(sh2_t *pSh2); 199 | 200 | typedef struct sh2_Op_s { 201 | uint32_t timeout_us; 202 | sh2_OpStart_t *start; 203 | sh2_OpRx_t *rx; 204 | sh2_OpReset_t *onReset; 205 | } sh2_Op_t; 206 | 207 | // Parameters and state information for the operation in progress 208 | typedef union { 209 | struct { 210 | CommandReq_t req; 211 | } sendCmd; 212 | struct { 213 | sh2_ProductIds_t *pProdIds; 214 | uint8_t nextEntry; 215 | uint8_t expectedEntries; 216 | } getProdIds; 217 | struct { 218 | sh2_SensorConfig_t *pConfig; 219 | sh2_SensorId_t sensorId; 220 | } getSensorConfig; 221 | struct { 222 | const sh2_SensorConfig_t *pConfig; 223 | sh2_SensorId_t sensorId; 224 | } setSensorConfig; 225 | struct { 226 | uint16_t frsType; 227 | uint32_t *pData; 228 | uint16_t *pWords; 229 | uint16_t nextOffset; 230 | } getFrs; 231 | struct { 232 | uint16_t frsType; 233 | uint32_t *pData; 234 | uint16_t words; 235 | uint16_t offset; 236 | } setFrs; 237 | struct { 238 | uint8_t severity; 239 | sh2_ErrorRecord_t *pErrors; 240 | uint16_t *pNumErrors; 241 | uint16_t errsRead; 242 | } getErrors; 243 | struct { 244 | sh2_SensorId_t sensorId; 245 | sh2_Counts_t *pCounts; 246 | } getCounts; 247 | struct { 248 | uint8_t sensors; 249 | } calConfig; 250 | struct { 251 | uint8_t *pSensors; 252 | } getCalConfig; 253 | struct { 254 | sh2_SensorId_t sensorId; 255 | } forceFlush; 256 | struct { 257 | sh2_OscType_t *pOscType; 258 | } getOscType; 259 | struct { 260 | uint32_t interval_us; 261 | } startCal; 262 | struct { 263 | sh2_CalStatus_t status; 264 | } finishCal; 265 | struct { 266 | uint8_t wheelIndex; 267 | uint32_t timestamp; 268 | int16_t wheelData; 269 | uint8_t dataType; 270 | } wheelRequest; 271 | } sh2_OpData_t; 272 | 273 | // Max length of an FRS record, words. 274 | #define MAX_FRS_WORDS (72) 275 | 276 | struct sh2_s { 277 | // Pointer to the SHTP HAL 278 | sh2_Hal_t *pHal; 279 | 280 | // associated SHTP instance 281 | void *pShtp; 282 | 283 | volatile bool resetComplete; 284 | char version[MAX_VER_LEN+1]; 285 | 286 | // Multi-step operation support 287 | const sh2_Op_t *pOp; 288 | int opStatus; 289 | sh2_OpData_t opData; 290 | uint8_t lastCmdId; 291 | uint8_t cmdSeq; 292 | uint8_t nextCmdSeq; 293 | 294 | // Event callback and it's cookie 295 | sh2_EventCallback_t *eventCallback; 296 | void * eventCookie; 297 | 298 | // Sensor callback and it's cookie 299 | sh2_SensorCallback_t *sensorCallback; 300 | void * sensorCookie; 301 | 302 | // Storage space for reading sensor metadata 303 | uint32_t frsData[MAX_FRS_WORDS]; 304 | uint16_t frsDataLen; 305 | 306 | // Stats 307 | uint32_t execBadPayload; 308 | uint32_t emptyPayloads; 309 | uint32_t unknownReportIds; 310 | 311 | }; 312 | 313 | #define SENSORHUB_BASE_TIMESTAMP_REF (0xFB) 314 | typedef PACKED_STRUCT { 315 | uint8_t reportId; 316 | int32_t timebase; 317 | } BaseTimestampRef_t; 318 | 319 | #define SENSORHUB_TIMESTAMP_REBASE (0xFA) 320 | typedef PACKED_STRUCT { 321 | uint8_t reportId; 322 | int32_t timebase; 323 | } TimestampRebase_t; 324 | 325 | // SENSORHUB_FORCE_SENSOR_FLUSH 326 | #define SENSORHUB_FORCE_SENSOR_FLUSH (0xF0) 327 | typedef PACKED_STRUCT { 328 | uint8_t reportId; 329 | uint8_t sensorId; 330 | } ForceFlushReq_t; 331 | 332 | // SENSORHUB_FLUSH_COMPLETED 333 | #define SENSORHUB_FLUSH_COMPLETED (0xEF) 334 | typedef PACKED_STRUCT { 335 | uint8_t reportId; 336 | uint8_t sensorId; 337 | } ForceFlushResp_t; 338 | 339 | typedef struct sh2_ReportLen_s { 340 | uint8_t id; 341 | uint8_t len; 342 | } sh2_ReportLen_t; 343 | 344 | // SENSORHUB_FRS_WRITE_REQ 345 | #define SENSORHUB_FRS_WRITE_REQ (0xF7) 346 | typedef PACKED_STRUCT { 347 | uint8_t reportId; 348 | uint8_t reserved; 349 | uint16_t length; 350 | uint16_t frsType; 351 | } FrsWriteReq_t; 352 | 353 | // SENSORHUB_FRS_WRITE_DATA_REQ 354 | #define SENSORHUB_FRS_WRITE_DATA_REQ (0xF6) 355 | typedef PACKED_STRUCT { 356 | uint8_t reportId; 357 | uint8_t reserved; 358 | uint16_t offset; 359 | uint32_t data0; 360 | uint32_t data1; 361 | } FrsWriteDataReq_t; 362 | 363 | // FRS write status values 364 | #define FRS_WRITE_STATUS_RECEIVED (0) 365 | #define FRS_WRITE_STATUS_UNRECOGNIZED_FRS_TYPE (1) 366 | #define FRS_WRITE_STATUS_BUSY (2) 367 | #define FRS_WRITE_STATUS_WRITE_COMPLETED (3) 368 | #define FRS_WRITE_STATUS_READY (4) 369 | #define FRS_WRITE_STATUS_FAILED (5) 370 | #define FRS_WRITE_STATUS_NOT_READY (6) // data received when not in write mode 371 | #define FRS_WRITE_STATUS_INVALID_LENGTH (7) 372 | #define FRS_WRITE_STATUS_RECORD_VALID (8) 373 | #define FRS_WRITE_STATUS_INVALID_RECORD (9) 374 | #define FRS_WRITE_STATUS_DEVICE_ERROR (10) 375 | #define FRS_WRITE_STATUS_READ_ONLY (11) 376 | 377 | // SENSORHUB_FRS_WRITE_RESP 378 | #define SENSORHUB_FRS_WRITE_RESP (0xF5) 379 | typedef PACKED_STRUCT { 380 | uint8_t reportId; 381 | uint8_t status; 382 | uint16_t wordOffset; 383 | } FrsWriteResp_t; 384 | 385 | // RESP_FRS_READ_REQ 386 | #define SENSORHUB_FRS_READ_REQ (0xF4) 387 | typedef PACKED_STRUCT { 388 | uint8_t reportId; 389 | uint8_t reserved; 390 | uint16_t readOffset; 391 | uint16_t frsType; 392 | uint16_t blockSize; 393 | } FrsReadReq_t; 394 | 395 | // Get Datalen portion of len_status field 396 | #define FRS_READ_DATALEN(x) (((x) >> 4) & 0x0F) 397 | 398 | // Get status portion of len_status field 399 | #define FRS_READ_STATUS(x) ((x) & 0x0F) 400 | 401 | // Status values 402 | #define FRS_READ_STATUS_NO_ERROR 0 403 | #define FRS_READ_STATUS_UNRECOGNIZED_FRS_TYPE 1 404 | #define FRS_READ_STATUS_BUSY 2 405 | #define FRS_READ_STATUS_READ_RECORD_COMPLETED 3 406 | #define FRS_READ_STATUS_OFFSET_OUT_OF_RANGE 4 407 | #define FRS_READ_STATUS_RECORD_EMPTY 5 408 | #define FRS_READ_STATUS_READ_BLOCK_COMPLETED 6 409 | #define FRS_READ_STATUS_READ_BLOCK_AND_RECORD_COMPLETED 7 410 | #define FRS_READ_STATUS_DEVICE_ERROR 8 411 | 412 | // SENSORHUB_FRS_READ_RESP 413 | #define SENSORHUB_FRS_READ_RESP (0xF3) 414 | typedef PACKED_STRUCT { 415 | uint8_t reportId; 416 | uint8_t len_status; // See FRS_READ... macros above 417 | uint16_t wordOffset; 418 | uint32_t data0; 419 | uint32_t data1; 420 | uint16_t frsType; 421 | uint8_t reserved0; 422 | uint8_t reserved1; 423 | } FrsReadResp_t; 424 | 425 | // ------------------------------------------------------------------------ 426 | // Private data 427 | 428 | // SH2 state 429 | sh2_t _sh2; 430 | 431 | // SH2 Async Event Message 432 | static sh2_AsyncEvent_t sh2AsyncEvent; 433 | 434 | // Lengths of reports by report id. 435 | static const sh2_ReportLen_t sh2ReportLens[] = { 436 | // Sensor reports 437 | {.id = SH2_ACCELEROMETER, .len = 10}, 438 | {.id = SH2_GYROSCOPE_CALIBRATED, .len = 10}, 439 | {.id = SH2_MAGNETIC_FIELD_CALIBRATED, .len = 10}, 440 | {.id = SH2_LINEAR_ACCELERATION, .len = 10}, 441 | {.id = SH2_ROTATION_VECTOR, .len = 14}, 442 | {.id = SH2_GRAVITY, .len = 10}, 443 | {.id = SH2_GYROSCOPE_UNCALIBRATED, .len = 16}, 444 | {.id = SH2_GAME_ROTATION_VECTOR, .len = 12}, 445 | {.id = SH2_GEOMAGNETIC_ROTATION_VECTOR, .len = 14}, 446 | {.id = SH2_PRESSURE, .len = 8}, 447 | {.id = SH2_AMBIENT_LIGHT, .len = 8}, 448 | {.id = SH2_HUMIDITY, .len = 6}, 449 | {.id = SH2_PROXIMITY, .len = 6}, 450 | {.id = SH2_TEMPERATURE, .len = 6}, 451 | {.id = SH2_MAGNETIC_FIELD_UNCALIBRATED, .len = 16}, 452 | {.id = SH2_TAP_DETECTOR, .len = 5}, 453 | {.id = SH2_STEP_COUNTER, .len = 12}, 454 | {.id = SH2_SIGNIFICANT_MOTION, .len = 6}, 455 | {.id = SH2_STABILITY_CLASSIFIER, .len = 6}, 456 | {.id = SH2_RAW_ACCELEROMETER, .len = 16}, 457 | {.id = SH2_RAW_GYROSCOPE, .len = 16}, 458 | {.id = SH2_RAW_MAGNETOMETER, .len = 16}, 459 | {.id = SH2_STEP_DETECTOR, .len = 8}, 460 | {.id = SH2_SHAKE_DETECTOR, .len = 6}, 461 | {.id = SH2_FLIP_DETECTOR, .len = 6}, 462 | {.id = SH2_PICKUP_DETECTOR, .len = 8}, 463 | {.id = SH2_STABILITY_DETECTOR, .len = 6}, 464 | {.id = SH2_PERSONAL_ACTIVITY_CLASSIFIER, .len = 16}, 465 | {.id = SH2_SLEEP_DETECTOR, .len = 6}, 466 | {.id = SH2_TILT_DETECTOR, .len = 6}, 467 | {.id = SH2_POCKET_DETECTOR, .len = 6}, 468 | {.id = SH2_CIRCLE_DETECTOR, .len = 6}, 469 | {.id = SH2_HEART_RATE_MONITOR, .len = 6}, 470 | {.id = SH2_ARVR_STABILIZED_RV, .len = 14}, 471 | {.id = SH2_ARVR_STABILIZED_GRV, .len = 12}, 472 | {.id = SH2_GYRO_INTEGRATED_RV, .len = 14}, 473 | {.id = SH2_IZRO_MOTION_REQUEST, .len = 6}, 474 | {.id = SH2_RAW_OPTICAL_FLOW, .len = 24}, 475 | {.id = SH2_DEAD_RECKONING_POSE, .len = 60}, 476 | {.id = SH2_WHEEL_ENCODER, .len = 12}, 477 | 478 | // Other response types 479 | {.id = SENSORHUB_FLUSH_COMPLETED, .len = 2}, 480 | {.id = SENSORHUB_COMMAND_RESP, .len = 16}, 481 | {.id = SENSORHUB_FRS_READ_RESP, .len = 16}, 482 | {.id = SENSORHUB_FRS_WRITE_RESP, .len = 4}, 483 | {.id = SENSORHUB_PROD_ID_RESP, .len = 16}, 484 | {.id = SENSORHUB_TIMESTAMP_REBASE, .len = 5}, 485 | {.id = SENSORHUB_BASE_TIMESTAMP_REF, .len = 5}, 486 | {.id = SENSORHUB_GET_FEATURE_RESP, .len = 17}, 487 | }; 488 | 489 | // ------------------------------------------------------------------------ 490 | // Private functions 491 | 492 | // SH-2 transaction phases 493 | static int opStart(sh2_t *pSh2, const sh2_Op_t *pOp) 494 | { 495 | // return error if another operation already in progress 496 | if (pSh2->pOp) return SH2_ERR_OP_IN_PROGRESS; 497 | 498 | // Establish this operation as the new operation in progress 499 | pSh2->pOp = pOp; 500 | pSh2->opStatus = SH2_OK; 501 | int rc = pOp->start(pSh2); // Call start method 502 | if (rc != SH2_OK) { 503 | // Unregister this operation 504 | pSh2->opStatus = rc; 505 | pSh2->pOp = 0; 506 | } 507 | 508 | return rc; 509 | } 510 | 511 | static void opRx(sh2_t *pSh2, const uint8_t *payload, uint16_t len) 512 | { 513 | if ((pSh2->pOp != 0) && // An operation is in progress 514 | (pSh2->pOp->rx != 0)) { // and it has an rx method 515 | pSh2->pOp->rx(pSh2, payload, len); // Call receive method 516 | } 517 | } 518 | 519 | static int opCompleted(sh2_t *pSh2, int status) 520 | { 521 | // Record status 522 | pSh2->opStatus = status; 523 | 524 | // Signal that op is done. 525 | pSh2->pOp = 0; 526 | 527 | return SH2_OK; 528 | } 529 | 530 | static void opOnReset(sh2_t *pSh2) 531 | { 532 | if (pSh2->pOp != 0) { 533 | if (pSh2->pOp->onReset != 0) { 534 | // This operation has its own reset handler so use it. 535 | pSh2->pOp->onReset(pSh2); 536 | } 537 | else { 538 | // No reset handler : abort the operation with SH2_ERR code 539 | opCompleted(pSh2, SH2_ERR); 540 | } 541 | } 542 | } 543 | 544 | static uint8_t getReportLen(uint8_t reportId) 545 | { 546 | for (unsigned n = 0; n < ARRAY_LEN(sh2ReportLens); n++) { 547 | if (sh2ReportLens[n].id == reportId) { 548 | return sh2ReportLens[n].len; 549 | } 550 | } 551 | 552 | return 0; 553 | } 554 | 555 | static void sensorhubControlHdlr(void *cookie, uint8_t *payload, uint16_t len, uint32_t timestamp) 556 | { 557 | (void)timestamp; // unused. 558 | 559 | sh2_t *pSh2 = (sh2_t *)cookie; 560 | 561 | uint16_t cursor = 0; 562 | uint32_t count = 0; 563 | CommandResp_t * pResp = 0; 564 | 565 | if (len == 0) { 566 | pSh2->emptyPayloads++; 567 | return; 568 | } 569 | 570 | while (cursor < len) { 571 | // Get next report id 572 | count++; 573 | uint8_t reportId = payload[cursor]; 574 | 575 | // Determine report length 576 | uint8_t reportLen = getReportLen(reportId); 577 | if (reportLen == 0) { 578 | // An unrecognized report id 579 | pSh2->unknownReportIds++; 580 | return; 581 | } 582 | else { 583 | // Check for unsolicited initialize response 584 | if (reportId == SENSORHUB_COMMAND_RESP) { 585 | pResp = (CommandResp_t *)(payload+cursor); 586 | if ((pResp->command == (SH2_CMD_INITIALIZE | SH2_INIT_UNSOLICITED)) && 587 | (pResp->r[1] == SH2_INIT_SYSTEM)) { 588 | // This is an unsolicited INIT message. 589 | // Ignore this. EXECUTABLE_DEVICE_RESP_RESET_COMPLETE makes it redundant. 590 | } 591 | 592 | } // Check for Get Feature Response 593 | else if (reportId == SENSORHUB_GET_FEATURE_RESP) { 594 | if (pSh2->eventCallback) { 595 | GetFeatureResp_t * pGetFeatureResp; 596 | pGetFeatureResp = (GetFeatureResp_t *)(payload + cursor); 597 | 598 | sh2AsyncEvent.eventId = SH2_GET_FEATURE_RESP; 599 | sh2AsyncEvent.sh2SensorConfigResp.sensorId = pGetFeatureResp->featureReportId; 600 | sh2AsyncEvent.sh2SensorConfigResp.sensorConfig.changeSensitivityEnabled = 601 | ((pGetFeatureResp->flags & FEAT_CHANGE_SENSITIVITY_ENABLED) != 0); 602 | sh2AsyncEvent.sh2SensorConfigResp.sensorConfig.changeSensitivityRelative = 603 | ((pGetFeatureResp->flags & FEAT_CHANGE_SENSITIVITY_RELATIVE) != 0); 604 | sh2AsyncEvent.sh2SensorConfigResp.sensorConfig.wakeupEnabled = 605 | ((pGetFeatureResp->flags & FEAT_WAKE_ENABLED) != 0); 606 | sh2AsyncEvent.sh2SensorConfigResp.sensorConfig.alwaysOnEnabled = 607 | ((pGetFeatureResp->flags & FEAT_ALWAYS_ON_ENABLED) != 0); 608 | sh2AsyncEvent.sh2SensorConfigResp.sensorConfig.changeSensitivity = 609 | pGetFeatureResp->changeSensitivity; 610 | sh2AsyncEvent.sh2SensorConfigResp.sensorConfig.reportInterval_us = 611 | pGetFeatureResp->reportInterval_uS; 612 | sh2AsyncEvent.sh2SensorConfigResp.sensorConfig.batchInterval_us = 613 | pGetFeatureResp->batchInterval_uS; 614 | sh2AsyncEvent.sh2SensorConfigResp.sensorConfig.sensorSpecific = 615 | pGetFeatureResp->sensorSpecific; 616 | 617 | pSh2->eventCallback(pSh2->eventCookie, &sh2AsyncEvent); 618 | } 619 | } 620 | 621 | // Hand off to operation in progress, if any 622 | opRx(pSh2, payload+cursor, reportLen); 623 | cursor += reportLen; 624 | } 625 | } 626 | } 627 | 628 | 629 | static int opProcess(sh2_t *pSh2, const sh2_Op_t *pOp) 630 | { 631 | int status = SH2_OK; 632 | uint32_t start_us = 0; 633 | 634 | start_us = pSh2->pHal->getTimeUs(pSh2->pHal); 635 | 636 | status = opStart(pSh2, pOp); 637 | if (status != SH2_OK) { 638 | return status; 639 | } 640 | 641 | uint32_t now_us = start_us; 642 | // While op not complete and not timed out. 643 | while ((pSh2->pOp != 0) && 644 | ((pOp->timeout_us == 0) || 645 | ((now_us-start_us) < pOp->timeout_us))) { 646 | 647 | if (pSh2->pShtp == 0) { 648 | // Was SH2 interface closed unexpectedly? 649 | pSh2->opStatus = SH2_ERR; 650 | break; 651 | } 652 | 653 | // Service SHTP to poll the device. 654 | shtp_service(pSh2->pShtp); 655 | 656 | // Update the time 657 | now_us = pSh2->pHal->getTimeUs(pSh2->pHal); 658 | } 659 | 660 | if (pSh2->pOp != 0) { 661 | // Operation has timed out. Clean up. 662 | pSh2->pOp = 0; 663 | pSh2->opStatus = SH2_ERR_TIMEOUT; 664 | } 665 | 666 | return pSh2->opStatus; 667 | } 668 | 669 | // Produce 64-bit microsecond timestamp for a sensor event 670 | static uint64_t touSTimestamp(uint32_t hostInt, int32_t referenceDelta, uint16_t delay) 671 | { 672 | static uint32_t lastHostInt = 0; 673 | static uint32_t rollovers = 0; 674 | uint64_t timestamp; 675 | 676 | // Count times hostInt timestamps rolled over to produce upper bits 677 | if (hostInt < lastHostInt) { 678 | rollovers++; 679 | } 680 | lastHostInt = hostInt; 681 | 682 | timestamp = ((uint64_t)rollovers << 32); 683 | timestamp += hostInt + (referenceDelta + delay) * 100; 684 | 685 | return timestamp; 686 | } 687 | 688 | static void sensorhubInputHdlr(sh2_t *pSh2, uint8_t *payload, uint16_t len, uint32_t timestamp) 689 | { 690 | sh2_SensorEvent_t event; 691 | uint16_t cursor = 0; 692 | 693 | int32_t referenceDelta = 0; 694 | 695 | while (cursor < len) { 696 | // Get next report id 697 | uint8_t reportId = payload[cursor]; 698 | 699 | // Determine report length 700 | uint8_t reportLen = getReportLen(reportId); 701 | if (reportLen == 0) { 702 | // An unrecognized report id 703 | pSh2->unknownReportIds++; 704 | return; 705 | } 706 | else { 707 | if (reportId == SENSORHUB_BASE_TIMESTAMP_REF) { 708 | const BaseTimestampRef_t *rpt = (const BaseTimestampRef_t *)(payload+cursor); 709 | 710 | // store base timestamp reference 711 | referenceDelta = -rpt->timebase; 712 | } 713 | else if (reportId == SENSORHUB_TIMESTAMP_REBASE) { 714 | const TimestampRebase_t *rpt = (const TimestampRebase_t *)(payload+cursor); 715 | 716 | referenceDelta += rpt->timebase; 717 | } 718 | else if (reportId == SENSORHUB_FLUSH_COMPLETED) { 719 | // Route this as if it arrived on command channel. 720 | opRx(pSh2, payload+cursor, reportLen); 721 | } 722 | else { 723 | // Sensor event. Call callback 724 | uint8_t *pReport = payload+cursor; 725 | uint16_t delay = ((pReport[2] & 0xFC) << 6) + pReport[3]; 726 | event.timestamp_uS = touSTimestamp(timestamp, referenceDelta, delay); 727 | event.delay_uS = (referenceDelta + delay) * 100; 728 | event.reportId = reportId; 729 | memcpy(event.report, pReport, reportLen); 730 | event.len = reportLen; 731 | if (pSh2->sensorCallback != 0) { 732 | pSh2->sensorCallback(pSh2->sensorCookie, &event); 733 | } 734 | } 735 | 736 | // Move to next report in the payload 737 | cursor += reportLen; 738 | } 739 | } 740 | } 741 | 742 | static void sensorhubInputNormalHdlr(void *cookie, uint8_t *payload, uint16_t len, uint32_t timestamp) 743 | { 744 | sh2_t *pSh2 = (sh2_t *)cookie; 745 | 746 | sensorhubInputHdlr(pSh2, payload, len, timestamp); 747 | } 748 | 749 | static void sensorhubInputWakeHdlr(void *cookie, uint8_t *payload, uint16_t len, uint32_t timestamp) 750 | { 751 | sh2_t *pSh2 = (sh2_t *)cookie; 752 | 753 | sensorhubInputHdlr(pSh2, payload, len, timestamp); 754 | } 755 | 756 | static void sensorhubInputGyroRvHdlr(void *cookie, uint8_t *payload, uint16_t len, uint32_t timestamp) 757 | { 758 | sh2_t *pSh2 = (sh2_t *)cookie; 759 | sh2_SensorEvent_t event; 760 | uint16_t cursor = 0; 761 | 762 | uint8_t reportId = SH2_GYRO_INTEGRATED_RV; 763 | uint8_t reportLen = getReportLen(reportId); 764 | 765 | while (cursor < len) { 766 | event.timestamp_uS = timestamp; 767 | event.reportId = reportId; 768 | memcpy(event.report, payload+cursor, reportLen); 769 | event.len = reportLen; 770 | 771 | if (pSh2->sensorCallback != 0) { 772 | pSh2->sensorCallback(pSh2->sensorCookie, &event); 773 | } 774 | 775 | cursor += reportLen; 776 | } 777 | } 778 | 779 | static void executableDeviceHdlr(void *cookie, uint8_t *payload, uint16_t len, uint32_t timestamp) 780 | { 781 | (void)timestamp; // unused 782 | 783 | sh2_t *pSh2 = (sh2_t *)cookie; 784 | 785 | // Discard if length is bad 786 | if (len != 1) { 787 | pSh2->execBadPayload++; 788 | return; 789 | } 790 | 791 | switch (payload[0]) { 792 | case EXECUTABLE_DEVICE_RESP_RESET_COMPLETE: 793 | // reset process is now done. 794 | pSh2->resetComplete = true; 795 | 796 | // Send reset event to SH2 operation processor. 797 | // Some commands may handle themselves. Most will be aborted with SH2_ERR. 798 | opOnReset(pSh2); 799 | 800 | // Notify client that reset is complete. 801 | sh2AsyncEvent.eventId = SH2_RESET; 802 | if (pSh2->eventCallback) { 803 | pSh2->eventCallback(pSh2->eventCookie, &sh2AsyncEvent); 804 | } 805 | break; 806 | default: 807 | pSh2->execBadPayload++; 808 | break; 809 | } 810 | } 811 | 812 | static int sendExecutable(sh2_t *pSh2, uint8_t cmd) 813 | { 814 | return shtp_send(pSh2->pShtp, CHAN_EXECUTABLE_DEVICE, &cmd, 1); 815 | } 816 | 817 | static int sendCtrl(sh2_t *pSh2, const uint8_t *data, uint16_t len) 818 | { 819 | return shtp_send(pSh2->pShtp, CHAN_SENSORHUB_CONTROL, data, len); 820 | } 821 | 822 | static int16_t toQ14(double x) 823 | { 824 | int16_t retval = (int16_t)(x * (1<<14)); 825 | 826 | return retval; 827 | } 828 | 829 | // ------------------------------------------------------------------------ 830 | // Get Product ID support 831 | 832 | // Get Product ID Op handler 833 | static int getProdIdStart(sh2_t *pSh2) 834 | { 835 | int rc = SH2_OK; 836 | ProdIdReq_t req; 837 | 838 | pSh2->opData.getProdIds.nextEntry = 0; 839 | pSh2->opData.getProdIds.expectedEntries = 4; // Most products supply 4 product ids. 840 | // When the first arrives, we'll know if 841 | // we need to adjust this. 842 | 843 | // Set up request to issue 844 | memset(&req, 0, sizeof(req)); 845 | req.reportId = SENSORHUB_PROD_ID_REQ; 846 | rc = sendCtrl(pSh2, (uint8_t *)&req, sizeof(req)); 847 | 848 | return rc; 849 | } 850 | 851 | static void getProdIdRx(sh2_t *pSh2, const uint8_t *payload, uint16_t len) 852 | { 853 | (void)len; // unused 854 | 855 | ProdIdResp_t *resp = (ProdIdResp_t *)payload; 856 | 857 | // skip this if it isn't the product id response. 858 | if (resp->reportId != SENSORHUB_PROD_ID_RESP) return; 859 | 860 | // Store this product id, if we can 861 | sh2_ProductIds_t *pProdIds = pSh2->opData.getProdIds.pProdIds; 862 | 863 | if (pProdIds) { 864 | // Store the product id response 865 | if (pSh2->opData.getProdIds.nextEntry < pSh2->opData.getProdIds.expectedEntries) { 866 | sh2_ProductId_t *pProdId = &pProdIds->entry[pSh2->opData.getProdIds.nextEntry]; 867 | 868 | pProdId->resetCause = resp->resetCause; 869 | pProdId->swVersionMajor = resp->swVerMajor; 870 | pProdId->swVersionMinor = resp->swVerMinor; 871 | pProdId->swPartNumber = resp->swPartNumber; 872 | pProdId->swBuildNumber = resp->swBuildNumber; 873 | pProdId->swVersionPatch = resp->swVerPatch; 874 | pProdId->reserved0 = resp->reserved0; 875 | pProdId->reserved1 = resp->reserved1; 876 | 877 | if ((pProdId->swPartNumber == 10004095) || 878 | (pProdId->swPartNumber == 10004818) || 879 | (pProdId->swPartNumber == 10005028)) { 880 | // FSP200 has 5 product id entries 881 | pSh2->opData.getProdIds.expectedEntries = 5; 882 | } 883 | 884 | 885 | pSh2->opData.getProdIds.nextEntry++; 886 | } 887 | } 888 | 889 | // Complete this operation if there is no storage for more product ids 890 | if ((pSh2->opData.getProdIds.pProdIds == 0) || 891 | (pSh2->opData.getProdIds.nextEntry >= pSh2->opData.getProdIds.expectedEntries)) { 892 | 893 | pSh2->opData.getProdIds.pProdIds->numEntries = pSh2->opData.getProdIds.nextEntry; 894 | opCompleted(pSh2, SH2_OK); 895 | } 896 | 897 | return; 898 | } 899 | 900 | const sh2_Op_t getProdIdOp = { 901 | .start = getProdIdStart, 902 | .rx = getProdIdRx, 903 | }; 904 | 905 | // ------------------------------------------------------------------------ 906 | // Set Sensor Config 907 | 908 | static int getSensorConfigStart(sh2_t *pSh2) 909 | { 910 | int rc = SH2_OK; 911 | GetFeatureReq_t req; 912 | 913 | // set up request to issue 914 | memset(&req, 0, sizeof(req)); 915 | req.reportId = SENSORHUB_GET_FEATURE_REQ; 916 | req.featureReportId = pSh2->opData.getSensorConfig.sensorId; 917 | rc = sendCtrl(pSh2, (uint8_t *)&req, sizeof(req)); 918 | 919 | return rc; 920 | } 921 | 922 | static void getSensorConfigRx(sh2_t *pSh2, const uint8_t *payload, uint16_t len) 923 | { 924 | (void)len; // unused 925 | 926 | GetFeatureResp_t *resp = (GetFeatureResp_t *)payload; 927 | sh2_SensorConfig_t *pConfig; 928 | 929 | // skip this if it isn't the response we're waiting for. 930 | if (resp->reportId != SENSORHUB_GET_FEATURE_RESP) return; 931 | if (resp->featureReportId != pSh2->opData.getSensorConfig.sensorId) return; 932 | 933 | // Copy out data 934 | pConfig = pSh2->opData.getSensorConfig.pConfig; 935 | 936 | pConfig->changeSensitivityEnabled = ((resp->flags & FEAT_CHANGE_SENSITIVITY_ENABLED) != 0); 937 | pConfig->changeSensitivityRelative = ((resp->flags & FEAT_CHANGE_SENSITIVITY_RELATIVE) != 0); 938 | pConfig->wakeupEnabled = ((resp->flags & FEAT_WAKE_ENABLED) != 0); 939 | pConfig->alwaysOnEnabled = ((resp->flags & FEAT_ALWAYS_ON_ENABLED) != 0); 940 | pConfig->sniffEnabled = ((resp->flags & FEAT_SNIFF_ENABLED) !=0); 941 | pConfig->changeSensitivity = resp->changeSensitivity; 942 | pConfig->reportInterval_us = resp->reportInterval_uS; 943 | pConfig->batchInterval_us = resp->batchInterval_uS; 944 | pConfig->sensorSpecific = resp->sensorSpecific; 945 | 946 | // Complete this operation 947 | opCompleted(pSh2, SH2_OK); 948 | 949 | return; 950 | } 951 | 952 | const sh2_Op_t getSensorConfigOp = { 953 | .start = getSensorConfigStart, 954 | .rx = getSensorConfigRx, 955 | }; 956 | 957 | // ------------------------------------------------------------------------ 958 | // Set Sensor Config 959 | 960 | // SENSORHUB_SET_FEATURE_CMD 961 | #define SENSORHUB_SET_FEATURE_CMD (0xFD) 962 | typedef PACKED_STRUCT { 963 | uint8_t reportId; // 0xFD 964 | uint8_t featureReportId; // sensor id 965 | uint8_t flags; // FEAT_... values 966 | uint16_t changeSensitivity; 967 | uint32_t reportInterval_uS; 968 | uint32_t batchInterval_uS; 969 | uint32_t sensorSpecific; 970 | } SetFeatureReport_t; 971 | 972 | static int setSensorConfigStart(sh2_t *pSh2) 973 | { 974 | SetFeatureReport_t req; 975 | uint8_t flags = 0; 976 | int rc; 977 | sh2_SensorConfig_t *pConfig = pSh2->opData.getSensorConfig.pConfig; 978 | 979 | if (pConfig->changeSensitivityEnabled) flags |= FEAT_CHANGE_SENSITIVITY_ENABLED; 980 | if (pConfig->changeSensitivityRelative) flags |= FEAT_CHANGE_SENSITIVITY_RELATIVE; 981 | if (pConfig->wakeupEnabled) flags |= FEAT_WAKE_ENABLED; 982 | if (pConfig->alwaysOnEnabled) flags |= FEAT_ALWAYS_ON_ENABLED; 983 | if (pConfig->sniffEnabled) flags |= FEAT_SNIFF_ENABLED; 984 | 985 | memset(&req, 0, sizeof(req)); 986 | req.reportId = SENSORHUB_SET_FEATURE_CMD; 987 | req.featureReportId = pSh2->opData.setSensorConfig.sensorId; 988 | req.flags = flags; 989 | req.changeSensitivity = pConfig->changeSensitivity; 990 | req.reportInterval_uS = pConfig->reportInterval_us; 991 | req.batchInterval_uS = pConfig->batchInterval_us; 992 | req.sensorSpecific = pConfig->sensorSpecific; 993 | 994 | rc = sendCtrl(pSh2, (uint8_t *)&req, sizeof(req)); 995 | opCompleted(pSh2, rc); 996 | 997 | return rc; 998 | } 999 | 1000 | const sh2_Op_t setSensorConfigOp = { 1001 | .start = setSensorConfigStart, 1002 | }; 1003 | 1004 | // ------------------------------------------------------------------------ 1005 | // Get FRS. 1006 | 1007 | 1008 | static int getFrsStart(sh2_t *pSh2) 1009 | { 1010 | int rc = SH2_OK; 1011 | FrsReadReq_t req; 1012 | 1013 | pSh2->opData.getFrs.nextOffset = 0; 1014 | 1015 | // set up request to issue 1016 | memset(&req, 0, sizeof(req)); 1017 | req.reportId = SENSORHUB_FRS_READ_REQ; 1018 | req.reserved = 0; 1019 | req.readOffset = 0; // read from start 1020 | req.frsType = pSh2->opData.getFrs.frsType; 1021 | req.blockSize = 0; // read all avail data 1022 | 1023 | rc = sendCtrl(pSh2, (uint8_t *)&req, sizeof(req)); 1024 | 1025 | return rc; 1026 | } 1027 | 1028 | static void getFrsRx(sh2_t *pSh2, const uint8_t *payload, uint16_t len) 1029 | { 1030 | (void)len; // unused 1031 | 1032 | FrsReadResp_t *resp = (FrsReadResp_t *)payload; 1033 | uint8_t status; 1034 | 1035 | // skip this if it isn't the response we're looking for 1036 | if (resp->reportId != SENSORHUB_FRS_READ_RESP) return; 1037 | 1038 | // Check for errors: Unrecognized FRS type, Busy, Out of range, Device error 1039 | status = FRS_READ_STATUS(resp->len_status); 1040 | if ((status == FRS_READ_STATUS_UNRECOGNIZED_FRS_TYPE) || 1041 | (status == FRS_READ_STATUS_BUSY) || 1042 | (status == FRS_READ_STATUS_OFFSET_OUT_OF_RANGE) || 1043 | (status == FRS_READ_STATUS_DEVICE_ERROR) 1044 | ) { 1045 | // Operation failed 1046 | opCompleted(pSh2, SH2_ERR_HUB); 1047 | return; 1048 | } 1049 | 1050 | if (status == FRS_READ_STATUS_RECORD_EMPTY) { 1051 | // Empty record, return zero length. 1052 | *(pSh2->opData.getFrs.pWords) = 0; 1053 | opCompleted(pSh2, SH2_OK); 1054 | } 1055 | 1056 | // Store the contents from this response 1057 | uint16_t offset = resp->wordOffset; 1058 | 1059 | // check for missed offsets, resulting in error. 1060 | if (offset != pSh2->opData.getFrs.nextOffset) { 1061 | // Some data was dropped. 1062 | *(pSh2->opData.getFrs.pWords) = 0; 1063 | opCompleted(pSh2, SH2_ERR_IO); 1064 | } 1065 | 1066 | // store first word, if we have room 1067 | if ((*(pSh2->opData.getFrs.pWords) == 0) || 1068 | (offset <= *(pSh2->opData.getFrs.pWords))) { 1069 | pSh2->opData.getFrs.pData[offset] = resp->data0; 1070 | pSh2->opData.getFrs.nextOffset = offset+1; 1071 | } 1072 | 1073 | // store second word if there is one and we have room 1074 | if ((FRS_READ_DATALEN(resp->len_status) == 2) && 1075 | ((*(pSh2->opData.getFrs.pWords) == 0) || 1076 | (offset <= *(pSh2->opData.getFrs.pWords)))) { 1077 | pSh2->opData.getFrs.pData[offset+1] = resp->data1; 1078 | pSh2->opData.getFrs.nextOffset = offset+2; 1079 | } 1080 | 1081 | // If read is done, complete the operation 1082 | if ((status == FRS_READ_STATUS_READ_RECORD_COMPLETED) || 1083 | (status == FRS_READ_STATUS_READ_BLOCK_COMPLETED) || 1084 | (status == FRS_READ_STATUS_READ_BLOCK_AND_RECORD_COMPLETED)) { 1085 | *(pSh2->opData.getFrs.pWords) = pSh2->opData.getFrs.nextOffset; 1086 | 1087 | opCompleted(pSh2, SH2_OK); 1088 | } 1089 | 1090 | return; 1091 | } 1092 | 1093 | const sh2_Op_t getFrsOp = { 1094 | .start = getFrsStart, 1095 | .rx = getFrsRx, 1096 | }; 1097 | 1098 | // ------------------------------------------------------------------------ 1099 | // Support for sh2_getMetadata 1100 | 1101 | static const struct { 1102 | sh2_SensorId_t sensorId; 1103 | uint16_t recordId; 1104 | } sensorToRecordMap[] = { 1105 | { SH2_RAW_ACCELEROMETER, FRS_ID_META_RAW_ACCELEROMETER }, 1106 | { SH2_ACCELEROMETER, FRS_ID_META_ACCELEROMETER }, 1107 | { SH2_LINEAR_ACCELERATION, FRS_ID_META_LINEAR_ACCELERATION }, 1108 | { SH2_GRAVITY, FRS_ID_META_GRAVITY }, 1109 | { SH2_RAW_GYROSCOPE, FRS_ID_META_RAW_GYROSCOPE }, 1110 | { SH2_GYROSCOPE_CALIBRATED, FRS_ID_META_GYROSCOPE_CALIBRATED }, 1111 | { SH2_GYROSCOPE_UNCALIBRATED, FRS_ID_META_GYROSCOPE_UNCALIBRATED }, 1112 | { SH2_RAW_MAGNETOMETER, FRS_ID_META_RAW_MAGNETOMETER }, 1113 | { SH2_MAGNETIC_FIELD_CALIBRATED, FRS_ID_META_MAGNETIC_FIELD_CALIBRATED }, 1114 | { SH2_MAGNETIC_FIELD_UNCALIBRATED, FRS_ID_META_MAGNETIC_FIELD_UNCALIBRATED }, 1115 | { SH2_ROTATION_VECTOR, FRS_ID_META_ROTATION_VECTOR }, 1116 | { SH2_GAME_ROTATION_VECTOR, FRS_ID_META_GAME_ROTATION_VECTOR }, 1117 | { SH2_GEOMAGNETIC_ROTATION_VECTOR, FRS_ID_META_GEOMAGNETIC_ROTATION_VECTOR }, 1118 | { SH2_PRESSURE, FRS_ID_META_PRESSURE }, 1119 | { SH2_AMBIENT_LIGHT, FRS_ID_META_AMBIENT_LIGHT }, 1120 | { SH2_HUMIDITY, FRS_ID_META_HUMIDITY }, 1121 | { SH2_PROXIMITY, FRS_ID_META_PROXIMITY }, 1122 | { SH2_TEMPERATURE, FRS_ID_META_TEMPERATURE }, 1123 | { SH2_TAP_DETECTOR, FRS_ID_META_TAP_DETECTOR }, 1124 | { SH2_STEP_DETECTOR, FRS_ID_META_STEP_DETECTOR }, 1125 | { SH2_STEP_COUNTER, FRS_ID_META_STEP_COUNTER }, 1126 | { SH2_SIGNIFICANT_MOTION, FRS_ID_META_SIGNIFICANT_MOTION }, 1127 | { SH2_STABILITY_CLASSIFIER, FRS_ID_META_STABILITY_CLASSIFIER }, 1128 | { SH2_SHAKE_DETECTOR, FRS_ID_META_SHAKE_DETECTOR }, 1129 | { SH2_FLIP_DETECTOR, FRS_ID_META_FLIP_DETECTOR }, 1130 | { SH2_PICKUP_DETECTOR, FRS_ID_META_PICKUP_DETECTOR }, 1131 | { SH2_STABILITY_DETECTOR, FRS_ID_META_STABILITY_DETECTOR }, 1132 | { SH2_PERSONAL_ACTIVITY_CLASSIFIER, FRS_ID_META_PERSONAL_ACTIVITY_CLASSIFIER }, 1133 | { SH2_SLEEP_DETECTOR, FRS_ID_META_SLEEP_DETECTOR }, 1134 | { SH2_TILT_DETECTOR, FRS_ID_META_TILT_DETECTOR }, 1135 | { SH2_POCKET_DETECTOR, FRS_ID_META_POCKET_DETECTOR }, 1136 | { SH2_CIRCLE_DETECTOR, FRS_ID_META_CIRCLE_DETECTOR }, 1137 | }; 1138 | 1139 | static void stuffMetadata(sh2_SensorMetadata_t *pData, uint32_t *frsData) 1140 | { 1141 | // Populate the sensorMetadata structure with results 1142 | pData->meVersion = (frsData[0] >> 0) & 0xFF; 1143 | pData->mhVersion = (frsData[0] >> 8) & 0xFF; 1144 | pData->shVersion = (frsData[0] >> 16) & 0xFF; 1145 | pData->range = frsData[1]; 1146 | pData->resolution = frsData[2]; 1147 | pData->power_mA = (frsData[3] >> 0) & 0xFFFF; // 16.10 format 1148 | pData->revision = (frsData[3] >> 16) & 0xFFFF; 1149 | pData->minPeriod_uS = frsData[4]; 1150 | pData->maxPeriod_uS = 0; // ...unless reading format 4 metadata below 1151 | pData->fifoMax = (frsData[5] >> 0) & 0xFFFF; 1152 | pData->fifoReserved = (frsData[5] >> 16) & 0xFFFF; 1153 | pData->batchBufferBytes = (frsData[6] >> 0) & 0xFFFF;; 1154 | pData->vendorIdLen = (frsData[6] >> 16) & 0xFFFF; 1155 | 1156 | // Init fields that may not be present, depending on metadata revision 1157 | pData->qPoint1 = 0; 1158 | pData->qPoint2 = 0; 1159 | pData->qPoint3 = 0; 1160 | pData->sensorSpecificLen = 0; 1161 | strcpy(pData->vendorId, ""); // init with empty string in case vendorIdLen == 0 1162 | 1163 | int vendorIdOffset = 8; 1164 | // Get revision-specific fields 1165 | if (pData->revision == 0) { 1166 | // No fixed fields, vendor id copied after if-else block 1167 | } 1168 | else if (pData->revision == 1) { 1169 | pData->qPoint1 = (frsData[7] >> 0) & 0xFFFF; 1170 | pData->qPoint2 = (frsData[7] >> 16) & 0xFFFF; 1171 | } 1172 | else if (pData->revision == 2) { 1173 | pData->qPoint1 = (frsData[7] >> 0) & 0xFFFF; 1174 | pData->qPoint2 = (frsData[7] >> 16) & 0xFFFF; 1175 | pData->sensorSpecificLen = (frsData[8] >> 0) & 0xFFFF; 1176 | memcpy(pData->sensorSpecific, (uint8_t *)&frsData[9], pData->sensorSpecificLen); 1177 | vendorIdOffset = 9 + ((pData->sensorSpecificLen+3)/4); // 9 + one word for every 4 bytes of SS data 1178 | } 1179 | else if (pData->revision == 3) { 1180 | pData->qPoint1 = (frsData[7] >> 0) & 0xFFFF; 1181 | pData->qPoint2 = (frsData[7] >> 16) & 0xFFFF; 1182 | pData->sensorSpecificLen = (frsData[8] >> 0) & 0xFFFF; 1183 | pData->qPoint3 = (frsData[8] >> 16) & 0xFFFF; 1184 | memcpy(pData->sensorSpecific, (uint8_t *)&frsData[9], pData->sensorSpecificLen); 1185 | vendorIdOffset = 9 + ((pData->sensorSpecificLen+3)/4); // 9 + one word for every 4 bytes of SS data 1186 | } 1187 | else if (pData->revision == 4) { 1188 | pData->qPoint1 = (frsData[7] >> 0) & 0xFFFF; 1189 | pData->qPoint2 = (frsData[7] >> 16) & 0xFFFF; 1190 | pData->sensorSpecificLen = (frsData[8] >> 0) & 0xFFFF; 1191 | pData->qPoint3 = (frsData[8] >> 16) & 0xFFFF; 1192 | pData->maxPeriod_uS = frsData[9]; 1193 | memcpy(pData->sensorSpecific, (uint8_t *)&frsData[10], pData->sensorSpecificLen); 1194 | vendorIdOffset = 10 + ((pData->sensorSpecificLen+3)/4); // 9 + one word for every 4 bytes of SS data 1195 | } 1196 | else { 1197 | // Unrecognized revision! 1198 | } 1199 | 1200 | // Copy vendor id 1201 | memcpy(pData->vendorId, (uint8_t *)&frsData[vendorIdOffset], 1202 | pData->vendorIdLen); 1203 | } 1204 | 1205 | // ------------------------------------------------------------------------ 1206 | // Set FRS. 1207 | 1208 | static int setFrsStart(sh2_t *pSh2) 1209 | { 1210 | int rc = SH2_OK; 1211 | FrsWriteReq_t req; 1212 | 1213 | pSh2->opData.setFrs.offset = 0; 1214 | 1215 | // set up request to issue 1216 | memset(&req, 0, sizeof(req)); 1217 | req.reportId = SENSORHUB_FRS_WRITE_REQ; 1218 | req.reserved = 0; 1219 | req.length = pSh2->opData.setFrs.words; 1220 | req.frsType = pSh2->opData.getFrs.frsType; 1221 | 1222 | rc = sendCtrl(pSh2, (uint8_t *)&req, sizeof(req)); 1223 | 1224 | return rc; 1225 | } 1226 | 1227 | static void setFrsRx(sh2_t *pSh2, const uint8_t *payload, uint16_t len) 1228 | { 1229 | (void)len; // unused 1230 | 1231 | FrsWriteResp_t *resp = (FrsWriteResp_t *)payload; 1232 | FrsWriteDataReq_t req; 1233 | uint8_t status; 1234 | bool sendMoreData = false; 1235 | bool completed = false; 1236 | int rc = SH2_OK; 1237 | 1238 | // skip this if it isn't the response we're looking for. 1239 | if (resp->reportId != SENSORHUB_FRS_WRITE_RESP) return; 1240 | 1241 | // Check for errors: Unrecognized FRS type, Busy, Out of range, Device error 1242 | status = resp->status; 1243 | switch(status) { 1244 | case FRS_WRITE_STATUS_RECEIVED: 1245 | case FRS_WRITE_STATUS_READY: 1246 | sendMoreData = true; 1247 | break; 1248 | case FRS_WRITE_STATUS_UNRECOGNIZED_FRS_TYPE: 1249 | case FRS_WRITE_STATUS_BUSY: 1250 | case FRS_WRITE_STATUS_FAILED: 1251 | case FRS_WRITE_STATUS_NOT_READY: 1252 | case FRS_WRITE_STATUS_INVALID_LENGTH: 1253 | case FRS_WRITE_STATUS_INVALID_RECORD: 1254 | case FRS_WRITE_STATUS_DEVICE_ERROR: 1255 | case FRS_WRITE_STATUS_READ_ONLY: 1256 | rc = SH2_ERR_HUB; 1257 | completed = true; 1258 | break; 1259 | case FRS_WRITE_STATUS_WRITE_COMPLETED: 1260 | // Successful completion 1261 | rc = SH2_OK; 1262 | completed = true; 1263 | break; 1264 | case FRS_WRITE_STATUS_RECORD_VALID: 1265 | // That's nice, keep waiting 1266 | break; 1267 | } 1268 | 1269 | // if we should send more data, do it. 1270 | if (sendMoreData && 1271 | (pSh2->opData.setFrs.offset < pSh2->opData.setFrs.words)) { 1272 | uint16_t offset = pSh2->opData.setFrs.offset; 1273 | 1274 | memset(&req, 0, sizeof(req)); 1275 | req.reportId = SENSORHUB_FRS_WRITE_DATA_REQ; 1276 | req.reserved = 0; 1277 | req.offset = offset; 1278 | req.data0 = pSh2->opData.setFrs.pData[offset++]; 1279 | if (offset < pSh2->opData.setFrs.words) { 1280 | req.data1 = pSh2->opData.setFrs.pData[offset++]; 1281 | } else { 1282 | req.data1 = 0; 1283 | } 1284 | pSh2->opData.setFrs.offset = offset; 1285 | 1286 | rc = sendCtrl(pSh2, (uint8_t *)&req, sizeof(req)); 1287 | } 1288 | 1289 | // if the operation is done or has to be aborted, complete it 1290 | if (completed) { 1291 | opCompleted(pSh2, rc); 1292 | } 1293 | 1294 | return; 1295 | } 1296 | 1297 | const sh2_Op_t setFrsOp = { 1298 | .start = setFrsStart, 1299 | .rx = setFrsRx, 1300 | }; 1301 | 1302 | // ------------------------------------------------------------------------ 1303 | // Support for sending commands 1304 | 1305 | static int sendCmd(sh2_t *pSh2, uint8_t cmd, uint8_t p[COMMAND_PARAMS]) 1306 | { 1307 | int rc = SH2_OK; 1308 | CommandReq_t req; 1309 | 1310 | // Clear request structure 1311 | memset(&req, 0, sizeof(req)); 1312 | 1313 | // Create a command sequence number for this command 1314 | pSh2->lastCmdId = cmd; 1315 | pSh2->cmdSeq = pSh2->nextCmdSeq++; 1316 | 1317 | // set up request to issue 1318 | req.reportId = SENSORHUB_COMMAND_REQ; 1319 | req.seq = pSh2->cmdSeq; 1320 | req.command = cmd; 1321 | for (int n = 0; n < COMMAND_PARAMS; n++) { 1322 | req.p[n] = p[n]; 1323 | } 1324 | rc = sendCtrl(pSh2, (uint8_t *)&req, sizeof(req)); 1325 | 1326 | return rc; 1327 | } 1328 | 1329 | // Send a command with 0 parameters 1330 | static int sendCmd0(sh2_t *pSh2, uint8_t cmd) 1331 | { 1332 | uint8_t p[COMMAND_PARAMS]; 1333 | 1334 | memset(p, 0, COMMAND_PARAMS); 1335 | 1336 | return sendCmd(pSh2, cmd, p); 1337 | } 1338 | 1339 | // Send a command with 1 parameter 1340 | static int sendCmd1(sh2_t *pSh2, uint8_t cmd, uint8_t p0) 1341 | { 1342 | uint8_t p[COMMAND_PARAMS]; 1343 | 1344 | memset(p, 0, COMMAND_PARAMS); 1345 | 1346 | p[0] = p0; 1347 | return sendCmd(pSh2, cmd, p); 1348 | } 1349 | 1350 | // Send a command with 2 parameters 1351 | static int sendCmd2(sh2_t *pSh2, uint8_t cmd, uint8_t p0, uint8_t p1) 1352 | { 1353 | uint8_t p[COMMAND_PARAMS]; 1354 | 1355 | memset(p, 0, COMMAND_PARAMS); 1356 | 1357 | p[0] = p0; 1358 | p[1] = p1; 1359 | return sendCmd(pSh2, cmd, p); 1360 | } 1361 | 1362 | static bool wrongResponse(sh2_t *pSh2, CommandResp_t *resp) 1363 | { 1364 | if (resp->reportId != SENSORHUB_COMMAND_RESP) return true; 1365 | if (resp->command != pSh2->lastCmdId) return true; 1366 | if (resp->commandSeq != pSh2->cmdSeq) return true; 1367 | 1368 | return false; 1369 | } 1370 | 1371 | // ------------------------------------------------------------------------ 1372 | // Get Errors 1373 | 1374 | static int getErrorsStart(sh2_t *pSh2) 1375 | { 1376 | // Initialize state 1377 | pSh2->opData.getErrors.errsRead = 0; 1378 | 1379 | return sendCmd1(pSh2, SH2_CMD_ERRORS, pSh2->opData.getErrors.severity); 1380 | } 1381 | 1382 | static void getErrorsRx(sh2_t *pSh2, const uint8_t *payload, uint16_t len) 1383 | { 1384 | (void)len; // unused 1385 | 1386 | CommandResp_t *resp = (CommandResp_t *)payload; 1387 | 1388 | // skip this if it isn't the right response 1389 | if (wrongResponse(pSh2, resp)) return; 1390 | 1391 | if (resp->r[2] == 255) { 1392 | // No error to report, operation is complete 1393 | *(pSh2->opData.getErrors.pNumErrors) = pSh2->opData.getErrors.errsRead; 1394 | opCompleted(pSh2, SH2_OK); 1395 | } else { 1396 | // Copy data for invoker. 1397 | unsigned int index = pSh2->opData.getErrors.errsRead; 1398 | if (index < *(pSh2->opData.getErrors.pNumErrors)) { 1399 | // We have room for this one. 1400 | pSh2->opData.getErrors.pErrors[index].severity = resp->r[0]; 1401 | pSh2->opData.getErrors.pErrors[index].sequence = resp->r[1]; 1402 | pSh2->opData.getErrors.pErrors[index].source = resp->r[2]; 1403 | pSh2->opData.getErrors.pErrors[index].error = resp->r[3]; 1404 | pSh2->opData.getErrors.pErrors[index].module = resp->r[4]; 1405 | pSh2->opData.getErrors.pErrors[index].code = resp->r[5]; 1406 | 1407 | pSh2->opData.getErrors.errsRead++; 1408 | } 1409 | } 1410 | 1411 | return; 1412 | } 1413 | 1414 | const sh2_Op_t getErrorsOp = { 1415 | .start = getErrorsStart, 1416 | .rx = getErrorsRx, 1417 | }; 1418 | 1419 | // ------------------------------------------------------------------------ 1420 | // Get Counts 1421 | 1422 | static int getCountsStart(sh2_t *pSh2) 1423 | { 1424 | return sendCmd2(pSh2, SH2_CMD_COUNTS, SH2_COUNTS_GET_COUNTS, pSh2->opData.getCounts.sensorId); 1425 | } 1426 | 1427 | static void getCountsRx(sh2_t *pSh2, const uint8_t *payload, uint16_t len) 1428 | { 1429 | (void)len; // unused 1430 | 1431 | CommandResp_t *resp = (CommandResp_t *)payload; 1432 | 1433 | if (wrongResponse(pSh2, resp)) return; 1434 | 1435 | // Store results 1436 | if (resp->respSeq == 0) { 1437 | pSh2->opData.getCounts.pCounts->offered = readu32(&resp->r[3]); 1438 | pSh2->opData.getCounts.pCounts->accepted = readu32(&resp->r[7]); 1439 | } 1440 | else { 1441 | pSh2->opData.getCounts.pCounts->on = readu32(&resp->r[3]); 1442 | pSh2->opData.getCounts.pCounts->attempted = readu32(&resp->r[7]); 1443 | } 1444 | 1445 | // Complete this operation if we've received last response 1446 | if (resp->respSeq == 1) { 1447 | opCompleted(pSh2, SH2_OK); 1448 | } 1449 | 1450 | return; 1451 | } 1452 | 1453 | const sh2_Op_t getCountsOp = { 1454 | .start = getCountsStart, 1455 | .rx = getCountsRx, 1456 | }; 1457 | 1458 | // ------------------------------------------------------------------------ 1459 | // Generic Send Command 1460 | 1461 | static int sendCmdStart(sh2_t *pSh2) 1462 | { 1463 | int status = sendCmd(pSh2, pSh2->opData.sendCmd.req.command, 1464 | pSh2->opData.sendCmd.req.p); 1465 | 1466 | opCompleted(pSh2, status); 1467 | 1468 | return status; 1469 | } 1470 | 1471 | const sh2_Op_t sendCmdOp = { 1472 | .start = sendCmdStart, 1473 | }; 1474 | 1475 | // ------------------------------------------------------------------------ 1476 | // Reinit 1477 | 1478 | static int reinitStart(sh2_t *pSh2) 1479 | { 1480 | int status = sendCmd1(pSh2, SH2_CMD_INITIALIZE, SH2_INIT_SYSTEM); 1481 | 1482 | return status; 1483 | } 1484 | 1485 | static void reinitRx(sh2_t *pSh2, const uint8_t *payload, uint16_t len) 1486 | { 1487 | (void)len; // unused 1488 | CommandResp_t *resp = (CommandResp_t *)payload; 1489 | 1490 | // Ignore message if it doesn't pertain to this operation 1491 | if (wrongResponse(pSh2, resp)) return; 1492 | 1493 | // Get return status 1494 | if (resp->r[0] != 0) { 1495 | pSh2->opStatus = SH2_ERR_HUB; 1496 | opCompleted(pSh2, pSh2->opStatus); 1497 | } 1498 | else { 1499 | pSh2->opStatus = SH2_OK; 1500 | opCompleted(pSh2, pSh2->opStatus); 1501 | } 1502 | } 1503 | 1504 | const sh2_Op_t reinitOp = { 1505 | .start = reinitStart, 1506 | .rx = reinitRx, 1507 | }; 1508 | 1509 | // ------------------------------------------------------------------------ 1510 | // Save DCD Now 1511 | 1512 | static int saveDcdNowStart(sh2_t *pSh2) 1513 | { 1514 | int status = sendCmd0(pSh2, SH2_CMD_DCD); 1515 | 1516 | return status; 1517 | } 1518 | 1519 | static void saveDcdNowRx(sh2_t *pSh2, const uint8_t *payload, uint16_t len) 1520 | { 1521 | (void)len; // unused 1522 | CommandResp_t *resp = (CommandResp_t *)payload; 1523 | 1524 | // Ignore message if it doesn't pertain to this operation 1525 | if (wrongResponse(pSh2, resp)) return; 1526 | 1527 | // Get return status 1528 | int status = SH2_OK; 1529 | if (resp->r[0] != 0) { 1530 | status = SH2_ERR_HUB; 1531 | } 1532 | 1533 | opCompleted(pSh2, status); 1534 | } 1535 | 1536 | const sh2_Op_t saveDcdNowOp = { 1537 | .start = saveDcdNowStart, 1538 | .rx = saveDcdNowRx, 1539 | }; 1540 | 1541 | // ------------------------------------------------------------------------ 1542 | // Get Osc Type 1543 | 1544 | static int getOscTypeStart(sh2_t *pSh2) 1545 | { 1546 | return sendCmd0(pSh2, SH2_CMD_GET_OSC_TYPE); 1547 | } 1548 | 1549 | static void getOscTypeRx(sh2_t *pSh2, const uint8_t *payload, uint16_t len) 1550 | { 1551 | (void)len; // unused 1552 | CommandResp_t *resp = (CommandResp_t *)payload; 1553 | sh2_OscType_t *pOscType; 1554 | 1555 | // Ignore message if it doesn't pertain to this operation 1556 | if (wrongResponse(pSh2, resp)) return; 1557 | 1558 | // Read out data 1559 | pOscType = pSh2->opData.getOscType.pOscType; 1560 | *pOscType = (sh2_OscType_t)resp->r[0]; 1561 | 1562 | // Complete this operation 1563 | opCompleted(pSh2, SH2_OK); 1564 | } 1565 | 1566 | const sh2_Op_t getOscTypeOp = { 1567 | .start = getOscTypeStart, 1568 | .rx = getOscTypeRx, 1569 | }; 1570 | 1571 | // ------------------------------------------------------------------------ 1572 | // Set Cal Config 1573 | 1574 | static int setCalConfigStart(sh2_t *pSh2) 1575 | { 1576 | uint8_t p[COMMAND_PARAMS]; 1577 | 1578 | // Clear p. (Importantly, set subcommand in p[3] to 0, CONFIGURE) 1579 | memset(p, 0, COMMAND_PARAMS); 1580 | 1581 | // Which cal processes to enable/disable 1582 | p[0] = (pSh2->opData.calConfig.sensors & SH2_CAL_ACCEL) ? 1 : 0; // accel cal 1583 | p[1] = (pSh2->opData.calConfig.sensors & SH2_CAL_GYRO) ? 1 : 0; // gyro cal 1584 | p[2] = (pSh2->opData.calConfig.sensors & SH2_CAL_MAG) ? 1 : 0; // mag cal 1585 | p[4] = (pSh2->opData.calConfig.sensors & SH2_CAL_PLANAR) ? 1 : 0; // planar cal 1586 | p[5] = (pSh2->opData.calConfig.sensors & SH2_CAL_ON_TABLE) ? 1 : 0; // on-table cal 1587 | 1588 | p[6] = (pSh2->opData.calConfig.sensors & SH2_CAL_ZERO_GYRO_CONTROL_MASK) >> 5; 1589 | 1590 | return sendCmd(pSh2, SH2_CMD_ME_CAL, p); 1591 | } 1592 | 1593 | static void setCalConfigRx(sh2_t *pSh2, const uint8_t *payload, uint16_t len) 1594 | { 1595 | (void)len; // unused 1596 | 1597 | CommandResp_t *resp = (CommandResp_t *)payload; 1598 | 1599 | // Ignore message if it doesn't pertain to this operation 1600 | if (wrongResponse(pSh2, resp)) return; 1601 | 1602 | // Read out data 1603 | int status = SH2_OK; 1604 | if (resp->r[0] != 0) { 1605 | status = SH2_ERR_HUB; 1606 | } 1607 | 1608 | // Complete this operation 1609 | opCompleted(pSh2, status); 1610 | } 1611 | 1612 | const sh2_Op_t setCalConfigOp = { 1613 | .start = setCalConfigStart, 1614 | .rx = setCalConfigRx, 1615 | }; 1616 | 1617 | // ------------------------------------------------------------------------ 1618 | // Get Cal Config 1619 | 1620 | static int getCalConfigStart(sh2_t *pSh2) 1621 | { 1622 | uint8_t p[COMMAND_PARAMS]; 1623 | 1624 | // Clear p. (Importantly, set subcommand in p[3] to 0, CONFIGURE) 1625 | memset(p, 0, COMMAND_PARAMS); 1626 | 1627 | // Subcommand: Get ME Calibration 1628 | p[3] = 0x01; 1629 | 1630 | return sendCmd(pSh2, SH2_CMD_ME_CAL, p); 1631 | } 1632 | 1633 | static void getCalConfigRx(sh2_t *pSh2, const uint8_t *payload, uint16_t len) 1634 | { 1635 | (void)len; // unused 1636 | CommandResp_t *resp = (CommandResp_t *)payload; 1637 | 1638 | // Ignore message if it doesn't pertain to this operation 1639 | if (wrongResponse(pSh2, resp)) return; 1640 | 1641 | // Read out data 1642 | int status = SH2_OK; 1643 | if (resp->r[0] != 0) { 1644 | status = SH2_ERR_HUB; 1645 | } 1646 | else { 1647 | // Unload results into pSensors 1648 | uint8_t sensors = 0; 1649 | if (resp->r[1]) sensors |= SH2_CAL_ACCEL; 1650 | if (resp->r[2]) sensors |= SH2_CAL_GYRO; 1651 | if (resp->r[3]) sensors |= SH2_CAL_MAG; 1652 | if (resp->r[4]) sensors |= SH2_CAL_PLANAR; 1653 | *(pSh2->opData.getCalConfig.pSensors) = sensors; 1654 | } 1655 | 1656 | // Complete this operation 1657 | opCompleted(pSh2, status); 1658 | } 1659 | 1660 | 1661 | const sh2_Op_t getCalConfigOp = { 1662 | .start = getCalConfigStart, 1663 | .rx = getCalConfigRx, 1664 | }; 1665 | 1666 | // ------------------------------------------------------------------------ 1667 | // Force Flush 1668 | 1669 | static int forceFlushStart(sh2_t *pSh2) 1670 | { 1671 | ForceFlushReq_t req; 1672 | 1673 | memset(&req, 0, sizeof(req)); 1674 | req.reportId = SENSORHUB_FORCE_SENSOR_FLUSH; 1675 | req.sensorId = pSh2->opData.forceFlush.sensorId; 1676 | 1677 | return sendCtrl(pSh2, (uint8_t *)&req, sizeof(req)); 1678 | } 1679 | 1680 | static void forceFlushRx(sh2_t *pSh2, const uint8_t *payload, uint16_t len) 1681 | { 1682 | (void)len; // unused 1683 | 1684 | ForceFlushResp_t *resp = (ForceFlushResp_t *)payload; 1685 | 1686 | // Ignore message if it doesn't pertain to this operation 1687 | if (resp->reportId != SENSORHUB_FLUSH_COMPLETED) return; 1688 | if (resp->sensorId != pSh2->opData.forceFlush.sensorId) return; 1689 | 1690 | // Complete this operation 1691 | opCompleted(pSh2, SH2_OK); 1692 | } 1693 | 1694 | const sh2_Op_t forceFlushOp = { 1695 | .start = forceFlushStart, 1696 | .rx = forceFlushRx, 1697 | }; 1698 | 1699 | // ------------------------------------------------------------------------ 1700 | // Clear DCD And Reset 1701 | 1702 | static int clearDcdAndResetStart(sh2_t *pSh2) 1703 | { 1704 | pSh2->resetComplete = false; 1705 | int status = sendCmd0(pSh2, SH2_CMD_CLEAR_DCD_AND_RESET); 1706 | 1707 | return status; 1708 | } 1709 | 1710 | static void clearDcdAndResetOnReset(sh2_t *pSh2) 1711 | { 1712 | // When reset is detected, this op is complete. 1713 | opCompleted(pSh2, SH2_OK); 1714 | } 1715 | 1716 | const sh2_Op_t clearDcdAndResetOp = { 1717 | .start = clearDcdAndResetStart, 1718 | .onReset = clearDcdAndResetOnReset, 1719 | }; 1720 | 1721 | // ------------------------------------------------------------------------ 1722 | // Start Cal 1723 | 1724 | static int startCalStart(sh2_t *pSh2) 1725 | { 1726 | uint8_t p[COMMAND_PARAMS]; 1727 | 1728 | // Clear p. (Importantly, set subcommand in p[3] to 0, CONFIGURE) 1729 | memset(p, 0, COMMAND_PARAMS); 1730 | 1731 | // Subcommand: Get ME Calibration 1732 | p[0] = SH2_CAL_START; 1733 | p[1] = pSh2->opData.startCal.interval_us & 0xFF; // LSB 1734 | p[2] = (pSh2->opData.startCal.interval_us >> 8) & 0xFF; 1735 | p[3] = (pSh2->opData.startCal.interval_us >> 16) & 0xFF; 1736 | p[4] = (pSh2->opData.startCal.interval_us >> 24) & 0xFF; // MSB 1737 | 1738 | return sendCmd(pSh2, SH2_CMD_CAL, p); 1739 | } 1740 | 1741 | static void startCalRx(sh2_t *pSh2, const uint8_t *payload, uint16_t len) 1742 | { 1743 | (void)len; // unused 1744 | 1745 | CommandResp_t *resp = (CommandResp_t *)payload; 1746 | 1747 | // Ignore message if it doesn't pertain to this operation 1748 | if (wrongResponse(pSh2, resp)) return; 1749 | 1750 | // Complete this operation 1751 | opCompleted(pSh2, SH2_OK); 1752 | } 1753 | 1754 | const sh2_Op_t startCalOp = { 1755 | .start = startCalStart, 1756 | .rx = startCalRx, 1757 | }; 1758 | 1759 | // ------------------------------------------------------------------------ 1760 | // Start Cal 1761 | 1762 | static int finishCalStart(sh2_t *pSh2) 1763 | { 1764 | return sendCmd1(pSh2, SH2_CMD_CAL, SH2_CAL_FINISH); 1765 | } 1766 | 1767 | static void finishCalRx(sh2_t *pSh2, const uint8_t *payload, uint16_t len) 1768 | { 1769 | (void)len; // unused 1770 | 1771 | CommandResp_t *resp = (CommandResp_t *)payload; 1772 | 1773 | // Ignore message if it doesn't pertain to this operation 1774 | if (wrongResponse(pSh2, resp)) return; 1775 | 1776 | pSh2->opData.finishCal.status = (sh2_CalStatus_t)resp->r[1]; 1777 | 1778 | // Complete this operation 1779 | if (pSh2->opData.finishCal.status == SH2_CAL_SUCCESS) { 1780 | opCompleted(pSh2, SH2_OK); 1781 | } 1782 | else { 1783 | opCompleted(pSh2, SH2_ERR_HUB); 1784 | } 1785 | } 1786 | 1787 | const sh2_Op_t finishCalOp = { 1788 | .start = finishCalStart, 1789 | .rx = finishCalRx, 1790 | }; 1791 | 1792 | // ----------------------------------------------------------------------- 1793 | static int sendWheelOpStart(sh2_t *pSh2) 1794 | { 1795 | uint8_t p[COMMAND_PARAMS]; 1796 | memset(p, 0, COMMAND_PARAMS); 1797 | p[0] = pSh2->opData.wheelRequest.wheelIndex; 1798 | p[1] = (pSh2->opData.wheelRequest.timestamp >> 0) & 0xFF; 1799 | p[2] = (pSh2->opData.wheelRequest.timestamp >> 8) & 0xFF; 1800 | p[3] = (pSh2->opData.wheelRequest.timestamp >> 16) & 0xFF; 1801 | p[4] = (pSh2->opData.wheelRequest.timestamp >> 24) & 0xFF; 1802 | p[5] = (pSh2->opData.wheelRequest.wheelData >> 0) & 0xFF; 1803 | p[6] = (pSh2->opData.wheelRequest.wheelData >> 8) & 0xFF; 1804 | p[7] = pSh2->opData.wheelRequest.dataType; 1805 | int status = sendCmd(pSh2, SH2_CMD_WHEEL_REQ, p); 1806 | opCompleted(pSh2, status); 1807 | return status; 1808 | } 1809 | 1810 | 1811 | const sh2_Op_t sendWheelOp = { 1812 | .start = sendWheelOpStart, 1813 | .timeout_us = 5000000, 1814 | }; 1815 | 1816 | 1817 | // ------------------------------------------------------------------------ 1818 | // SHTP Event Callback 1819 | 1820 | static void shtpEventCallback(void *cookie, shtp_Event_t shtpEvent) { 1821 | (void)cookie; // unused 1822 | 1823 | sh2_t *pSh2 = &_sh2; 1824 | 1825 | sh2AsyncEvent.eventId = SH2_SHTP_EVENT; 1826 | sh2AsyncEvent.shtpEvent = shtpEvent; 1827 | if (pSh2->eventCallback) { 1828 | pSh2->eventCallback(pSh2->eventCookie, &sh2AsyncEvent); 1829 | } 1830 | } 1831 | 1832 | // ------------------------------------------------------------------------ 1833 | // Public functions 1834 | 1835 | /** 1836 | * @brief Open a session with a sensor hub. 1837 | * 1838 | * This function should be called before others in this API. 1839 | * An instance of an SH2 HAL should be passed in. 1840 | * This call will result in the open() function of the HAL being called. 1841 | * 1842 | * As part of the initialization process, a callback function is registered that will 1843 | * be invoked when the device generates certain events. (See sh2_AsyncEventId) 1844 | * 1845 | * @param pHal Pointer to an SH2 HAL instance, provided by the target system. 1846 | * @param eventCallback Will be called when events, such as reset complete, occur. 1847 | * @param eventCookie Will be passed to eventCallback. 1848 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 1849 | */ 1850 | int sh2_open(sh2_Hal_t *pHal, 1851 | sh2_EventCallback_t *eventCallback, void *eventCookie) 1852 | { 1853 | sh2_t *pSh2 = &_sh2; 1854 | 1855 | // Validate parameters 1856 | if (pHal == 0) return SH2_ERR_BAD_PARAM; 1857 | 1858 | // Clear everything in sh2 structure. 1859 | memset(pSh2, 0, sizeof(sh2_t)); 1860 | 1861 | // will go true after reset response from SH. 1862 | pSh2->resetComplete = false; 1863 | 1864 | // Store reference to HAL for future use. 1865 | pSh2->pHal = pHal; 1866 | pSh2->eventCallback = eventCallback; 1867 | pSh2->eventCookie = eventCookie; 1868 | pSh2->sensorCallback = 0; 1869 | pSh2->sensorCookie = 0; 1870 | 1871 | // Open SHTP layer 1872 | pSh2->pShtp = shtp_open(pSh2->pHal); 1873 | if (pSh2->pShtp == 0) { 1874 | // Error opening SHTP 1875 | return SH2_ERR; 1876 | } 1877 | 1878 | // Register SHTP event callback 1879 | shtp_setEventCallback(pSh2->pShtp, shtpEventCallback, pSh2); 1880 | 1881 | // Register with SHTP 1882 | // Register SH2 handlers 1883 | shtp_listenChan(pSh2->pShtp, CHAN_SENSORHUB_CONTROL, sensorhubControlHdlr, pSh2); 1884 | shtp_listenChan(pSh2->pShtp, CHAN_SENSORHUB_INPUT, sensorhubInputNormalHdlr, pSh2); 1885 | shtp_listenChan(pSh2->pShtp, CHAN_SENSORHUB_INPUT_WAKE, sensorhubInputWakeHdlr, pSh2); 1886 | shtp_listenChan(pSh2->pShtp, CHAN_SENSORHUB_INPUT_GIRV, sensorhubInputGyroRvHdlr, pSh2); 1887 | 1888 | // Register EXECUTABLE handlers 1889 | shtp_listenChan(pSh2->pShtp, CHAN_EXECUTABLE_DEVICE, executableDeviceHdlr, pSh2); 1890 | 1891 | // Wait for reset notifications to arrive. 1892 | // The client can't talk to the sensor hub until that happens. 1893 | uint32_t start_us = pSh2->pHal->getTimeUs(pSh2->pHal); 1894 | uint32_t now_us = start_us; 1895 | while (((now_us - start_us) < ADVERT_TIMEOUT_US) && 1896 | (!pSh2->resetComplete)) 1897 | { 1898 | shtp_service(pSh2->pShtp); 1899 | now_us = pSh2->pHal->getTimeUs(pSh2->pHal); 1900 | } 1901 | 1902 | // No errors. 1903 | return SH2_OK; 1904 | } 1905 | 1906 | /** 1907 | * @brief Close a session with a sensor hub. 1908 | * 1909 | * This should be called at the end of a sensor hub session. 1910 | * The underlying SHTP and HAL instances will be closed. 1911 | */ 1912 | void sh2_close(void) 1913 | { 1914 | sh2_t *pSh2 = &_sh2; 1915 | 1916 | if (pSh2->pShtp != 0) { 1917 | shtp_close(pSh2->pShtp); 1918 | } 1919 | 1920 | // Clear everything in sh2 structure. 1921 | memset(pSh2, 0, sizeof(sh2_t)); 1922 | } 1923 | 1924 | /** 1925 | * @brief Service the SH2 device, reading any data that is available and dispatching callbacks. 1926 | * 1927 | * This function should be called periodically by the host system to service an open sensor hub. 1928 | */ 1929 | void sh2_service(void) 1930 | { 1931 | sh2_t *pSh2 = &_sh2; 1932 | 1933 | if (pSh2->pShtp != 0) { 1934 | shtp_service(pSh2->pShtp); 1935 | } 1936 | } 1937 | 1938 | /** 1939 | * @brief Register a function to receive sensor events. 1940 | * 1941 | * @param callback A function that will be called each time a sensor event is received. 1942 | * @param cookie A value that will be passed to the sensor callback function. 1943 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 1944 | */ 1945 | int sh2_setSensorCallback(sh2_SensorCallback_t *callback, void *cookie) 1946 | { 1947 | sh2_t *pSh2 = &_sh2; 1948 | 1949 | pSh2->sensorCallback = callback; 1950 | pSh2->sensorCookie = cookie; 1951 | 1952 | return SH2_OK; 1953 | } 1954 | 1955 | /** 1956 | * @brief Reset the sensor hub device by sending RESET (1) command on "device" channel. 1957 | * 1958 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 1959 | */ 1960 | int sh2_devReset(void) 1961 | { 1962 | sh2_t *pSh2 = &_sh2; 1963 | 1964 | if (pSh2->pShtp == 0) { 1965 | return SH2_ERR; // sh2 API isn't open 1966 | } 1967 | 1968 | return sendExecutable(pSh2, EXECUTABLE_DEVICE_CMD_RESET); 1969 | } 1970 | 1971 | /** 1972 | * @brief Turn sensor hub on by sending ON (2) command on "device" channel. 1973 | * 1974 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 1975 | */ 1976 | int sh2_devOn(void) 1977 | { 1978 | sh2_t *pSh2 = &_sh2; 1979 | 1980 | if (pSh2->pShtp == 0) { 1981 | return SH2_ERR; // sh2 API isn't open 1982 | } 1983 | 1984 | return sendExecutable(pSh2, EXECUTABLE_DEVICE_CMD_ON); 1985 | } 1986 | 1987 | /** 1988 | * @brief Put sensor hub in sleep state by sending SLEEP (3) command on "device" channel. 1989 | * 1990 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 1991 | */ 1992 | int sh2_devSleep(void) 1993 | { 1994 | sh2_t *pSh2 = &_sh2; 1995 | 1996 | if (pSh2->pShtp == 0) { 1997 | return SH2_ERR; // sh2 API isn't open 1998 | } 1999 | 2000 | return sendExecutable(pSh2, EXECUTABLE_DEVICE_CMD_SLEEP); 2001 | } 2002 | 2003 | /** 2004 | * @brief Get Product ID information from Sensorhub. 2005 | * 2006 | * @param prodIds Pointer to structure that will receive results. 2007 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 2008 | */ 2009 | int sh2_getProdIds(sh2_ProductIds_t *prodIds) 2010 | { 2011 | sh2_t *pSh2 = &_sh2; 2012 | 2013 | if (pSh2->pShtp == 0) { 2014 | return SH2_ERR; // sh2 API isn't open 2015 | } 2016 | 2017 | // clear opData 2018 | memset(&pSh2->opData, 0, sizeof(sh2_OpData_t)); 2019 | 2020 | pSh2->opData.getProdIds.pProdIds = prodIds; 2021 | 2022 | return opProcess(pSh2, &getProdIdOp); 2023 | } 2024 | 2025 | /** 2026 | * @brief Get sensor configuration. 2027 | * 2028 | * @param sensorId Which sensor to query. 2029 | * @param config SensorConfig structure to store results. 2030 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 2031 | */ 2032 | int sh2_getSensorConfig(sh2_SensorId_t sensorId, sh2_SensorConfig_t *pConfig) 2033 | { 2034 | sh2_t *pSh2 = &_sh2; 2035 | 2036 | if (pSh2->pShtp == 0) { 2037 | return SH2_ERR; // sh2 API isn't open 2038 | } 2039 | 2040 | // clear opData 2041 | memset(&pSh2->opData, 0, sizeof(sh2_OpData_t)); 2042 | 2043 | // Set up operation 2044 | pSh2->opData.getSensorConfig.sensorId = sensorId; 2045 | pSh2->opData.getSensorConfig.pConfig = pConfig; 2046 | 2047 | return opProcess(pSh2, &getSensorConfigOp); 2048 | } 2049 | 2050 | /** 2051 | * @brief Set sensor configuration. (e.g enable a sensor at a particular rate.) 2052 | * 2053 | * @param sensorId Which sensor to configure. 2054 | * @param pConfig Pointer to structure holding sensor configuration. 2055 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 2056 | */ 2057 | int sh2_setSensorConfig(sh2_SensorId_t sensorId, const sh2_SensorConfig_t *pConfig) 2058 | { 2059 | sh2_t *pSh2 = &_sh2; 2060 | 2061 | if (pSh2->pShtp == 0) { 2062 | return SH2_ERR; // sh2 API isn't open 2063 | } 2064 | 2065 | // clear opData 2066 | memset(&pSh2->opData, 0, sizeof(sh2_OpData_t)); 2067 | 2068 | // Set up operation 2069 | pSh2->opData.setSensorConfig.sensorId = sensorId; 2070 | pSh2->opData.setSensorConfig.pConfig = pConfig; 2071 | 2072 | return opProcess(pSh2, &setSensorConfigOp); 2073 | } 2074 | 2075 | /** 2076 | * @brief Get metadata related to a sensor. 2077 | * 2078 | * @param sensorId Which sensor to query. 2079 | * @param pData Pointer to structure to receive the results. 2080 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 2081 | */ 2082 | int sh2_getMetadata(sh2_SensorId_t sensorId, sh2_SensorMetadata_t *pData) 2083 | { 2084 | sh2_t *pSh2 = &_sh2; 2085 | 2086 | if (pSh2->pShtp == 0) { 2087 | return SH2_ERR; // sh2 API isn't open 2088 | } 2089 | 2090 | // pData must be non-null 2091 | if (pData == 0) return SH2_ERR_BAD_PARAM; 2092 | 2093 | // Convert sensorId to metadata recordId 2094 | unsigned i; 2095 | for (i = 0; i < ARRAY_LEN(sensorToRecordMap); i++) { 2096 | if (sensorToRecordMap[i].sensorId == sensorId) { 2097 | break; 2098 | } 2099 | } 2100 | if (i >= ARRAY_LEN(sensorToRecordMap)) { 2101 | // no match was found 2102 | return SH2_ERR_BAD_PARAM; 2103 | } 2104 | uint16_t recordId = sensorToRecordMap[i].recordId; 2105 | 2106 | // clear opData 2107 | memset(&pSh2->opData, 0, sizeof(sh2_OpData_t)); 2108 | 2109 | // Set up an FRS read operation 2110 | pSh2->opData.getFrs.frsType = recordId; 2111 | pSh2->opData.getFrs.pData = pSh2->frsData; 2112 | pSh2->frsDataLen = ARRAY_LEN(pSh2->frsData); 2113 | pSh2->opData.getFrs.pWords = &pSh2->frsDataLen; 2114 | 2115 | // Read an FRS record 2116 | int status = opProcess(pSh2, &getFrsOp); 2117 | 2118 | // Copy the results into pData 2119 | if (status == SH2_OK) { 2120 | stuffMetadata(pData, pSh2->frsData); 2121 | } 2122 | 2123 | return status; 2124 | } 2125 | 2126 | /** 2127 | * @brief Get an FRS record. 2128 | * 2129 | * @param recordId Which FRS Record to retrieve. 2130 | * @param pData pointer to buffer to receive the results 2131 | * @param[in] words Size of pData buffer, in 32-bit words. 2132 | * @param[out] words Number of 32-bit words retrieved. 2133 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 2134 | */ 2135 | int sh2_getFrs(uint16_t recordId, uint32_t *pData, uint16_t *words) 2136 | { 2137 | sh2_t *pSh2 = &_sh2; 2138 | 2139 | if (pSh2->pShtp == 0) { 2140 | return SH2_ERR; // sh2 API isn't open 2141 | } 2142 | 2143 | if ((pData == 0) || (words == 0)) { 2144 | return SH2_ERR_BAD_PARAM; 2145 | } 2146 | 2147 | // clear opData 2148 | memset(&pSh2->opData, 0, sizeof(sh2_OpData_t)); 2149 | 2150 | // Store params for this op 2151 | pSh2->opData.getFrs.frsType = recordId; 2152 | pSh2->opData.getFrs.pData = pData; 2153 | pSh2->opData.getFrs.pWords = words; 2154 | 2155 | return opProcess(pSh2, &getFrsOp); 2156 | } 2157 | 2158 | /** 2159 | * @brief Set an FRS record 2160 | * 2161 | * @param recordId Which FRS Record to set. 2162 | * @param pData pointer to buffer containing the new data. 2163 | * @param words number of 32-bit words to write. (0 to delete record.) 2164 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 2165 | */ 2166 | int sh2_setFrs(uint16_t recordId, uint32_t *pData, uint16_t words) 2167 | { 2168 | sh2_t *pSh2 = &_sh2; 2169 | 2170 | if (pSh2->pShtp == 0) { 2171 | return SH2_ERR; // sh2 API isn't open 2172 | } 2173 | 2174 | if ((pData == 0) && (words != 0)) { 2175 | return SH2_ERR_BAD_PARAM; 2176 | } 2177 | 2178 | // clear opData 2179 | memset(&pSh2->opData, 0, sizeof(sh2_OpData_t)); 2180 | 2181 | pSh2->opData.setFrs.frsType = recordId; 2182 | pSh2->opData.setFrs.pData = pData; 2183 | pSh2->opData.setFrs.words = words; 2184 | 2185 | return opProcess(pSh2, &setFrsOp); 2186 | } 2187 | 2188 | /** 2189 | * @brief Get error counts. 2190 | * 2191 | * @param severity Only errors of this severity or greater are returned. 2192 | * @param pErrors Buffer to receive error codes. 2193 | * @param numErrors size of pErrors array 2194 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 2195 | */ 2196 | int sh2_getErrors(uint8_t severity, sh2_ErrorRecord_t *pErrors, uint16_t *numErrors) 2197 | { 2198 | sh2_t *pSh2 = &_sh2; 2199 | 2200 | if (pSh2->pShtp == 0) { 2201 | return SH2_ERR; // sh2 API isn't open 2202 | } 2203 | 2204 | // clear opData 2205 | memset(&pSh2->opData, 0, sizeof(sh2_OpData_t)); 2206 | 2207 | pSh2->opData.getErrors.severity = severity; 2208 | pSh2->opData.getErrors.pErrors = pErrors; 2209 | pSh2->opData.getErrors.pNumErrors = numErrors; 2210 | 2211 | return opProcess(pSh2, &getErrorsOp); 2212 | } 2213 | 2214 | /** 2215 | * @brief Read counters related to a sensor. 2216 | * 2217 | * @param sensorId Which sensor to operate on. 2218 | * @param pCounts Pointer to Counts structure that will receive data. 2219 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 2220 | */ 2221 | int sh2_getCounts(sh2_SensorId_t sensorId, sh2_Counts_t *pCounts) 2222 | { 2223 | sh2_t *pSh2 = &_sh2; 2224 | 2225 | if (pSh2->pShtp == 0) { 2226 | return SH2_ERR; // sh2 API isn't open 2227 | } 2228 | 2229 | // clear opData 2230 | memset(&pSh2->opData, 0, sizeof(sh2_OpData_t)); 2231 | 2232 | pSh2->opData.getCounts.sensorId = sensorId; 2233 | pSh2->opData.getCounts.pCounts = pCounts; 2234 | 2235 | return opProcess(pSh2, &getCountsOp); 2236 | } 2237 | 2238 | /** 2239 | * @brief Clear counters related to a sensor. 2240 | * 2241 | * @param sensorId which sensor to operate on. 2242 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 2243 | */ 2244 | int sh2_clearCounts(sh2_SensorId_t sensorId) 2245 | { 2246 | sh2_t *pSh2 = &_sh2; 2247 | 2248 | if (pSh2->pShtp == 0) { 2249 | return SH2_ERR; // sh2 API isn't open 2250 | } 2251 | 2252 | // clear opData 2253 | memset(&pSh2->opData, 0, sizeof(sh2_OpData_t)); 2254 | 2255 | 2256 | pSh2->opData.sendCmd.req.command = SH2_CMD_COUNTS; 2257 | pSh2->opData.sendCmd.req.p[0] = SH2_COUNTS_CLEAR_COUNTS; 2258 | pSh2->opData.sendCmd.req.p[1] = sensorId; 2259 | 2260 | return opProcess(pSh2, &sendCmdOp); 2261 | } 2262 | 2263 | /** 2264 | * @brief Perform a tare operation on one or more axes. 2265 | * 2266 | * @param axes Bit mask specifying which axes should be tared. 2267 | * @param basis Which rotation vector to use as the basis for Tare adjustment. 2268 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 2269 | */ 2270 | int sh2_setTareNow(uint8_t axes, // SH2_TARE_X | SH2_TARE_Y | SH2_TARE_Z 2271 | sh2_TareBasis_t basis) 2272 | { 2273 | sh2_t *pSh2 = &_sh2; 2274 | 2275 | if (pSh2->pShtp == 0) { 2276 | return SH2_ERR; // sh2 API isn't open 2277 | } 2278 | 2279 | // clear opData 2280 | memset(&pSh2->opData, 0, sizeof(sh2_OpData_t)); 2281 | 2282 | 2283 | pSh2->opData.sendCmd.req.command = SH2_CMD_TARE; 2284 | pSh2->opData.sendCmd.req.p[0] = SH2_TARE_TARE_NOW; 2285 | pSh2->opData.sendCmd.req.p[1] = axes; 2286 | pSh2->opData.sendCmd.req.p[2] = basis; 2287 | 2288 | return opProcess(pSh2, &sendCmdOp); 2289 | } 2290 | 2291 | /** 2292 | * @brief Clears the previously applied tare operation. 2293 | * 2294 | * @return SH2_OK \n"); 2295 | */ 2296 | int sh2_clearTare(void) 2297 | { 2298 | sh2_t *pSh2 = &_sh2; 2299 | 2300 | if (pSh2->pShtp == 0) { 2301 | return SH2_ERR; // sh2 API isn't open 2302 | } 2303 | 2304 | // clear opData 2305 | memset(&pSh2->opData, 0, sizeof(sh2_OpData_t)); 2306 | 2307 | 2308 | pSh2->opData.sendCmd.req.command = SH2_CMD_TARE; 2309 | pSh2->opData.sendCmd.req.p[0] = SH2_TARE_SET_REORIENTATION; 2310 | 2311 | return opProcess(pSh2, &sendCmdOp); 2312 | } 2313 | 2314 | /** 2315 | * @brief Persist the results of last tare operation to flash. 2316 | * 2317 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 2318 | */ 2319 | int sh2_persistTare(void) 2320 | { 2321 | sh2_t *pSh2 = &_sh2; 2322 | 2323 | if (pSh2->pShtp == 0) { 2324 | return SH2_ERR; // sh2 API isn't open 2325 | } 2326 | 2327 | // clear opData 2328 | memset(&pSh2->opData, 0, sizeof(sh2_OpData_t)); 2329 | 2330 | 2331 | pSh2->opData.sendCmd.req.command = SH2_CMD_TARE; 2332 | pSh2->opData.sendCmd.req.p[0] = SH2_TARE_PERSIST_TARE; 2333 | 2334 | return opProcess(pSh2, &sendCmdOp); 2335 | } 2336 | 2337 | /** 2338 | * @brief Set the current run-time sensor reorientation. (Set to zero to clear tare.) 2339 | * 2340 | * @param orientation Quaternion rotation vector to apply as new tare. 2341 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 2342 | */ 2343 | int sh2_setReorientation(sh2_Quaternion_t *orientation) 2344 | { 2345 | sh2_t *pSh2 = &_sh2; 2346 | 2347 | if (pSh2->pShtp == 0) { 2348 | return SH2_ERR; // sh2 API isn't open 2349 | } 2350 | 2351 | // clear opData 2352 | memset(&pSh2->opData, 0, sizeof(sh2_OpData_t)); 2353 | 2354 | 2355 | pSh2->opData.sendCmd.req.command = SH2_CMD_TARE; 2356 | pSh2->opData.sendCmd.req.p[0] = SH2_TARE_SET_REORIENTATION; 2357 | 2358 | // save me a lot of typing and you a lot of reading 2359 | uint8_t *p = pSh2->opData.sendCmd.req.p; 2360 | 2361 | // Put new orientation in command parameters 2362 | writeu16(&p[1], toQ14(orientation->x)); 2363 | writeu16(&p[3], toQ14(orientation->y)); 2364 | writeu16(&p[5], toQ14(orientation->z)); 2365 | writeu16(&p[7], toQ14(orientation->w)); 2366 | 2367 | return opProcess(pSh2, &sendCmdOp); 2368 | } 2369 | 2370 | /** 2371 | * @brief Command the sensorhub to reset. 2372 | * 2373 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 2374 | */ 2375 | int sh2_reinitialize(void) 2376 | { 2377 | sh2_t *pSh2 = &_sh2; 2378 | 2379 | if (pSh2->pShtp == 0) { 2380 | return SH2_ERR; // sh2 API isn't open 2381 | } 2382 | 2383 | return opProcess(pSh2, &reinitOp); 2384 | } 2385 | 2386 | /** 2387 | * @brief Save Dynamic Calibration Data to flash. 2388 | * 2389 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 2390 | */ 2391 | int sh2_saveDcdNow(void) 2392 | { 2393 | sh2_t *pSh2 = &_sh2; 2394 | 2395 | if (pSh2->pShtp == 0) { 2396 | return SH2_ERR; // sh2 API isn't open 2397 | } 2398 | 2399 | return opProcess(pSh2, &saveDcdNowOp); 2400 | } 2401 | 2402 | /** 2403 | * @brief Get Oscillator type. 2404 | * 2405 | * @param pOscType pointer to data structure to receive results. 2406 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 2407 | */ 2408 | int sh2_getOscType(sh2_OscType_t *pOscType) 2409 | { 2410 | sh2_t *pSh2 = &_sh2; 2411 | 2412 | if (pSh2->pShtp == 0) { 2413 | return SH2_ERR; // sh2 API isn't open 2414 | } 2415 | 2416 | pSh2->opData.getOscType.pOscType = pOscType; 2417 | 2418 | return opProcess(pSh2, &getOscTypeOp); 2419 | } 2420 | 2421 | /** 2422 | * @brief Enable/Disable dynamic calibration for certain sensors 2423 | * 2424 | * @param sensors Bit mask to configure which sensors are affected. 2425 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 2426 | */ 2427 | int sh2_setCalConfig(uint8_t sensors) 2428 | { 2429 | sh2_t *pSh2 = &_sh2; 2430 | 2431 | if (pSh2->pShtp == 0) { 2432 | return SH2_ERR; // sh2 API isn't open 2433 | } 2434 | 2435 | pSh2->opData.calConfig.sensors = sensors; 2436 | 2437 | return opProcess(pSh2, &setCalConfigOp); 2438 | } 2439 | 2440 | /** 2441 | * @brief Get dynamic calibration configuration settings. 2442 | * 2443 | * @param pSensors pointer to Bit mask, set on return. 2444 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 2445 | */ 2446 | int sh2_getCalConfig(uint8_t *pSensors) 2447 | { 2448 | sh2_t *pSh2 = &_sh2; 2449 | 2450 | if (pSh2->pShtp == 0) { 2451 | return SH2_ERR; // sh2 API isn't open 2452 | } 2453 | 2454 | pSh2->opData.getCalConfig.pSensors = pSensors; 2455 | 2456 | return opProcess(pSh2, &getCalConfigOp); 2457 | } 2458 | 2459 | /** 2460 | * @brief Configure automatic saving of dynamic calibration data. 2461 | * 2462 | * @param enabled Enable or Disable DCD auto-save. 2463 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 2464 | */ 2465 | int sh2_setDcdAutoSave(bool enabled) 2466 | { 2467 | sh2_t *pSh2 = &_sh2; 2468 | 2469 | if (pSh2->pShtp == 0) { 2470 | return SH2_ERR; // sh2 API isn't open 2471 | } 2472 | 2473 | // clear opData 2474 | memset(&pSh2->opData, 0, sizeof(sh2_OpData_t)); 2475 | 2476 | pSh2->opData.sendCmd.req.command = SH2_CMD_DCD_SAVE; 2477 | pSh2->opData.sendCmd.req.p[0] = enabled ? 0 : 1; 2478 | 2479 | return opProcess(pSh2, &sendCmdOp); 2480 | } 2481 | 2482 | /** 2483 | * @brief Immediately issue all buffered sensor reports from a given sensor. 2484 | * 2485 | * @param sensorId Which sensor reports to flush. 2486 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 2487 | */ 2488 | int sh2_flush(sh2_SensorId_t sensorId) 2489 | { 2490 | sh2_t *pSh2 = &_sh2; 2491 | 2492 | if (pSh2->pShtp == 0) { 2493 | return SH2_ERR; // sh2 API isn't open 2494 | } 2495 | 2496 | // clear opData 2497 | memset(&pSh2->opData, 0, sizeof(sh2_OpData_t)); 2498 | 2499 | pSh2->opData.forceFlush.sensorId = sensorId; 2500 | 2501 | return opProcess(pSh2, &forceFlushOp); 2502 | } 2503 | 2504 | /** 2505 | * @brief Command clear DCD in RAM, then reset sensor hub. 2506 | * 2507 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 2508 | */ 2509 | int sh2_clearDcdAndReset(void) 2510 | { 2511 | sh2_t *pSh2 = &_sh2; 2512 | 2513 | if (pSh2->pShtp == 0) { 2514 | return SH2_ERR; // sh2 API isn't open 2515 | } 2516 | 2517 | return opProcess(pSh2, &clearDcdAndResetOp); 2518 | } 2519 | 2520 | /** 2521 | * @brief Start simple self-calibration procedure. 2522 | * 2523 | * @parameter interval_us sensor report interval, uS. 2524 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 2525 | */ 2526 | int sh2_startCal(uint32_t interval_us) 2527 | { 2528 | sh2_t *pSh2 = &_sh2; 2529 | 2530 | if (pSh2->pShtp == 0) { 2531 | return SH2_ERR; // sh2 API isn't open 2532 | } 2533 | 2534 | // clear opData 2535 | memset(&pSh2->opData, 0, sizeof(sh2_OpData_t)); 2536 | 2537 | pSh2->opData.startCal.interval_us = interval_us; 2538 | 2539 | return opProcess(pSh2, &startCalOp); 2540 | } 2541 | 2542 | /** 2543 | * @brief Finish simple self-calibration procedure. 2544 | * 2545 | * @parameter status contains calibration status code on return. 2546 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 2547 | */ 2548 | int sh2_finishCal(sh2_CalStatus_t *status) 2549 | { 2550 | sh2_t *pSh2 = &_sh2; 2551 | 2552 | if (pSh2->pShtp == 0) { 2553 | return SH2_ERR; // sh2 API isn't open 2554 | } 2555 | 2556 | // clear opData 2557 | memset(&pSh2->opData, 0, sizeof(sh2_OpData_t)); 2558 | 2559 | int retval = opProcess(pSh2, &finishCalOp); 2560 | if (status != NULL) { 2561 | *status = pSh2->opData.finishCal.status; 2562 | } 2563 | 2564 | return retval; 2565 | } 2566 | 2567 | /** 2568 | * @brief send Interactive ZRO Request. 2569 | * 2570 | * @parameter intent Inform the sensor hub what sort of motion should be in progress. 2571 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 2572 | */ 2573 | int sh2_setIZro(sh2_IZroMotionIntent_t intent) 2574 | { 2575 | sh2_t *pSh2 = &_sh2; 2576 | 2577 | if (pSh2->pShtp == 0) { 2578 | return SH2_ERR; // sh2 API isn't open 2579 | } 2580 | 2581 | // clear opData 2582 | memset(&pSh2->opData, 0, sizeof(sh2_OpData_t)); 2583 | 2584 | // set up opData for iZRO request 2585 | pSh2->opData.sendCmd.req.command = SH2_CMD_INTERACTIVE_ZRO; 2586 | pSh2->opData.sendCmd.req.p[0] = intent; 2587 | 2588 | // Send command 2589 | return opProcess(pSh2, &sendCmdOp); 2590 | } 2591 | 2592 | 2593 | int sh2_reportWheelEncoder(uint8_t wheelIndex, uint32_t timestamp, int16_t wheelData, uint8_t dataType){ 2594 | sh2_t *pSh2 = &_sh2; 2595 | 2596 | if (pSh2->pShtp == 0) { 2597 | return SH2_ERR; // sh2 API isn't open 2598 | } 2599 | 2600 | //No callback (am i doing this right?) 2601 | pSh2->pOp = 0; 2602 | memset(&pSh2->opData, 0, sizeof(sh2_OpData_t)); 2603 | pSh2->opData.wheelRequest.wheelIndex = wheelIndex; 2604 | pSh2->opData.wheelRequest.timestamp = timestamp; 2605 | pSh2->opData.wheelRequest.wheelData = wheelData; 2606 | pSh2->opData.wheelRequest.dataType = dataType; 2607 | int rc = opProcess(pSh2, &sendWheelOp); 2608 | return rc; 2609 | } 2610 | 2611 | int sh2_saveDeadReckoningCalNow(void){ 2612 | sh2_t *pSh2 = &_sh2; 2613 | 2614 | if (pSh2->pShtp == 0) { 2615 | return SH2_ERR; // sh2 API isn't open 2616 | } 2617 | 2618 | memset(&pSh2->opData, 0, sizeof(sh2_OpData_t)); 2619 | pSh2->opData.sendCmd.req.command = SH2_CMD_DR_CAL_SAVE; 2620 | 2621 | return opProcess(pSh2, &sendCmdOp); 2622 | } 2623 | -------------------------------------------------------------------------------- /sh2.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2015-2021 CEVA, 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 CEVA, 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 SH-2 Sensor Hub. 23 | * 24 | * The sh2 API provides an interface to the CEVA sensor hub devices. 25 | */ 26 | 27 | #ifndef SH2_H 28 | #define SH2_H 29 | 30 | #include 31 | #include 32 | 33 | #include "sh2_hal.h" 34 | 35 | /*************************************************************************************** 36 | * Public type definitions 37 | ***************************************************************************************/ 38 | 39 | /** 40 | * @brief Sensor Event 41 | * 42 | * See the SH-2 Reference Manual for more detail. 43 | */ 44 | #define SH2_MAX_SENSOR_EVENT_LEN (60) 45 | typedef struct sh2_SensorEvent { 46 | uint64_t timestamp_uS; 47 | int64_t delay_uS; 48 | uint8_t len; 49 | uint8_t reportId; 50 | uint8_t report[SH2_MAX_SENSOR_EVENT_LEN]; 51 | } sh2_SensorEvent_t; 52 | 53 | typedef void (sh2_SensorCallback_t)(void * cookie, sh2_SensorEvent_t *pEvent); 54 | 55 | /** 56 | * @brief Product Id value 57 | * 58 | * See the SH-2 Reference Manual for more detail. 59 | */ 60 | typedef struct sh2_ProductId_s { 61 | uint8_t resetCause; 62 | uint8_t swVersionMajor; 63 | uint8_t swVersionMinor; 64 | uint32_t swPartNumber; 65 | uint32_t swBuildNumber; 66 | uint16_t swVersionPatch; 67 | uint8_t reserved0; 68 | uint8_t reserved1; 69 | } sh2_ProductId_t; 70 | 71 | #define SH2_MAX_PROD_ID_ENTRIES (5) 72 | typedef struct sh2_ProductIds_s { 73 | sh2_ProductId_t entry[SH2_MAX_PROD_ID_ENTRIES]; 74 | uint8_t numEntries; 75 | } sh2_ProductIds_t; 76 | 77 | /** 78 | * @brief List of sensor types supported by the hub 79 | * 80 | * See the SH-2 Reference Manual for more information on each type. 81 | */ 82 | enum sh2_SensorId_e { 83 | SH2_RAW_ACCELEROMETER = 0x14, 84 | SH2_ACCELEROMETER = 0x01, 85 | SH2_LINEAR_ACCELERATION = 0x04, 86 | SH2_GRAVITY = 0x06, 87 | SH2_RAW_GYROSCOPE = 0x15, 88 | SH2_GYROSCOPE_CALIBRATED = 0x02, 89 | SH2_GYROSCOPE_UNCALIBRATED = 0x07, 90 | SH2_RAW_MAGNETOMETER = 0x16, 91 | SH2_MAGNETIC_FIELD_CALIBRATED = 0x03, 92 | SH2_MAGNETIC_FIELD_UNCALIBRATED = 0x0f, 93 | SH2_ROTATION_VECTOR = 0x05, 94 | SH2_GAME_ROTATION_VECTOR = 0x08, 95 | SH2_GEOMAGNETIC_ROTATION_VECTOR = 0x09, 96 | SH2_PRESSURE = 0x0a, 97 | SH2_AMBIENT_LIGHT = 0x0b, 98 | SH2_HUMIDITY = 0x0c, 99 | SH2_PROXIMITY = 0x0d, 100 | SH2_TEMPERATURE = 0x0e, 101 | SH2_RESERVED = 0x17, 102 | SH2_TAP_DETECTOR = 0x10, 103 | SH2_STEP_DETECTOR = 0x18, 104 | SH2_STEP_COUNTER = 0x11, 105 | SH2_SIGNIFICANT_MOTION = 0x12, 106 | SH2_STABILITY_CLASSIFIER = 0x13, 107 | SH2_SHAKE_DETECTOR = 0x19, 108 | SH2_FLIP_DETECTOR = 0x1a, 109 | SH2_PICKUP_DETECTOR = 0x1b, 110 | SH2_STABILITY_DETECTOR = 0x1c, 111 | SH2_PERSONAL_ACTIVITY_CLASSIFIER = 0x1e, 112 | SH2_SLEEP_DETECTOR = 0x1f, 113 | SH2_TILT_DETECTOR = 0x20, 114 | SH2_POCKET_DETECTOR = 0x21, 115 | SH2_CIRCLE_DETECTOR = 0x22, 116 | SH2_HEART_RATE_MONITOR = 0x23, 117 | SH2_ARVR_STABILIZED_RV = 0x28, 118 | SH2_ARVR_STABILIZED_GRV = 0x29, 119 | SH2_GYRO_INTEGRATED_RV = 0x2A, 120 | SH2_IZRO_MOTION_REQUEST = 0x2B, 121 | SH2_RAW_OPTICAL_FLOW = 0x2C, 122 | SH2_DEAD_RECKONING_POSE = 0x2D, 123 | SH2_WHEEL_ENCODER = 0x2E, 124 | 125 | // UPDATE to reflect greatest sensor id 126 | SH2_MAX_SENSOR_ID = 0x2E, 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 | 148 | bool sniffEnabled; /**< @brief Any output from this sensor should 149 | be sent to host, but reportInterval_us and 150 | sensorSpecific do not influence sensor 151 | operation. Not supported by all sensors. */ 152 | 153 | /* 16-bit signed fixed point integer representing the value a 154 | * sensor output must exceed in order to trigger another input 155 | * report. A setting of 0 causes all reports to be sent. 156 | */ 157 | uint16_t changeSensitivity; /**< @brief Report-on-change threshold */ 158 | 159 | /* Interval in microseconds between asynchronous input reports. */ 160 | uint32_t reportInterval_us; /**< @brief [uS] Report interval */ 161 | 162 | /* Reserved field, not used. */ 163 | uint32_t batchInterval_us; /**< @brief [uS] Batch interval */ 164 | 165 | /* Meaning is sensor specific */ 166 | uint32_t sensorSpecific; /**< @brief See SH-2 Reference Manual for details. */ 167 | } sh2_SensorConfig_t; 168 | 169 | /** 170 | * @brief Sensor Metadata Record 171 | * 172 | * See the SH-2 Reference Manual for more detail. 173 | */ 174 | typedef struct sh2_SensorMetadata { 175 | uint8_t meVersion; /**< @brief Motion Engine Version */ 176 | uint8_t mhVersion; /**< @brief Motion Hub Version */ 177 | uint8_t shVersion; /**< @brief SensorHub Version */ 178 | uint32_t range; /**< @brief Same units as sensor reports */ 179 | uint32_t resolution; /**< @brief Same units as sensor reports */ 180 | uint16_t revision; /**< @brief Metadata record format revision */ 181 | uint16_t power_mA; /**< @brief [mA] Fixed point 16Q10 format */ 182 | uint32_t minPeriod_uS; /**< @brief [uS] */ 183 | uint32_t maxPeriod_uS; /**< @brief [uS] */ 184 | uint32_t fifoReserved; /**< @brief (Unused) */ 185 | uint32_t fifoMax; /**< @brief (Unused) */ 186 | uint32_t batchBufferBytes; /**< @brief (Unused) */ 187 | uint16_t qPoint1; /**< @brief q point for sensor values */ 188 | uint16_t qPoint2; /**< @brief q point for accuracy or bias fields */ 189 | uint16_t qPoint3; /**< @brief q point for sensor data change sensitivity */ 190 | uint32_t vendorIdLen; /**< @brief [bytes] */ 191 | char vendorId[48]; /**< @brief Vendor name and part number */ 192 | uint32_t sensorSpecificLen; /**< @brief [bytes] */ 193 | uint8_t sensorSpecific[48]; /**< @brief See SH-2 Reference Manual */ 194 | } sh2_SensorMetadata_t; 195 | 196 | /** 197 | * @brief SensorHub Error Record 198 | * 199 | * See the SH-2 Reference Manual for more detail. 200 | */ 201 | typedef struct sh2_ErrorRecord { 202 | uint8_t severity; /**< @brief Error severity, 0: most severe. */ 203 | uint8_t sequence; /**< @brief Sequence number (by severity) */ 204 | uint8_t source; /**< @brief 1-MotionEngine, 2-MotionHub, 3-SensorHub, 4-Chip */ 205 | uint8_t error; /**< @brief See SH-2 Reference Manual */ 206 | uint8_t module; /**< @brief See SH-2 Reference Manual */ 207 | uint8_t code; /**< @brief See SH-2 Reference Manual */ 208 | } sh2_ErrorRecord_t; 209 | 210 | /** 211 | * @brief SensorHub Counter Record 212 | * 213 | * See the SH-2 Reference Manual for more detail. 214 | */ 215 | typedef struct sh2_Counts { 216 | uint32_t offered; /**< @brief [events] */ 217 | uint32_t accepted; /**< @brief [events] */ 218 | uint32_t on; /**< @brief [events] */ 219 | uint32_t attempted; /**< @brief [events] */ 220 | } sh2_Counts_t; 221 | 222 | /** 223 | * @brief Values for specifying tare basis 224 | * 225 | * See the SH-2 Reference Manual for more detail. 226 | */ 227 | typedef enum sh2_TareBasis { 228 | SH2_TARE_BASIS_ROTATION_VECTOR = 0, /**< @brief Use Rotation Vector */ 229 | SH2_TARE_BASIS_GAMING_ROTATION_VECTOR = 1, /**< @brief Use Game Rotation Vector */ 230 | SH2_TARE_BASIS_GEOMAGNETIC_ROTATION_VECTOR = 2, /**< @brief Use Geomagnetic R.V. */ 231 | } sh2_TareBasis_t; 232 | 233 | /** 234 | * @brief Bit Fields for specifying tare axes. 235 | * 236 | * See the SH-2 Reference Manual for more detail. 237 | */ 238 | typedef enum sh2_TareAxis { 239 | SH2_TARE_X = 1, /**< @brief sh2_tareNow() axes bit field */ 240 | SH2_TARE_Y = 2, /**< @brief sh2_tareNow() axes bit field */ 241 | SH2_TARE_Z = 4, /**< @brief sh2_tareNow() axes bit field */ 242 | SH2_TARE_CONTROL_VECTOR_X = (1 << 3), /**< @brief Use X axis of source and frame to perform tare */ 243 | SH2_TARE_CONTROL_VECTOR_Y = (0 << 3), /**< @brief Use Y axis of source and frame to perform tare */ 244 | SH2_TARE_CONTROL_VECTOR_Z = (2 << 3), /**< @brief Use Z axis of source and frame to perform tare */ 245 | SH2_TARE_CONTROL_SEQUENCE_DEFAULT = (0 << 5), /**< @brief Tare "typical" toration for source/axis combination */ 246 | SH2_TARE_CONTROL_SEQUENCE_PRE = (1 << 5), /**< @brief Apply to pre-rotation (tare world to device) */ 247 | SH2_TARE_CONTROL_SEQUENCE_POST = (2 << 5), /**< @brief Apply to post-rotation (tare device to world) */ 248 | } sh2_TareAxis_t; 249 | 250 | /** 251 | * @brief Quaternion (double precision floating point representation.) 252 | * 253 | * See the SH-2 Reference Manual for more detail. 254 | */ 255 | typedef struct sh2_Quaternion { 256 | double x; 257 | double y; 258 | double z; 259 | double w; 260 | } sh2_Quaternion_t; 261 | 262 | /** 263 | * @brief Oscillator type: Internal or External 264 | * 265 | * See the SH-2 Reference Manual for more detail. 266 | */ 267 | typedef enum { 268 | SH2_OSC_INTERNAL = 0, 269 | SH2_OSC_EXT_CRYSTAL = 1, 270 | SH2_OSC_EXT_CLOCK = 2, 271 | } sh2_OscType_t; 272 | 273 | /** 274 | * @brief Calibration result 275 | * 276 | * See the SH-2 Reference Manual, Finish Calibration Response. 277 | */ 278 | typedef enum { 279 | SH2_CAL_SUCCESS = 0, 280 | SH2_CAL_NO_ZRO, 281 | SH2_CAL_NO_STATIONARY_DETECTION, 282 | SH2_CAL_ROTATION_OUTSIDE_SPEC, 283 | SH2_CAL_ZRO_OUTSIDE_SPEC, 284 | SH2_CAL_ZGO_OUTSIDE_SPEC, 285 | SH2_CAL_GYRO_GAIN_OUTSIDE_SPEC, 286 | SH2_CAL_GYRO_PERIOD_OUTSIDE_SPEC, 287 | SH2_CAL_GYRO_DROPS_OUTSIDE_SPEC, 288 | } sh2_CalStatus_t; 289 | 290 | // FRS Record Ids 291 | #define STATIC_CALIBRATION_AGM (0x7979) 292 | #define NOMINAL_CALIBRATION (0x4D4D) 293 | #define STATIC_CALIBRATION_SRA (0x8A8A) 294 | #define NOMINAL_CALIBRATION_SRA (0x4E4E) 295 | #define DYNAMIC_CALIBRATION (0x1F1F) 296 | #define ME_POWER_MGMT (0xD3E2) 297 | #define SYSTEM_ORIENTATION (0x2D3E) 298 | #define ACCEL_ORIENTATION (0x2D41) 299 | #define SCREEN_ACCEL_ORIENTATION (0x2D43) 300 | #define GYROSCOPE_ORIENTATION (0x2D46) 301 | #define MAGNETOMETER_ORIENTATION (0x2D4C) 302 | #define ARVR_STABILIZATION_RV (0x3E2D) 303 | #define ARVR_STABILIZATION_GRV (0x3E2E) 304 | #define TAP_DETECT_CONFIG (0xC269) 305 | #define SIG_MOTION_DETECT_CONFIG (0xC274) 306 | #define SHAKE_DETECT_CONFIG (0x7D7D) 307 | #define MAX_FUSION_PERIOD (0xD7D7) 308 | #define SERIAL_NUMBER (0x4B4B) 309 | #define ES_PRESSURE_CAL (0x39AF) 310 | #define ES_TEMPERATURE_CAL (0x4D20) 311 | #define ES_HUMIDITY_CAL (0x1AC9) 312 | #define ES_AMBIENT_LIGHT_CAL (0x39B1) 313 | #define ES_PROXIMITY_CAL (0x4DA2) 314 | #define ALS_CAL (0xD401) 315 | #define PROXIMITY_SENSOR_CAL (0xD402) 316 | #define PICKUP_DETECTOR_CONFIG (0x1B2A) 317 | #define FLIP_DETECTOR_CONFIG (0xFC94) 318 | #define STABILITY_DETECTOR_CONFIG (0xED85) 319 | #define ACTIVITY_TRACKER_CONFIG (0xED88) 320 | #define SLEEP_DETECTOR_CONFIG (0xED87) 321 | #define TILT_DETECTOR_CONFIG (0xED89) 322 | #define POCKET_DETECTOR_CONFIG (0xEF27) 323 | #define CIRCLE_DETECTOR_CONFIG (0xEE51) 324 | #define USER_RECORD (0x74B4) 325 | #define ME_TIME_SOURCE_SELECT (0xD403) 326 | #define UART_FORMAT (0xA1A1) 327 | #define GYRO_INTEGRATED_RV_CONFIG (0xA1A2) 328 | #define DR_IMU_CONFIG (0xDED2) 329 | #define DR_VEL_EST_CONFIG (0xDED3) 330 | #define DR_SYNC_CONFIG (0xDED4) 331 | #define DR_QUAL_CONFIG (0xDED5) 332 | #define DR_CAL_CONFIG (0xDED6) 333 | #define DR_LIGHT_REC_CONFIG (0xDED8) 334 | #define DR_FUSION_CONFIG (0xDED9) 335 | #define DR_OF_CONFIG (0xDEDA) 336 | #define DR_WHEEL_CONFIG (0xDEDB) 337 | #define DR_CAL (0xDEDC) 338 | #define DR_WHEEL_SELECT (0xDEDF) 339 | #define FRS_ID_META_RAW_ACCELEROMETER (0xE301) 340 | #define FRS_ID_META_ACCELEROMETER (0xE302) 341 | #define FRS_ID_META_LINEAR_ACCELERATION (0xE303) 342 | #define FRS_ID_META_GRAVITY (0xE304) 343 | #define FRS_ID_META_RAW_GYROSCOPE (0xE305) 344 | #define FRS_ID_META_GYROSCOPE_CALIBRATED (0xE306) 345 | #define FRS_ID_META_GYROSCOPE_UNCALIBRATED (0xE307) 346 | #define FRS_ID_META_RAW_MAGNETOMETER (0xE308) 347 | #define FRS_ID_META_MAGNETIC_FIELD_CALIBRATED (0xE309) 348 | #define FRS_ID_META_MAGNETIC_FIELD_UNCALIBRATED (0xE30A) 349 | #define FRS_ID_META_ROTATION_VECTOR (0xE30B) 350 | #define FRS_ID_META_GAME_ROTATION_VECTOR (0xE30C) 351 | #define FRS_ID_META_GEOMAGNETIC_ROTATION_VECTOR (0xE30D) 352 | #define FRS_ID_META_PRESSURE (0xE30E) 353 | #define FRS_ID_META_AMBIENT_LIGHT (0xE30F) 354 | #define FRS_ID_META_HUMIDITY (0xE310) 355 | #define FRS_ID_META_PROXIMITY (0xE311) 356 | #define FRS_ID_META_TEMPERATURE (0xE312) 357 | #define FRS_ID_META_TAP_DETECTOR (0xE313) 358 | #define FRS_ID_META_STEP_DETECTOR (0xE314) 359 | #define FRS_ID_META_STEP_COUNTER (0xE315) 360 | #define FRS_ID_META_SIGNIFICANT_MOTION (0xE316) 361 | #define FRS_ID_META_STABILITY_CLASSIFIER (0xE317) 362 | #define FRS_ID_META_SHAKE_DETECTOR (0xE318) 363 | #define FRS_ID_META_FLIP_DETECTOR (0xE319) 364 | #define FRS_ID_META_PICKUP_DETECTOR (0xE31A) 365 | #define FRS_ID_META_STABILITY_DETECTOR (0xE31B) 366 | #define FRS_ID_META_PERSONAL_ACTIVITY_CLASSIFIER (0xE31C) 367 | #define FRS_ID_META_SLEEP_DETECTOR (0xE31D) 368 | #define FRS_ID_META_TILT_DETECTOR (0xE31E) 369 | #define FRS_ID_META_POCKET_DETECTOR (0xE31F) 370 | #define FRS_ID_META_CIRCLE_DETECTOR (0xE320) 371 | #define FRS_ID_META_HEART_RATE_MONITOR (0xE321) 372 | #define FRS_ID_META_ARVR_STABILIZED_RV (0xE322) 373 | #define FRS_ID_META_ARVR_STABILIZED_GRV (0xE323) 374 | #define FRS_ID_META_GYRO_INTEGRATED_RV (0xE324) 375 | #define FRS_ID_META_RAW_OPTICAL_FLOW (0xE326) 376 | 377 | /** 378 | * @brief Interactive ZRO Motion Intent 379 | * 380 | * See the SH-2 Reference Manual, 6.4.13 381 | */ 382 | typedef enum { 383 | SH2_IZRO_MI_UNKNOWN = 0, 384 | SH2_IZRO_MI_STATIONARY_NO_VIBRATION, 385 | SH2_IZRO_MI_STATIONARY_WITH_VIBRATION, 386 | SH2_IZRO_MI_IN_MOTION, 387 | SH2_IZRO_MI_ACCELERATING, 388 | } sh2_IZroMotionIntent_t; 389 | 390 | /** 391 | * @brief Interactive ZRO Motion Intent 392 | * 393 | * See the SH-2 Reference Manual, 6.4.13 394 | */ 395 | typedef enum { 396 | SH2_IZRO_MR_NO_REQUEST = 0, 397 | SH2_IZRO_MR_STAY_STATIONARY, 398 | SH2_IZRO_MR_STATIONARY_NON_URGENT, 399 | SH2_IZRO_MR_STATIONARY_URGENT, 400 | } sh2_IZroMotionRequest_t; 401 | 402 | 403 | /** 404 | * @brief Asynchronous Event 405 | * 406 | * Represents reset events and other non-sensor events received from SH-2 sensor hub. 407 | */ 408 | 409 | enum sh2_AsyncEventId_e { 410 | SH2_RESET, 411 | SH2_SHTP_EVENT, 412 | SH2_GET_FEATURE_RESP, 413 | }; 414 | typedef enum sh2_AsyncEventId_e sh2_AsyncEventId_t; 415 | 416 | enum sh2_ShtpEvent_e { 417 | SH2_SHTP_TX_DISCARD = 0, 418 | SH2_SHTP_SHORT_FRAGMENT = 1, 419 | SH2_SHTP_TOO_LARGE_PAYLOADS = 2, 420 | SH2_SHTP_BAD_RX_CHAN = 3, 421 | SH2_SHTP_BAD_TX_CHAN = 4, 422 | SH2_SHTP_BAD_FRAGMENT = 5, 423 | SH2_SHTP_BAD_SN = 6, 424 | SH2_SHTP_INTERRUPTED_PAYLOAD = 7, 425 | }; 426 | typedef uint8_t sh2_ShtpEvent_t; 427 | 428 | typedef struct sh2_SensorConfigResp_e { 429 | sh2_SensorId_t sensorId; 430 | sh2_SensorConfig_t sensorConfig; 431 | } sh2_SensorConfigResp_t; 432 | 433 | typedef struct sh2_AsyncEvent { 434 | uint32_t eventId; 435 | union { 436 | sh2_ShtpEvent_t shtpEvent; 437 | sh2_SensorConfigResp_t sh2SensorConfigResp; 438 | }; 439 | } sh2_AsyncEvent_t; 440 | 441 | typedef void (sh2_EventCallback_t)(void * cookie, sh2_AsyncEvent_t *pEvent); 442 | 443 | 444 | /*************************************************************************************** 445 | * Public API 446 | **************************************************************************************/ 447 | 448 | /** 449 | * @brief Open a session with a sensor hub. 450 | * 451 | * This function should be called before others in this API. 452 | * An instance of an SH2 HAL should be passed in. 453 | * This call will result in the open() function of the HAL being called. 454 | * 455 | * As part of the initialization process, a callback function is registered that will 456 | * be invoked when the device generates certain events. (See sh2_AsyncEventId) 457 | * 458 | * @param pHal Pointer to an SH2 HAL instance, provided by the target system. 459 | * @param eventCallback Will be called when events, such as reset complete, occur. 460 | * @param eventCookie Will be passed to eventCallback. 461 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 462 | */ 463 | int sh2_open(sh2_Hal_t *pHal, 464 | sh2_EventCallback_t *eventCallback, void *eventCookie); 465 | 466 | /** 467 | * @brief Close a session with a sensor hub. 468 | * 469 | * This should be called at the end of a sensor hub session. 470 | * The underlying SHTP and HAL instances will be closed. 471 | * 472 | */ 473 | void sh2_close(void); 474 | 475 | /** 476 | * @brief Service the SH2 device, reading any data that is available and dispatching callbacks. 477 | * 478 | * This function should be called periodically by the host system to service an open sensor hub. 479 | * 480 | */ 481 | void sh2_service(void); 482 | 483 | /** 484 | * @brief Register a function to receive sensor events. 485 | * 486 | * @param callback A function that will be called each time a sensor event is received. 487 | * @param cookie A value that will be passed to the sensor callback function. 488 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 489 | */ 490 | int sh2_setSensorCallback(sh2_SensorCallback_t *callback, void *cookie); 491 | 492 | /** 493 | * @brief Reset the sensor hub device by sending RESET (1) command on "device" channel. 494 | * 495 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 496 | */ 497 | int sh2_devReset(void); 498 | 499 | /** 500 | * @brief Turn sensor hub on by sending ON (2) command on "device" channel. 501 | * 502 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 503 | */ 504 | int sh2_devOn(void); 505 | 506 | /** 507 | * @brief Put sensor hub in sleep state by sending SLEEP (3) command on "device" channel. 508 | * 509 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 510 | */ 511 | int sh2_devSleep(void); 512 | 513 | /** 514 | * @brief Get Product ID information from Sensorhub. 515 | * 516 | * @param prodIds Pointer to structure that will receive results. 517 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 518 | */ 519 | int sh2_getProdIds(sh2_ProductIds_t *prodIds); 520 | 521 | /** 522 | * @brief Get sensor configuration. 523 | * 524 | * @param sensorId Which sensor to query. 525 | * @param config SensorConfig structure to store results. 526 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 527 | */ 528 | int sh2_getSensorConfig(sh2_SensorId_t sensorId, sh2_SensorConfig_t *config); 529 | 530 | /** 531 | * @brief Set sensor configuration. (e.g enable a sensor at a particular rate.) 532 | * 533 | * @param sensorId Which sensor to configure. 534 | * @param pConfig Pointer to structure holding sensor configuration. 535 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 536 | */ 537 | int sh2_setSensorConfig(sh2_SensorId_t sensorId, const sh2_SensorConfig_t *pConfig); 538 | 539 | /** 540 | * @brief Get metadata related to a sensor. 541 | * 542 | * @param sensorId Which sensor to query. 543 | * @param pData Pointer to structure to receive the results. 544 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 545 | */ 546 | int sh2_getMetadata(sh2_SensorId_t sensorId, sh2_SensorMetadata_t *pData); 547 | 548 | /** 549 | * @brief Get an FRS record. 550 | * 551 | * @param recordId Which FRS Record to retrieve. 552 | * @param pData pointer to buffer to receive the results 553 | * @param[in] words Size of pData buffer, in 32-bit words. 554 | * @param[out] words Number of 32-bit words retrieved. 555 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 556 | */ 557 | int sh2_getFrs(uint16_t recordId, uint32_t *pData, uint16_t *words); 558 | 559 | /** 560 | * @brief Set an FRS record 561 | * 562 | * @param recordId Which FRS Record to set. 563 | * @param pData pointer to buffer containing the new data. 564 | * @param words number of 32-bit words to write. (0 to delete record.) 565 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 566 | */ 567 | int sh2_setFrs(uint16_t recordId, uint32_t *pData, uint16_t words); 568 | 569 | /** 570 | * @brief Get error counts. 571 | * 572 | * @param severity Only errors of this severity or greater are returned. 573 | * @param pErrors Buffer to receive error codes. 574 | * @param numErrors size of pErrors array 575 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 576 | */ 577 | int sh2_getErrors(uint8_t severity, sh2_ErrorRecord_t *pErrors, uint16_t *numErrors); 578 | 579 | /** 580 | * @brief Read counters related to a sensor. 581 | * 582 | * @param sensorId Which sensor to operate on. 583 | * @param pCounts Pointer to Counts structure that will receive data. 584 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 585 | */ 586 | int sh2_getCounts(sh2_SensorId_t sensorId, sh2_Counts_t *pCounts); 587 | 588 | /** 589 | * @brief Clear counters related to a sensor. 590 | * 591 | * @param sensorId which sensor to operate on. 592 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 593 | */ 594 | int sh2_clearCounts(sh2_SensorId_t sensorId); 595 | 596 | /** 597 | * @brief Perform a tare operation on one or more axes. 598 | * 599 | * @param axes Bit mask specifying which axes should be tared. 600 | * @param basis Which rotation vector to use as the basis for Tare adjustment. 601 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 602 | */ 603 | int sh2_setTareNow(uint8_t axes, // SH2_TARE_X | SH2_TARE_Y | SH2_TARE_Z 604 | sh2_TareBasis_t basis); 605 | 606 | /** 607 | * @brief Clears the previously applied tare operation. 608 | * 609 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 610 | */ 611 | int sh2_clearTare(void); 612 | 613 | /** 614 | * @brief Persist the results of last tare operation to flash. 615 | * 616 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 617 | */ 618 | int sh2_persistTare(void); 619 | 620 | /** 621 | * @brief Set the current run-time sensor reorientation. (Set to zero to clear tare.) 622 | * 623 | * @param orientation Quaternion rotation vector to apply as new tare. 624 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 625 | */ 626 | int sh2_setReorientation(sh2_Quaternion_t *orientation); 627 | 628 | /** 629 | * @brief Command the sensorhub to reset. 630 | * 631 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 632 | */ 633 | int sh2_reinitialize(void); 634 | 635 | /** 636 | * @brief Save Dynamic Calibration Data to flash. 637 | * 638 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 639 | */ 640 | int sh2_saveDcdNow(void); 641 | 642 | /** 643 | * @brief Get Oscillator type. 644 | * 645 | * @param pOscType pointer to data structure to receive results. 646 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 647 | */ 648 | int sh2_getOscType(sh2_OscType_t *pOscType); 649 | 650 | // Flags for sensors field of sh_calConfig 651 | #define SH2_CAL_ACCEL (0x01) 652 | #define SH2_CAL_GYRO (0x02) 653 | #define SH2_CAL_MAG (0x04) 654 | #define SH2_CAL_PLANAR (0x08) 655 | #define SH2_CAL_ON_TABLE (0x10) 656 | 657 | // Bits 5 and 6 encode cal zero gyro control value. 658 | #define SH2_CAL_ZERO_GYRO_CONTROL_MASK (0x60) 659 | #define SH2_CAL_ZERO_GYRO_CONTROL_ON_TABLE_DETECT (0 << 5) 660 | #define SH2_CAL_ZERO_GYRO_CONTROL_NEVER (1 << 5) 661 | #define SH2_CAL_ZERO_GYRO_CONTROL_ON_TABLE_CLASS (2 << 5) 662 | #define SH2_CAL_ZERO_GYRO_CONTROL_ON_TABLE_CLASS_OR_LONG_TERM_STABLE (3 << 5) 663 | 664 | /** 665 | * @brief Enable/Disable dynamic calibration for certain sensors 666 | * 667 | * @param sensors Bit mask to configure which sensors are affected. 668 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 669 | */ 670 | int sh2_setCalConfig(uint8_t sensors); 671 | 672 | /** 673 | * @brief Get dynamic calibration configuration settings. 674 | * 675 | * @param pSensors pointer to Bit mask, set on return. 676 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 677 | */ 678 | int sh2_getCalConfig(uint8_t *pSensors); 679 | 680 | /** 681 | * @brief Configure automatic saving of dynamic calibration data. 682 | * 683 | * @param enabled Enable or Disable DCD auto-save. 684 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 685 | */ 686 | int sh2_setDcdAutoSave(bool enabled); 687 | 688 | /** 689 | * @brief Immediately issue all buffered sensor reports from a given sensor. 690 | * 691 | * @param sensorId Which sensor reports to flush. 692 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 693 | */ 694 | int sh2_flush(sh2_SensorId_t sensorId); 695 | 696 | /** 697 | * @brief Command clear DCD in RAM, then reset sensor hub. 698 | * 699 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 700 | */ 701 | int sh2_clearDcdAndReset(void); 702 | 703 | /** 704 | * @brief Start simple self-calibration procedure. 705 | * 706 | * @parameter interval_us sensor report interval, uS. 707 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 708 | */ 709 | int sh2_startCal(uint32_t interval_us); 710 | 711 | /** 712 | * @brief Finish simple self-calibration procedure. 713 | * 714 | * @parameter status contains calibration status code on return. 715 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 716 | */ 717 | int sh2_finishCal(sh2_CalStatus_t *status); 718 | 719 | /** 720 | * @brief send Interactive ZRO Request. 721 | * 722 | * @parameter intent Inform the sensor hub what sort of motion should be in progress. 723 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 724 | */ 725 | int sh2_setIZro(sh2_IZroMotionIntent_t intent); 726 | 727 | /** 728 | * @brief Report wheel position/velocity to sensor hub. 729 | * @parameter wheelIndex platform-dependent: 0= left, 1= right for 730 | * typical differential drive robot 731 | * @parameter timestamp microsecond timestamp (hub scale) of measurement 732 | * @parameter wheelData raw wheel position or velocity 733 | * @parameter dataType 0 if data is position, 1 if data is velocity 734 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 735 | */ 736 | int sh2_reportWheelEncoder(uint8_t wheelIndex, uint32_t timestamp, int16_t wheelData, uint8_t dataType); 737 | 738 | /** 739 | * @brief Save Dead Reckoning Calibration Data to flash. 740 | * 741 | * @return SH2_OK (0), on success. Negative value from sh2_err.h on error. 742 | */ 743 | int sh2_saveDeadReckoningCalNow(void); 744 | 745 | #endif 746 | -------------------------------------------------------------------------------- /sh2_SensorValue.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2015-21 CEVA, 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 CEVA, 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 | #include 27 | 28 | #define SCALE_Q(n) (1.0f / (1 << n)) 29 | 30 | const float scaleRadToDeg = 180.0f / 3.14159265358f; 31 | 32 | // ------------------------------------------------------------------------ 33 | // Forward declarations 34 | 35 | static int decodeRawAccelerometer(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 36 | static int decodeAccelerometer(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 37 | static int decodeLinearAcceleration(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 38 | static int decodeGravity(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 39 | static int decodeRawGyroscope(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 40 | static int decodeGyroscopeCalibrated(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 41 | static int decodeGyroscopeUncal(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 42 | static int decodeRawMagnetometer(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 43 | static int decodeMagneticFieldCalibrated(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 44 | static int decodeMagneticFieldUncal(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 45 | static int decodeRotationVector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 46 | static int decodeGameRotationVector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 47 | static int decodeGeomagneticRotationVector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 48 | static int decodePressure(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 49 | static int decodeAmbientLight(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 50 | static int decodeHumidity(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 51 | static int decodeProximity(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 52 | static int decodeTemperature(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 53 | static int decodeReserved(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 54 | static int decodeTapDetector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 55 | static int decodeStepDetector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 56 | static int decodeStepCounter(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 57 | static int decodeSignificantMotion(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 58 | static int decodeStabilityClassifier(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 59 | static int decodeShakeDetector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 60 | static int decodeFlipDetector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 61 | static int decodePickupDetector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 62 | static int decodeStabilityDetector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 63 | static int decodePersonalActivityClassifier(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 64 | static int decodeSleepDetector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 65 | static int decodeTiltDetector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 66 | static int decodePocketDetector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 67 | static int decodeCircleDetector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 68 | static int decodeHeartRateMonitor(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 69 | static int decodeArvrStabilizedRV(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 70 | static int decodeArvrStabilizedGRV(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 71 | static int decodeGyroIntegratedRV(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 72 | static int decodeIZroRequest(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 73 | static int decodeRawOptFlow(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 74 | static int decodeDeadReckoningPose(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 75 | static int decodeWheelEncoder(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 76 | 77 | // ------------------------------------------------------------------------ 78 | // Public API 79 | 80 | int sh2_decodeSensorEvent(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 81 | { 82 | // Fill out fields of *value based on *event, converting data from message representation 83 | // to natural representation. 84 | 85 | int rc = SH2_OK; 86 | 87 | value->sensorId = event->reportId; 88 | value->timestamp = event->timestamp_uS; 89 | 90 | if (value->sensorId != SH2_GYRO_INTEGRATED_RV) { 91 | value->sequence = event->report[1]; 92 | value->status = event->report[2] & 0x03; 93 | } 94 | else { 95 | value->sequence = 0; 96 | value->status = 0; 97 | } 98 | 99 | // extract delay field (100uS units) 100 | 101 | 102 | switch (value->sensorId) { 103 | case SH2_RAW_ACCELEROMETER: 104 | rc = decodeRawAccelerometer(value, event); 105 | break; 106 | case SH2_ACCELEROMETER: 107 | rc = decodeAccelerometer(value, event); 108 | break; 109 | case SH2_LINEAR_ACCELERATION: 110 | rc = decodeLinearAcceleration(value, event); 111 | break; 112 | case SH2_GRAVITY: 113 | rc = decodeGravity(value, event); 114 | break; 115 | case SH2_RAW_GYROSCOPE: 116 | rc = decodeRawGyroscope(value, event); 117 | break; 118 | case SH2_GYROSCOPE_CALIBRATED: 119 | rc = decodeGyroscopeCalibrated(value, event); 120 | break; 121 | case SH2_GYROSCOPE_UNCALIBRATED: 122 | rc = decodeGyroscopeUncal(value, event); 123 | break; 124 | case SH2_RAW_MAGNETOMETER: 125 | rc = decodeRawMagnetometer(value, event); 126 | break; 127 | case SH2_MAGNETIC_FIELD_CALIBRATED: 128 | rc = decodeMagneticFieldCalibrated(value, event); 129 | break; 130 | case SH2_MAGNETIC_FIELD_UNCALIBRATED: 131 | rc = decodeMagneticFieldUncal(value, event); 132 | break; 133 | case SH2_ROTATION_VECTOR: 134 | rc = decodeRotationVector(value, event); 135 | break; 136 | case SH2_GAME_ROTATION_VECTOR: 137 | rc = decodeGameRotationVector(value, event); 138 | break; 139 | case SH2_GEOMAGNETIC_ROTATION_VECTOR: 140 | rc = decodeGeomagneticRotationVector(value, event); 141 | break; 142 | case SH2_PRESSURE: 143 | rc = decodePressure(value, event); 144 | break; 145 | case SH2_AMBIENT_LIGHT: 146 | rc = decodeAmbientLight(value, event); 147 | break; 148 | case SH2_HUMIDITY: 149 | rc = decodeHumidity(value, event); 150 | break; 151 | case SH2_PROXIMITY: 152 | rc = decodeProximity(value, event); 153 | break; 154 | case SH2_TEMPERATURE: 155 | rc = decodeTemperature(value, event); 156 | break; 157 | case SH2_RESERVED: 158 | rc = decodeReserved(value, event); 159 | break; 160 | case SH2_TAP_DETECTOR: 161 | rc = decodeTapDetector(value, event); 162 | break; 163 | case SH2_STEP_DETECTOR: 164 | rc = decodeStepDetector(value, event); 165 | break; 166 | case SH2_STEP_COUNTER: 167 | rc = decodeStepCounter(value, event); 168 | break; 169 | case SH2_SIGNIFICANT_MOTION: 170 | rc = decodeSignificantMotion(value, event); 171 | break; 172 | case SH2_STABILITY_CLASSIFIER: 173 | rc = decodeStabilityClassifier(value, event); 174 | break; 175 | case SH2_SHAKE_DETECTOR: 176 | rc = decodeShakeDetector(value, event); 177 | break; 178 | case SH2_FLIP_DETECTOR: 179 | rc = decodeFlipDetector(value, event); 180 | break; 181 | case SH2_PICKUP_DETECTOR: 182 | rc = decodePickupDetector(value, event); 183 | break; 184 | case SH2_STABILITY_DETECTOR: 185 | rc = decodeStabilityDetector(value, event); 186 | break; 187 | case SH2_PERSONAL_ACTIVITY_CLASSIFIER: 188 | rc = decodePersonalActivityClassifier(value, event); 189 | break; 190 | case SH2_SLEEP_DETECTOR: 191 | rc = decodeSleepDetector(value, event); 192 | break; 193 | case SH2_TILT_DETECTOR: 194 | rc = decodeTiltDetector(value, event); 195 | break; 196 | case SH2_POCKET_DETECTOR: 197 | rc = decodePocketDetector(value, event); 198 | break; 199 | case SH2_CIRCLE_DETECTOR: 200 | rc = decodeCircleDetector(value, event); 201 | break; 202 | case SH2_HEART_RATE_MONITOR: 203 | rc = decodeHeartRateMonitor(value, event); 204 | break; 205 | case SH2_ARVR_STABILIZED_RV: 206 | rc = decodeArvrStabilizedRV(value, event); 207 | break; 208 | case SH2_ARVR_STABILIZED_GRV: 209 | rc = decodeArvrStabilizedGRV(value, event); 210 | break; 211 | case SH2_GYRO_INTEGRATED_RV: 212 | rc = decodeGyroIntegratedRV(value, event); 213 | break; 214 | case SH2_IZRO_MOTION_REQUEST: 215 | rc = decodeIZroRequest(value, event); 216 | break; 217 | case SH2_RAW_OPTICAL_FLOW: 218 | rc = decodeRawOptFlow(value, event); 219 | break; 220 | case SH2_DEAD_RECKONING_POSE: 221 | rc = decodeDeadReckoningPose(value, event); 222 | break; 223 | case SH2_WHEEL_ENCODER: 224 | rc = decodeWheelEncoder(value, event); 225 | break; 226 | default: 227 | // Unknown report id 228 | rc = SH2_ERR; 229 | break; 230 | } 231 | 232 | return rc; 233 | } 234 | 235 | // ------------------------------------------------------------------------ 236 | // Private utility functions 237 | 238 | static int decodeRawAccelerometer(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 239 | { 240 | value->un.rawAccelerometer.x = read16(&event->report[4]); 241 | value->un.rawAccelerometer.y = read16(&event->report[6]); 242 | value->un.rawAccelerometer.z = read16(&event->report[8]); 243 | value->un.rawAccelerometer.timestamp = read32(&event->report[12]); 244 | 245 | return SH2_OK; 246 | } 247 | 248 | static int decodeAccelerometer(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 249 | { 250 | value->un.accelerometer.x = read16(&event->report[4]) * SCALE_Q(8); 251 | value->un.accelerometer.y = read16(&event->report[6]) * SCALE_Q(8); 252 | value->un.accelerometer.z = read16(&event->report[8]) * SCALE_Q(8); 253 | 254 | return SH2_OK; 255 | } 256 | 257 | static int decodeLinearAcceleration(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 258 | { 259 | value->un.linearAcceleration.x = read16(&event->report[4]) * SCALE_Q(8); 260 | value->un.linearAcceleration.y = read16(&event->report[6]) * SCALE_Q(8); 261 | value->un.linearAcceleration.z = read16(&event->report[8]) * SCALE_Q(8); 262 | 263 | return SH2_OK; 264 | } 265 | 266 | static int decodeGravity(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 267 | { 268 | value->un.gravity.x = read16(&event->report[4]) * SCALE_Q(8); 269 | value->un.gravity.y = read16(&event->report[6]) * SCALE_Q(8); 270 | value->un.gravity.z = read16(&event->report[8]) * SCALE_Q(8); 271 | 272 | return SH2_OK; 273 | } 274 | 275 | static int decodeRawGyroscope(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 276 | { 277 | value->un.rawGyroscope.x = read16(&event->report[4]); 278 | value->un.rawGyroscope.y = read16(&event->report[6]); 279 | value->un.rawGyroscope.z = read16(&event->report[8]); 280 | value->un.rawGyroscope.temperature = read16(&event->report[10]); 281 | value->un.rawGyroscope.timestamp = read32(&event->report[12]); 282 | 283 | return SH2_OK; 284 | } 285 | 286 | static int decodeGyroscopeCalibrated(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 287 | { 288 | value->un.gyroscope.x = read16(&event->report[4]) * SCALE_Q(9); 289 | value->un.gyroscope.y = read16(&event->report[6]) * SCALE_Q(9); 290 | value->un.gyroscope.z = read16(&event->report[8]) * SCALE_Q(9); 291 | 292 | return SH2_OK; 293 | } 294 | 295 | static int decodeGyroscopeUncal(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 296 | { 297 | value->un.gyroscopeUncal.x = read16(&event->report[4]) * SCALE_Q(9); 298 | value->un.gyroscopeUncal.y = read16(&event->report[6]) * SCALE_Q(9); 299 | value->un.gyroscopeUncal.z = read16(&event->report[8]) * SCALE_Q(9); 300 | 301 | value->un.gyroscopeUncal.biasX = read16(&event->report[10]) * SCALE_Q(9); 302 | value->un.gyroscopeUncal.biasY = read16(&event->report[12]) * SCALE_Q(9); 303 | value->un.gyroscopeUncal.biasZ = read16(&event->report[14]) * SCALE_Q(9); 304 | 305 | return SH2_OK; 306 | } 307 | 308 | static int decodeRawMagnetometer(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 309 | { 310 | value->un.rawMagnetometer.x = read16(&event->report[4]); 311 | value->un.rawMagnetometer.y = read16(&event->report[6]); 312 | value->un.rawMagnetometer.z = read16(&event->report[8]); 313 | value->un.rawMagnetometer.timestamp = read32(&event->report[12]); 314 | 315 | return SH2_OK; 316 | } 317 | 318 | static int decodeMagneticFieldCalibrated(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 319 | { 320 | value->un.magneticField.x = read16(&event->report[4]) * SCALE_Q(4); 321 | value->un.magneticField.y = read16(&event->report[6]) * SCALE_Q(4); 322 | value->un.magneticField.z = read16(&event->report[8]) * SCALE_Q(4); 323 | 324 | return SH2_OK; 325 | } 326 | 327 | static int decodeMagneticFieldUncal(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 328 | { 329 | value->un.magneticFieldUncal.x = read16(&event->report[4]) * SCALE_Q(4); 330 | value->un.magneticFieldUncal.y = read16(&event->report[6]) * SCALE_Q(4); 331 | value->un.magneticFieldUncal.z = read16(&event->report[8]) * SCALE_Q(4); 332 | 333 | value->un.magneticFieldUncal.biasX = read16(&event->report[10]) * SCALE_Q(4); 334 | value->un.magneticFieldUncal.biasY = read16(&event->report[12]) * SCALE_Q(4); 335 | value->un.magneticFieldUncal.biasZ = read16(&event->report[14]) * SCALE_Q(4); 336 | 337 | return SH2_OK; 338 | } 339 | 340 | static int decodeRotationVector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 341 | { 342 | value->un.rotationVector.i = read16(&event->report[4]) * SCALE_Q(14); 343 | value->un.rotationVector.j = read16(&event->report[6]) * SCALE_Q(14); 344 | value->un.rotationVector.k = read16(&event->report[8]) * SCALE_Q(14); 345 | value->un.rotationVector.real = read16(&event->report[10]) * SCALE_Q(14); 346 | value->un.rotationVector.accuracy = read16(&event->report[12]) * SCALE_Q(12); 347 | 348 | return SH2_OK; 349 | } 350 | 351 | static int decodeGameRotationVector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 352 | { 353 | value->un.gameRotationVector.i = read16(&event->report[4]) * SCALE_Q(14); 354 | value->un.gameRotationVector.j = read16(&event->report[6]) * SCALE_Q(14); 355 | value->un.gameRotationVector.k = read16(&event->report[8]) * SCALE_Q(14); 356 | value->un.gameRotationVector.real = read16(&event->report[10]) * SCALE_Q(14); 357 | 358 | return SH2_OK; 359 | } 360 | 361 | static int decodeGeomagneticRotationVector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 362 | { 363 | value->un.geoMagRotationVector.i = read16(&event->report[4]) * SCALE_Q(14); 364 | value->un.geoMagRotationVector.j = read16(&event->report[6]) * SCALE_Q(14); 365 | value->un.geoMagRotationVector.k = read16(&event->report[8]) * SCALE_Q(14); 366 | value->un.geoMagRotationVector.real = read16(&event->report[10]) * SCALE_Q(14); 367 | value->un.geoMagRotationVector.accuracy = read16(&event->report[12]) * SCALE_Q(12); 368 | 369 | return SH2_OK; 370 | } 371 | 372 | static int decodePressure(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 373 | { 374 | value->un.pressure.value = read32(&event->report[4]) * SCALE_Q(20); 375 | 376 | return SH2_OK; 377 | } 378 | 379 | static int decodeAmbientLight(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 380 | { 381 | value->un.ambientLight.value = read32(&event->report[4]) * SCALE_Q(8); 382 | 383 | return SH2_OK; 384 | } 385 | 386 | static int decodeHumidity(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 387 | { 388 | value->un.humidity.value = read16(&event->report[4]) * SCALE_Q(8); 389 | 390 | return SH2_OK; 391 | } 392 | 393 | static int decodeProximity(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 394 | { 395 | value->un.proximity.value = read16(&event->report[4]) * SCALE_Q(4); 396 | 397 | return SH2_OK; 398 | } 399 | 400 | static int decodeTemperature(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 401 | { 402 | value->un.temperature.value = read16(&event->report[4]) * SCALE_Q(7); 403 | 404 | return SH2_OK; 405 | } 406 | 407 | static int decodeReserved(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 408 | { 409 | value->un.reserved.tbd = read16(&event->report[4]) * SCALE_Q(7); 410 | 411 | return SH2_OK; 412 | } 413 | 414 | static int decodeTapDetector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 415 | { 416 | value->un.tapDetector.flags = event->report[4]; 417 | 418 | return SH2_OK; 419 | } 420 | 421 | static int decodeStepDetector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 422 | { 423 | value->un.stepDetector.latency = readu32(&event->report[4]); 424 | 425 | return SH2_OK; 426 | } 427 | 428 | static int decodeStepCounter(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 429 | { 430 | value->un.stepCounter.latency = readu32(&event->report[4]); 431 | value->un.stepCounter.steps = readu32(&event->report[8]); 432 | 433 | return SH2_OK; 434 | } 435 | 436 | static int decodeSignificantMotion(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 437 | { 438 | value->un.sigMotion.motion = readu16(&event->report[4]); 439 | 440 | return SH2_OK; 441 | } 442 | 443 | static int decodeStabilityClassifier(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 444 | { 445 | value->un.stabilityClassifier.classification = event->report[4]; 446 | 447 | return SH2_OK; 448 | } 449 | 450 | static int decodeShakeDetector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 451 | { 452 | value->un.shakeDetector.shake = readu16(&event->report[4]); 453 | 454 | return SH2_OK; 455 | } 456 | 457 | static int decodeFlipDetector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 458 | { 459 | value->un.flipDetector.flip = readu16(&event->report[4]); 460 | 461 | return SH2_OK; 462 | } 463 | 464 | static int decodePickupDetector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 465 | { 466 | value->un.pickupDetector.pickup = readu16(&event->report[4]); 467 | 468 | return SH2_OK; 469 | } 470 | 471 | static int decodeStabilityDetector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 472 | { 473 | value->un.stabilityDetector.stability = readu16(&event->report[4]); 474 | 475 | return SH2_OK; 476 | } 477 | 478 | static int decodePersonalActivityClassifier(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 479 | { 480 | value->un.personalActivityClassifier.page = event->report[4] & 0x7F; 481 | value->un.personalActivityClassifier.lastPage = ((event->report[4] & 0x80) != 0); 482 | value->un.personalActivityClassifier.mostLikelyState = event->report[5]; 483 | for (int n = 0; n < 10; n++) { 484 | value->un.personalActivityClassifier.confidence[n] = event->report[6+n]; 485 | } 486 | 487 | return SH2_OK; 488 | } 489 | 490 | static int decodeSleepDetector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 491 | { 492 | value->un.sleepDetector.sleepState = event->report[4]; 493 | 494 | return SH2_OK; 495 | } 496 | 497 | static int decodeTiltDetector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 498 | { 499 | value->un.tiltDetector.tilt = readu16(&event->report[4]); 500 | 501 | return SH2_OK; 502 | } 503 | 504 | static int decodePocketDetector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 505 | { 506 | value->un.pocketDetector.pocket = readu16(&event->report[4]); 507 | 508 | return SH2_OK; 509 | } 510 | 511 | static int decodeCircleDetector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 512 | { 513 | value->un.circleDetector.circle = readu16(&event->report[4]); 514 | 515 | return SH2_OK; 516 | } 517 | 518 | static int decodeHeartRateMonitor(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 519 | { 520 | value->un.heartRateMonitor.heartRate = readu16(&event->report[4]); 521 | 522 | return SH2_OK; 523 | } 524 | 525 | static int decodeArvrStabilizedRV(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 526 | { 527 | value->un.arvrStabilizedRV.i = read16(&event->report[4]) * SCALE_Q(14); 528 | value->un.arvrStabilizedRV.j = read16(&event->report[6]) * SCALE_Q(14); 529 | value->un.arvrStabilizedRV.k = read16(&event->report[8]) * SCALE_Q(14); 530 | value->un.arvrStabilizedRV.real = read16(&event->report[10]) * SCALE_Q(14); 531 | value->un.arvrStabilizedRV.accuracy = read16(&event->report[12]) * SCALE_Q(12); 532 | 533 | return SH2_OK; 534 | } 535 | 536 | static int decodeArvrStabilizedGRV(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 537 | { 538 | value->un.arvrStabilizedGRV.i = read16(&event->report[4]) * SCALE_Q(14); 539 | value->un.arvrStabilizedGRV.j = read16(&event->report[6]) * SCALE_Q(14); 540 | value->un.arvrStabilizedGRV.k = read16(&event->report[8]) * SCALE_Q(14); 541 | value->un.arvrStabilizedGRV.real = read16(&event->report[10]) * SCALE_Q(14); 542 | 543 | return SH2_OK; 544 | } 545 | 546 | static int decodeGyroIntegratedRV(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 547 | { 548 | value->un.gyroIntegratedRV.i = read16(&event->report[0]) * SCALE_Q(14); 549 | value->un.gyroIntegratedRV.j = read16(&event->report[2]) * SCALE_Q(14); 550 | value->un.gyroIntegratedRV.k = read16(&event->report[4]) * SCALE_Q(14); 551 | value->un.gyroIntegratedRV.real = read16(&event->report[6]) * SCALE_Q(14); 552 | value->un.gyroIntegratedRV.angVelX = read16(&event->report[8]) * SCALE_Q(10); 553 | value->un.gyroIntegratedRV.angVelY = read16(&event->report[10]) * SCALE_Q(10); 554 | value->un.gyroIntegratedRV.angVelZ = read16(&event->report[12]) * SCALE_Q(10); 555 | 556 | return SH2_OK; 557 | } 558 | 559 | static int decodeIZroRequest(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 560 | { 561 | value->un.izroRequest.intent = (sh2_IZroMotionIntent_t)event->report[4]; 562 | value->un.izroRequest.request = (sh2_IZroMotionRequest_t)event->report[5]; 563 | 564 | return SH2_OK; 565 | } 566 | 567 | static int decodeRawOptFlow(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event) 568 | { 569 | // Decode Raw optical flow 570 | value->un.rawOptFlow.dx = read16(&event->report[4]); 571 | value->un.rawOptFlow.dy = read16(&event->report[6]); 572 | value->un.rawOptFlow.iq = read16(&event->report[8]); 573 | value->un.rawOptFlow.resX = read8(&event->report[10]); 574 | value->un.rawOptFlow.resY = read8(&event->report[11]); 575 | value->un.rawOptFlow.shutter = read8(&event->report[12]); 576 | value->un.rawOptFlow.frameMax = read8(&event->report[13]); 577 | value->un.rawOptFlow.frameAvg = read8(&event->report[14]); 578 | value->un.rawOptFlow.frameMin = read8(&event->report[15]); 579 | value->un.rawOptFlow.laserOn = read8(&event->report[16]); 580 | value->un.rawOptFlow.dt = read16(&event->report[18]); 581 | value->un.rawOptFlow.timestamp = read32(&event->report[20]); 582 | 583 | return SH2_OK; 584 | } 585 | 586 | static int decodeDeadReckoningPose(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event){ 587 | value->un.deadReckoningPose.timestamp = read32(&event->report[4]); 588 | value->un.deadReckoningPose.linPosX = read32(&event->report[8]) * SCALE_Q(17); 589 | value->un.deadReckoningPose.linPosY = read32(&event->report[12]) * SCALE_Q(17); 590 | value->un.deadReckoningPose.linPosZ = read32(&event->report[16]) * SCALE_Q(17); 591 | 592 | value->un.deadReckoningPose.i = read32(&event->report[20]) * SCALE_Q(30); 593 | value->un.deadReckoningPose.j = read32(&event->report[24]) * SCALE_Q(30); 594 | value->un.deadReckoningPose.k = read32(&event->report[28]) * SCALE_Q(30); 595 | value->un.deadReckoningPose.real = read32(&event->report[32]) * SCALE_Q(30); 596 | 597 | value->un.deadReckoningPose.linVelX = read32(&event->report[36]) * SCALE_Q(25); 598 | value->un.deadReckoningPose.linVelY = read32(&event->report[40]) * SCALE_Q(25); 599 | value->un.deadReckoningPose.linVelZ = read32(&event->report[44]) * SCALE_Q(25); 600 | 601 | value->un.deadReckoningPose.angVelX = read32(&event->report[48]) * SCALE_Q(25); 602 | value->un.deadReckoningPose.angVelY = read32(&event->report[52]) * SCALE_Q(25); 603 | value->un.deadReckoningPose.angVelZ = read32(&event->report[56]) * SCALE_Q(25); 604 | return SH2_OK; 605 | } 606 | 607 | static int decodeWheelEncoder(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event){ 608 | value->un.wheelEncoder.timestamp = read32(&event->report[4]); 609 | value->un.wheelEncoder.wheelIndex = read8(&event->report[8]); 610 | value->un.wheelEncoder.dataType = read8(&event->report[9]); 611 | value->un.wheelEncoder.data = read16(&event->report[10]); 612 | return SH2_OK; 613 | } 614 | -------------------------------------------------------------------------------- /sh2_SensorValue.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2015-21 CEVA, 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 CEVA, 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 | #include 30 | 31 | #include "sh2.h" 32 | 33 | /* Note on quaternion naming conventions: 34 | * Quaternions are values with four real components that are usually 35 | * interpreted as coefficients in the complex quantity, Q. 36 | * 37 | * As in, Q = W + Xi + Yj + Zk 38 | * 39 | * Where i, j and k represent the three imaginary dimensions. 40 | * 41 | * So W represents the Real components and X, Y and Z the Imaginary ones. 42 | * 43 | * In the Hillcrest datasheets and in this code, however, the four components 44 | * are named real, i, j and k, to make it explicit which is which. If you 45 | * need to translate these names into the "wxyz" or "xyzw" convention, then, the 46 | * appropriate mapping is this: 47 | * w = real 48 | * x = i 49 | * y = j 50 | * z = k 51 | */ 52 | 53 | /** 54 | * @brief Raw Accelerometer 55 | * 56 | * See the SH-2 Reference Manual for more detail. 57 | */ 58 | typedef struct sh2_RawAccelerometer { 59 | /* Units are ADC counts */ 60 | int16_t x; /**< @brief [ADC counts] */ 61 | int16_t y; /**< @brief [ADC counts] */ 62 | int16_t z; /**< @brief [ADC counts] */ 63 | 64 | /* Microseconds */ 65 | uint32_t timestamp; /**< @brief [uS] */ 66 | } sh2_RawAccelerometer_t; 67 | 68 | /** 69 | * @brief Accelerometer 70 | * 71 | * See the SH-2 Reference Manual for more detail. 72 | */ 73 | typedef struct sh2_Accelerometer { 74 | float x; 75 | float y; 76 | float z; 77 | } sh2_Accelerometer_t; 78 | 79 | /** 80 | * @brief Raw gyroscope 81 | * 82 | * See the SH-2 Reference Manual for more detail. 83 | */ 84 | typedef struct sh2_RawGyroscope { 85 | /* Units are ADC counts */ 86 | int16_t x; /**< @brief [ADC Counts] */ 87 | int16_t y; /**< @brief [ADC Counts] */ 88 | int16_t z; /**< @brief [ADC Counts] */ 89 | int16_t temperature; /**< @brief [ADC Counts] */ 90 | 91 | /* Microseconds */ 92 | uint32_t timestamp; /**< @brief [uS] */ 93 | } sh2_RawGyroscope_t; 94 | 95 | /** 96 | * @brief Gyroscope 97 | * 98 | * See the SH-2 Reference Manual for more detail. 99 | */ 100 | typedef struct sh2_Gyroscope { 101 | /* Units are rad/s */ 102 | float x; 103 | float y; 104 | float z; 105 | } sh2_Gyroscope_t; 106 | 107 | /** 108 | * @brief Uncalibrated gyroscope 109 | * 110 | * See the SH-2 Reference Manual for more detail. 111 | */ 112 | typedef struct sh2_GyroscopeUncalibrated { 113 | /* Units are rad/s */ 114 | float x; /**< @brief [rad/s] */ 115 | float y; /**< @brief [rad/s] */ 116 | float z; /**< @brief [rad/s] */ 117 | float biasX; /**< @brief [rad/s] */ 118 | float biasY; /**< @brief [rad/s] */ 119 | float biasZ; /**< @brief [rad/s] */ 120 | } sh2_GyroscopeUncalibrated_t; 121 | 122 | /** 123 | * @brief Raw Magnetometer 124 | * 125 | * See the SH-2 Reference Manual for more detail. 126 | */ 127 | typedef struct sh2_RawMagnetometer { 128 | /* Units are ADC counts */ 129 | int16_t x; /**< @brief [ADC Counts] */ 130 | int16_t y; /**< @brief [ADC Counts] */ 131 | int16_t z; /**< @brief [ADC Counts] */ 132 | 133 | /* Microseconds */ 134 | uint32_t timestamp; /**< @brief [uS] */ 135 | } sh2_RawMagnetometer_t; 136 | 137 | /** 138 | * @brief Magnetic field 139 | * 140 | * See the SH-2 Reference Manual for more detail. 141 | */ 142 | typedef struct sh2_MagneticField { 143 | /* Units are uTesla */ 144 | float x; /**< @brief [uTesla] */ 145 | float y; /**< @brief [uTesla] */ 146 | float z; /**< @brief [uTesla] */ 147 | } sh2_MagneticField_t; 148 | 149 | /** 150 | * @brief Uncalibrated magnetic field 151 | * 152 | * See the SH-2 Reference Manual for more detail. 153 | */ 154 | typedef struct sh2_MagneticFieldUncalibrated { 155 | /* Units are uTesla */ 156 | float x; /**< @brief [uTesla] */ 157 | float y; /**< @brief [uTesla] */ 158 | float z; /**< @brief [uTesla] */ 159 | float biasX; /**< @brief [uTesla] */ 160 | float biasY; /**< @brief [uTesla] */ 161 | float biasZ; /**< @brief [uTesla] */ 162 | } sh2_MagneticFieldUncalibrated_t; 163 | 164 | /** 165 | * @brief Rotation Vector with Accuracy 166 | * 167 | * See the SH-2 Reference Manual for more detail. 168 | */ 169 | typedef struct sh2_RotationVectorWAcc { 170 | float i; /**< @brief Quaternion component i */ 171 | float j; /**< @brief Quaternion component j */ 172 | float k; /**< @brief Quaternion component k */ 173 | float real; /**< @brief Quaternion component, real */ 174 | float accuracy; /**< @brief Accuracy estimate [radians] */ 175 | } sh2_RotationVectorWAcc_t; 176 | 177 | /** 178 | * @brief Rotation Vector 179 | * 180 | * See the SH-2 Reference Manual for more detail. 181 | */ 182 | typedef struct sh2_RotationVector { 183 | float i; /**< @brief Quaternion component i */ 184 | float j; /**< @brief Quaternion component j */ 185 | float k; /**< @brief Quaternion component k */ 186 | float real; /**< @brief Quaternion component real */ 187 | } sh2_RotationVector_t; 188 | 189 | /** 190 | * @brief Atmospheric Pressure 191 | * 192 | * See the SH-2 Reference Manual for more detail. 193 | */ 194 | typedef struct sh2_Pressure { 195 | float value; /**< @brief Atmospheric Pressure. [hectopascals] */ 196 | } sh2_Pressure_t; 197 | 198 | /** 199 | * @brief Ambient Light 200 | * 201 | * See the SH-2 Reference Manual for more detail. 202 | */ 203 | typedef struct sh2_AmbientLight { 204 | float value; /**< @brief Ambient Light. [lux] */ 205 | } sh2_AmbientLight_t; 206 | 207 | /** 208 | * @brief Humidity 209 | * 210 | * See the SH-2 Reference Manual for more detail. 211 | */ 212 | typedef struct sh2_Humidity { 213 | float value; /**< @brief Relative Humidity. [percent] */ 214 | } sh2_Humidity_t; 215 | 216 | /** 217 | * @brief Proximity 218 | * 219 | * See the SH-2 Reference Manual for more detail. 220 | */ 221 | typedef struct sh2_Proximity { 222 | float value; /**< @brief Proximity. [cm] */ 223 | } sh2_Proximity_t; 224 | 225 | /** 226 | * @brief Temperature 227 | * 228 | * See the SH-2 Reference Manual for more detail. 229 | */ 230 | typedef struct sh2_Temperature { 231 | float value; /**< @brief Temperature. [C] */ 232 | } sh2_Temperature_t; 233 | 234 | /** 235 | * @brief Reserved 236 | * 237 | * See the SH-2 Reference Manual for more detail. 238 | */ 239 | typedef struct sh2_Reserved { 240 | float tbd; /**< @brief Reserved */ 241 | } sh2_Reserved_t; 242 | 243 | /** 244 | * @brief TapDetector 245 | * 246 | * See the SH-2 Reference Manual for more detail. 247 | */ 248 | #define TAPDET_X (1) 249 | #define TAPDET_X_POS (2) 250 | #define TAPDET_Y (4) 251 | #define TAPDET_Y_POS (8) 252 | #define TAPDET_Z (16) 253 | #define TAPDET_Z_POS (32) 254 | #define TAPDET_DOUBLE (64) 255 | typedef struct sh2_TapDetector { 256 | uint8_t flags; /**< @brief TapDetector. */ 257 | } sh2_TapDetector_t; 258 | 259 | /** 260 | * @brief StepDetector 261 | * 262 | * See the SH-2 Reference Manual for more detail. 263 | */ 264 | typedef struct sh2_StepDetector { 265 | uint32_t latency; /**< @brief Step detect latency [uS]. */ 266 | } sh2_StepDetector_t; 267 | 268 | /** 269 | * @brief StepCounter 270 | * 271 | * See the SH-2 Reference Manual for more detail. 272 | */ 273 | typedef struct sh2_StepCounter { 274 | uint32_t latency; /**< @brief Step counter latency [uS]. */ 275 | uint16_t steps; /**< @brief Steps counted. */ 276 | } sh2_StepCounter_t; 277 | 278 | /** 279 | * @brief SigMotion 280 | * 281 | * See the SH-2 Reference Manual for more detail. 282 | */ 283 | typedef struct sh2_SigMotion { 284 | uint16_t motion; 285 | } sh2_SigMotion_t; 286 | 287 | /** 288 | * @brief StabilityClassifier 289 | * 290 | * See the SH-2 Reference Manual for more detail. 291 | */ 292 | #define STABILITY_CLASSIFIER_UNKNOWN (0) 293 | #define STABILITY_CLASSIFIER_ON_TABLE (1) 294 | #define STABILITY_CLASSIFIER_STATIONARY (2) 295 | #define STABILITY_CLASSIFIER_STABLE (3) 296 | #define STABILITY_CLASSIFIER_MOTION (4) 297 | typedef struct sh2_StabilityClassifier { 298 | uint8_t classification; 299 | } sh2_StabilityClassifier_t; 300 | 301 | /** 302 | * @brief ShakeDetector 303 | * 304 | * See the SH-2 Reference Manual for more detail. 305 | */ 306 | #define SHAKE_X (1) 307 | #define SHAKE_Y (2) 308 | #define SHAKE_Z (4) 309 | typedef struct sh2_ShakeDetector { 310 | uint16_t shake; 311 | } sh2_ShakeDetector_t; 312 | 313 | /** 314 | * @brief flipDetector 315 | * 316 | * See the SH-2 Reference Manual for more detail. 317 | */ 318 | typedef struct sh2_FlipDetector { 319 | uint16_t flip; 320 | } sh2_FlipDetector_t; 321 | 322 | /** 323 | * @brief pickupDetector 324 | * 325 | * See the SH-2 Reference Manual for more detail. 326 | */ 327 | #define PICKUP_LEVEL_TO_NOT_LEVEL (1) 328 | #define PICKUP_STOP_WITHIN_REGION (2) 329 | typedef struct sh2_PickupDetector { 330 | uint16_t pickup; /**< flag field with bits defined above. */ 331 | } sh2_PickupDetector_t; 332 | 333 | /** 334 | * @brief stabilityDetector 335 | * 336 | * See the SH-2 Reference Manual for more detail. 337 | */ 338 | #define STABILITY_ENTERED (1) 339 | #define STABILITY_EXITED (2) 340 | typedef struct sh2_StabilityDetector { 341 | uint16_t stability; /**< flag field with bits defined above. */ 342 | } sh2_StabilityDetector_t; 343 | 344 | /** 345 | * @brief Personal Activity Classifier 346 | * 347 | * See the SH-2 Reference Manual for more detail. 348 | */ 349 | #define PAC_UNKNOWN (0) 350 | #define PAC_IN_VEHICLE (1) 351 | #define PAC_ON_BICYCLE (2) 352 | #define PAC_ON_FOOT (3) 353 | #define PAC_STILL (4) 354 | #define PAC_TILTING (5) 355 | #define PAC_WALKING (6) 356 | #define PAC_RUNNING (7) 357 | typedef struct sh2_PersonalActivityClassifier { 358 | uint8_t page; 359 | bool lastPage; 360 | uint8_t mostLikelyState; 361 | uint8_t confidence[10]; 362 | } sh2_PersonalActivityClassifier_t; 363 | 364 | /** 365 | * @brief sleepDetector 366 | * 367 | * See the SH-2 Reference Manual for more detail. 368 | */ 369 | typedef struct sh2_SleepDetector { 370 | uint8_t sleepState; 371 | } sh2_SleepDetector_t; 372 | 373 | /** 374 | * @brief tiltDetector 375 | * 376 | * See the SH-2 Reference Manual for more detail. 377 | */ 378 | typedef struct sh2_TiltDetector { 379 | uint16_t tilt; 380 | } sh2_TiltDetector_t; 381 | 382 | /** 383 | * @brief pocketDetector 384 | * 385 | * See the SH-2 Reference Manual for more detail. 386 | */ 387 | typedef struct sh2_PocketDetector { 388 | uint16_t pocket; 389 | } sh2_PocketDetector_t; 390 | 391 | /** 392 | * @brief circleDetector 393 | * 394 | * See the SH-2 Reference Manual for more detail. 395 | */ 396 | typedef struct sh2_CircleDetector { 397 | uint16_t circle; 398 | } sh2_CircleDetector_t; 399 | 400 | /** 401 | * @brief heartRateMonitor 402 | * 403 | * See SH-2 Reference Manual for details. 404 | */ 405 | typedef struct sh2_HeartRateMonitor { 406 | uint16_t heartRate; /**< heart rate in beats per minute. */ 407 | } sh2_HeartRateMonitor_t; 408 | 409 | /** 410 | * @brief Gyro Integrated Rotation Vector 411 | * 412 | * See SH-2 Reference Manual for details. 413 | */ 414 | typedef struct sh2_GyroIntegratedRV { 415 | float i; /**< @brief Quaternion component i */ 416 | float j; /**< @brief Quaternion component j */ 417 | float k; /**< @brief Quaternion component k */ 418 | float real; /**< @brief Quaternion component real */ 419 | float angVelX; /**< @brief Angular velocity about x [rad/s] */ 420 | float angVelY; /**< @brief Angular velocity about y [rad/s] */ 421 | float angVelZ; /**< @brief Angular velocity about z [rad/s] */ 422 | } sh2_GyroIntegratedRV_t; 423 | 424 | typedef struct sh2_IZroRequest { 425 | sh2_IZroMotionIntent_t intent; 426 | sh2_IZroMotionRequest_t request; 427 | } sh2_IZroRequest_t; 428 | 429 | typedef struct sh2_RawOptFlow { 430 | uint32_t timestamp; 431 | int16_t dt; 432 | int16_t dx; 433 | int16_t dy; 434 | int16_t iq; 435 | uint8_t resX; 436 | uint8_t resY; 437 | uint8_t shutter; 438 | uint8_t frameMax; 439 | uint8_t frameAvg; 440 | uint8_t frameMin; 441 | uint8_t laserOn; 442 | } sh2_RawOptFlow_t; 443 | 444 | typedef struct sh2_DeadReckoningPose { 445 | uint32_t timestamp; 446 | float linPosX; 447 | float linPosY; 448 | float linPosZ; 449 | float i; 450 | float j; 451 | float k; 452 | float real; 453 | float linVelX; 454 | float linVelY; 455 | float linVelZ; 456 | float angVelX; 457 | float angVelY; 458 | float angVelZ; 459 | } sh2_DeadReckoningPose_t; 460 | 461 | typedef struct sh2_WheelEncoder { 462 | uint32_t timestamp; 463 | uint8_t wheelIndex; 464 | uint8_t dataType; 465 | uint16_t data; 466 | } sh2_WheelEncoder_t; 467 | 468 | 469 | typedef struct sh2_SensorValue { 470 | 471 | /** Which sensor produced this event. */ 472 | uint8_t sensorId; 473 | 474 | /** @brief 8-bit unsigned integer used to track reports. 475 | * 476 | * The sequence number increments once for each report sent. Gaps 477 | * in the sequence numbers indicate missing or dropped reports. 478 | */ 479 | uint8_t sequence; 480 | 481 | /* Status of a sensor 482 | * 0 - Unreliable 483 | * 1 - Accuracy low 484 | * 2 - Accuracy medium 485 | * 3 - Accuracy high 486 | */ 487 | uint8_t status; /**< @brief bits 7-5: reserved, 4-2: exponent delay, 1-0: Accuracy */ 488 | 489 | uint64_t timestamp; /**< [uS] */ 490 | 491 | uint32_t delay; /**< @brief [uS] value is delay * 2^exponent (see status) */ 492 | 493 | /** @brief Sensor Data 494 | * 495 | * Use the structure based on the value of the sensor 496 | * field. 497 | */ 498 | union { 499 | sh2_RawAccelerometer_t rawAccelerometer; 500 | sh2_Accelerometer_t accelerometer; 501 | sh2_Accelerometer_t linearAcceleration; 502 | sh2_Accelerometer_t gravity; 503 | sh2_RawGyroscope_t rawGyroscope; 504 | sh2_Gyroscope_t gyroscope; 505 | sh2_GyroscopeUncalibrated_t gyroscopeUncal; 506 | sh2_RawMagnetometer_t rawMagnetometer; 507 | sh2_MagneticField_t magneticField; 508 | sh2_MagneticFieldUncalibrated_t magneticFieldUncal; 509 | sh2_RotationVectorWAcc_t rotationVector; 510 | sh2_RotationVector_t gameRotationVector; 511 | sh2_RotationVectorWAcc_t geoMagRotationVector; 512 | sh2_Pressure_t pressure; 513 | sh2_AmbientLight_t ambientLight; 514 | sh2_Humidity_t humidity; 515 | sh2_Proximity_t proximity; 516 | sh2_Temperature_t temperature; 517 | sh2_Reserved_t reserved; 518 | sh2_TapDetector_t tapDetector; 519 | sh2_StepDetector_t stepDetector; 520 | sh2_StepCounter_t stepCounter; 521 | sh2_SigMotion_t sigMotion; 522 | sh2_StabilityClassifier_t stabilityClassifier; 523 | sh2_ShakeDetector_t shakeDetector; 524 | sh2_FlipDetector_t flipDetector; 525 | sh2_PickupDetector_t pickupDetector; 526 | sh2_StabilityDetector_t stabilityDetector; 527 | sh2_PersonalActivityClassifier_t personalActivityClassifier; 528 | sh2_SleepDetector_t sleepDetector; 529 | sh2_TiltDetector_t tiltDetector; 530 | sh2_PocketDetector_t pocketDetector; 531 | sh2_CircleDetector_t circleDetector; 532 | sh2_HeartRateMonitor_t heartRateMonitor; 533 | sh2_RotationVectorWAcc_t arvrStabilizedRV; 534 | sh2_RotationVector_t arvrStabilizedGRV; 535 | sh2_GyroIntegratedRV_t gyroIntegratedRV; 536 | sh2_IZroRequest_t izroRequest; 537 | sh2_RawOptFlow_t rawOptFlow; 538 | sh2_DeadReckoningPose_t deadReckoningPose; 539 | sh2_WheelEncoder_t wheelEncoder; 540 | } un; 541 | } sh2_SensorValue_t; 542 | 543 | int sh2_decodeSensorEvent(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event); 544 | 545 | #endif 546 | -------------------------------------------------------------------------------- /sh2_err.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2015-21 CEVA, 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 CEVA, 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 the SH2 API. 23 | * 24 | * Struct and type definitions supporting the SH2 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 | -------------------------------------------------------------------------------- /sh2_hal.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018-2021 CEVA, 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 CEVA, 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 20 | */ 21 | 22 | // Include guard 23 | #ifndef SH2_HAL_H 24 | #define SH2_HAL_H 25 | 26 | #include 27 | 28 | // Maximum SHTP Transfer and Payload sizes 29 | #define SH2_HAL_MAX_TRANSFER_OUT (128) 30 | #define SH2_HAL_MAX_PAYLOAD_OUT (128) 31 | 32 | #define SH2_HAL_MAX_TRANSFER_IN (1024) 33 | #define SH2_HAL_MAX_PAYLOAD_IN (1024) 34 | 35 | typedef struct sh2_Hal_s sh2_Hal_t; 36 | 37 | // The SH2 interface uses these functions to access the underlying 38 | // communications device. The system designer will need to 39 | // implement these. At system intialization time, an sh2_Hal_t 40 | // structure should be initialized with pointers to all the hardware 41 | // access layer functions. A pointer to this structure must then be 42 | // passed to sh2_open() to initialize the SH2 interface. 43 | // 44 | // If the DFU (download firmware update) capability is needed, the 45 | // example DFU code also uses this interface but each function has 46 | // somewhat different requirements. So a separate instance of an 47 | // sh2_Hal_t structure, pointing to different functions, is 48 | // necessary to support DFU. 49 | 50 | struct sh2_Hal_s { 51 | // This function initializes communications with the device. It 52 | // can initialize any GPIO pins and peripheral devices used to 53 | // interface with the sensor hub. 54 | // It should also perform a reset cycle on the sensor hub to 55 | // ensure communications start from a known state. 56 | int (*open)(sh2_Hal_t *self); 57 | 58 | // This function completes communications with the sensor hub. 59 | // It should put the device in reset then de-initialize any 60 | // peripherals or hardware resources that were used. 61 | void (*close)(sh2_Hal_t *self); 62 | 63 | // This function supports reading data from the sensor hub. 64 | // It will be called frequently to service the device. 65 | // 66 | // If the HAL has received a full SHTP transfer, this function 67 | // should load the data into pBuffer, set the timestamp to the 68 | // time the interrupt was detected, and return the non-zero length 69 | // of data in this transfer. 70 | // 71 | // If the HAL has not recevied a full SHTP transfer, this function 72 | // should return 0. 73 | // 74 | // Because this function is called regularly, it can be used to 75 | // perform other housekeeping operations. (In the case of UART 76 | // interfacing, bytes transmitted are staggered in time and this 77 | // function can be used to keep the transmission flowing.) 78 | int (*read)(sh2_Hal_t *self, uint8_t *pBuffer, unsigned len, uint32_t *t_us); 79 | 80 | // This function supports writing data to the sensor hub. 81 | // It is called each time the application has a block of data to 82 | // transfer to the device. 83 | // 84 | // If the device isn't ready to receive data, this function can 85 | // return 0 without performing the transmit function. 86 | // 87 | // If the transmission can be started, this function needs to 88 | // copy the data from pBuffer and return the number of bytes 89 | // accepted. It need not block. The actual transmission of 90 | // the data can continue after this function returns. 91 | int (*write)(sh2_Hal_t *self, uint8_t *pBuffer, unsigned len); 92 | 93 | // This function should return a 32-bit value representing a 94 | // microsecond counter. The count may roll over after 2^32 95 | // microseconds. 96 | uint32_t (*getTimeUs)(sh2_Hal_t *self); 97 | }; 98 | 99 | // End of include guard 100 | #endif 101 | -------------------------------------------------------------------------------- /sh2_util.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2015-21 CEVA, 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 CEVA, 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 | -------------------------------------------------------------------------------- /sh2_util.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2015-21 CEVA, 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 CEVA, 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 | -------------------------------------------------------------------------------- /shtp.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2015-21 CEVA, 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 CEVA, 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 | * Sensor Hub Transport Protocol (SHTP) API 20 | */ 21 | 22 | #include "shtp.h" 23 | #include "sh2_err.h" 24 | 25 | #include 26 | 27 | // ------------------------------------------------------------------------ 28 | // Private types 29 | 30 | #define SHTP_INSTANCES (1) // Number of SHTP devices supported 31 | #define SHTP_MAX_CHANS (8) // Max channels per SHTP device 32 | #define SHTP_HDR_LEN (4) 33 | 34 | typedef struct shtp_Channel_s { 35 | uint8_t nextOutSeq; 36 | uint8_t nextInSeq; 37 | shtp_Callback_t *callback; 38 | void *cookie; 39 | } shtp_Channel_t; 40 | 41 | // Per-instance data for SHTP 42 | typedef struct shtp_s { 43 | // Associated SHTP HAL 44 | // If 0, this indicates the SHTP instance is available for new opens 45 | sh2_Hal_t *pHal; 46 | 47 | // Asynchronous Event callback and it's cookie 48 | shtp_EventCallback_t *eventCallback; 49 | void * eventCookie; 50 | 51 | // Transmit support 52 | uint8_t outTransfer[SH2_HAL_MAX_TRANSFER_OUT]; 53 | 54 | // Receive support 55 | uint16_t inRemaining; 56 | uint8_t inChan; 57 | uint8_t inPayload[SH2_HAL_MAX_PAYLOAD_IN]; 58 | uint16_t inCursor; 59 | uint32_t inTimestamp; 60 | uint8_t inTransfer[SH2_HAL_MAX_TRANSFER_IN]; 61 | 62 | // SHTP Channels 63 | shtp_Channel_t chan[SHTP_MAX_CHANS]; 64 | 65 | // Stats 66 | uint32_t rxBadChan; 67 | uint32_t rxShortFragments; 68 | uint32_t rxTooLargePayloads; 69 | uint32_t rxInterruptedPayloads; 70 | 71 | uint32_t badTxChan; 72 | uint32_t txDiscards; 73 | uint32_t txTooLargePayloads; 74 | 75 | } shtp_t; 76 | 77 | 78 | // ------------------------------------------------------------------------ 79 | // Private data 80 | 81 | static shtp_t instances[SHTP_INSTANCES]; 82 | 83 | static bool shtp_initialized = false; 84 | 85 | // ------------------------------------------------------------------------ 86 | // Private functions 87 | 88 | static void shtp_init(void) 89 | { 90 | // Clear pHal pointer in every instance. This marks them as unallocated. 91 | for (int n = 0; n < SHTP_INSTANCES; n++) { 92 | instances[n].pHal = 0; 93 | } 94 | 95 | // Set the initialized flag so this doesn't happen again. 96 | shtp_initialized = true; 97 | } 98 | 99 | static shtp_t *getInstance(void) 100 | { 101 | for (int n = 0; n < SHTP_INSTANCES; n++) { 102 | if (instances[n].pHal == 0) { 103 | // This instance is free 104 | return &instances[n]; 105 | } 106 | } 107 | 108 | // Can't give an instance, none are free 109 | return 0; 110 | } 111 | 112 | 113 | static inline uint16_t min_u16(uint16_t a, uint16_t b) 114 | { 115 | if (a < b) { 116 | return a; 117 | } 118 | else { 119 | return b; 120 | } 121 | } 122 | 123 | // Send a cargo as a sequence of transports 124 | static int txProcess(shtp_t *pShtp, uint8_t chan, const uint8_t* pData, uint32_t len) 125 | { 126 | int status = SH2_OK; 127 | 128 | bool continuation = false; 129 | uint16_t cursor = 0; 130 | uint16_t remaining; 131 | uint16_t transferLen; // length of transfer, minus the header 132 | uint16_t lenField; 133 | 134 | cursor = 0; 135 | remaining = len; 136 | while (remaining > 0) { 137 | // How much data (not header) can we send in next transfer 138 | transferLen = min_u16(remaining, SH2_HAL_MAX_TRANSFER_OUT-SHTP_HDR_LEN); 139 | 140 | // Length field will be transferLen + SHTP_HDR_LEN 141 | lenField = transferLen + SHTP_HDR_LEN; 142 | 143 | // Put the header in the out buffer 144 | pShtp->outTransfer[0] = lenField & 0xFF; 145 | pShtp->outTransfer[1] = (lenField >> 8) & 0x7F; 146 | if (continuation) { 147 | pShtp->outTransfer[1] |= 0x80; 148 | } 149 | pShtp->outTransfer[2] = chan; 150 | pShtp->outTransfer[3] = pShtp->chan[chan].nextOutSeq++; 151 | 152 | // Stage one tranfer in the out buffer 153 | memcpy(pShtp->outTransfer+SHTP_HDR_LEN, pData+cursor, transferLen); 154 | remaining -= transferLen; 155 | cursor += transferLen; 156 | 157 | // Transmit (try repeatedly while HAL write returns 0) 158 | status = pShtp->pHal->write(pShtp->pHal, pShtp->outTransfer, lenField); 159 | while (status == 0) 160 | { 161 | shtp_service(pShtp); 162 | status = pShtp->pHal->write(pShtp->pHal, pShtp->outTransfer, lenField); 163 | } 164 | 165 | if (status < 0) 166 | { 167 | // Error, throw away this cargo 168 | pShtp->txDiscards++; 169 | return status; 170 | } 171 | 172 | // For the rest of this transmission, packets are continuations. 173 | continuation = true; 174 | } 175 | 176 | return SH2_OK; 177 | } 178 | 179 | static void rxAssemble(shtp_t *pShtp, uint8_t *in, uint16_t len, uint32_t t_us) 180 | { 181 | uint16_t payloadLen; 182 | bool continuation; 183 | uint8_t chan = 0; 184 | uint8_t seq = 0; 185 | 186 | // discard invalid short fragments 187 | if (len < SHTP_HDR_LEN) { 188 | pShtp->rxShortFragments++; 189 | if (pShtp->eventCallback) { 190 | pShtp->eventCallback(pShtp->eventCookie, SHTP_SHORT_FRAGMENT); 191 | } 192 | return; 193 | } 194 | 195 | // Interpret header fields 196 | payloadLen = (in[0] + (in[1] << 8)) & (~0x8000); 197 | continuation = ((in[1] & 0x80) != 0); 198 | chan = in[2]; 199 | seq = in[3]; 200 | 201 | if (seq != pShtp->chan[chan].nextInSeq){ 202 | if (pShtp->eventCallback) { 203 | pShtp->eventCallback(pShtp->eventCookie, 204 | SHTP_BAD_SN); 205 | } 206 | } 207 | 208 | if (payloadLen < SHTP_HDR_LEN) { 209 | pShtp->rxShortFragments++; 210 | if (pShtp->eventCallback) { 211 | pShtp->eventCallback(pShtp->eventCookie, SHTP_SHORT_FRAGMENT); 212 | } 213 | return; 214 | } 215 | 216 | if (chan >= SHTP_MAX_CHANS) { 217 | // Invalid channel id. 218 | pShtp->rxBadChan++; 219 | 220 | if (pShtp->eventCallback) { 221 | pShtp->eventCallback(pShtp->eventCookie, SHTP_BAD_RX_CHAN); 222 | } 223 | return; 224 | } 225 | 226 | // Discard earlier assembly in progress if the received data doesn't match it. 227 | if (pShtp->inRemaining) { 228 | // Check this against previously received data. 229 | if (!continuation || 230 | (chan != pShtp->inChan) || 231 | (seq != pShtp->chan[chan].nextInSeq) || 232 | (payloadLen-SHTP_HDR_LEN != pShtp->inRemaining)) { 233 | 234 | if (pShtp->eventCallback) { 235 | pShtp->eventCallback(pShtp->eventCookie, 236 | SHTP_BAD_FRAGMENT); 237 | } 238 | 239 | // This fragment doesn't fit with previous one, discard earlier data 240 | pShtp->inRemaining = 0; 241 | 242 | pShtp->rxInterruptedPayloads++; 243 | if (pShtp->eventCallback) { 244 | pShtp->eventCallback(pShtp->eventCookie, SHTP_INTERRUPTED_PAYLOAD); 245 | } 246 | } 247 | } 248 | 249 | // Remember next sequence number we expect for this channel. 250 | pShtp->chan[chan].nextInSeq = seq + 1; 251 | 252 | if (pShtp->inRemaining == 0) { 253 | if (payloadLen > sizeof(pShtp->inPayload)) { 254 | // Error: This payload won't fit! Discard it. 255 | pShtp->rxTooLargePayloads++; 256 | 257 | if (pShtp->eventCallback) { 258 | pShtp->eventCallback(pShtp->eventCookie, SHTP_TOO_LARGE_PAYLOADS); 259 | } 260 | 261 | return; 262 | } 263 | 264 | // This represents a new payload 265 | 266 | // Store timestamp 267 | pShtp->inTimestamp = t_us; 268 | 269 | // Start a new assembly. 270 | pShtp->inCursor = 0; 271 | pShtp->inChan = chan; 272 | } 273 | 274 | // Append the new fragment to the payload under construction. 275 | if (len > payloadLen) { 276 | // Only use the valid portion of the transfer 277 | len = payloadLen; 278 | } 279 | memcpy(pShtp->inPayload + pShtp->inCursor, in+SHTP_HDR_LEN, len-SHTP_HDR_LEN); 280 | pShtp->inCursor += len-SHTP_HDR_LEN; 281 | pShtp->inRemaining = payloadLen - len; 282 | 283 | // If whole payload received, deliver it to channel listener. 284 | if (pShtp->inRemaining == 0) { 285 | 286 | // Call callback if there is one. 287 | if (pShtp->chan[chan].callback != 0) { 288 | pShtp->chan[chan].callback(pShtp->chan[chan].cookie, 289 | pShtp->inPayload, pShtp->inCursor, 290 | pShtp->inTimestamp); 291 | } 292 | } 293 | } 294 | 295 | // ------------------------------------------------------------------------ 296 | // Public functions 297 | 298 | // Takes HAL pointer, returns shtp ID for use in future calls. 299 | // HAL will be opened by this call. 300 | void *shtp_open(sh2_Hal_t *pHal) 301 | { 302 | if (!shtp_initialized) { 303 | // Perform one-time module initialization 304 | shtp_init(); 305 | } 306 | 307 | // Validate params 308 | if (pHal == 0) { 309 | // Error 310 | return 0; 311 | } 312 | 313 | // Find an available instance for this open 314 | shtp_t *pShtp = getInstance(); 315 | if (pShtp == 0) { 316 | // No instances available, return error 317 | return 0; 318 | } 319 | 320 | // Clear the SHTP instance as a shortcut to initializing all fields 321 | memset(pShtp, 0, sizeof(shtp_t)); 322 | 323 | // Open HAL 324 | int status = pHal->open(pHal); 325 | if (status != SH2_OK) { 326 | return 0; 327 | } 328 | 329 | // Store reference to the HAL 330 | pShtp->pHal = pHal; 331 | 332 | return pShtp; 333 | } 334 | 335 | // Releases resources associated with this SHTP instance. 336 | // HAL will not be closed. 337 | void shtp_close(void *pInstance) 338 | { 339 | shtp_t *pShtp = (shtp_t *)pInstance; 340 | 341 | pShtp->pHal->close(pShtp->pHal); 342 | 343 | // Deallocate the SHTP instance. 344 | // (Setting pHal to 0 marks it as free.) 345 | pShtp->pHal = 0; 346 | } 347 | 348 | // Register the pointer of the callback function for reporting asynchronous events 349 | void shtp_setEventCallback(void *pInstance, 350 | shtp_EventCallback_t * eventCallback, 351 | void *eventCookie) { 352 | shtp_t *pShtp = (shtp_t *)pInstance; 353 | 354 | pShtp->eventCallback = eventCallback; 355 | pShtp->eventCookie = eventCookie; 356 | } 357 | 358 | // Register a listener for an SHTP channel 359 | int shtp_listenChan(void *pInstance, 360 | uint8_t channel, 361 | shtp_Callback_t *callback, void * cookie) 362 | { 363 | shtp_t *pShtp = (shtp_t *)pInstance; 364 | 365 | // Balk if channel is invalid 366 | if ((channel == 0) || (channel >= SHTP_MAX_CHANS)) { 367 | return SH2_ERR_BAD_PARAM; 368 | } 369 | 370 | pShtp->chan[channel].callback = callback; 371 | pShtp->chan[channel].cookie = cookie; 372 | 373 | return SH2_OK; 374 | } 375 | 376 | // Send an SHTP payload on a particular channel 377 | int shtp_send(void *pInstance, 378 | uint8_t channel, 379 | const uint8_t *payload, uint16_t len) 380 | { 381 | shtp_t *pShtp = (shtp_t *)pInstance; 382 | 383 | if (len > SH2_HAL_MAX_PAYLOAD_OUT) { 384 | pShtp->txTooLargePayloads++; 385 | return SH2_ERR_BAD_PARAM; 386 | } 387 | if (channel >= SHTP_MAX_CHANS) { 388 | pShtp->badTxChan++; 389 | return SH2_ERR_BAD_PARAM; 390 | } 391 | 392 | return txProcess(pShtp, channel, payload, len); 393 | } 394 | 395 | // Check for received data and process it. 396 | void shtp_service(void *pInstance) 397 | { 398 | shtp_t *pShtp = (shtp_t *)pInstance; 399 | uint32_t t_us = 0; 400 | 401 | int len = pShtp->pHal->read(pShtp->pHal, pShtp->inTransfer, sizeof(pShtp->inTransfer), &t_us); 402 | if (len > 0) { 403 | rxAssemble(pShtp, pShtp->inTransfer, len, t_us); 404 | } 405 | } 406 | -------------------------------------------------------------------------------- /shtp.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2015-21 CEVA, 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 CEVA, 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 | * 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 | typedef enum shtp_Event_e { 31 | SHTP_SHORT_FRAGMENT = 1, 32 | SHTP_TOO_LARGE_PAYLOADS = 2, 33 | SHTP_BAD_RX_CHAN = 3, 34 | SHTP_BAD_TX_CHAN = 4, 35 | SHTP_BAD_FRAGMENT = 5, 36 | SHTP_BAD_SN = 6, 37 | SHTP_INTERRUPTED_PAYLOAD = 7, 38 | } shtp_Event_t; 39 | 40 | typedef void shtp_Callback_t(void * cookie, uint8_t *payload, uint16_t len, uint32_t timestamp); 41 | typedef void shtp_EventCallback_t(void *cookie, shtp_Event_t shtpEvent); 42 | 43 | // Open the SHTP communications session. 44 | // Takes a pointer to a HAL, which will be opened by this function. 45 | // Returns a pointer referencing the open SHTP session. (Pass this as pInstance to later calls.) 46 | void * shtp_open(sh2_Hal_t *pHal); 47 | 48 | // Closes and SHTP session. 49 | // The associated HAL will be closed. 50 | void shtp_close(void *pShtp); 51 | 52 | // Set the callback function for reporting SHTP asynchronous events 53 | void shtp_setEventCallback(void *pInstance, 54 | shtp_EventCallback_t * eventCallback, 55 | void *eventCookie); 56 | 57 | // Register a listener for an SHTP channel 58 | int shtp_listenChan(void *pShtp, 59 | uint8_t channel, 60 | shtp_Callback_t *callback, void * cookie); 61 | 62 | // Send an SHTP payload on a particular channel 63 | int shtp_send(void *pShtp, 64 | uint8_t channel, const uint8_t *payload, uint16_t len); 65 | 66 | // Check for received data and process it. 67 | void shtp_service(void *pShtp); 68 | 69 | // #ifdef SHTP_H 70 | #endif 71 | --------------------------------------------------------------------------------