ROS 2 与 App 通讯 03：发布 BNO086 IMU 数据app/sketch/lib/Adafruit_BNO08x_Ventuno/src/sh2.c
/*
* Copyright 2015-2018 Hillcrest Laboratories, Inc.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License and
* any applicable agreements you may have with Hillcrest Laboratories, Inc.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
/**
* @file sh2.c
* @author David Wheeler
* @date 22 Sept 2015
* @brief API Definition for Hillcrest SH-2 Sensor Hub.
*
* The sh2 API provides functions for opening a session with
* the sensor hub and performing all supported operations with it.
* This includes enabling sensors and reading events as well as
* other housekeeping functions.
*
*/
#include "sh2.h"
#include "sh2_err.h"
#include "shtp.h"
#include "sh2_util.h"
#include <string.h>
// ------------------------------------------------------------------------
// Private type definitions
#define GUID_EXECUTABLE (1)
#define GUID_SENSORHUB (2)
// executable/device channel responses
#define EXECUTABLE_DEVICE_CMD_RESET (1)
#define EXECUTABLE_DEVICE_CMD_ON (2)
#define EXECUTABLE_DEVICE_CMD_SLEEP (3)
// executable/device channel responses
#define EXECUTABLE_DEVICE_RESP_RESET_COMPLETE (1)
// Tags for sensorhub app advertisements.
#define TAG_SH2_VERSION (0x80)
#define TAG_SH2_REPORT_LENGTHS (0x81)
// Max length of sensorhub version string.
#define MAX_VER_LEN (16)
// Max number of report ids supported
#define SH2_MAX_REPORT_IDS (64)
#if defined(_MSC_VER)
#define PACKED_STRUCT struct
#pragma pack(push, 1)
#elif defined(__GNUC__)
#define PACKED_STRUCT struct __attribute__((packed))
#else
#define PACKED_STRUCT __packed struct
#endif
#define ADVERT_TIMEOUT_US (200000)
// Command and Subcommand values
#define SH2_CMD_ERRORS 1
#define SH2_CMD_COUNTS 2
#define SH2_COUNTS_GET_COUNTS 0
#define SH2_COUNTS_CLEAR_COUNTS 1
#define SH2_CMD_TARE 3
#define SH2_TARE_TARE_NOW 0
#define SH2_TARE_PERSIST_TARE 1
#define SH2_TARE_SET_REORIENTATION 2
#define SH2_CMD_INITIALIZE 4
#define SH2_INIT_SYSTEM 1
#define SH2_INIT_UNSOLICITED 0x80
// #define SH2_CMD_FRS 5 /* Depreciated */
#define SH2_CMD_DCD 6
#define SH2_CMD_ME_CAL 7
#define SH2_CMD_DCD_SAVE 9
#define SH2_CMD_GET_OSC_TYPE 0x0A
#define SH2_CMD_CLEAR_DCD_AND_RESET 0x0B
#define SH2_CMD_CAL 0x0C
#define SH2_CAL_START 0
#define SH2_CAL_FINISH 1
#define SH2_CMD_BOOTLOADER 0x0D /* SH-2 Reference Manual 6.4.12 */
#define SH2_BL_MODE_REQ 0
#define SH2_BL_STATUS_REQ 1
#define SH2_CMD_INTERACTIVE_ZRO 0x0E /* SH-2 Reference Manual 6.4.13 */
// SENSORHUB_COMMAND_REQ
#define SENSORHUB_COMMAND_REQ (0xF2)
#define COMMAND_PARAMS (9)
typedef PACKED_STRUCT {
uint8_t reportId;
uint8_t seq;
uint8_t command;
uint8_t p[COMMAND_PARAMS];
} CommandReq_t;
// SENSORHUB_COMMAND_RESP
#define SENSORHUB_COMMAND_RESP (0xF1)
#define RESPONSE_VALUES (11)
typedef PACKED_STRUCT {
uint8_t reportId;
uint8_t seq;
uint8_t command;
uint8_t commandSeq;
uint8_t respSeq;
uint8_t r[RESPONSE_VALUES];
} CommandResp_t;
// SENSORHUB_PROD_ID_REQ
#define SENSORHUB_PROD_ID_REQ (0xF9)
typedef PACKED_STRUCT {
uint8_t reportId;
uint8_t reserved;
} ProdIdReq_t;
// SENSORHUB_PROD_ID_RESP
#define SENSORHUB_PROD_ID_RESP (0xF8)
typedef PACKED_STRUCT {
uint8_t reportId;
uint8_t resetCause;
uint8_t swVerMajor;
uint8_t swVerMinor;
uint32_t swPartNumber;
uint32_t swBuildNumber;
uint16_t swVerPatch;
uint8_t reserved0;
uint8_t reserved1;
} ProdIdResp_t;
// Report definitions
// Bit fields for Feature Report flags
#define FEAT_CHANGE_SENSITIVITY_RELATIVE (1)
#define FEAT_CHANGE_SENSITIVITY_ABSOLUTE (0)
#define FEAT_CHANGE_SENSITIVITY_ENABLED (2)
#define FEAT_CHANGE_SENSITIVITY_DISABLED (0)
#define FEAT_WAKE_ENABLED (4)
#define FEAT_WAKE_DISABLED (0)
#define FEAT_ALWAYS_ON_ENABLED (8)
#define FEAT_ALWAYS_ON_DISABLED (0)
// GET_FEATURE_REQ
#define SENSORHUB_GET_FEATURE_REQ (0xFE)
typedef PACKED_STRUCT{
uint8_t reportId;
uint8_t featureReportId;
} GetFeatureReq_t;
// SENSORHUB_GET_FEATURE_RESP
#define SENSORHUB_GET_FEATURE_RESP (0xFC)
typedef PACKED_STRUCT{
uint8_t reportId;
uint8_t featureReportId; // sensor id
uint8_t flags; // FEAT_... values
uint16_t changeSensitivity;
uint32_t reportInterval_uS;
uint32_t batchInterval_uS;
uint32_t sensorSpecific;
} GetFeatureResp_t;
typedef struct sh2_s sh2_t;
typedef int (sh2_OpStart_t)(sh2_t *pSh2);
typedef void (sh2_OpRx_t)(sh2_t *pSh2, const uint8_t *payload, uint16_t len);
typedef struct sh2_Op_s {
uint32_t timeout_us;
sh2_OpStart_t *start;
sh2_OpRx_t *rx;
} sh2_Op_t;
// Parameters and state information for the operation in progress
typedef union {
struct {
CommandReq_t req;
} sendCmd;
struct {
sh2_ProductIds_t *pProdIds;
uint8_t nextEntry;
uint8_t expectedEntries;
} getProdIds;
struct {
sh2_SensorConfig_t *pConfig;
sh2_SensorId_t sensorId;
} getSensorConfig;
struct {
const sh2_SensorConfig_t *pConfig;
sh2_SensorId_t sensorId;
} setSensorConfig;
struct {
uint16_t frsType;
uint32_t *pData;
uint16_t *pWords;
uint16_t nextOffset;
} getFrs;
struct {
uint16_t frsType;
uint32_t *pData;
uint16_t words;
uint16_t offset;
} setFrs;
struct {
uint8_t severity;
sh2_ErrorRecord_t *pErrors;
uint16_t *pNumErrors;
uint16_t errsRead;
} getErrors;
struct {
sh2_SensorId_t sensorId;
sh2_Counts_t *pCounts;
} getCounts;
struct {
uint8_t sensors;
} calConfig;
struct {
uint8_t *pSensors;
} getCalConfig;
struct {
sh2_SensorId_t sensorId;
} forceFlush;
struct {
sh2_OscType_t *pOscType;
} getOscType;
struct {
uint32_t interval_us;
} startCal;
struct {
sh2_CalStatus_t status;
} finishCal;
} sh2_OpData_t;
// Max length of an FRS record, words.
#define MAX_FRS_WORDS (72)
struct sh2_s {
// Pointer to the SHTP HAL
sh2_Hal_t *pHal;
// associated SHTP instance
void *pShtp;
volatile bool resetComplete;
bool advertDone;
uint8_t executableChan;
uint8_t controlChan;
char version[MAX_VER_LEN+1];
// Report lengths
struct {
uint8_t id;
uint8_t len;
} report[SH2_MAX_REPORT_IDS];
// Multi-step operation support
const sh2_Op_t *pOp;
int opStatus;
sh2_OpData_t opData;
uint8_t lastCmdId;
uint8_t cmdSeq;
uint8_t nextCmdSeq;
// Event callback and it's cookie
sh2_EventCallback_t *eventCallback;
void * eventCookie;
// Sensor callback and it's cookie
sh2_SensorCallback_t *sensorCallback;
void * sensorCookie;
// Storage space for reading sensor metadata
uint32_t frsData[MAX_FRS_WORDS];
uint16_t frsDataLen;
// Stats
uint32_t execBadPayload;
uint32_t emptyPayloads;
uint32_t unknownReportIds;
};
#define SENSORHUB_BASE_TIMESTAMP_REF (0xFB)
typedef PACKED_STRUCT {
uint8_t reportId;
uint32_t timebase;
} BaseTimestampRef_t;
#define SENSORHUB_TIMESTAMP_REBASE (0xFA)
typedef PACKED_STRUCT {
uint8_t reportId;
int32_t timebase;
} TimestampRebase_t;
// SENSORHUB_FORCE_SENSOR_FLUSH
#define SENSORHUB_FORCE_SENSOR_FLUSH (0xF0)
typedef PACKED_STRUCT {
uint8_t reportId;
uint8_t sensorId;
} ForceFlushReq_t;
// SENSORHUB_FLUSH_COMPLETED
#define SENSORHUB_FLUSH_COMPLETED (0xEF)
typedef PACKED_STRUCT {
uint8_t reportId;
uint8_t sensorId;
} ForceFlushResp_t;
// ------------------------------------------------------------------------
// Private data
// SH2 state
sh2_t _sh2;
// SH2 Async Event Message
static sh2_AsyncEvent_t sh2AsyncEvent;
// ------------------------------------------------------------------------
// Private functions
// SH-2 transaction phases
static int opStart(sh2_t *pSh2, const sh2_Op_t *pOp)
{
// return error if another operation already in progress
if (pSh2->pOp) return SH2_ERR_OP_IN_PROGRESS;
// Establish this operation as the new operation in progress
pSh2->pOp = pOp;
pSh2->opStatus = SH2_OK;
int rc = pOp->start(pSh2); // Call start method
if (rc != SH2_OK) {
// Unregister this operation
pSh2->opStatus = rc;
pSh2->pOp = 0;
}
return rc;
}
static void opRx(sh2_t *pSh2, const uint8_t *payload, uint16_t len)
{
if ((pSh2->pOp != 0) && // An operation is in progress
(pSh2->pOp->rx != 0)) { // and it has an rx method
pSh2->pOp->rx(pSh2, payload, len); // Call receive method
}
}
static void sensorhubAdvertHdlr(void *cookie, uint8_t tag, uint8_t len, uint8_t *value)
{
sh2_t *pSh2 = (sh2_t *)cookie;
switch (tag) {
case TAG_SH2_VERSION:
strcpy(pSh2->version, (const char *)value);
break;
case TAG_SH2_REPORT_LENGTHS:
{
uint8_t reports = len/2;
if (reports > SH2_MAX_REPORT_IDS) {
// Hub gave us more report lengths than we can store!
reports = SH2_MAX_REPORT_IDS;
}
for (int n = 0; n < reports; n++) {
pSh2->report[n].id = value[n*2];
pSh2->report[n].len = value[n*2 + 1];
}
break;
}
case 0:
{
// 0 tag indicates end of advertisements for this app
// At this time, the SHTP layer can give us our channel numbers
pSh2->executableChan = shtp_chanNo(pSh2->pShtp, "executable", "device");
pSh2->controlChan = shtp_chanNo(pSh2->pShtp, "sensorhub", "control");
pSh2->advertDone = true;
break;
}
default:
break;
}
}
static void sensorhubControlHdlr(void *cookie, uint8_t *payload, uint16_t len, uint32_t timestamp)
{
sh2_t *pSh2 = (sh2_t *)cookie;
uint16_t cursor = 0;
uint32_t count = 0;
CommandResp_t * pResp = 0;
if (len == 0) {
pSh2->emptyPayloads++;
return;
}
while (cursor < len) {
// Get next report id
count++;
uint8_t reportId = payload[cursor];
// Determine report length
uint8_t reportLen = 0;
for (int n = 0; n < SH2_MAX_REPORT_IDS; n++) {
if (pSh2->report[n].id == reportId) {
reportLen = pSh2->report[n].len;
break;
}
}
if (reportLen == 0) {
// An unrecognized report id
pSh2->unknownReportIds++;
return;
}
else {
// Check for unsolicited initialize response
if (reportId == SENSORHUB_COMMAND_RESP) {
pResp = (CommandResp_t *)(payload+cursor);
if ((pResp->command == (SH2_CMD_INITIALIZE | SH2_INIT_UNSOLICITED)) &&
(pResp->r[1] == SH2_INIT_SYSTEM)) {
// This is an unsolicited INIT message.
// Is it time to call reset callback?
}
} // Check for Get Feature Response
else if (reportId == SENSORHUB_GET_FEATURE_RESP) {
if (pSh2->eventCallback) {
GetFeatureResp_t * pGetFeatureResp;
pGetFeatureResp = (GetFeatureResp_t *)(payload + cursor);
sh2AsyncEvent.eventId = SH2_GET_FEATURE_RESP;
sh2AsyncEvent.sh2SensorConfigResp.sensorId = pGetFeatureResp->featureReportId;
sh2AsyncEvent.sh2SensorConfigResp.sensorConfig.changeSensitivityEnabled = ((pGetFeatureResp->flags & FEAT_CHANGE_SENSITIVITY_ENABLED) != 0);
sh2AsyncEvent.sh2SensorConfigResp.sensorConfig.changeSensitivityRelative = ((pGetFeatureResp->flags & FEAT_CHANGE_SENSITIVITY_RELATIVE) != 0);
sh2AsyncEvent.sh2SensorConfigResp.sensorConfig.wakeupEnabled = ((pGetFeatureResp->flags & FEAT_WAKE_ENABLED) != 0);
sh2AsyncEvent.sh2SensorConfigResp.sensorConfig.alwaysOnEnabled = ((pGetFeatureResp->flags & FEAT_ALWAYS_ON_ENABLED) != 0);
sh2AsyncEvent.sh2SensorConfigResp.sensorConfig.changeSensitivity = pGetFeatureResp->changeSensitivity;
sh2AsyncEvent.sh2SensorConfigResp.sensorConfig.reportInterval_us = pGetFeatureResp->reportInterval_uS;
sh2AsyncEvent.sh2SensorConfigResp.sensorConfig.batchInterval_us = pGetFeatureResp->batchInterval_uS;
sh2AsyncEvent.sh2SensorConfigResp.sensorConfig.sensorSpecific = pGetFeatureResp->sensorSpecific;
pSh2->eventCallback(pSh2->eventCookie, &sh2AsyncEvent);
}
}
// Hand off to operation in progress, if any
opRx(pSh2, payload+cursor, reportLen);
cursor += reportLen;
}
}
}
static int opCompleted(sh2_t *pSh2, int status)
{
// Record status
pSh2->opStatus = status;
// Signal that op is done.
pSh2->pOp = 0;
return SH2_OK;
}
static int opProcess(sh2_t *pSh2, const sh2_Op_t *pOp)
{
int status = SH2_OK;
uint32_t start_us = 0;
start_us = pSh2->pHal->getTimeUs(pSh2->pHal);
status = opStart(&_sh2, pOp);
if (status != SH2_OK) {
return status;
}
uint32_t now_us = start_us;
// While op not complete and not timed out.
while ((pSh2->pOp != 0) &&
((pOp->timeout_us == 0) ||
((now_us-start_us) < pOp->timeout_us))) {
// Service SHTP to poll the device.
shtp_service(pSh2->pShtp);
// Update the time
now_us = pSh2->pHal->getTimeUs(pSh2->pHal);
}
if (pSh2->pOp != 0) {
// Operation has timed out. Clean up.
pSh2->pOp = 0;
pSh2->opStatus = SH2_ERR_TIMEOUT;
}
return pSh2->opStatus;
}
static uint8_t getReportLen(sh2_t *pSh2, uint8_t reportId)
{
for (int n = 0; n < SH2_MAX_REPORT_IDS; n++) {
if (pSh2->report[n].id == reportId) {
return pSh2->report[n].len;
}
}
return 0;
}
// Produce 64-bit microsecond timestamp for a sensor event
static uint64_t touSTimestamp(uint32_t hostInt, int32_t referenceDelta, uint16_t delay)
{
static uint32_t lastHostInt = 0;
static uint32_t rollovers = 0;
uint64_t timestamp;
// Count times hostInt timestamps rolled over to produce upper bits
if (hostInt < lastHostInt) {
rollovers++;
}
lastHostInt = hostInt;
timestamp = ((uint64_t)rollovers << 32);
timestamp += hostInt + (referenceDelta + delay) * 100;
return timestamp;
}
static void sensorhubInputHdlr(sh2_t *pSh2, uint8_t *payload, uint16_t len, uint32_t timestamp)
{
sh2_SensorEvent_t event;
uint16_t cursor = 0;
uint32_t referenceDelta;
referenceDelta = 0;
while (cursor < len) {
// Get next report id
uint8_t reportId = payload[cursor];
// Determine report length
uint8_t reportLen = getReportLen(pSh2, reportId);
if (reportLen == 0) {
// An unrecognized report id
pSh2->unknownReportIds++;
return;
}
else {
if (reportId == SENSORHUB_BASE_TIMESTAMP_REF) {
const BaseTimestampRef_t *rpt = (const BaseTimestampRef_t *)(payload+cursor);
// store base timestamp reference
referenceDelta = -rpt->timebase;
}
else if (reportId == SENSORHUB_TIMESTAMP_REBASE) {
const TimestampRebase_t *rpt = (const TimestampRebase_t *)(payload+cursor);
referenceDelta += rpt->timebase;
}
else if (reportId == SENSORHUB_FLUSH_COMPLETED) {
// Route this as if it arrived on command channel.
opRx(pSh2, payload+cursor, reportLen);
}
else {
uint8_t *pReport = payload+cursor;
uint16_t delay = ((pReport[2] & 0xFC) << 6) + pReport[3];
event.timestamp_uS = touSTimestamp(timestamp, referenceDelta, delay);
event.reportId = reportId;
memcpy(event.report, pReport, reportLen);
event.len = reportLen;
if (pSh2->sensorCallback != 0) {
pSh2->sensorCallback(pSh2->sensorCookie, &event);
}
}
cursor += reportLen;
}
}
}
static void sensorhubInputNormalHdlr(void *cookie, uint8_t *payload, uint16_t len, uint32_t timestamp)
{
sh2_t *pSh2 = (sh2_t *)cookie;
sensorhubInputHdlr(pSh2, payload, len, timestamp);
}
static void sensorhubInputWakeHdlr(void *cookie, uint8_t *payload, uint16_t len, uint32_t timestamp)
{
sh2_t *pSh2 = (sh2_t *)cookie;
sensorhubInputHdlr(pSh2, payload, len, timestamp);
}
static void sensorhubInputGyroRvHdlr(void *cookie, uint8_t *payload, uint16_t len, uint32_t timestamp)
{
sh2_t *pSh2 = (sh2_t *)cookie;
sh2_SensorEvent_t event;
uint16_t cursor = 0;
uint8_t reportId = SH2_GYRO_INTEGRATED_RV;
uint8_t reportLen = getReportLen(pSh2, reportId);
while (cursor < len) {
event.timestamp_uS = timestamp;
event.reportId = reportId;
memcpy(event.report, payload+cursor, reportLen);
event.len = reportLen;
if (pSh2->sensorCallback != 0) {
pSh2->sensorCallback(pSh2->sensorCookie, &event);
}
cursor += reportLen;
}
}
static void executableAdvertHdlr(void *cookie, uint8_t tag, uint8_t len, uint8_t *value)
{
// Ignore. No known TLV tags for this app.
}
static void executableDeviceHdlr(void *cookie, uint8_t *payload, uint16_t len, uint32_t timestamp)
{
sh2_t *pSh2 = (sh2_t *)cookie;
// Discard if length is bad
if (len != 1) {
pSh2->execBadPayload++;
return;
}
switch (payload[0]) {
case EXECUTABLE_DEVICE_RESP_RESET_COMPLETE:
// reset process is now done.
pSh2->resetComplete = true;
// Notify client that reset is complete.
sh2AsyncEvent.eventId = SH2_RESET;
if (pSh2->eventCallback) {
pSh2->eventCallback(pSh2->eventCookie, &sh2AsyncEvent);
}
break;
default:
pSh2->execBadPayload++;
break;
}
}
static int sendExecutable(sh2_t *pSh2, uint8_t cmd)
{
return shtp_send(pSh2->pShtp, pSh2->executableChan, &cmd, 1);
}
static int sendCtrl(sh2_t *pSh2, const uint8_t *data, uint16_t len)
{
return shtp_send(pSh2->pShtp, pSh2->controlChan, data, len);
}
static int16_t toQ14(double x)
{
int16_t retval = (int16_t)(x * (1<<14));
return retval;
}
// ------------------------------------------------------------------------
// Get Product ID support
// Get Product ID Op handler
static int getProdIdStart(sh2_t *pSh2)
{
int rc = SH2_OK;
ProdIdReq_t req;
pSh2->opData.getProdIds.nextEntry = 0;
pSh2->opData.getProdIds.expectedEntries = 4; // Most products supply 4 product ids.
// When the first arrives, we'll know if
// we need to adjust this.
// Set up request to issue
memset(&req, 0, sizeof(req));
req.reportId = SENSORHUB_PROD_ID_REQ;
rc = sendCtrl(pSh2, (uint8_t *)&req, sizeof(req));
return rc;
}
static void getProdIdRx(sh2_t *pSh2, const uint8_t *payload, uint16_t len)
{
ProdIdResp_t *resp = (ProdIdResp_t *)payload;
// skip this if it isn't the product id response.
if (resp->reportId != SENSORHUB_PROD_ID_RESP) return;
// Store this product id, if we can
sh2_ProductIds_t *pProdIds = pSh2->opData.getProdIds.pProdIds;
if (pProdIds) {
// Store the product id response
if (pSh2->opData.getProdIds.nextEntry < pSh2->opData.getProdIds.expectedEntries) {
sh2_ProductId_t *pProdId = &pProdIds->entry[pSh2->opData.getProdIds.nextEntry];
pProdId->resetCause = resp->resetCause;
pProdId->swVersionMajor = resp->swVerMajor;
pProdId->swVersionMinor = resp->swVerMinor;
pProdId->swPartNumber = resp->swPartNumber;
pProdId->swBuildNumber = resp->swBuildNumber;
pProdId->swVersionPatch = resp->swVerPatch;
pProdId->reserved0 = resp->reserved0;
pProdId->reserved1 = resp->reserved1;
if (pProdId->swPartNumber == 10004095) {
// FSP200 has 5 product id entries
pSh2->opData.getProdIds.expectedEntries = 5;
}
pSh2->opData.getProdIds.nextEntry++;
}
}
// Complete this operation if there is no storage for more product ids
if ((pSh2->opData.getProdIds.pProdIds == 0) ||
(pSh2->opData.getProdIds.nextEntry >= pSh2->opData.getProdIds.expectedEntries)) {
pSh2->opData.getProdIds.pProdIds->numEntries = pSh2->opData.getProdIds.nextEntry;
opCompleted(pSh2, SH2_OK);
}
return;
}
const sh2_Op_t getProdIdOp = {
.start = getProdIdStart,
.rx = getProdIdRx,
};
// ------------------------------------------------------------------------
// Set Sensor Config
static int getSensorConfigStart(sh2_t *pSh2)
{
int rc = SH2_OK;
GetFeatureReq_t req;
// set up request to issue
memset(&req, 0, sizeof(req));
req.reportId = SENSORHUB_GET_FEATURE_REQ;
req.featureReportId = pSh2->opData.getSensorConfig.sensorId;
rc = sendCtrl(pSh2, (uint8_t *)&req, sizeof(req));
return rc;
}
static void getSensorConfigRx(sh2_t *pSh2, const uint8_t *payload, uint16_t len)
{
GetFeatureResp_t *resp = (GetFeatureResp_t *)payload;
sh2_SensorConfig_t *pConfig;
// skip this if it isn't the response we're waiting for.
if (resp->reportId != SENSORHUB_GET_FEATURE_RESP) return;
if (resp->featureReportId != pSh2->opData.getSensorConfig.sensorId) return;
// Copy out data
pConfig = pSh2->opData.getSensorConfig.pConfig;
pConfig->changeSensitivityEnabled = ((resp->flags & FEAT_CHANGE_SENSITIVITY_ENABLED) != 0);
pConfig->changeSensitivityRelative = ((resp->flags & FEAT_CHANGE_SENSITIVITY_RELATIVE) != 0);
pConfig->wakeupEnabled = ((resp->flags & FEAT_WAKE_ENABLED) != 0);
pConfig->alwaysOnEnabled = ((resp->flags & FEAT_ALWAYS_ON_ENABLED) != 0);
pConfig->changeSensitivity = resp->changeSensitivity;
pConfig->reportInterval_us = resp->reportInterval_uS;
pConfig->batchInterval_us = resp->batchInterval_uS;
pConfig->sensorSpecific = resp->sensorSpecific;
// Complete this operation
opCompleted(pSh2, SH2_OK);
return;
}
const sh2_Op_t getSensorConfigOp = {
.start = getSensorConfigStart,
.rx = getSensorConfigRx,
};
// ------------------------------------------------------------------------
// Set Sensor Config
// SENSORHUB_SET_FEATURE_CMD
#define SENSORHUB_SET_FEATURE_CMD (0xFD)
typedef PACKED_STRUCT {
uint8_t reportId; // 0xFD
uint8_t featureReportId; // sensor id
uint8_t flags; // FEAT_... values
uint16_t changeSensitivity;
uint32_t reportInterval_uS;
uint32_t batchInterval_uS;
uint32_t sensorSpecific;
} SetFeatureReport_t;
static int setSensorConfigStart(sh2_t *pSh2)
{
SetFeatureReport_t req;
uint8_t flags = 0;
int rc;
sh2_SensorConfig_t *pConfig = pSh2->opData.getSensorConfig.pConfig;
if (pConfig->changeSensitivityEnabled) flags |= FEAT_CHANGE_SENSITIVITY_ENABLED;
if (pConfig->changeSensitivityRelative) flags |= FEAT_CHANGE_SENSITIVITY_RELATIVE;
if (pConfig->wakeupEnabled) flags |= FEAT_WAKE_ENABLED;
if (pConfig->alwaysOnEnabled) flags |= FEAT_ALWAYS_ON_ENABLED;
memset(&req, 0, sizeof(req));
req.reportId = SENSORHUB_SET_FEATURE_CMD;
req.featureReportId = pSh2->opData.setSensorConfig.sensorId;
req.flags = flags;
req.changeSensitivity = pConfig->changeSensitivity;
req.reportInterval_uS = pConfig->reportInterval_us;
req.batchInterval_uS = pConfig->batchInterval_us;
req.sensorSpecific = pConfig->sensorSpecific;
rc = sendCtrl(pSh2, (uint8_t *)&req, sizeof(req));
opCompleted(pSh2, rc);
return rc;
}
const sh2_Op_t setSensorConfigOp = {
.start = setSensorConfigStart,
};
// ------------------------------------------------------------------------
// Get FRS.
// SENSORHUB_FRS_WRITE_REQ
#define SENSORHUB_FRS_WRITE_REQ (0xF7)
typedef PACKED_STRUCT {
uint8_t reportId;
uint8_t reserved;
uint16_t length;
uint16_t frsType;
} FrsWriteReq_t;
// SENSORHUB_FRS_WRITE_DATA_REQ
#define SENSORHUB_FRS_WRITE_DATA_REQ (0xF6)
typedef PACKED_STRUCT {
uint8_t reportId;
uint8_t reserved;
uint16_t offset;
uint32_t data0;
uint32_t data1;
} FrsWriteDataReq_t;
// FRS write status values
#define FRS_WRITE_STATUS_RECEIVED (0)
#define FRS_WRITE_STATUS_UNRECOGNIZED_FRS_TYPE (1)
#define FRS_WRITE_STATUS_BUSY (2)
#define FRS_WRITE_STATUS_WRITE_COMPLETED (3)
#define FRS_WRITE_STATUS_READY (4)
#define FRS_WRITE_STATUS_FAILED (5)
#define FRS_WRITE_STATUS_NOT_READY (6) // data received when not in write mode
#define FRS_WRITE_STATUS_INVALID_LENGTH (7)
#define FRS_WRITE_STATUS_RECORD_VALID (8)
#define FRS_WRITE_STATUS_INVALID_RECORD (9)
#define FRS_WRITE_STATUS_DEVICE_ERROR (10)
#define FRS_WRITE_STATUS_READ_ONLY (11)
// SENSORHUB_FRS_WRITE_RESP
#define SENSORHUB_FRS_WRITE_RESP (0xF5)
typedef PACKED_STRUCT {
uint8_t reportId;
uint8_t status;
uint16_t wordOffset;
} FrsWriteResp_t;
// RESP_FRS_READ_REQ
#define SENSORHUB_FRS_READ_REQ (0xF4)
typedef PACKED_STRUCT {
uint8_t reportId;
uint8_t reserved;
uint16_t readOffset;
uint16_t frsType;
uint16_t blockSize;
} FrsReadReq_t;
// Get Datalen portion of len_status field
#define FRS_READ_DATALEN(x) ((x >> 4) & 0x0F)
// Get status portion of len_status field
#define FRS_READ_STATUS(x) ((x) & 0x0F)
// Status values
#define FRS_READ_STATUS_NO_ERROR 0
#define FRS_READ_STATUS_UNRECOGNIZED_FRS_TYPE 1
#define FRS_READ_STATUS_BUSY 2
#define FRS_READ_STATUS_READ_RECORD_COMPLETED 3
#define FRS_READ_STATUS_OFFSET_OUT_OF_RANGE 4
#define FRS_READ_STATUS_RECORD_EMPTY 5
#define FRS_READ_STATUS_READ_BLOCK_COMPLETED 6
#define FRS_READ_STATUS_READ_BLOCK_AND_RECORD_COMPLETED 7
#define FRS_READ_STATUS_DEVICE_ERROR 8
// SENSORHUB_FRS_READ_RESP
#define SENSORHUB_FRS_READ_RESP (0xF3)
typedef PACKED_STRUCT {
uint8_t reportId;
uint8_t len_status; // See FRS_READ... macros above
uint16_t wordOffset;
uint32_t data0;
uint32_t data1;
uint16_t frsType;
uint8_t reserved0;
uint8_t reserved1;
} FrsReadResp_t;
static int getFrsStart(sh2_t *pSh2)
{
int rc = SH2_OK;
FrsReadReq_t req;
pSh2->opData.getFrs.nextOffset = 0;
// set up request to issue
memset(&req, 0, sizeof(req));
req.reportId = SENSORHUB_FRS_READ_REQ;
req.reserved = 0;
req.readOffset = 0; // read from start
req.frsType = pSh2->opData.getFrs.frsType;
req.blockSize = 0; // read all avail data
rc = sendCtrl(pSh2, (uint8_t *)&req, sizeof(req));
return rc;
}
static void getFrsRx(sh2_t *pSh2, const uint8_t *payload, uint16_t len)
{
FrsReadResp_t *resp = (FrsReadResp_t *)payload;
uint8_t status;
// skip this if it isn't the response we're looking for
if (resp->reportId != SENSORHUB_FRS_READ_RESP) return;
// Check for errors: Unrecognized FRS type, Busy, Out of range, Device error
status = FRS_READ_STATUS(resp->len_status);
if ((status == FRS_READ_STATUS_UNRECOGNIZED_FRS_TYPE) ||
(status == FRS_READ_STATUS_BUSY) ||
(status == FRS_READ_STATUS_OFFSET_OUT_OF_RANGE) ||
(status == FRS_READ_STATUS_DEVICE_ERROR)
) {
// Operation failed
opCompleted(pSh2, SH2_ERR_HUB);
return;
}
if (status == FRS_READ_STATUS_RECORD_EMPTY) {
// Empty record, return zero length.
*(pSh2->opData.getFrs.pWords) = 0;
opCompleted(pSh2, SH2_OK);
}
// Store the contents from this response
uint16_t offset = resp->wordOffset;
// check for missed offsets, resulting in error.
if (offset != pSh2->opData.getFrs.nextOffset) {
// Some data was dropped.
*(pSh2->opData.getFrs.pWords) = 0;
opCompleted(pSh2, SH2_ERR_IO);
}
// store first word, if we have room
if ((*(pSh2->opData.getFrs.pWords) == 0) ||
(offset <= *(pSh2->opData.getFrs.pWords))) {
pSh2->opData.getFrs.pData[offset] = resp->data0;
pSh2->opData.getFrs.nextOffset = offset+1;
}
// store second word if there is one and we have room
if ((FRS_READ_DATALEN(resp->len_status) == 2) &&
((*(pSh2->opData.getFrs.pWords) == 0) ||
(offset <= *(pSh2->opData.getFrs.pWords)))) {
pSh2->opData.getFrs.pData[offset+1] = resp->data1;
pSh2->opData.getFrs.nextOffset = offset+2;
}
// If read is done, complete the operation
if ((status == FRS_READ_STATUS_READ_RECORD_COMPLETED) ||
(status == FRS_READ_STATUS_READ_BLOCK_COMPLETED) ||
(status == FRS_READ_STATUS_READ_BLOCK_AND_RECORD_COMPLETED)) {
*(pSh2->opData.getFrs.pWords) = pSh2->opData.getFrs.nextOffset;
opCompleted(pSh2, SH2_OK);
}
return;
}
const sh2_Op_t getFrsOp = {
.start = getFrsStart,
.rx = getFrsRx,
};
// ------------------------------------------------------------------------
// Support for sh2_getMetadata
const static struct {
sh2_SensorId_t sensorId;
uint16_t recordId;
} sensorToRecordMap[] = {
{ SH2_RAW_ACCELEROMETER, FRS_ID_META_RAW_ACCELEROMETER },
{ SH2_ACCELEROMETER, FRS_ID_META_ACCELEROMETER },
{ SH2_LINEAR_ACCELERATION, FRS_ID_META_LINEAR_ACCELERATION },
{ SH2_GRAVITY, FRS_ID_META_GRAVITY },
{ SH2_RAW_GYROSCOPE, FRS_ID_META_RAW_GYROSCOPE },
{ SH2_GYROSCOPE_CALIBRATED, FRS_ID_META_GYROSCOPE_CALIBRATED },
{ SH2_GYROSCOPE_UNCALIBRATED, FRS_ID_META_GYROSCOPE_UNCALIBRATED },
{ SH2_RAW_MAGNETOMETER, FRS_ID_META_RAW_MAGNETOMETER },
{ SH2_MAGNETIC_FIELD_CALIBRATED, FRS_ID_META_MAGNETIC_FIELD_CALIBRATED },
{ SH2_MAGNETIC_FIELD_UNCALIBRATED, FRS_ID_META_MAGNETIC_FIELD_UNCALIBRATED },
{ SH2_ROTATION_VECTOR, FRS_ID_META_ROTATION_VECTOR },
{ SH2_GAME_ROTATION_VECTOR, FRS_ID_META_GAME_ROTATION_VECTOR },
{ SH2_GEOMAGNETIC_ROTATION_VECTOR, FRS_ID_META_GEOMAGNETIC_ROTATION_VECTOR },
{ SH2_PRESSURE, FRS_ID_META_PRESSURE },
{ SH2_AMBIENT_LIGHT, FRS_ID_META_AMBIENT_LIGHT },
{ SH2_HUMIDITY, FRS_ID_META_HUMIDITY },
{ SH2_PROXIMITY, FRS_ID_META_PROXIMITY },
{ SH2_TEMPERATURE, FRS_ID_META_TEMPERATURE },
{ SH2_TAP_DETECTOR, FRS_ID_META_TAP_DETECTOR },
{ SH2_STEP_DETECTOR, FRS_ID_META_STEP_DETECTOR },
{ SH2_STEP_COUNTER, FRS_ID_META_STEP_COUNTER },
{ SH2_SIGNIFICANT_MOTION, FRS_ID_META_SIGNIFICANT_MOTION },
{ SH2_STABILITY_CLASSIFIER, FRS_ID_META_STABILITY_CLASSIFIER },
{ SH2_SHAKE_DETECTOR, FRS_ID_META_SHAKE_DETECTOR },
{ SH2_FLIP_DETECTOR, FRS_ID_META_FLIP_DETECTOR },
{ SH2_PICKUP_DETECTOR, FRS_ID_META_PICKUP_DETECTOR },
{ SH2_STABILITY_DETECTOR, FRS_ID_META_STABILITY_DETECTOR },
{ SH2_PERSONAL_ACTIVITY_CLASSIFIER, FRS_ID_META_PERSONAL_ACTIVITY_CLASSIFIER },
{ SH2_SLEEP_DETECTOR, FRS_ID_META_SLEEP_DETECTOR },
{ SH2_TILT_DETECTOR, FRS_ID_META_TILT_DETECTOR },
{ SH2_POCKET_DETECTOR, FRS_ID_META_POCKET_DETECTOR },
{ SH2_CIRCLE_DETECTOR, FRS_ID_META_CIRCLE_DETECTOR },
};
static void stuffMetadata(sh2_SensorMetadata_t *pData, uint32_t *frsData)
{
// Populate the sensorMetadata structure with results
pData->meVersion = (frsData[0] >> 0) & 0xFF;
pData->mhVersion = (frsData[0] >> 8) & 0xFF;
pData->shVersion = (frsData[0] >> 16) & 0xFF;
pData->range = frsData[1];
pData->resolution = frsData[2];
pData->power_mA = (frsData[3] >> 0) & 0xFFFF; // 16.10 format
pData->revision = (frsData[3] >> 16) & 0xFFFF;
pData->minPeriod_uS = frsData[4];
pData->maxPeriod_uS = 0; // ...unless reading format 4 metadata below
pData->fifoMax = (frsData[5] >> 0) & 0xFFFF;
pData->fifoReserved = (frsData[5] >> 16) & 0xFFFF;
pData->batchBufferBytes = (frsData[6] >> 0) & 0xFFFF;;
pData->vendorIdLen = (frsData[6] >> 16) & 0xFFFF;
// Init fields that may not be present, depending on metadata revision
pData->qPoint1 = 0;
pData->qPoint2 = 0;
pData->qPoint3 = 0;
pData->sensorSpecificLen = 0;
strcpy(pData->vendorId, ""); // init with empty string in case vendorIdLen == 0
int vendorIdOffset = 8;
// Get revision-specific fields
if (pData->revision == 0) {
// No fixed fields, vendor id copied after if-else block
}
else if (pData->revision == 1) {
pData->qPoint1 = (frsData[7] >> 0) & 0xFFFF;
pData->qPoint2 = (frsData[7] >> 16) & 0xFFFF;
}
else if (pData->revision == 2) {
pData->qPoint1 = (frsData[7] >> 0) & 0xFFFF;
pData->qPoint2 = (frsData[7] >> 16) & 0xFFFF;
pData->sensorSpecificLen = (frsData[8] >> 0) & 0xFFFF;
memcpy(pData->sensorSpecific, (uint8_t *)&frsData[9], pData->sensorSpecificLen);
vendorIdOffset = 9 + ((pData->sensorSpecificLen+3)/4); // 9 + one word for every 4 bytes of SS data
}
else if (pData->revision == 3) {
pData->qPoint1 = (frsData[7] >> 0) & 0xFFFF;
pData->qPoint2 = (frsData[7] >> 16) & 0xFFFF;
pData->sensorSpecificLen = (frsData[8] >> 0) & 0xFFFF;
pData->qPoint3 = (frsData[8] >> 16) & 0xFFFF;
memcpy(pData->sensorSpecific, (uint8_t *)&frsData[9], pData->sensorSpecificLen);
vendorIdOffset = 9 + ((pData->sensorSpecificLen+3)/4); // 9 + one word for every 4 bytes of SS data
}
else if (pData->revision == 4) {
pData->qPoint1 = (frsData[7] >> 0) & 0xFFFF;
pData->qPoint2 = (frsData[7] >> 16) & 0xFFFF;
pData->sensorSpecificLen = (frsData[8] >> 0) & 0xFFFF;
pData->qPoint3 = (frsData[8] >> 16) & 0xFFFF;
pData->maxPeriod_uS = frsData[9];
memcpy(pData->sensorSpecific, (uint8_t *)&frsData[10], pData->sensorSpecificLen);
vendorIdOffset = 10 + ((pData->sensorSpecificLen+3)/4); // 9 + one word for every 4 bytes of SS data
}
else {
// Unrecognized revision!
}
// Copy vendor id
memcpy(pData->vendorId, (uint8_t *)&frsData[vendorIdOffset],
pData->vendorIdLen);
}
// ------------------------------------------------------------------------
// Set FRS.
static int setFrsStart(sh2_t *pSh2)
{
int rc = SH2_OK;
FrsWriteReq_t req;
pSh2->opData.setFrs.offset = 0;
// set up request to issue
memset(&req, 0, sizeof(req));
req.reportId = SENSORHUB_FRS_WRITE_REQ;
req.reserved = 0;
req.length = pSh2->opData.setFrs.words;
req.frsType = pSh2->opData.getFrs.frsType;
rc = sendCtrl(pSh2, (uint8_t *)&req, sizeof(req));
return rc;
}
static void setFrsRx(sh2_t *pSh2, const uint8_t *payload, uint16_t len)
{
FrsWriteResp_t *resp = (FrsWriteResp_t *)payload;
FrsWriteDataReq_t req;
uint8_t status;
bool sendMoreData = false;
bool completed = false;
int rc = SH2_OK;
// skip this if it isn't the response we're looking for.
if (resp->reportId != SENSORHUB_FRS_WRITE_RESP) return;
// Check for errors: Unrecognized FRS type, Busy, Out of range, Device error
status = resp->status;
switch(status) {
case FRS_WRITE_STATUS_RECEIVED:
case FRS_WRITE_STATUS_READY:
sendMoreData = true;
break;
case FRS_WRITE_STATUS_UNRECOGNIZED_FRS_TYPE:
case FRS_WRITE_STATUS_BUSY:
case FRS_WRITE_STATUS_FAILED:
case FRS_WRITE_STATUS_NOT_READY:
case FRS_WRITE_STATUS_INVALID_LENGTH:
case FRS_WRITE_STATUS_INVALID_RECORD:
case FRS_WRITE_STATUS_DEVICE_ERROR:
case FRS_WRITE_STATUS_READ_ONLY:
rc = SH2_ERR_HUB;
completed = true;
break;
case FRS_WRITE_STATUS_WRITE_COMPLETED:
// Successful completion
rc = SH2_OK;
completed = true;
break;
case FRS_WRITE_STATUS_RECORD_VALID:
// That's nice, keep waiting
break;
}
// if we should send more data, do it.
if (sendMoreData &&
(pSh2->opData.setFrs.offset < pSh2->opData.setFrs.words)) {
uint16_t offset = pSh2->opData.setFrs.offset;
memset(&req, 0, sizeof(req));
req.reportId = SENSORHUB_FRS_WRITE_DATA_REQ;
req.reserved = 0;
req.offset = offset;
req.data0 = pSh2->opData.setFrs.pData[offset++];
if (offset < pSh2->opData.setFrs.words) {
req.data1 = pSh2->opData.setFrs.pData[offset++];
} else {
req.data1 = 0;
}
pSh2->opData.setFrs.offset = offset;
rc = sendCtrl(pSh2, (uint8_t *)&req, sizeof(req));
}
// if the operation is done or has to be aborted, complete it
if (completed) {
opCompleted(pSh2, rc);
}
return;
}
const sh2_Op_t setFrsOp = {
.start = setFrsStart,
.rx = setFrsRx,
};
// ------------------------------------------------------------------------
// Support for sending commands
static int sendCmd(sh2_t *pSh2, uint8_t cmd, uint8_t p[COMMAND_PARAMS])
{
int rc = SH2_OK;
CommandReq_t req;
// Clear request structure
memset(&req, 0, sizeof(req));
// Create a command sequence number for this command
pSh2->lastCmdId = cmd;
pSh2->cmdSeq = pSh2->nextCmdSeq++;
// set up request to issue
req.reportId = SENSORHUB_COMMAND_REQ;
req.seq = pSh2->cmdSeq;
req.command = cmd;
for (int n = 0; n < COMMAND_PARAMS; n++) {
req.p[n] = p[n];
}
rc = sendCtrl(pSh2, (uint8_t *)&req, sizeof(req));
return rc;
}
// Send a command with 0 parameters
static int sendCmd0(sh2_t *pSh2, uint8_t cmd)
{
uint8_t p[COMMAND_PARAMS];
memset(p, 0, COMMAND_PARAMS);
return sendCmd(pSh2, cmd, p);
}
// Send a command with 1 parameter
static int sendCmd1(sh2_t *pSh2, uint8_t cmd, uint8_t p0)
{
uint8_t p[COMMAND_PARAMS];
memset(p, 0, COMMAND_PARAMS);
p[0] = p0;
return sendCmd(pSh2, cmd, p);
}
// Send a command with 2 parameters
static int sendCmd2(sh2_t *pSh2, uint8_t cmd, uint8_t p0, uint8_t p1)
{
uint8_t p[COMMAND_PARAMS];
memset(p, 0, COMMAND_PARAMS);
p[0] = p0;
p[1] = p1;
return sendCmd(pSh2, cmd, p);
}
static bool wrongResponse(sh2_t *pSh2, CommandResp_t *resp)
{
if (resp->reportId != SENSORHUB_COMMAND_RESP) return true;
if (resp->command != pSh2->lastCmdId) return true;
if (resp->commandSeq != pSh2->cmdSeq) return true;
return false;
}
// ------------------------------------------------------------------------
// Get Errors
static int getErrorsStart(sh2_t *pSh2)
{
// Initialize state
pSh2->opData.getErrors.errsRead = 0;
return sendCmd1(pSh2, SH2_CMD_ERRORS, pSh2->opData.getErrors.severity);
}
static void getErrorsRx(sh2_t *pSh2, const uint8_t *payload, uint16_t len)
{
CommandResp_t *resp = (CommandResp_t *)payload;
// skip this if it isn't the right response
if (wrongResponse(pSh2, resp)) return;
if (resp->r[2] == 255) {
// No error to report, operation is complete
*(pSh2->opData.getErrors.pNumErrors) = pSh2->opData.getErrors.errsRead;
opCompleted(pSh2, SH2_OK);
} else {
// Copy data for invoker.
unsigned int index = pSh2->opData.getErrors.errsRead;
if (index < *(pSh2->opData.getErrors.pNumErrors)) {
// We have room for this one.
pSh2->opData.getErrors.pErrors[index].severity = resp->r[0];
pSh2->opData.getErrors.pErrors[index].sequence = resp->r[1];
pSh2->opData.getErrors.pErrors[index].source = resp->r[2];
pSh2->opData.getErrors.pErrors[index].error = resp->r[3];
pSh2->opData.getErrors.pErrors[index].module = resp->r[4];
pSh2->opData.getErrors.pErrors[index].code = resp->r[5];
pSh2->opData.getErrors.errsRead++;
}
}
return;
}
const sh2_Op_t getErrorsOp = {
.start = getErrorsStart,
.rx = getErrorsRx,
};
// ------------------------------------------------------------------------
// Get Counts
static int getCountsStart(sh2_t *pSh2)
{
return sendCmd2(pSh2, SH2_CMD_COUNTS, SH2_COUNTS_GET_COUNTS, pSh2->opData.getCounts.sensorId);
}
static void getCountsRx(sh2_t *pSh2, const uint8_t *payload, uint16_t len)
{
CommandResp_t *resp = (CommandResp_t *)payload;
if (wrongResponse(pSh2, resp)) return;
// Store results
if (resp->respSeq == 0) {
pSh2->opData.getCounts.pCounts->offered = readu32(&resp->r[3]);
pSh2->opData.getCounts.pCounts->accepted = readu32(&resp->r[7]);
}
else {
pSh2->opData.getCounts.pCounts->on = readu32(&resp->r[3]);
pSh2->opData.getCounts.pCounts->attempted = readu32(&resp->r[7]);
}
// Complete this operation if we've received last response
if (resp->respSeq == 1) {
opCompleted(pSh2, SH2_OK);
}
return;
}
const sh2_Op_t getCountsOp = {
.start = getCountsStart,
.rx = getCountsRx,
};
// ------------------------------------------------------------------------
// Generic Send Command
static int sendCmdStart(sh2_t *pSh2)
{
int status = sendCmd(pSh2, pSh2->opData.sendCmd.req.command,
pSh2->opData.sendCmd.req.p);
opCompleted(pSh2, status);
return status;
}
const sh2_Op_t sendCmdOp = {
.start = sendCmdStart,
};
// ------------------------------------------------------------------------
// Reinit
static int reinitStart(sh2_t *pSh2)
{
int status = sendCmd1(pSh2, SH2_CMD_INITIALIZE, SH2_INIT_SYSTEM);
return status;
}
static void reinitRx(sh2_t *pSh2, const uint8_t *payload, uint16_t len)
{
CommandResp_t *resp = (CommandResp_t *)payload;
// Ignore message if it doesn't pertain to this operation
if (wrongResponse(pSh2, resp)) return;
// Get return status
int status = SH2_OK;
if (resp->r[0] != 0) {
status = SH2_ERR_HUB;
}
opCompleted(pSh2, status);
}
const sh2_Op_t reinitOp = {
.start = reinitStart,
.rx = reinitRx,
};
// ------------------------------------------------------------------------
// Save DCD Now
static int saveDcdNowStart(sh2_t *pSh2)
{
int status = sendCmd0(pSh2, SH2_CMD_DCD);
return status;
}
static void saveDcdNowRx(sh2_t *pSh2, const uint8_t *payload, uint16_t len)
{
CommandResp_t *resp = (CommandResp_t *)payload;
// Ignore message if it doesn't pertain to this operation
if (wrongResponse(pSh2, resp)) return;
// Get return status
int status = SH2_OK;
if (resp->r[0] != 0) {
status = SH2_ERR_HUB;
}
opCompleted(pSh2, status);
}
const sh2_Op_t saveDcdNowOp = {
.start = saveDcdNowStart,
.rx = saveDcdNowRx,
};
// ------------------------------------------------------------------------
// Get Osc Type
static int getOscTypeStart(sh2_t *pSh2)
{
return sendCmd0(pSh2, SH2_CMD_GET_OSC_TYPE);
}
static void getOscTypeRx(sh2_t *pSh2, const uint8_t *payload, uint16_t len)
{
CommandResp_t *resp = (CommandResp_t *)payload;
sh2_OscType_t *pOscType;
// Ignore message if it doesn't pertain to this operation
if (wrongResponse(pSh2, resp)) return;
// Read out data
pOscType = pSh2->opData.getOscType.pOscType;
*pOscType = (sh2_OscType_t)resp->r[0];
// Complete this operation
opCompleted(pSh2, SH2_OK);
}
const sh2_Op_t getOscTypeOp = {
.start = getOscTypeStart,
.rx = getOscTypeRx,
};
// ------------------------------------------------------------------------
// Set Cal Config
static int setCalConfigStart(sh2_t *pSh2)
{
uint8_t p[COMMAND_PARAMS];
// Clear p. (Importantly, set subcommand in p[3] to 0, CONFIGURE)
memset(p, 0, COMMAND_PARAMS);
// Which cal processes to enable/disable
p[0] = (pSh2->opData.calConfig.sensors & SH2_CAL_ACCEL) ? 1 : 0; // accel cal
p[1] = (pSh2->opData.calConfig.sensors & SH2_CAL_GYRO) ? 1 : 0; // gyro cal
p[2] = (pSh2->opData.calConfig.sensors & SH2_CAL_MAG) ? 1 : 0; // mag cal
p[4] = (pSh2->opData.calConfig.sensors & SH2_CAL_PLANAR) ? 1 : 0; // planar cal
return sendCmd(pSh2, SH2_CMD_ME_CAL, p);
}
static void setCalConfigRx(sh2_t *pSh2, const uint8_t *payload, uint16_t len)
{
CommandResp_t *resp = (CommandResp_t *)payload;
// Ignore message if it doesn't pertain to this operation
if (wrongResponse(pSh2, resp)) return;
// Read out data
int status = SH2_OK;
if (resp->r[0] != 0) {
status = SH2_ERR_HUB;
}
// Complete this operation
opCompleted(pSh2, status);
}
const sh2_Op_t setCalConfigOp = {
.start = setCalConfigStart,
.rx = setCalConfigRx,
};
// ------------------------------------------------------------------------
// Get Cal Config
static int getCalConfigStart(sh2_t *pSh2)
{
uint8_t p[COMMAND_PARAMS];
// Clear p. (Importantly, set subcommand in p[3] to 0, CONFIGURE)
memset(p, 0, COMMAND_PARAMS);
// Subcommand: Get ME Calibration
p[3] = 0x01;
return sendCmd(pSh2, SH2_CMD_ME_CAL, p);
}
static void getCalConfigRx(sh2_t *pSh2, const uint8_t *payload, uint16_t len)
{
CommandResp_t *resp = (CommandResp_t *)payload;
// Ignore message if it doesn't pertain to this operation
if (wrongResponse(pSh2, resp)) return;
// Read out data
int status = SH2_OK;
if (resp->r[0] != 0) {
status = SH2_ERR_HUB;
}
else {
// Unload results into pSensors
uint8_t sensors = 0;
if (resp->r[1]) sensors |= SH2_CAL_ACCEL;
if (resp->r[2]) sensors |= SH2_CAL_GYRO;
if (resp->r[3]) sensors |= SH2_CAL_MAG;
if (resp->r[4]) sensors |= SH2_CAL_PLANAR;
*(pSh2->opData.getCalConfig.pSensors) = sensors;
}
// Complete this operation
opCompleted(pSh2, status);
}
const sh2_Op_t getCalConfigOp = {
.start = getCalConfigStart,
.rx = getCalConfigRx,
};
// ------------------------------------------------------------------------
// Force Flush
static int forceFlushStart(sh2_t *pSh2)
{
ForceFlushReq_t req;
memset(&req, 0, sizeof(req));
req.reportId = SENSORHUB_FORCE_SENSOR_FLUSH;
req.sensorId = pSh2->opData.forceFlush.sensorId;
return sendCtrl(pSh2, (uint8_t *)&req, sizeof(req));
}
static void forceFlushRx(sh2_t *pSh2, const uint8_t *payload, uint16_t len)
{
ForceFlushResp_t *resp = (ForceFlushResp_t *)payload;
// Ignore message if it doesn't pertain to this operation
if (resp->reportId != SENSORHUB_FLUSH_COMPLETED) return;
if (resp->sensorId != pSh2->opData.forceFlush.sensorId) return;
// Complete this operation
opCompleted(pSh2, SH2_OK);
}
const sh2_Op_t forceFlushOp = {
.start = forceFlushStart,
.rx = forceFlushRx,
};
// ------------------------------------------------------------------------
// Start Cal
static int startCalStart(sh2_t *pSh2)
{
uint8_t p[COMMAND_PARAMS];
// Clear p. (Importantly, set subcommand in p[3] to 0, CONFIGURE)
memset(p, 0, COMMAND_PARAMS);
// Subcommand: Get ME Calibration
p[0] = SH2_CAL_START;
p[1] = pSh2->opData.startCal.interval_us & 0xFF; // LSB
p[2] = (pSh2->opData.startCal.interval_us >> 8) & 0xFF;
p[3] = (pSh2->opData.startCal.interval_us >> 16) & 0xFF;
p[4] = (pSh2->opData.startCal.interval_us >> 24) & 0xFF; // MSB
return sendCmd(pSh2, SH2_CMD_CAL, p);
}
static void startCalRx(sh2_t *pSh2, const uint8_t *payload, uint16_t len)
{
CommandResp_t *resp = (CommandResp_t *)payload;
// Ignore message if it doesn't pertain to this operation
if (wrongResponse(pSh2, resp)) return;
// Complete this operation
opCompleted(pSh2, SH2_OK);
}
const sh2_Op_t startCalOp = {
.start = startCalStart,
.rx = startCalRx,
};
// ------------------------------------------------------------------------
// Start Cal
static int finishCalStart(sh2_t *pSh2)
{
return sendCmd1(pSh2, SH2_CMD_CAL, SH2_CAL_FINISH);
}
static void finishCalRx(sh2_t *pSh2, const uint8_t *payload, uint16_t len)
{
CommandResp_t *resp = (CommandResp_t *)payload;
// Ignore message if it doesn't pertain to this operation
if (wrongResponse(pSh2, resp)) return;
pSh2->opData.finishCal.status = (sh2_CalStatus_t)resp->r[1];
// Complete this operation
if (pSh2->opData.finishCal.status == SH2_CAL_SUCCESS) {
opCompleted(pSh2, SH2_OK);
}
else {
opCompleted(pSh2, SH2_ERR_HUB);
}
}
const sh2_Op_t finishCalOp = {
.start = finishCalStart,
.rx = finishCalRx,
};
// ------------------------------------------------------------------------
// SHTP Event Callback
static void shtpEventCallback(void *cookie, shtp_Event_t shtpEvent) {
sh2_t *pSh2 = &_sh2;
sh2AsyncEvent.eventId = SH2_SHTP_EVENT;
sh2AsyncEvent.shtpEvent = shtpEvent;
if (pSh2->eventCallback) {
pSh2->eventCallback(pSh2->eventCookie, &sh2AsyncEvent);
}
}
// ------------------------------------------------------------------------
// Public functions
/**
* @brief Open a session with a sensor hub.
*
* This function should be called before others in this API.
* An instance of an SH2 HAL should be passed in.
* This call will result in the open() function of the HAL being called.
*
* As part of the initialization process, a callback function is registered that will
* be invoked when the device generates certain events. (See sh2_AsyncEventId)
*
* @param pHal Pointer to an SH2 HAL instance, provided by the target system.
* @param eventCallback Will be called when events, such as reset complete, occur.
* @param eventCookie Will be passed to eventCallback.
* @return SH2_OK (0), on success. Negative value from sh2_err.h on error.
*/
int sh2_open(sh2_Hal_t *pHal,
sh2_EventCallback_t *eventCallback, void *eventCookie)
{
sh2_t *pSh2 = &_sh2;
// Validate parameters
if (pHal == 0) return SH2_ERR_BAD_PARAM;
// Clear everything in sh2 structure.
memset(&_sh2, 0, sizeof(_sh2));
pSh2->resetComplete = false; // will go true after reset response from SH.
pSh2->controlChan = 0xFF; // An invalid value since we don't know yet.
// Store reference to HAL for future use.
pSh2->pHal = pHal;
pSh2->eventCallback = eventCallback;
pSh2->eventCookie = eventCookie;
pSh2->sensorCallback = 0;
pSh2->sensorCookie = 0;
// Open SHTP layer
pSh2->pShtp = shtp_open(pSh2->pHal);
if (pSh2->pShtp == 0) {
// Error opening SHTP
return SH2_ERR;
}
// Register SHTP event callback
shtp_setEventCallback(pSh2->pShtp, shtpEventCallback, &_sh2);
// Register with SHTP
// Register SH2 handlers
shtp_listenAdvert(pSh2->pShtp, GUID_SENSORHUB, sensorhubAdvertHdlr, &_sh2);
shtp_listenChan(pSh2->pShtp, GUID_SENSORHUB, "control", sensorhubControlHdlr, &_sh2);
shtp_listenChan(pSh2->pShtp, GUID_SENSORHUB, "inputNormal", sensorhubInputNormalHdlr, &_sh2);
shtp_listenChan(pSh2->pShtp, GUID_SENSORHUB, "inputWake", sensorhubInputWakeHdlr, &_sh2);
shtp_listenChan(pSh2->pShtp, GUID_SENSORHUB, "inputGyroRv", sensorhubInputGyroRvHdlr, &_sh2);
// Register EXECUTABLE handlers
shtp_listenAdvert(pSh2->pShtp, GUID_EXECUTABLE, executableAdvertHdlr, &_sh2);
shtp_listenChan(pSh2->pShtp, GUID_EXECUTABLE, "device", executableDeviceHdlr, &_sh2);
// Wait for reset notifications to arrive.
// The client can't talk to the sensor hub until that happens.
uint32_t start_us = pSh2->pHal->getTimeUs(pSh2->pHal);
uint32_t now_us = start_us;
while (((now_us - start_us) < ADVERT_TIMEOUT_US) &&
(!pSh2->resetComplete))
{
shtp_service(pSh2->pShtp);
now_us = pSh2->pHal->getTimeUs(pSh2->pHal);
}
// No errors.
return SH2_OK;
}
/**
* @brief Close a session with a sensor hub.
*
* This should be called at the end of a sensor hub session.
* The underlying SHTP and HAL instances will be closed.
*/
void sh2_close(void)
{
sh2_t *pSh2 = &_sh2;
shtp_close(pSh2->pShtp);
// Clear everything in sh2 structure.
memset(pSh2, 0, sizeof(sh2_t));
}
/**
* @brief Service the SH2 device, reading any data that is available and dispatching callbacks.
*
* This function should be called periodically by the host system to service an open sensor hub.
*/
void sh2_service(void)
{
sh2_t *pSh2 = &_sh2;
shtp_service(pSh2->pShtp);
}
/**
* @brief Register a function to receive sensor events.
*
* @param callback A function that will be called each time a sensor event is received.
* @param cookie A value that will be passed to the sensor callback function.
* @return SH2_OK (0), on success. Negative value from sh2_err.h on error.
*/
int sh2_setSensorCallback(sh2_SensorCallback_t *callback, void *cookie)
{
sh2_t *pSh2 = &_sh2;
pSh2->sensorCallback = callback;
pSh2->sensorCookie = cookie;
return SH2_OK;
}
/**
* @brief Reset the sensor hub device by sending RESET (1) command on "device" channel.
*
* @return SH2_OK (0), on success. Negative value from sh2_err.h on error.
*/
int sh2_devReset(void)
{
sh2_t *pSh2 = &_sh2;
return sendExecutable(pSh2, EXECUTABLE_DEVICE_CMD_RESET);
}
/**
* @brief Turn sensor hub on by sending RESET (1) command on "device" channel.
*
* @return SH2_OK (0), on success. Negative value from sh2_err.h on error.
*/
int sh2_devOn(void)
{
sh2_t *pSh2 = &_sh2;
return sendExecutable(pSh2, EXECUTABLE_DEVICE_CMD_ON);
}
/**
* @brief Put sensor hub in sleep state by sending SLEEP (2) command on "device" channel.
*
* @return SH2_OK (0), on success. Negative value from sh2_err.h on error.
*/
int sh2_devSleep(void)
{
sh2_t *pSh2 = &_sh2;
return sendExecutable(pSh2, EXECUTABLE_DEVICE_CMD_SLEEP);
}
/**
* @brief Get Product ID information from Sensorhub.
*
* @param prodIds Pointer to structure that will receive results.
* @return SH2_OK (0), on success. Negative value from sh2_err.h on error.
*/
int sh2_getProdIds(sh2_ProductIds_t *prodIds)
{
sh2_t *pSh2 = &_sh2;
// clear opData
memset(&pSh2->opData, 0, sizeof(sh2_OpData_t));
pSh2->opData.getProdIds.pProdIds = prodIds;
return opProcess(pSh2, &getProdIdOp);
}
/**
* @brief Get sensor configuration.
*
* @param sensorId Which sensor to query.
* @param config SensorConfig structure to store results.
* @return SH2_OK (0), on success. Negative value from sh2_err.h on error.
*/
int sh2_getSensorConfig(sh2_SensorId_t sensorId, sh2_SensorConfig_t *pConfig)
{
sh2_t *pSh2 = &_sh2;
// clear opData
memset(&pSh2->opData, 0, sizeof(sh2_OpData_t));
// Set up operation
pSh2->opData.getSensorConfig.sensorId = sensorId;
pSh2->opData.getSensorConfig.pConfig = pConfig;
return opProcess(pSh2, &getSensorConfigOp);
}
/**
* @brief Set sensor configuration. (e.g enable a sensor at a particular rate.)
*
* @param sensorId Which sensor to configure.
* @param pConfig Pointer to structure holding sensor configuration.
* @return SH2_OK (0), on success. Negative value from sh2_err.h on error.
*/
int sh2_setSensorConfig(sh2_SensorId_t sensorId, const sh2_SensorConfig_t *pConfig)
{
sh2_t *pSh2 = &_sh2;
// clear opData
memset(&pSh2->opData, 0, sizeof(sh2_OpData_t));
// Set up operation
pSh2->opData.setSensorConfig.sensorId = sensorId;
pSh2->opData.setSensorConfig.pConfig = pConfig;
return opProcess(pSh2, &setSensorConfigOp);
}
/**
* @brief Get metadata related to a sensor.
*
* @param sensorId Which sensor to query.
* @param pData Pointer to structure to receive the results.
* @return SH2_OK (0), on success. Negative value from sh2_err.h on error.
*/
int sh2_getMetadata(sh2_SensorId_t sensorId, sh2_SensorMetadata_t *pData)
{
sh2_t *pSh2 = &_sh2;
// pData must be non-null
if (pData == 0) return SH2_ERR_BAD_PARAM;
// Convert sensorId to metadata recordId
int i;
for (i = 0; i < ARRAY_LEN(sensorToRecordMap); i++) {
if (sensorToRecordMap[i].sensorId == sensorId) {
break;
}
}
if (i >= ARRAY_LEN(sensorToRecordMap)) {
// no match was found
return SH2_ERR_BAD_PARAM;
}
uint16_t recordId = sensorToRecordMap[i].recordId;
// clear opData
memset(&pSh2->opData, 0, sizeof(sh2_OpData_t));
// Set up an FRS read operation
pSh2->opData.getFrs.frsType = recordId;
pSh2->opData.getFrs.pData = pSh2->frsData;
pSh2->frsDataLen = ARRAY_LEN(pSh2->frsData);
pSh2->opData.getFrs.pWords = &pSh2->frsDataLen;
// Read an FRS record
int status = opProcess(pSh2, &getFrsOp);
// Copy the results into pData
if (status == SH2_OK) {
stuffMetadata(pData, pSh2->frsData);
}
return status;
}
/**
* @brief Get an FRS record.
*
* @param recordId Which FRS Record to retrieve.
* @param pData pointer to buffer to receive the results
* @param[in] words Size of pData buffer, in 32-bit words.
* @param[out] words Number of 32-bit words retrieved.
* @return SH2_OK (0), on success. Negative value from sh2_err.h on error.
*/
int sh2_getFrs(uint16_t recordId, uint32_t *pData, uint16_t *words)
{
sh2_t *pSh2 = &_sh2;
if ((pData == 0) || (words == 0)) {
return SH2_ERR_BAD_PARAM;
}
// clear opData
memset(&pSh2->opData, 0, sizeof(sh2_OpData_t));
// Store params for this op
pSh2->opData.getFrs.frsType = recordId;
pSh2->opData.getFrs.pData = pData;
pSh2->opData.getFrs.pWords = words;
return opProcess(pSh2, &getFrsOp);
}
/**
* @brief Set an FRS record
*
* @param recordId Which FRS Record to set.
* @param pData pointer to buffer containing the new data.
* @param words number of 32-bit words to write. (0 to delete record.)
* @return SH2_OK (0), on success. Negative value from sh2_err.h on error.
*/
int sh2_setFrs(uint16_t recordId, uint32_t *pData, uint16_t words)
{
sh2_t *pSh2 = &_sh2;
if ((pData == 0) && (words != 0)) {
return SH2_ERR_BAD_PARAM;
}
// clear opData
memset(&pSh2->opData, 0, sizeof(sh2_OpData_t));
pSh2->opData.setFrs.frsType = recordId;
pSh2->opData.setFrs.pData = pData;
pSh2->opData.setFrs.words = words;
return opProcess(pSh2, &setFrsOp);
}
/**
* @brief Get error counts.
*
* @param severity Only errors of this severity or greater are returned.
* @param pErrors Buffer to receive error codes.
* @param numErrors size of pErrors array
* @return SH2_OK (0), on success. Negative value from sh2_err.h on error.
*/
int sh2_getErrors(uint8_t severity, sh2_ErrorRecord_t *pErrors, uint16_t *numErrors)
{
sh2_t *pSh2 = &_sh2;
// clear opData
memset(&pSh2->opData, 0, sizeof(sh2_OpData_t));
pSh2->opData.getErrors.severity = severity;
pSh2->opData.getErrors.pErrors = pErrors;
pSh2->opData.getErrors.pNumErrors = numErrors;
return opProcess(pSh2, &getErrorsOp);
}
/**
* @brief Read counters related to a sensor.
*
* @param sensorId Which sensor to operate on.
* @param pCounts Pointer to Counts structure that will receive data.
* @return SH2_OK (0), on success. Negative value from sh2_err.h on error.
*/
int sh2_getCounts(sh2_SensorId_t sensorId, sh2_Counts_t *pCounts)
{
sh2_t *pSh2 = &_sh2;
// clear opData
memset(&pSh2->opData, 0, sizeof(sh2_OpData_t));
pSh2->opData.getCounts.sensorId = sensorId;
pSh2->opData.getCounts.pCounts = pCounts;
return opProcess(pSh2, &getCountsOp);
}
/**
* @brief Clear counters related to a sensor.
*
* @param sensorId which sensor to operate on.
* @return SH2_OK (0), on success. Negative value from sh2_err.h on error.
*/
int sh2_clearCounts(sh2_SensorId_t sensorId)
{
sh2_t *pSh2 = &_sh2;
// clear opData
memset(&pSh2->opData, 0, sizeof(sh2_OpData_t));
pSh2->opData.sendCmd.req.command = SH2_CMD_COUNTS;
pSh2->opData.sendCmd.req.p[0] = SH2_COUNTS_CLEAR_COUNTS;
pSh2->opData.sendCmd.req.p[1] = sensorId;
return opProcess(pSh2, &sendCmdOp);
}
/**
* @brief Perform a tare operation on one or more axes.
*
* @param axes Bit mask specifying which axes should be tared.
* @param basis Which rotation vector to use as the basis for Tare adjustment.
* @return SH2_OK (0), on success. Negative value from sh2_err.h on error.
*/
int sh2_setTareNow(uint8_t axes, // SH2_TARE_X | SH2_TARE_Y | SH2_TARE_Z
sh2_TareBasis_t basis)
{
sh2_t *pSh2 = &_sh2;
// clear opData
memset(&pSh2->opData, 0, sizeof(sh2_OpData_t));
pSh2->opData.sendCmd.req.command = SH2_CMD_TARE;
pSh2->opData.sendCmd.req.p[0] = SH2_TARE_TARE_NOW;
pSh2->opData.sendCmd.req.p[1] = axes;
pSh2->opData.sendCmd.req.p[2] = basis;
return opProcess(pSh2, &sendCmdOp);
}
/**
* @brief Clears the previously applied tare operation.
*
* @return SH2_OK \n");
*/
int sh2_clearTare(void)
{
sh2_t *pSh2 = &_sh2;
// clear opData
memset(&pSh2->opData, 0, sizeof(sh2_OpData_t));
pSh2->opData.sendCmd.req.command = SH2_CMD_TARE;
pSh2->opData.sendCmd.req.p[0] = SH2_TARE_SET_REORIENTATION;
return opProcess(pSh2, &sendCmdOp);
}
/**
* @brief Persist the results of last tare operation to flash.
*
* @return SH2_OK (0), on success. Negative value from sh2_err.h on error.
*/
int sh2_persistTare(void)
{
sh2_t *pSh2 = &_sh2;
// clear opData
memset(&pSh2->opData, 0, sizeof(sh2_OpData_t));
pSh2->opData.sendCmd.req.command = SH2_CMD_TARE;
pSh2->opData.sendCmd.req.p[0] = SH2_TARE_PERSIST_TARE;
return opProcess(pSh2, &sendCmdOp);
}
/**
* @brief Set the current run-time sensor reorientation. (Set to zero to clear tare.)
*
* @param orientation Quaternion rotation vector to apply as new tare.
* @return SH2_OK (0), on success. Negative value from sh2_err.h on error.
*/
int sh2_setReorientation(sh2_Quaternion_t *orientation)
{
sh2_t *pSh2 = &_sh2;
// clear opData
memset(&pSh2->opData, 0, sizeof(sh2_OpData_t));
pSh2->opData.sendCmd.req.command = SH2_CMD_TARE;
pSh2->opData.sendCmd.req.p[0] = SH2_TARE_SET_REORIENTATION;
// save me a lot of typing and you a lot of reading
uint8_t *p = pSh2->opData.sendCmd.req.p;
// Put new orientation in command parameters
writeu16(&p[1], toQ14(orientation->x));
writeu16(&p[3], toQ14(orientation->y));
writeu16(&p[5], toQ14(orientation->z));
writeu16(&p[7], toQ14(orientation->w));
return opProcess(pSh2, &sendCmdOp);
}
/**
* @brief Command the sensorhub to reset.
*
* @return SH2_OK (0), on success. Negative value from sh2_err.h on error.
*/
int sh2_reinitialize(void)
{
sh2_t *pSh2 = &_sh2;
return opProcess(pSh2, &reinitOp);
}
/**
* @brief Save Dynamic Calibration Data to flash.
*
* @return SH2_OK (0), on success. Negative value from sh2_err.h on error.
*/
int sh2_saveDcdNow(void)
{
sh2_t *pSh2 = &_sh2;
return opProcess(pSh2, &saveDcdNowOp);
}
/**
* @brief Get Oscillator type.
*
* @param pOscType pointer to data structure to receive results.
* @return SH2_OK (0), on success. Negative value from sh2_err.h on error.
*/
int sh2_getOscType(sh2_OscType_t *pOscType)
{
sh2_t *pSh2 = &_sh2;
pSh2->opData.getOscType.pOscType = pOscType;
return opProcess(pSh2, &getOscTypeOp);
}
/**
* @brief Enable/Disable dynamic calibration for certain sensors
*
* @param sensors Bit mask to configure which sensors are affected.
* @return SH2_OK (0), on success. Negative value from sh2_err.h on error.
*/
int sh2_setCalConfig(uint8_t sensors)
{
sh2_t *pSh2 = &_sh2;
pSh2->opData.calConfig.sensors = sensors;
return opProcess(pSh2, &setCalConfigOp);
}
/**
* @brief Get dynamic calibration configuration settings.
*
* @param pSensors pointer to Bit mask, set on return.
* @return SH2_OK (0), on success. Negative value from sh2_err.h on error.
*/
int sh2_getCalConfig(uint8_t *pSensors)
{
sh2_t *pSh2 = &_sh2;
pSh2->opData.getCalConfig.pSensors = pSensors;
return opProcess(pSh2, &getCalConfigOp);
}
/**
* @brief Configure automatic saving of dynamic calibration data.
*
* @param enabled Enable or Disable DCD auto-save.
* @return SH2_OK (0), on success. Negative value from sh2_err.h on error.
*/
int sh2_setDcdAutoSave(bool enabled)
{
sh2_t *pSh2 = &_sh2;
// clear opData
memset(&pSh2->opData, 0, sizeof(sh2_OpData_t));
pSh2->opData.sendCmd.req.command = SH2_CMD_DCD_SAVE;
pSh2->opData.sendCmd.req.p[0] = enabled ? 0 : 1;
return opProcess(pSh2, &sendCmdOp);
}
/**
* @brief Immediately issue all buffered sensor reports from a given sensor.
*
* @param sensorId Which sensor reports to flush.
* @return SH2_OK (0), on success. Negative value from sh2_err.h on error.
*/
int sh2_flush(sh2_SensorId_t sensorId)
{
sh2_t *pSh2 = &_sh2;
// clear opData
memset(&pSh2->opData, 0, sizeof(sh2_OpData_t));
pSh2->opData.forceFlush.sensorId = sensorId;
return opProcess(pSh2, &forceFlushOp);
}
/**
* @brief Command clear DCD in RAM, then reset sensor hub.
*
* @return SH2_OK (0), on success. Negative value from sh2_err.h on error.
*/
int sh2_clearDcdAndReset(void)
{
sh2_t *pSh2 = &_sh2;
// clear opData
memset(&pSh2->opData, 0, sizeof(sh2_OpData_t));
pSh2->opData.sendCmd.req.command = SH2_CMD_CLEAR_DCD_AND_RESET;
return opProcess(pSh2, &sendCmdOp);
}
/**
* @brief Start simple self-calibration procedure.
*
* @parameter interval_us sensor report interval, uS.
* @return SH2_OK (0), on success. Negative value from sh2_err.h on error.
*/
int sh2_startCal(uint32_t interval_us)
{
sh2_t *pSh2 = &_sh2;
// clear opData
memset(&pSh2->opData, 0, sizeof(sh2_OpData_t));
pSh2->opData.startCal.interval_us = interval_us;
return opProcess(pSh2, &startCalOp);
}
/**
* @brief Finish simple self-calibration procedure.
*
* @parameter status contains calibration status code on return.
* @return SH2_OK (0), on success. Negative value from sh2_err.h on error.
*/
int sh2_finishCal(sh2_CalStatus_t *status)
{
sh2_t *pSh2 = &_sh2;
// clear opData
memset(&pSh2->opData, 0, sizeof(sh2_OpData_t));
return opProcess(pSh2, &finishCalOp);
}
/**
* @brief send Interactive ZRO Request.
*
* @parameter intent Inform the sensor hub what sort of motion should be in progress.
* @return SH2_OK (0), on success. Negative value from sh2_err.h on error.
*/
int sh2_setIZro(sh2_IZroMotionIntent_t intent)
{
sh2_t *pSh2 = &_sh2;
// clear opData
memset(&pSh2->opData, 0, sizeof(sh2_OpData_t));
// set up opData for iZRO request
pSh2->opData.sendCmd.req.command = SH2_CMD_INTERACTIVE_ZRO;
pSh2->opData.sendCmd.req.p[0] = intent;
// Send command
return opProcess(pSh2, &sendCmdOp);
}