From c19bb23bbf5c0cf008785ab3924e85e6a0239e99 Mon Sep 17 00:00:00 2001 From: YOUR_NAME Date: Mon, 6 Dec 2021 15:47:40 +0800 Subject: [PATCH] feat: add gravity drive v10 Signed-off-by: YOUR_NAME --- .../sensor/driver/accel/sensor_accel_driver.c | 28 +- .../sensor/driver/accel/sensor_accel_driver.h | 7 +- .../driver/accel/sensor_gravity_driver.c | 335 ++++++++++++++++++ .../driver/accel/sensor_gravity_driver.h | 44 +++ .../driver/chipset/accel/accel_bmi160.c | 31 +- .../driver/chipset/accel/accel_bmi160.h | 3 +- 6 files changed, 426 insertions(+), 22 deletions(-) mode change 100644 => 100755 model/sensor/driver/accel/sensor_accel_driver.c mode change 100644 => 100755 model/sensor/driver/accel/sensor_accel_driver.h create mode 100755 model/sensor/driver/accel/sensor_gravity_driver.c create mode 100755 model/sensor/driver/accel/sensor_gravity_driver.h mode change 100644 => 100755 model/sensor/driver/chipset/accel/accel_bmi160.c mode change 100644 => 100755 model/sensor/driver/chipset/accel/accel_bmi160.h diff --git a/model/sensor/driver/accel/sensor_accel_driver.c b/model/sensor/driver/accel/sensor_accel_driver.c old mode 100644 new mode 100755 index f967792d..e6365048 --- a/model/sensor/driver/accel/sensor_accel_driver.c +++ b/model/sensor/driver/accel/sensor_accel_driver.c @@ -29,6 +29,19 @@ static struct AccelDrvData *AccelGetDrvData(void) static struct SensorRegCfgGroupNode *g_regCfgGroup[SENSOR_GROUP_MAX] = { NULL }; +int32_t SubscribeAccelDataCallbackFunc(GravitySubscribeAccelCallback cb) +{ + CHECK_NULL_PTR_RETURN_VALUE(cb, HDF_ERR_INVALID_PARAM); + + struct AccelDrvData *drvData = AccelGetDrvData(); + + CHECK_NULL_PTR_RETURN_VALUE(drvData, HDF_ERR_INVALID_PARAM); + + drvData->cb = cb; + + return HDF_SUCCESS; +} + int32_t AccelRegisterChipOps(const struct AccelOpsCall *ops) { struct AccelDrvData *drvData = AccelGetDrvData(); @@ -44,6 +57,7 @@ int32_t AccelRegisterChipOps(const struct AccelOpsCall *ops) static void AccelDataWorkEntry(void *arg) { struct AccelDrvData *drvData = NULL; + struct SensorReportEvent event; drvData = (struct AccelDrvData *)arg; CHECK_NULL_PTR_RETURN(drvData); @@ -52,8 +66,19 @@ static void AccelDataWorkEntry(void *arg) HDF_LOGI("%s: Accel ReadData function NULl", __func__); return; } - if (drvData->ops.ReadData(drvData->accelCfg) != HDF_SUCCESS) { + + if (drvData->ops.ReadData(drvData->accelCfg, &event) != HDF_SUCCESS) { HDF_LOGE("%s: Accel read data failed", __func__); + return; + } + + if (ReportSensorEvent(&event) != HDF_SUCCESS) { + HDF_LOGE("%s: report accel data failed", __func__); + return; + } + + if (drvData->cb != NULL) { + drvData->cb((int32_t*)event.data, event.dataLen); } } @@ -342,6 +367,7 @@ int32_t AccelInitDriver(struct HdfDeviceObject *device) } drvData->accelCfg->regCfgGroup = &g_regCfgGroup[0]; + drvData->cb = NULL; HDF_LOGI("%s: Init accel driver success", __func__); return HDF_SUCCESS; diff --git a/model/sensor/driver/accel/sensor_accel_driver.h b/model/sensor/driver/accel/sensor_accel_driver.h old mode 100644 new mode 100755 index 4162ae0d..5a7d0044 --- a/model/sensor/driver/accel/sensor_accel_driver.h +++ b/model/sensor/driver/accel/sensor_accel_driver.h @@ -32,6 +32,8 @@ enum AccelAxisPart { ACCEL_AXIS_BUTT, }; +typedef int32_t (*GravitySubscribeAccelCallback)(int32_t *rawData, int32_t size); + struct AccelData { int32_t x; int32_t y; @@ -40,7 +42,7 @@ struct AccelData { struct AccelOpsCall { int32_t (*Init)(struct SensorCfgData *data); - int32_t (*ReadData)(struct SensorCfgData *data); + int32_t (*ReadData)(struct SensorCfgData *data, struct SensorReportEvent *event); }; struct AccelDrvData { @@ -54,10 +56,11 @@ struct AccelDrvData { int64_t interval; struct SensorCfgData *accelCfg; struct AccelOpsCall ops; + GravitySubscribeAccelCallback cb; }; int32_t AccelRegisterChipOps(const struct AccelOpsCall *ops); struct SensorCfgData *AccelCreateCfgData(const struct DeviceResourceNode *node); void AccelReleaseCfgData(struct SensorCfgData *sensorCfgData); - +int32_t SubscribeAccelDataCallbackFunc(GravitySubscribeAccelCallback cb); #endif /* SENSOR_ACCEL_DRIVER_H */ diff --git a/model/sensor/driver/accel/sensor_gravity_driver.c b/model/sensor/driver/accel/sensor_gravity_driver.c new file mode 100755 index 00000000..8666ec46 --- /dev/null +++ b/model/sensor/driver/accel/sensor_gravity_driver.c @@ -0,0 +1,335 @@ +/* + * Copyright (c) 2021 Huawei Device Co., Ltd. + * + * HDF is dual licensed: you can use it either under the terms of + * the GPL, or the BSD license, at your option. + * See the LICENSE file in the root of this repository for complete details. + */ + +#include "sensor_gravity_driver.h" +#include "sensor_accel_driver.h" +#include +#include "hdf_base.h" +#include "hdf_device_desc.h" +#include "osal_math.h" +#include "osal_mem.h" +#include "osal_time.h" +#include "sensor_device_manager.h" +#include "sensor_platform_if.h" + +#define HDF_LOG_TAG sensor_gravity_driver_c + +#define HDF_GRAVITY_WORK_QUEUE_NAME "hdf_gravity_work_queue" + +static struct GravityDrvData *g_gravityDrvData = NULL; +static int32_t g_accelRawData[GRAVITY_AXIS_NUM]; + +static struct GravityDrvData *GravityGetDrvData(void) +{ + return g_gravityDrvData; +} + +static int32_t GravitySubscribeAccelData(int32_t *accelData, int32_t size) +{ + CHECK_NULL_PTR_RETURN_VALUE(accelData, HDF_ERR_INVALID_PARAM); + + if (size > GRAVITY_AXIS_NUM * sizeof(int32_t)) { + HDF_LOGE("%s: size exceed max value", __func__); + return HDF_FAILURE; + } + + g_accelRawData[GRAVITY_X_AXIS] = accelData[GRAVITY_X_AXIS]; + g_accelRawData[GRAVITY_Y_AXIS] = accelData[GRAVITY_Y_AXIS]; + g_accelRawData[GRAVITY_Z_AXIS] = accelData[GRAVITY_Z_AXIS]; + + return HDF_SUCCESS; +} + +static void GravityDataWorkEntry(void *arg) +{ + struct GravityDrvData *drvData = NULL; + static int32_t GravityRawData[GRAVITY_AXIS_NUM]; + struct SensorReportEvent event; + drvData = (struct GravityDrvData *)arg; + CHECK_NULL_PTR_RETURN(drvData); + OsalTimespec time; + + GravityRawData[GRAVITY_X_AXIS] = (85 * (GravityRawData[GRAVITY_X_AXIS] - g_accelRawData[GRAVITY_X_AXIS]) + + g_accelRawData[GRAVITY_X_AXIS] * 100) / 100; + GravityRawData[GRAVITY_Y_AXIS] = (85 * (GravityRawData[GRAVITY_Y_AXIS] - g_accelRawData[GRAVITY_Y_AXIS]) + + g_accelRawData[GRAVITY_Y_AXIS] * 100) / 100; + GravityRawData[GRAVITY_Z_AXIS] = (85 * (GravityRawData[GRAVITY_Z_AXIS] - g_accelRawData[GRAVITY_Z_AXIS]) + + g_accelRawData[GRAVITY_Z_AXIS] * 100) / 100; + + if (OsalGetTime(&time) != HDF_SUCCESS) { + HDF_LOGE("%s: Get time failed", __func__); + return; + } + event.timestamp = time.sec * SENSOR_SECOND_CONVERT_NANOSECOND + time.usec * SENSOR_CONVERT_UNIT; /* unit nanosecond */ + event.sensorId = SENSOR_TAG_GRAVITY; + event.option = 0; + event.mode = SENSOR_WORK_MODE_REALTIME; + event.dataLen = sizeof(GravityRawData); + event.data = (uint8_t *)&GravityRawData; + + if (ReportSensorEvent(&event) != HDF_SUCCESS) { + HDF_LOGE("%s: report gravity data failed", __func__); + } +} + +static void GravityTimerEntry(uintptr_t arg) +{ + int32_t ret; + struct GravityDrvData *drvData = (struct GravityDrvData *)arg; + CHECK_NULL_PTR_RETURN(drvData); + + if (!HdfAddWork(&drvData->gravityWorkQueue, &drvData->gravityWork)) { + HDF_LOGE("%s: Gravity add work queue failed", __func__); + } + + ret = OsalTimerSetTimeout(&drvData->gravityTimer, drvData->interval); + if (ret != HDF_SUCCESS) { + HDF_LOGE("%s: Gravity modify time failed", __func__); + } +} + +static int32_t InitGravityData(struct GravityDrvData *drvData) +{ + if (HdfWorkQueueInit(&drvData->gravityWorkQueue, HDF_GRAVITY_WORK_QUEUE_NAME) != HDF_SUCCESS) { + HDF_LOGE("%s: Gravity init work queue failed", __func__); + return HDF_FAILURE; + } + + if (HdfWorkInit(&drvData->gravityWork, GravityDataWorkEntry, drvData) != HDF_SUCCESS) { + HDF_LOGE("%s: Gravity create thread failed", __func__); + return HDF_FAILURE; + } + + drvData->interval = GRAVITY_TIMER_MAX_TIME; + drvData->enable = false; + + return HDF_SUCCESS; +} + +static int32_t SetGravityEnable(void) +{ + int32_t ret; + struct GravityDrvData *drvData = GravityGetDrvData(); + + CHECK_NULL_PTR_RETURN_VALUE(drvData, HDF_ERR_INVALID_PARAM); + CHECK_NULL_PTR_RETURN_VALUE(drvData->gravityCfg, HDF_ERR_INVALID_PARAM); + + if (drvData->enable) { + HDF_LOGE("%s: Gravity sensor is enabled", __func__); + return HDF_SUCCESS; + } + + ret = OsalTimerCreate(&drvData->gravityTimer, GRAVITY_TIMER_MAX_TIME, GravityTimerEntry, (uintptr_t)drvData); + if (ret != HDF_SUCCESS) { + HDF_LOGE("%s: Gravity create timer failed[%d]", __func__, ret); + return ret; + } + + ret = OsalTimerStartLoop(&drvData->gravityTimer); + if (ret != HDF_SUCCESS) { + HDF_LOGE("%s: Gravity start timer failed[%d]", __func__, ret); + return ret; + } + drvData->enable = true; + + return HDF_SUCCESS; +} + +static int32_t SetGravityDisable(void) +{ + int32_t ret; + struct GravityDrvData *drvData = GravityGetDrvData(); + + CHECK_NULL_PTR_RETURN_VALUE(drvData, HDF_ERR_INVALID_PARAM); + CHECK_NULL_PTR_RETURN_VALUE(drvData->gravityCfg, HDF_ERR_INVALID_PARAM); + + if (!drvData->enable) { + HDF_LOGE("%s: Gravity sensor had disable", __func__); + return HDF_SUCCESS; + } + + ret = OsalTimerDelete(&drvData->gravityTimer); + if (ret != HDF_SUCCESS) { + HDF_LOGE("%s: Gravity delete timer failed", __func__); + return ret; + } + drvData->enable = false; + + return HDF_SUCCESS; +} + +static int32_t SetGravityBatch(int64_t samplingInterval, int64_t interval) +{ + (void)interval; + + int64_t ms; + struct GravityDrvData *drvData = NULL; + + drvData = GravityGetDrvData(); + CHECK_NULL_PTR_RETURN_VALUE(drvData, HDF_ERR_INVALID_PARAM); + + ms = OsalDivS64(samplingInterval, (SENSOR_CONVERT_UNIT * SENSOR_CONVERT_UNIT)); + + drvData->interval = (ms <= GRAVITY_TIMER_MIN_TIME) ? GRAVITY_TIMER_MIN_TIME : GRAVITY_TIMER_MAX_TIME; + + return HDF_SUCCESS; +} + +static int32_t SetGravityMode(int32_t mode) +{ + return (mode == SENSOR_WORK_MODE_REALTIME) ? HDF_SUCCESS : HDF_FAILURE; +} + +static int32_t SetGravityOption(uint32_t option) +{ + (void)option; + return HDF_SUCCESS; +} + +static int32_t DispatchGravity(struct HdfDeviceIoClient *client, + int cmd, struct HdfSBuf *data, struct HdfSBuf *reply) +{ + (void)client; + (void)cmd; + (void)data; + (void)reply; + + return HDF_SUCCESS; +} + +int32_t GravityBindDriver(struct HdfDeviceObject *device) +{ + CHECK_NULL_PTR_RETURN_VALUE(device, HDF_ERR_INVALID_PARAM); + + struct GravityDrvData *drvData = (struct GravityDrvData *)OsalMemCalloc(sizeof(*drvData)); + if (drvData == NULL) { + HDF_LOGE("%s: Malloc gravity drv data fail!", __func__); + return HDF_ERR_MALLOC_FAIL; + } + + drvData->ioService.Dispatch = DispatchGravity; + drvData->device = device; + device->service = &drvData->ioService; + g_gravityDrvData = drvData; + return HDF_SUCCESS; +} + +static int32_t InitGravityOps(struct SensorCfgData *config, struct SensorDeviceInfo *deviceInfo) +{ + CHECK_NULL_PTR_RETURN_VALUE(config, HDF_ERR_INVALID_PARAM); + + deviceInfo->ops.Enable = SetGravityEnable; + deviceInfo->ops.Disable = SetGravityDisable; + deviceInfo->ops.SetBatch = SetGravityBatch; + deviceInfo->ops.SetMode = SetGravityMode; + deviceInfo->ops.SetOption = SetGravityOption; + + if (memcpy_s(&deviceInfo->sensorInfo, sizeof(deviceInfo->sensorInfo), + &config->sensorInfo, sizeof(config->sensorInfo)) != EOK) { + HDF_LOGE("%s: Copy sensor info failed", __func__); + return HDF_FAILURE; + } + + if (SubscribeAccelDataCallbackFunc(GravitySubscribeAccelData) != HDF_SUCCESS) { + HDF_LOGE("%s: Gravity register accel failed", __func__); + return HDF_FAILURE; + } + + return HDF_SUCCESS; +} + +static int32_t InitGravityAfterBase(struct SensorCfgData *config) +{ + struct SensorDeviceInfo deviceInfo; + CHECK_NULL_PTR_RETURN_VALUE(config, HDF_ERR_INVALID_PARAM); + + if (InitGravityOps(config, &deviceInfo) != HDF_SUCCESS) { + HDF_LOGE("%s: Init gravity ops failed", __func__); + return HDF_FAILURE; + } + + if (AddSensorDevice(&deviceInfo) != HDF_SUCCESS) { + HDF_LOGE("%s: Add gravity device failed", __func__); + return HDF_FAILURE; + } + return HDF_SUCCESS; +} + +static int32_t GravityCreateCfgData(const struct DeviceResourceNode *node) +{ + struct GravityDrvData *drvData = GravityGetDrvData(); + + CHECK_NULL_PTR_RETURN_VALUE(node, HDF_ERR_INVALID_PARAM); + CHECK_NULL_PTR_RETURN_VALUE(drvData, HDF_ERR_INVALID_PARAM); + CHECK_NULL_PTR_RETURN_VALUE(drvData->gravityCfg, HDF_ERR_INVALID_PARAM); + + if (GetSensorBaseConfigData(node, drvData->gravityCfg) != HDF_SUCCESS) { + HDF_LOGE("%s: Get sensor base config failed", __func__); + return HDF_FAILURE; + } + + if (InitGravityAfterBase(drvData->gravityCfg) != HDF_SUCCESS) { + HDF_LOGE("%s: Init gravity sensor device failed", __func__); + return HDF_FAILURE; + } + return HDF_SUCCESS; +} + +int32_t GravityInitDriver(struct HdfDeviceObject *device) +{ + CHECK_NULL_PTR_RETURN_VALUE(device, HDF_ERR_INVALID_PARAM); + struct GravityDrvData *drvData = (struct GravityDrvData *)device->service; + CHECK_NULL_PTR_RETURN_VALUE(drvData, HDF_ERR_INVALID_PARAM); + + if (InitGravityData(drvData) != HDF_SUCCESS) { + HDF_LOGE("%s: Init gravity config failed", __func__); + return HDF_FAILURE; + } + + drvData->gravityCfg = (struct SensorCfgData *)OsalMemCalloc(sizeof(*drvData->gravityCfg)); + if (drvData->gravityCfg == NULL) { + HDF_LOGE("%s: Malloc gravity config data failed", __func__); + return HDF_FAILURE; + } + + if (GravityCreateCfgData(device->property) != HDF_SUCCESS) { + HDF_LOGE("%s: create gravity device failed", __func__); + OsalMemFree(drvData->gravityCfg); + return HDF_FAILURE; + } + HDF_LOGI("%s: Init gravity driver success", __func__); + return HDF_SUCCESS; +} + +void GravityReleaseDriver(struct HdfDeviceObject *device) +{ + CHECK_NULL_PTR_RETURN(device); + + struct GravityDrvData *drvData = (struct GravityDrvData *)device->service; + CHECK_NULL_PTR_RETURN(drvData); + + if (drvData->gravityCfg != NULL) { + (void)DeleteSensorDevice(&drvData->gravityCfg->sensorInfo); + OsalMemFree(drvData->gravityCfg); + drvData->gravityCfg = NULL; + } + + HdfWorkDestroy(&drvData->gravityWork); + HdfWorkQueueDestroy(&drvData->gravityWorkQueue); + OsalMemFree(drvData); +} + +struct HdfDriverEntry g_sensorGravityDevEntry = { + .moduleVersion = 1, + .moduleName = "HDF_SENSOR_GRAVITY", + .Bind = GravityBindDriver, + .Init = GravityInitDriver, + .Release = GravityReleaseDriver, +}; + +HDF_INIT(g_sensorGravityDevEntry); \ No newline at end of file diff --git a/model/sensor/driver/accel/sensor_gravity_driver.h b/model/sensor/driver/accel/sensor_gravity_driver.h new file mode 100755 index 00000000..ab54fa23 --- /dev/null +++ b/model/sensor/driver/accel/sensor_gravity_driver.h @@ -0,0 +1,44 @@ +/* + * Copyright (c) 2021 Huawei Device Co., Ltd. + * + * HDF is dual licensed: you can use it either under the terms of + * the GPL, or the BSD license, at your option. + * See the LICENSE file in the root of this repository for complete details. + */ + +#ifndef SENSOR_GRAVITY_DRIVER_H +#define SENSOR_GRAVITY_DRIVER_H + +#include "hdf_workqueue.h" +#include "osal_mutex.h" +#include "osal_timer.h" +#include "sensor_config_parser.h" +#include "sensor_platform_if.h" + +#define GRAVITY_TIMER_MIN_TIME 10 +#define GRAVITY_TIMER_MAX_TIME 20 +enum GravityAxisNum { + GRAVITY_X_AXIS = 0, + GRAVITY_Y_AXIS = 1, + GRAVITY_Z_AXIS = 2, + GRAVITY_AXIS_NUM = 3, +}; + +struct GravityData { + int32_t x; + int32_t y; + int32_t z; +}; + +struct GravityDrvData { + struct IDeviceIoService ioService; + struct HdfDeviceObject *device; + HdfWorkQueue gravityWorkQueue; + HdfWork gravityWork; + OsalTimer gravityTimer; + struct SensorCfgData *gravityCfg; + int64_t interval; + bool enable; +}; + +#endif /* SENSOR_GRAVITY_DRIVER_H */ \ No newline at end of file diff --git a/model/sensor/driver/chipset/accel/accel_bmi160.c b/model/sensor/driver/chipset/accel/accel_bmi160.c old mode 100644 new mode 100755 index 8472938f..6fb7ca2c --- a/model/sensor/driver/chipset/accel/accel_bmi160.c +++ b/model/sensor/driver/chipset/accel/accel_bmi160.c @@ -79,24 +79,24 @@ static int32_t ReadBmi160RawData(struct SensorCfgData *data, struct AccelData *r return HDF_SUCCESS; } -int32_t ReadBmi160Data(struct SensorCfgData *data) +int32_t ReadBmi160Data(struct SensorCfgData *cfg, struct SensorReportEvent *event) { int32_t ret; struct AccelData rawData = { 0, 0, 0 }; - int32_t tmp[ACCEL_AXIS_NUM]; - struct SensorReportEvent event; + static int32_t tmp[ACCEL_AXIS_NUM]; - (void)memset_s(&event, sizeof(event), 0, sizeof(event)); + CHECK_NULL_PTR_RETURN_VALUE(cfg, HDF_ERR_INVALID_PARAM); + CHECK_NULL_PTR_RETURN_VALUE(event, HDF_ERR_INVALID_PARAM); - ret = ReadBmi160RawData(data, &rawData, &event.timestamp); + ret = ReadBmi160RawData(cfg, &rawData, &event->timestamp); if (ret != HDF_SUCCESS) { HDF_LOGE("%s: BMI160 read raw data failed", __func__); return HDF_FAILURE; } - event.sensorId = SENSOR_TAG_ACCELEROMETER; - event.option = 0; - event.mode = SENSOR_WORK_MODE_REALTIME; + event->sensorId = SENSOR_TAG_ACCELEROMETER; + event->option = 0; + event->mode = SENSOR_WORK_MODE_REALTIME; rawData.x = rawData.x * BMI160_ACC_SENSITIVITY_2G; rawData.y = rawData.y * BMI160_ACC_SENSITIVITY_2G; @@ -106,24 +106,21 @@ int32_t ReadBmi160Data(struct SensorCfgData *data) tmp[ACCEL_Y_AXIS] = (rawData.y * SENSOR_1K_UNIT) / SENSOR_CONVERT_UNIT; tmp[ACCEL_Z_AXIS] = (rawData.z * SENSOR_1K_UNIT) / SENSOR_CONVERT_UNIT; - ret = SensorRawDataToRemapData(data->direction, tmp, sizeof(tmp) / sizeof(tmp[0])); + ret = SensorRawDataToRemapData(cfg->direction, tmp, sizeof(tmp) / sizeof(tmp[0])); if (ret != HDF_SUCCESS) { HDF_LOGE("%s: BMI160 convert raw data failed", __func__); return HDF_FAILURE; } - - event.dataLen = sizeof(tmp); - event.data = (uint8_t *)&tmp; - ret = ReportSensorEvent(&event); - if (ret != HDF_SUCCESS) { - HDF_LOGE("%s: BMI160 report data failed", __func__); - } + + event->dataLen = sizeof(tmp); + event->data = (uint8_t *)&tmp; + return ret; } static int32_t InitBmi160(struct SensorCfgData *data) { - int32_t ret; + int32_t ret; CHECK_NULL_PTR_RETURN_VALUE(data, HDF_ERR_INVALID_PARAM); ret = SetSensorRegCfgArray(&data->busCfg, data->regCfgGroup[SENSOR_INIT_GROUP]); diff --git a/model/sensor/driver/chipset/accel/accel_bmi160.h b/model/sensor/driver/chipset/accel/accel_bmi160.h old mode 100644 new mode 100755 index 1f1fb4de..49ad6b05 --- a/model/sensor/driver/chipset/accel/accel_bmi160.h +++ b/model/sensor/driver/chipset/accel/accel_bmi160.h @@ -59,8 +59,7 @@ #define BMI160_ACCEL_DATA_READY_MASK 0x80 int32_t DetectAccelBim160Chip(struct SensorCfgData *data); -int32_t ReadBmi160Data(struct SensorCfgData *data); - +int32_t ReadBmi160Data(struct SensorCfgData *cfg, struct SensorReportEvent *event); struct Bmi160DrvData { struct IDeviceIoService ioService; struct HdfDeviceObject *device;