Repository: incubator-mynewt-core Updated Branches: refs/heads/sensors_branch 1b0f37576 -> 5a76b68cc
Initial Commit BNO055 Driver - Adding BNO055 Driver - Also adding a shell for it - Cleaning up tsl driver Project: http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/repo Commit: http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/commit/5a76b68c Tree: http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/tree/5a76b68c Diff: http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/diff/5a76b68c Branch: refs/heads/sensors_branch Commit: 5a76b68ccc34fe3f342811d512f2d35e7ddb9d0b Parents: 1b0f375 Author: Vipul Rahane <vipulrah...@apache.org> Authored: Thu Feb 9 17:55:24 2017 -0800 Committer: Vipul Rahane <vipulrah...@apache.org> Committed: Thu Feb 9 17:55:24 2017 -0800 ---------------------------------------------------------------------- apps/slinky/pkg.yml | 1 + apps/slinky/src/main.c | 37 +- apps/slinky/syscfg.yml | 5 +- .../sensors/bno055/include/bno055/bno055.h | 128 +++ hw/drivers/sensors/bno055/pkg.yml | 32 + hw/drivers/sensors/bno055/src/bno055.c | 828 +++++++++++++++++++ hw/drivers/sensors/bno055/src/bno055_priv.h | 235 ++++++ hw/drivers/sensors/bno055/src/bno055_shell.c | 424 ++++++++++ hw/drivers/sensors/bno055/syscfg.yml | 35 + hw/drivers/sensors/tsl2561/src/tsl2561.c | 8 +- hw/sensor/include/sensor/euler.h | 50 ++ hw/sensor/include/sensor/quat.h | 50 ++ hw/sensor/include/sensor/sensor.h | 18 +- 13 files changed, 1836 insertions(+), 15 deletions(-) ---------------------------------------------------------------------- http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/blob/5a76b68c/apps/slinky/pkg.yml ---------------------------------------------------------------------- diff --git a/apps/slinky/pkg.yml b/apps/slinky/pkg.yml index 2f84f69..db9cc10 100644 --- a/apps/slinky/pkg.yml +++ b/apps/slinky/pkg.yml @@ -34,6 +34,7 @@ pkg.deps: - hw/drivers/sensors/sim - hw/drivers/sensors/lsm303dlhc - hw/drivers/sensors/tsl2561 + - hw/drivers/sensors/bno055 - boot/bootutil - sys/shell - sys/config http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/blob/5a76b68c/apps/slinky/src/main.c ---------------------------------------------------------------------- diff --git a/apps/slinky/src/main.c b/apps/slinky/src/main.c index 718d690..aa58fcb 100755 --- a/apps/slinky/src/main.c +++ b/apps/slinky/src/main.c @@ -32,6 +32,7 @@ #include <sensor/sensor.h> #include <lsm303dlhc/lsm303dlhc.h> #include <tsl2561/tsl2561.h> +#include <bno055/bno055.h> #include "flash_map/flash_map.h" #include <hal/hal_system.h> #if MYNEWT_VAL(SPLIT_LOADER) @@ -47,9 +48,6 @@ #include <reboot/log_reboot.h> #include <os/os_time.h> #include <id/id.h> -#if MYNEWT_VAL(TSL2561_CLI) -#include <tsl2561/tsl2561.h> -#endif #ifdef ARCH_sim #include <mcu/mcu_sim.h> @@ -252,6 +250,10 @@ config_sensor(void) dev = (struct os_dev *) os_dev_open("light0", OS_TIMEOUT_NEVER, NULL); assert(dev != NULL); rc = tsl2561_init(dev, NULL); + if (rc) { + os_dev_close(dev); + goto err; + } /* Gain set to 1X and Inetgration time set to 13ms */ tslcfg.gain = TSL2561_LIGHT_GAIN_1X; @@ -271,6 +273,10 @@ config_sensor(void) assert(dev != NULL); rc = lsm303dlhc_init(dev, NULL); + if (rc) { + os_dev_close(dev); + goto err; + } /* read once per sec. API should take this value in ms. */ lsmcfg.accel_rate = LSM303DLHC_ACCEL_RATE_1; @@ -283,6 +289,27 @@ config_sensor(void) } #endif +#if MYNEWT_VAL(BNO055_PRESENT) + struct bno055_cfg bcfg; + + dev = (struct os_dev *) os_dev_open("accel0", OS_TIMEOUT_NEVER, NULL); + assert(dev != NULL); + + rc = bno055_init(dev, NULL); + if (rc) { + os_dev_close(dev); + assert(0); + goto err; + } + + rc = bno055_config((struct bno055 *) dev, &bcfg); + if (rc) { + os_dev_close(dev); + assert(0); + goto err; + } +#endif + os_dev_close(dev); return (0); @@ -384,6 +411,10 @@ main(int argc, char **argv) tsl2561_shell_init(); #endif +#if MYNEWT_VAL(BNO055_CLI) + bno055_shell_init(); +#endif + config_sensor(); /* http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/blob/5a76b68c/apps/slinky/syscfg.yml ---------------------------------------------------------------------- diff --git a/apps/slinky/syscfg.yml b/apps/slinky/syscfg.yml index d324731..d184302 100644 --- a/apps/slinky/syscfg.yml +++ b/apps/slinky/syscfg.yml @@ -42,7 +42,7 @@ syscfg.vals: CONFIG_NEWTMGR: 1 OS_MAIN_TASK_PRIO: 10 - OS_MAIN_STACKS_SIZE: 512 + OS_MAIN_STACK_SIZE: 512 syscfg.defs: TSL2561_PRESENT: @@ -51,3 +51,6 @@ syscfg.defs: LSM303DLHC_PRESENT: description: 'LSM303 is present' value: 0 + BNO055_PRESENT: + description: 'BNO055 is present' + value: 0 http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/blob/5a76b68c/hw/drivers/sensors/bno055/include/bno055/bno055.h ---------------------------------------------------------------------- diff --git a/hw/drivers/sensors/bno055/include/bno055/bno055.h b/hw/drivers/sensors/bno055/include/bno055/bno055.h new file mode 100644 index 0000000..a540628 --- /dev/null +++ b/hw/drivers/sensors/bno055/include/bno055/bno055.h @@ -0,0 +1,128 @@ +/* + * Licensed to the Apache Software Foundation (ASF) under one + * or more contributor license agreements. See the NOTICE file + * distributed with this work for additional information + * regarding copyright ownership. The ASF licenses this file + * to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance + * with the License. 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. + */ + +#ifndef __BNO055_H__ +#define __BNO055_H__ + +#include <os/os.h> +#include "os/os_dev.h" +#include "sensor/sensor.h" + +#ifdef __cplusplus +extern "C" { +#endif + +struct bno055_cfg { + uint8_t bc_mode; +}; + +struct bno055 { + struct os_dev dev; + struct sensor sensor; + struct bno055_cfg cfg; + os_time_t last_read_time; +}; + +struct bno055_rev_info { + uint8_t bri_accel_rev; + uint8_t bri_mag_rev; + uint8_t bri_gyro_rev; + uint8_t bri_bl_rev; + uint16_t bri_sw_rev; +}; + +/** + * Initialize the bno055. This function is normally called by sysinit. + * + * @param dev Pointer to the bno055_dev device descriptor + */ +int bno055_init(struct os_dev *dev, void *arg); + +int bno055_config(struct bno055 *, struct bno055_cfg *); + +int +bno055_get_vector_data(void *datastruct, int type); + +/** + * Get quat data from sensor + * + * @param sensor quat data to be filled up + * @return 0 on success, non-zero on error + */ +int +bno055_get_quat_data(void *sqd); + +/** + * Get temperature from bno055 sensor + * + * @return temperature in degree celcius + */ +int +bno055_get_temp(int8_t *temp); + +/** + * Setting mode for the bno055 sensor + * + * @param Operation mode for the sensor + * @return 0 on success, non-zero on failure + */ +int +bno055_set_mode(uint8_t mode); + +#if MYNEWT_VAL(BNO055_CLI) +int bno055_shell_init(void); +#endif + +/** + * Get Revision info for different sensors in the bno055 + * + * @param pass the pointer to the revision structure + * @return 0 on success, non-zero on error + */ +int +bno055_get_rev_info(struct bno055_rev_info *ri); + +/** + * Gets system status, test results and errors if any from the sensor + * + * @param ptr to system status + * @param ptr to self test result + * @param ptr to system error + */ +int +bno055_get_sys_status(uint8_t *system_status, uint8_t *self_test_result, + uint8_t *system_error); + +int +bno055_get_chip_id(uint8_t *id); + +/** + * Read current operational mode of the sensor + * + * @return mode + */ +uint8_t +bno055_get_mode(void); + + +#ifdef __cplusplus +} +#endif + +#endif /* __BNO055_H__ */ http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/blob/5a76b68c/hw/drivers/sensors/bno055/pkg.yml ---------------------------------------------------------------------- diff --git a/hw/drivers/sensors/bno055/pkg.yml b/hw/drivers/sensors/bno055/pkg.yml new file mode 100644 index 0000000..5589276 --- /dev/null +++ b/hw/drivers/sensors/bno055/pkg.yml @@ -0,0 +1,32 @@ +# +# Licensed to the Apache Software Foundation (ASF) under one +# or more contributor license agreements. See the NOTICE file +# distributed with this work for additional information +# regarding copyright ownership. The ASF licenses this file +# to you under the Apache License, Version 2.0 (the +# "License"); you may not use this file except in compliance +# with the License. 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. +# + +pkg.name: hw/drivers/sensors/bno055 +pkg.description: Driver for the BNO055 +pkg.author: "Adafruit <supp...@adafruit.com>" +pkg.homepage: "http://www.adafruit.com/" +pkg.keywords: + - adafruit + - bno055 + - i2c + - sensor + +pkg.deps: + - "@apache-mynewt-core/kernel/os" + - "@apache-mynewt-core/hw/hal" http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/blob/5a76b68c/hw/drivers/sensors/bno055/src/bno055.c ---------------------------------------------------------------------- diff --git a/hw/drivers/sensors/bno055/src/bno055.c b/hw/drivers/sensors/bno055/src/bno055.c new file mode 100644 index 0000000..665db28 --- /dev/null +++ b/hw/drivers/sensors/bno055/src/bno055.c @@ -0,0 +1,828 @@ +/* + * Licensed to the Apache Software Foundation (ASF) under one + * or more contributor license agreements. See the NOTICE file + * distributed with this work for additional information + * resarding copyright ownership. The ASF licenses this file + * to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance + * with the License. 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. + */ + +#include <string.h> +#include <errno.h> +#include <assert.h> + +#include "defs/error.h" +#include "os/os.h" +#include "sysinit/sysinit.h" +#include "hal/hal_i2c.h" +#include "sensor/sensor.h" +#include "sensor/accel.h" +#include "sensor/mag.h" +#include "sensor/quat.h" +#include "sensor/euler.h" +#include "bno055/bno055.h" +#include "bno055_priv.h" + +#if MYNEWT_VAL(BNO055_LOG) +#include "log/log.h" +#endif + +#if MYNEWT_VAL(BNO055_STATS) +#include "stats/stats.h" +#endif + +#if MYNEWT_VAL(BNO055_STATS) +/* Define the stats section and records */ +STATS_SECT_START(bno055_stat_section) + STATS_SECT_ENTRY(samples_acc_2g) + STATS_SECT_ENTRY(samples_acc_4g) + STATS_SECT_ENTRY(samples_acc_8g) + STATS_SECT_ENTRY(samples_acc_16g) + STATS_SECT_ENTRY(samples_mag_1_3g) + STATS_SECT_ENTRY(samples_mag_1_9g) + STATS_SECT_ENTRY(samples_mag_2_5g) + STATS_SECT_ENTRY(samples_mag_4_0g) + STATS_SECT_ENTRY(samples_mag_4_7g) + STATS_SECT_ENTRY(samples_mag_5_6g) + STATS_SECT_ENTRY(samples_mag_8_1g) + STATS_SECT_ENTRY(errors) +STATS_SECT_END + +/* Define stat names for querying */ +STATS_NAME_START(bno055_stat_section) + STATS_NAME(bno055_stat_section, samples_acc_2g) + STATS_NAME(bno055_stat_section, samples_acc_4g) + STATS_NAME(bno055_stat_section, samples_acc_8g) + STATS_NAME(bno055_stat_section, samples_acc_16g) + STATS_NAME(bno055_stat_section, samples_mag_1_3g) + STATS_NAME(bno055_stat_section, samples_mag_1_9g) + STATS_NAME(bno055_stat_section, samples_mag_2_5g) + STATS_NAME(bno055_stat_section, samples_mag_4_0g) + STATS_NAME(bno055_stat_section, samples_mag_4_7g) + STATS_NAME(bno055_stat_section, samples_mag_5_6g) + STATS_NAME(bno055_stat_section, samples_mag_8_1g) + STATS_NAME(bno055_stat_section, errors) +STATS_NAME_END(bno055_stat_section) + +/* Global variable used to hold stats data */ +STATS_SECT_DECL(bno055_stat_section) g_bno055stats; +#endif + +#if MYNEWT_VAL(BNO055_LOG) +#define LOG_MODULE_BNO055 (303) +#define BNO055_INFO(...) LOG_INFO(&_log, LOG_MODULE_BNO055, __VA_ARGS__) +#define BNO055_ERR(...) LOG_ERROR(&_log, LOG_MODULE_BNO055, __VA_ARGS__) +static struct log _log; +#else +#define BNO055_INFO(...) +#define BNO055_ERR(...) +#endif + +/* Exports for the sensor interface. + */ +static void *bno055_sensor_get_interface(struct sensor *, sensor_type_t); +static int bno055_sensor_read(struct sensor *, sensor_type_t, + sensor_data_func_t, void *, uint32_t); +static int bno055_sensor_get_config(struct sensor *, sensor_type_t, + struct sensor_cfg *); + +static const struct sensor_driver g_bno055_sensor_driver = { + bno055_sensor_get_interface, + bno055_sensor_read, + bno055_sensor_get_config +}; + +static uint8_t g_bno055_mode; + +/** + * Writes a single byte to the specified register + * + * @param The register address to write to + * @param The value to write + * + * @return 0 on success, non-zero error on failure. + */ +int +bno055_write8(uint8_t reg, uint32_t value) +{ + int rc; + uint8_t payload[2] = { reg, value & 0xFF }; + + struct hal_i2c_master_data data_struct = { + .address = MYNEWT_VAL(BNO055_I2CADDR), + .len = 2, + .buffer = payload + }; + + rc = hal_i2c_master_write(MYNEWT_VAL(BNO055_I2CBUS), &data_struct, + OS_TICKS_PER_SEC / 10, 1); + if (rc) { + BNO055_ERR("Failed to write to 0x%02X:0x%02X with value 0x%02X\n", + data_struct.address, reg, value); +#if MYNEWT_VAL(BNO055_STATS) + STATS_INC(g_bno055stats, errors); +#endif + } + + return rc; +} + +/** + * Reads a single byte from the specified register + * + * @param The register address to read from + * @param Pointer to where the register value should be written + * + * @return 0 on success, non-zero error on failure. + */ +int +bno055_read8(uint8_t reg, uint8_t *value) +{ + int rc; + uint8_t payload; + + struct hal_i2c_master_data data_struct = { + .address = MYNEWT_VAL(BNO055_I2CADDR), + .len = 1, + .buffer = &payload + }; + + /* Register write */ + payload = reg; + rc = hal_i2c_master_write(MYNEWT_VAL(BNO055_I2CBUS), &data_struct, + OS_TICKS_PER_SEC / 10, 1); + if (rc) { + BNO055_ERR("I2C register write failed at address 0x%02X:0x%02X\n", + data_struct.address, reg); +#if MYNEWT_VAL(BNO055_STATS) + STATS_INC(g_bno055stats, errors); +#endif + goto err; + } + + /* Read one byte back */ + payload = 0; + rc = hal_i2c_master_read(MYNEWT_VAL(BNO055_I2CBUS), &data_struct, + OS_TICKS_PER_SEC / 10, 1); + *value = payload; + if (rc) { + BNO055_ERR("Failed to read from 0x%02X:0x%02X\n", addr, reg); +#if MYNEWT_VAL(BNO055_STATS) + STATS_INC(g_bno055stats, errors); +#endif + } + +err: + return rc; +} + +/** + * Read data from the sensor of variable length (MAX: 8 bytes) + * + * + * @param Register to read from + * @param Bufer to read into + * @param Length of the buffer + * + * @return 0 on success and non-zero on failure + */ +static int +bno055_readlen(uint8_t reg, uint8_t *buffer, uint8_t len) +{ + int rc; + uint8_t payload[8] = { reg, 0, 0, 0, 0, 0, 0, 0}; + + struct hal_i2c_master_data data_struct = { + .address = reg, + .len = 1, + .buffer = payload + }; + + if (len > 8) { + rc = SYS_EINVAL; + goto err; + } + + /* Clear the supplied buffer */ + memset(buffer, 0, 8); + + /* Register write */ + rc = hal_i2c_master_write(MYNEWT_VAL(BNO055_I2CBUS), &data_struct, + OS_TICKS_PER_SEC / 10, 1); + if (rc) { + BNO055_ERR("I2C access failed at address 0x%02X\n", addr); +#if MYNEWT_VAL(BNO055_STATS) + STATS_INC(g_bno055stats, errors); +#endif + goto err; + } + + /* Read six bytes back */ + memset(payload, 0, sizeof(payload)); + data_struct.len = len; + rc = hal_i2c_master_read(MYNEWT_VAL(BNO055_I2CBUS), &data_struct, + OS_TICKS_PER_SEC / 10, 1); + + if (rc) { + BNO055_ERR("Failed to read from 0x%02X:0x%02X\n", addr, reg); +#if MYNEWT_VAL(BNO055_STATS) + STATS_INC(g_bno055stats, errors); +#endif + goto err; + } + + /* Copy the I2C results into the supplied buffer */ + memcpy(buffer, payload, len); + + return 0; +err: + return rc; +} + + +/** + * Setting mode for the bno055 sensor + * + * @param Operation mode for the sensor + * @return 0 on success, non-zero on failure + */ +int +bno055_set_mode(uint8_t mode) +{ + int rc; + + rc = bno055_write8(BNO055_OPR_MODE_ADDR, mode); + if (rc) { + goto err; + } + + if (mode == BNO055_OPERATION_MODE_CONFIG) { + os_time_delay(OS_TICKS_PER_SEC/1000 * 19); + } else { + os_time_delay(OS_TICKS_PER_SEC/1000 * 7); + } + + g_bno055_mode = mode; + return 0; +err: + return rc; +} + +/** + * Read current operational mode of the sensor + * + * @return mode + */ +uint8_t +bno055_get_mode(void) +{ + return g_bno055_mode; +} + +/** + * Expects to be called back through os_dev_create(). + * + * @param The device object associated with this accellerometer + * @param Argument passed to OS device init, unused + * + * @return 0 on success, non-zero error on failure. + */ +int +bno055_init(struct os_dev *dev, void *arg) +{ + struct bno055 *bno055; + struct sensor *sensor; + int rc; + + bno055 = (struct bno055 *) dev; + +#if MYNEWT_VAL(BNO055_LOG) + log_register("bno055", &_log, &log_console_handler, NULL, LOG_SYSLEVEL); +#endif + + sensor = &bno055->sensor; + +#if MYNEWT_VAL(BNO055_STATS) + /* Initialise the stats entry */ + rc = stats_init( + STATS_HDR(g_bno055stats), + STATS_SIZE_INIT_PARMS(g_bno055stats, STATS_SIZE_32), + STATS_NAME_INIT_PARMS(bno055_stat_section)); + SYSINIT_PANIC_ASSERT(rc == 0); + /* Register the entry with the stats registry */ + rc = stats_register("bno055", STATS_HDR(g_bno055stats)); + SYSINIT_PANIC_ASSERT(rc == 0); +#endif + + rc = sensor_init(sensor, dev); + if (rc != 0) { + goto err; + } + + /* Add the accelerometer/magnetometer driver */ + rc = sensor_set_driver(sensor, SENSOR_TYPE_ACCELEROMETER | + SENSOR_TYPE_MAGNETIC_FIELD, + (struct sensor_driver *) &g_bno055_sensor_driver); + if (rc != 0) { + goto err; + } + + rc = sensor_mgr_register(sensor); + if (rc != 0) { + goto err; + } + + return (0); +err: + return (rc); +} + +int +bno055_get_chip_id(uint8_t *id) +{ + int rc; + uint8_t idtmp; + + /* Check if we can read the chip address */ + rc = bno055_read8(BNO055_CHIP_ID_ADDR, &idtmp); + if (rc) { + goto err; + } + + *id = idtmp; + + return 0; +err: + return rc; +} + +int +bno055_config(struct bno055 *bno055, struct bno055_cfg *cfg) +{ + int rc; + uint8_t id; + uint8_t prev_mode; + + prev_mode = g_bno055_mode; + + /* Check if we can read the chip address */ + rc = bno055_read8(BNO055_CHIP_ID_ADDR, &id); + if (rc) { + goto err; + } + + if (id != BNO055_ID) { + os_time_delay(OS_TICKS_PER_SEC/2); + + rc = bno055_read8(BNO055_CHIP_ID_ADDR, &id); + if (rc) { + goto err; + } + + if(id != BNO055_ID) { + goto err; + } + } + + rc = bno055_set_mode(BNO055_OPERATION_MODE_CONFIG); + if (rc) { + goto err; + } + + /* Reset sensor */ + rc = bno055_write8(BNO055_SYS_TRIGGER_ADDR, 0x20); + if (rc) { + goto err; + } + + /* Wait for about 100 ms */ + os_time_delay(OS_TICKS_PER_SEC/1000 * 100); + + /* Set to normal power mode */ + rc = bno055_write8(BNO055_PWR_MODE_ADDR, BNO055_POWER_MODE_NORMAL); + if (rc) { + goto err; + } + + rc = bno055_write8(BNO055_SYS_TRIGGER_ADDR, 0x0); + if (rc) { + goto err; + } + + os_time_delay(OS_TICKS_PER_SEC/1000 * 10); + + /* Overwrite the configuration data. */ + memcpy(&bno055->cfg, cfg, sizeof(*cfg)); + + /* Change back to previous mode */ + rc = bno055_set_mode(prev_mode); + if (rc) { + goto err; + } + + os_time_delay(OS_TICKS_PER_SEC/1000 * 20); + + return 0; +err: + return rc; +} + +/** + * Get quat data from sensor + * + * @param sensor quat data to be filled up + * @return 0 on success, non-zero on error + */ +int +bno055_get_quat_data(void *datastruct) +{ + uint8_t buffer[8]; + double scale; + int rc; + struct sensor_quat_data *sqd; + + sqd = (struct sensor_quat_data *)datastruct; + + /* As per Section 3.6.5.5 Orientation (Quaternion) */ + scale = (1.0 / (1<<14)); + + memset (buffer, 0, 8); + + /* Read quat data */ + rc = bno055_readlen(BNO055_QUATERNION_DATA_W_LSB_ADDR, buffer, 8); + + if (rc) { + goto err; + } + + sqd->sqd_w = ((((uint16_t)buffer[1]) << 8) | ((uint16_t)buffer[0])) * scale; + sqd->sqd_x = ((((uint16_t)buffer[3]) << 8) | ((uint16_t)buffer[2])) * scale; + sqd->sqd_y = ((((uint16_t)buffer[5]) << 8) | ((uint16_t)buffer[4])) * scale; + sqd->sqd_z = ((((uint16_t)buffer[7]) << 8) | ((uint16_t)buffer[6])) * scale; + + return 0; +err: + return rc; +} + +/** + * Find register based on sensor type + * + * @return register address + */ +static int +bno055_find_reg(sensor_type_t type, uint8_t *reg) +{ + int rc; + + rc = 0; + switch(type) { + case SENSOR_TYPE_ACCELEROMETER: + *reg = BNO055_ACCEL_DATA_X_LSB_ADDR; + break; + case SENSOR_TYPE_GYROSCOPE: + *reg = BNO055_GYRO_DATA_X_LSB_ADDR; + break; + case SENSOR_TYPE_MAGNETIC_FIELD: + *reg = BNO055_MAG_DATA_X_LSB_ADDR; + break; + case SENSOR_TYPE_EULER: + *reg = BNO055_EULER_H_LSB_ADDR; + break; + case SENSOR_TYPE_LINEAR_ACCEL: + *reg = BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR; + break; + case SENSOR_TYPE_GRAVITY: + *reg = BNO055_GRAVITY_DATA_X_LSB_ADDR; + break; + default: + BNO055_ERR("Not supported sensor type: %d\n", type); + rc = SYS_EINVAL; + break; + } + + return rc; +} + +int +bno055_get_vector_data(void *datastruct, int type) +{ + + uint8_t payload[6]; + int16_t x, y, z; + struct sensor_mag_data *smd; + struct sensor_accel_data *sad; + struct sensor_euler_data *sed; + uint8_t reg; + int rc; + + memset (payload, 0, 6); + + x = y = z = 0; + + rc = bno055_find_reg(type, ®); + if (rc) { + goto err; + } + + rc = bno055_readlen(reg, payload, 6); + if (rc) { + goto err; + } + + x = ((int16_t)payload[0]) | (((int16_t)payload[1]) << 8); + y = ((int16_t)payload[2]) | (((int16_t)payload[3]) << 8); + z = ((int16_t)payload[4]) | (((int16_t)payload[5]) << 8); + + /** + * Convert the value to an appropriate range (section 3.6.4) + */ + switch(type) { + case SENSOR_TYPE_MAGNETIC_FIELD: + smd = datastruct; + /* 1uT = 16 LSB */ + smd->smd_x = ((double)x)/16.0; + smd->smd_y = ((double)y)/16.0; + smd->smd_z = ((double)z)/16.0; + break; + case SENSOR_TYPE_GYROSCOPE: + sad = datastruct; + /* 1rps = 900 LSB */ + sad->sad_x = ((double)x)/900.0; + sad->sad_y = ((double)y)/900.0; + sad->sad_z = ((double)z)/900.0; + break; + case SENSOR_TYPE_EULER: + sad = datastruct; + /* 1 degree = 16 LSB */ + sed->sed_h = ((double)x)/16.0; + sed->sed_r = ((double)y)/16.0; + sed->sed_p = ((double)z)/16.0; + break; + case SENSOR_TYPE_ACCELEROMETER: + case SENSOR_TYPE_LINEAR_ACCEL: + case SENSOR_TYPE_GRAVITY: + sad = datastruct; + /* 1m/s^2 = 100 LSB */ + sad->sad_x = ((double)x)/100.0; + sad->sad_y = ((double)y)/100.0; + sad->sad_z = ((double)z)/100.0; + break; + default: + BNO055_ERR("Not supported sensor type: %d\n", type); + rc = SYS_EINVAL; + goto err; + } + + return 0; +err: + return rc; +} + +/** + * Get temperature from bno055 sensor + * + * @return temperature in degree celcius + */ +int +bno055_get_temp(int8_t *temp) +{ + int rc; + + rc = bno055_read8(BNO055_TEMP_ADDR, (uint8_t *)temp); + + return rc; +} + +/** + * Get sensor data of specific type. This function also allocates a buffer + * to fill up the data in. + * + * @param Sensor structure used by the SensorAPI + * @param Sensor type + * @param Data function provided by the caler of the API + * @param Argument for the data function + * @param Timeout if any for reading + * @return 0 on success, non-zero on error + */ +static int +bno055_sensor_read(struct sensor *sensor, sensor_type_t type, + sensor_data_func_t data_func, void *data_arg, uint32_t timeout) +{ + void *databuf; + int rc; + + /* Since this is the biggest struct, malloc space for it */ + databuf = malloc(sizeof(struct sensor_quat_data)); + + if (type == SENSOR_TYPE_ROTATION_VECTOR) { + /* Quaternion is a rotation vector */ + rc = bno055_get_quat_data(databuf); + if (rc) { + goto err; + } + } else if (type == SENSOR_TYPE_TEMPERATURE) { + rc = bno055_get_temp(databuf); + if (rc) { + goto err; + } + } else { + /* Get vector data, accel or gravity values */ + rc = bno055_get_vector_data(databuf, type); + if (rc) { + goto err; + } + } + + /* Call data function */ + rc = data_func(sensor, data_arg, databuf); + if (rc) { + goto err; + } + + /* Free the data buffer */ + free(databuf); + + return 0; +err: + return rc; +} + +/** + * Gets system status, test results and errors if any from the sensor + * + * @param ptr to system status + * @param ptr to self test result + * @param ptr to system error + */ +int +bno055_get_sys_status(uint8_t *system_status, uint8_t *self_test_result, uint8_t *system_error) +{ + int rc; + + rc = bno055_write8(BNO055_PAGE_ID_ADDR, 0); + if (rc) { + goto err; + } + /** + * System Status (see section 4.3.58) + * --------------------------------- + * bit 0: Idle + * bit 1: System Error + * bit 2: Initializing Peripherals + * bit 3: System Iniitalization + * bit 4: Executing Self-Test + * bit 5: Sensor fusion algorithm running + * bit 6: System running without fusion algorithms + */ + + if (system_status != 0) { + rc = bno055_read8(BNO055_SYS_STAT_ADDR, system_status); + if (rc) { + goto err; + } + } + + /** + * Self Test Results (see section ) + * -------------------------------- + * 1: test passed, 0: test failed + * bit 0: Accelerometer self test + * bit 1: Magnetometer self test + * bit 2: Gyroscope self test + * bit 3: MCU self test + * + * 0x0F : All Good + */ + + if (self_test_result != 0) { + rc = bno055_read8(BNO055_SELFTEST_RESULT_ADDR, self_test_result); + if (rc) { + goto err; + } + } + + /** + * System Error (see section 4.3.59) + * --------------------------------- + * bit 0 : No error + * bit 1 : Peripheral initialization error + * bit 2 : System initialization error + * bit 3 : Self test result failed + * bit 4 : Register map value out of range + * bit 5 : Register map address out of range + * bit 6 : Register map write error + * bit 7 : BNO low power mode not available for selected operat ion mode + * bit 8 : Accelerometer power mode not available + * bit 9 : Fusion algorithm configuration error + * bit 10 : Sensor configuration error + */ + + if (system_error != 0) { + rc = bno055_read8(BNO055_SYS_ERR_ADDR, system_error); + if (rc) { + goto err; + } + } + + os_time_delay(OS_TICKS_PER_SEC/1000 * 200); + + return 0; +err: + return rc; +} + +/** + * Get Revision info for different sensors in the bno055 + * + * @param pass the pointer to the revision structure + * @return 0 on success, non-zero on error + */ +int +bno055_get_rev_info(struct bno055_rev_info *ri) +{ + uint8_t sw_rev_l, sw_rev_h; + int rc; + + memset(ri, 0, sizeof(struct bno055_rev_info)); + + /* Check the accelerometer revision */ + rc = bno055_read8(BNO055_ACCEL_REV_ID_ADDR, &(ri->bri_accel_rev)); + if (rc) { + goto err; + } + + /* Check the magnetometer revision */ + rc = bno055_read8(BNO055_MAG_REV_ID_ADDR, &(ri->bri_mag_rev)); + if (rc) { + goto err; + } + + /* Check the gyroscope revision */ + rc = bno055_read8(BNO055_GYRO_REV_ID_ADDR, &(ri->bri_gyro_rev)); + if (rc) { + goto err; + } + + rc = bno055_read8(BNO055_BL_REV_ID_ADDR, &(ri->bri_bl_rev)); + if (rc) { + goto err; + } + + /* Check the SW revision */ + rc = bno055_read8(BNO055_SW_REV_ID_LSB_ADDR, &sw_rev_l); + if (rc) { + goto err; + } + + rc = bno055_read8(BNO055_SW_REV_ID_MSB_ADDR, &sw_rev_h); + if (rc) { + goto err; + } + + ri->bri_sw_rev = (((uint16_t)sw_rev_h) << 8) | ((uint16_t)sw_rev_l); + + return 0; +err: + return rc; +} + +static void * +bno055_sensor_get_interface(struct sensor *sensor, sensor_type_t type) +{ + return (NULL); +} + +static int +bno055_sensor_get_config(struct sensor *sensor, sensor_type_t type, + struct sensor_cfg *cfg) +{ + int rc; + + if ((type != SENSOR_TYPE_ACCELEROMETER) && + (type != SENSOR_TYPE_MAGNETIC_FIELD) && + (type != SENSOR_TYPE_TEMPERATURE) && + (type != SENSOR_TYPE_ROTATION_VECTOR) && + (type != SENSOR_TYPE_LINEAR_ACCEL) && + (type != SENSOR_TYPE_GRAVITY) && + (type != SENSOR_TYPE_EULER)) { + rc = SYS_EINVAL; + goto err; + } + + cfg->sc_valtype = SENSOR_VALUE_TYPE_FLOAT_TRIPLET; + + return (0); +err: + return (rc); +} http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/blob/5a76b68c/hw/drivers/sensors/bno055/src/bno055_priv.h ---------------------------------------------------------------------- diff --git a/hw/drivers/sensors/bno055/src/bno055_priv.h b/hw/drivers/sensors/bno055/src/bno055_priv.h new file mode 100644 index 0000000..3a2da68 --- /dev/null +++ b/hw/drivers/sensors/bno055/src/bno055_priv.h @@ -0,0 +1,235 @@ +/* + * Licensed to the Apache Software Foundation (ASF) under one + * or more contributor license agreements. See the NOTICE file + * distributed with this work for additional information + * resarding copyright ownership. The ASF licenses this file + * to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance + * with the License. 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. + */ + +/* Page id register definition */ +#define BNO055_PAGE_ID_ADDR 0X07 + +/* PAGE0 REGISTER DEFINITION START*/ +#define BNO055_CHIP_ID_ADDR 0x00 +#define BNO055_ACCEL_REV_ID_ADDR 0x01 +#define BNO055_MAG_REV_ID_ADDR 0x02 +#define BNO055_GYRO_REV_ID_ADDR 0x03 +#define BNO055_SW_REV_ID_LSB_ADDR 0x04 +#define BNO055_SW_REV_ID_MSB_ADDR 0x05 +#define BNO055_BL_REV_ID_ADDR 0X06 +/* Accel data register */ +#define BNO055_ACCEL_DATA_X_LSB_ADDR 0X08 +#define BNO055_ACCEL_DATA_X_MSB_ADDR 0X09 +#define BNO055_ACCEL_DATA_Y_LSB_ADDR 0X0A +#define BNO055_ACCEL_DATA_Y_MSB_ADDR 0X0B +#define BNO055_ACCEL_DATA_Z_LSB_ADDR 0X0C +#define BNO055_ACCEL_DATA_Z_MSB_ADDR 0X0D + +/* Mag data register */ +#define BNO055_MAG_DATA_X_LSB_ADDR 0X0E +#define BNO055_MAG_DATA_X_MSB_ADDR 0X0F +#define BNO055_MAG_DATA_Y_LSB_ADDR 0X10 +#define BNO055_MAG_DATA_Y_MSB_ADDR 0X11 +#define BNO055_MAG_DATA_Z_LSB_ADDR 0X12 +#define BNO055_MAG_DATA_Z_MSB_ADDR 0X13 + +/* Gyro data registers */ +#define BNO055_GYRO_DATA_X_LSB_ADDR 0X14 +#define BNO055_GYRO_DATA_X_MSB_ADDR 0X15 +#define BNO055_GYRO_DATA_Y_LSB_ADDR 0X16 +#define BNO055_GYRO_DATA_Y_MSB_ADDR 0X17 +#define BNO055_GYRO_DATA_Z_LSB_ADDR 0X18 +#define BNO055_GYRO_DATA_Z_MSB_ADDR 0X19 + +/* Euler data registers */ +#define BNO055_EULER_H_LSB_ADDR 0X1A +#define BNO055_EULER_H_MSB_ADDR 0X1B +#define BNO055_EULER_R_LSB_ADDR 0X1C +#define BNO055_EULER_R_MSB_ADDR 0X1D +#define BNO055_EULER_P_LSB_ADDR 0X1E +#define BNO055_EULER_P_MSB_ADDR 0X1F + +/* Quaternion data registers */ +#define BNO055_QUATERNION_DATA_W_LSB_ADDR 0X20 +#define BNO055_QUATERNION_DATA_W_MSB_ADDR 0X21 +#define BNO055_QUATERNION_DATA_X_LSB_ADDR 0X22 +#define BNO055_QUATERNION_DATA_X_MSB_ADDR 0X23 +#define BNO055_QUATERNION_DATA_Y_LSB_ADDR 0X24 +#define BNO055_QUATERNION_DATA_Y_MSB_ADDR 0X25 +#define BNO055_QUATERNION_DATA_Z_LSB_ADDR 0X26 +#define BNO055_QUATERNION_DATA_Z_MSB_ADDR 0X27 + +/* Linear acceleration data registers */ +#define BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR 0X28 +#define BNO055_LINEAR_ACCEL_DATA_X_MSB_ADDR 0X29 +#define BNO055_LINEAR_ACCEL_DATA_Y_LSB_ADDR 0X2A +#define BNO055_LINEAR_ACCEL_DATA_Y_MSB_ADDR 0X2B +#define BNO055_LINEAR_ACCEL_DATA_Z_LSB_ADDR 0X2C +#define BNO055_LINEAR_ACCEL_DATA_Z_MSB_ADDR 0X2D + +/* Gravity data registers */ +#define BNO055_GRAVITY_DATA_X_LSB_ADDR 0X2E +#define BNO055_GRAVITY_DATA_X_MSB_ADDR 0X2F +#define BNO055_GRAVITY_DATA_Y_LSB_ADDR 0X30 +#define BNO055_GRAVITY_DATA_Y_MSB_ADDR 0X31 +#define BNO055_GRAVITY_DATA_Z_LSB_ADDR 0X32 +#define BNO055_GRAVITY_DATA_Z_MSB_ADDR 0X33 + +/* Temperature data register */ +#define BNO055_TEMP_ADDR 0X34 + +/* Status registers */ +#define BNO055_CALIB_STAT_ADDR 0X35 +#define BNO055_SELFTEST_RESULT_ADDR 0X36 +#define BNO055_INTR_STAT_ADDR 0X37 + +#define BNO055_SYS_CLK_STAT_ADDR 0X38 +#define BNO055_SYS_STAT_ADDR 0X39 +#define BNO055_SYS_ERR_ADDR 0X3A + +/* Unit selection register */ +#define BNO055_UNIT_SEL_ADDR 0X3B +#define BNO055_DATA_SELECT_ADDR 0X3C + +/* Mode registers */ +#define BNO055_OPR_MODE_ADDR 0X3D +#define BNO055_PWR_MODE_ADDR 0X3E + +#define BNO055_SYS_TRIGGER_ADDR 0X3F +#define BNO055_TEMP_SOURCE_ADDR 0X40 + +/* Axis remap registers */ +#define BNO055_AXIS_MAP_CONFIG_ADDR 0X41 +#define BNO055_AXIS_MAP_SIGN_ADDR 0X42 + +/* SIC registers */ +#define BNO055_SIC_MATRIX_0_LSB_ADDR 0X43 +#define BNO055_SIC_MATRIX_0_MSB_ADDR 0X44 +#define BNO055_SIC_MATRIX_1_LSB_ADDR 0X45 +#define BNO055_SIC_MATRIX_1_MSB_ADDR 0X46 +#define BNO055_SIC_MATRIX_2_LSB_ADDR 0X47 +#define BNO055_SIC_MATRIX_2_MSB_ADDR 0X48 +#define BNO055_SIC_MATRIX_3_LSB_ADDR 0X49 +#define BNO055_SIC_MATRIX_3_MSB_ADDR 0X4A +#define BNO055_SIC_MATRIX_4_LSB_ADDR 0X4B +#define BNO055_SIC_MATRIX_4_MSB_ADDR 0X4C +#define BNO055_SIC_MATRIX_5_LSB_ADDR 0X4D +#define BNO055_SIC_MATRIX_5_MSB_ADDR 0X4E +#define BNO055_SIC_MATRIX_6_LSB_ADDR 0X4F +#define BNO055_SIC_MATRIX_6_MSB_ADDR 0X50 +#define BNO055_SIC_MATRIX_7_LSB_ADDR 0X51 +#define BNO055_SIC_MATRIX_7_MSB_ADDR 0X52 +#define BNO055_SIC_MATRIX_8_LSB_ADDR 0X53 +#define BNO055_SIC_MATRIX_8_MSB_ADDR 0X54 + +/* Accelerometer Offset registers */ +#define BNO055_ACCEL_OFFSET_X_LSB_ADDR 0X55 +#define BNO055_ACCEL_OFFSET_X_MSB_ADDR 0X56 +#define BNO055_ACCEL_OFFSET_Y_LSB_ADDR 0X57 +#define BNO055_ACCEL_OFFSET_Y_MSB_ADDR 0X58 +#define BNO055_ACCEL_OFFSET_Z_LSB_ADDR 0X59 +#define BNO055_ACCEL_OFFSET_Z_MSB_ADDR 0X5A + +/* Magnetometer Offset registers */ +#define BNO055_MAG_OFFSET_X_LSB_ADDR 0X5B +#define BNO055_MAG_OFFSET_X_MSB_ADDR 0X5C +#define BNO055_MAG_OFFSET_Y_LSB_ADDR 0X5D +#define BNO055_MAG_OFFSET_Y_MSB_ADDR 0X5E +#define BNO055_MAG_OFFSET_Z_LSB_ADDR 0X5F +#define BNO055_MAG_OFFSET_Z_MSB_ADDR 0X60 + +/* Gyroscope Offset register s*/ +#define BNO055_GYRO_OFFSET_X_LSB_ADDR 0X61 +#define BNO055_GYRO_OFFSET_X_MSB_ADDR 0X62 +#define BNO055_GYRO_OFFSET_Y_LSB_ADDR 0X63 +#define BNO055_GYRO_OFFSET_Y_MSB_ADDR 0X64 +#define BNO055_GYRO_OFFSET_Z_LSB_ADDR 0X65 +#define BNO055_GYRO_OFFSET_Z_MSB_ADDR 0X66 + +/* Radius registers */ +#define BNO055_ACCEL_RADIUS_LSB_ADDR 0X67 +#define BNO055_ACCEL_RADIUS_MSB_ADDR 0X68 +#define BNO055_MAG_RADIUS_LSB_ADDR 0X69 +#define BNO055_MAG_RADIUS_MSB_ADDR 0X6A + +/* Power modes */ +#define BNO055_POWER_MODE_NORMAL 0X00 +#define BNO055_POWER_MODE_LOWPOWER 0X01 +#define BNO055_POWER_MODE_SUSPEND 0X02 + +/* Operation Modes */ +#define BNO055_OPERATION_MODE_CONFIG 0X00 +#define BNO055_OPERATION_MODE_ACCONLY 0X01 +#define BNO055_OPERATION_MODE_MAGONLY 0X02 +#define BNO055_OPERATION_MODE_GYRONLY 0X03 +#define BNO055_OPERATION_MODE_ACCMAG 0X04 +#define BNO055_OPERATION_MODE_ACCGYRO 0X05 +#define BNO055_OPERATION_MODE_MAGGYRO 0X06 +#define BNO055_OPERATION_MODE_AMG 0X07 +#define BNO055_OPERATION_MODE_IMUPLUS 0X08 +#define BNO055_OPERATION_MODE_COMPASS 0X09 +#define BNO055_OPERATION_MODE_M4G 0X0A +#define BNO055_OPERATION_MODE_NDOF_FMC_OFF 0X0B +#define BNO055_OPERATION_MODE_NDOF 0X0C + +/* Remap config - Default: 0x24 */ +#define BNO055_REMAP_CONFIG_P0 0x21 +#define BNO055_REMAP_CONFIG_P1 0x24 +#define BNO055_REMAP_CONFIG_P2 0x24 +#define BNO055_REMAP_CONFIG_P3 0x21 +#define BNO055_REMAP_CONFIG_P4 0x24 +#define BNO055_REMAP_CONFIG_P5 0x21 +#define BNO055_REMAP_CONFIG_P6 0x21 +#define BNO055_REMAP_CONFIG_P7 0x24 + +/* Remap Sign - Default: 0x00 */ +#define BNO055_REMAP_SIGN_P0 0x04 +#define BNO055_REMAP_SIGN_P1 0x00 +#define BNO055_REMAP_SIGN_P2 0x06 +#define BNO055_REMAP_SIGN_P3 0x02 +#define BNO055_REMAP_SIGN_P4 0x03 +#define BNO055_REMAP_SIGN_P5 0x01 +#define BNO055_REMAP_SIGN_P6 0x07 +#define BNO055_REMAP_SIGN_P7 0x05 + +/* PAGE1 REGISTERS DEFINITION START*/ +/* Configuration registers*/ +#define BNO055_ACCEL_CONFIG_ADDR 0X08 +#define BNO055_MAG_CONFIG_ADDR 0X09 +#define BNO055_GYRO_CONFIG_ADDR 0X0A +#define BNO055_GYRO_MODE_CONFIG_ADDR 0X0B +#define BNO055_ACCEL_SLEEP_CONFIG_ADDR 0X0C +#define BNO055_GYRO_SLEEP_CONFIG_ADDR 0X0D +#define BNO055_MAG_SLEEP_CONFIG_ADDR 0x0E + +/* Interrupt Registers */ +#define BNO055_INT_MASK_ADDR 0X0F +#define BNO055_INT_ADDR 0X10 +#define BNO055_ACCEL_ANY_MOTION_THRES_ADDR 0X11 +#define BNO055_ACCEL_INTR_SETTINGS_ADDR 0X12 +#define BNO055_ACCEL_HIGH_G_DURN_ADDR 0X13 +#define BNO055_ACCEL_HIGH_G_THRES_ADDR 0X14 +#define BNO055_ACCEL_NO_MOTION_THRES_ADDR 0X15 +#define BNO055_ACCEL_NO_MOTION_SET_ADDR 0X16 +#define BNO055_GYRO_INTR_SETING_ADDR 0X17 +#define BNO055_GYRO_HIGHRATE_X_SET_ADDR 0X18 +#define BNO055_GYRO_DURN_X_ADDR 0X19 +#define BNO055_GYRO_HIGHRATE_Y_SET_ADDR 0X1A +#define BNO055_GYRO_DURN_Y_ADDR 0X1B +#define BNO055_GYRO_HIGHRATE_Z_SET_ADDR 0X1C +#define BNO055_GYRO_DURN_Z_ADDR 0X1D +#define BNO055_GYRO_ANY_MOTION_THRES_ADDR 0X1E +#define BNO055_GYRO_ANY_MOTION_SET_ADDR 0X1F + +#define BNO055_ID 0xA0 http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/blob/5a76b68c/hw/drivers/sensors/bno055/src/bno055_shell.c ---------------------------------------------------------------------- diff --git a/hw/drivers/sensors/bno055/src/bno055_shell.c b/hw/drivers/sensors/bno055/src/bno055_shell.c new file mode 100644 index 0000000..3f39a38 --- /dev/null +++ b/hw/drivers/sensors/bno055/src/bno055_shell.c @@ -0,0 +1,424 @@ +/* + * Licensed to the Apache Software Foundation (ASF) under one + * or more contributor license agreements. See the NOTICE file + * distributed with this work for additional information + * regarding copyright ownership. The ASF licenses this file + * to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance + * with the License. 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. + */ + +#include <string.h> +#include <errno.h> +#include "sysinit/sysinit.h" +#include "console/console.h" +#include "shell/shell.h" +#include "hal/hal_gpio.h" +#include "bno055_priv.h" +#include "bno055/bno055.h" +#include "sensor/sensor.h" +#include "sensor/accel.h" +#include "sensor/mag.h" +#include "sensor/quat.h" +#include "sensor/euler.h" + +#if MYNEWT_VAL(BNO055_CLI) +extern uint8_t g_bno055_mode; + +static int bno055_shell_cmd(int argc, char **argv); + +static struct shell_cmd bno055_shell_cmd_struct = { + .sc_cmd = "bno055", + .sc_cmd_func = bno055_shell_cmd +}; + +static int +bno055_shell_stol(char *param_val, long min, long max, long *output) +{ + char *endptr; + long lval; + + lval = strtol(param_val, &endptr, 10); /* Base 10 */ + if (param_val != '\0' && *endptr == '\0' && + lval >= min && lval <= max) { + *output = lval; + } else { + return EINVAL; + } + + return 0; +} + +static int +bno055_shell_err_too_many_args(char *cmd_name) +{ + console_printf("Error: too many arguments for command \"%s\"\n", + cmd_name); + return EINVAL; +} + +static int +bno055_shell_err_unknown_arg(char *cmd_name) +{ + console_printf("Error: unknown argument \"%s\"\n", + cmd_name); + return EINVAL; +} + +static int +bno055_shell_err_invalid_arg(char *cmd_name) +{ + console_printf("Error: invalid argument \"%s\"\n", + cmd_name); + return EINVAL; +} + +static int +bno055_shell_help(void) +{ + console_printf("%s cmd [flags...]\n", bno055_shell_cmd_struct.sc_cmd); + console_printf("cmd:\n"); + console_printf("\tr [n_samples] [ 0-acc | 1 -mag | 2 -gyro | 4 -temp |\n" + "\t 9-quat | 26-linearacc | 27-gravity | 28-euler ]\n\n"); + console_printf("\tmode [0-config | 1-acc | 2 -mag | 3 -gyro | 4 -accmag |\n" + "\t 5-accgyro | 6-maggyro | 7 -amg | 8 -imuplus | 9 -compass|\n" + "\t 9-m4g |11-NDOF_FMC_OFF | 12-NDOF ]\n"); + console_printf("\tchip_id\n"); + console_printf("\trev\n"); + console_printf("\treset\n"); + console_printf("\tpmode [0-normal | 1-lowpower | 2-suspend]\n"); + console_printf("\tdumpreg [addr]\n"); + + return 0; +} + +static int +bno055_shell_cmd_get_chip_id(int argc, char **argv) +{ + uint8_t id; + int rc; + + if (argc > 3) { + return bno055_shell_err_too_many_args(argv[1]); + } + + /* Display the chip id */ + if (argc == 2) { + rc = bno055_get_chip_id(&id); + if (rc) { + console_printf("Read failed %d", rc); + } + console_printf("%x\n", id ? 16u : 1u); + } + + return 0; +} + +static int +bno055_shell_cmd_get_rev_info(int argc, char **argv) +{ + int rc; + struct bno055_rev_info ri; + + if (argc > 3) { + return bno055_shell_err_too_many_args(argv[1]); + } + + /* Display the rev ids */ + if (argc == 2) { + rc = bno055_get_rev_info(&ri); + if (rc) { + console_printf("Read failed %d", rc); + } + console_printf("accel_rev:%x\nmag_rev:%x\ngyro_rev:%x\n" + "sw_rev:%x\nbl_rev:%x\n", ri.bri_accel_rev, + ri.bri_mag_rev, ri.bri_gyro_rev, ri.bri_sw_rev, + ri.bri_bl_rev); + } + + return 0; +} + +static int +bno055_shell_cmd_read(int argc, char **argv) +{ + uint16_t samples = 1; + long val; + int rc; + void *databuf; + struct sensor_quat_data *sqd; + struct sensor_euler_data *sed; + struct sensor_accel_data *sad; + int type; + + type = 0; + if (argc > 4) { + return bno055_shell_err_too_many_args(argv[1]); + } + + /* Since this is the biggest struct, malloc space for it */ + databuf = malloc(sizeof(*sqd)); + + + /* Check if more than one sample requested */ + if (argc == 4) { + if (bno055_shell_stol(argv[2], 1, UINT16_MAX, &val)) { + return bno055_shell_err_invalid_arg(argv[2]); + } + samples = (uint16_t)val; + + if (bno055_shell_stol(argv[2], 1, UINT16_MAX, &val)) { + return bno055_shell_err_invalid_arg(argv[2]); + } + type = (uint16_t)(1 << val); + } + + while(samples--) { + + if (type == SENSOR_TYPE_ROTATION_VECTOR) { + rc = bno055_get_quat_data(databuf); + if (rc) { + console_printf("Read failed: %d\n", rc); + goto err; + } + sqd = databuf; + console_printf("x:%u y:%u z:%u w:%u\n", (unsigned int)sqd->sqd_x, + (unsigned int)sqd->sqd_y, (unsigned int)sqd->sqd_z, + (unsigned int)sqd->sqd_w); + } else if (type == SENSOR_TYPE_EULER) { + rc = bno055_get_vector_data(databuf, type); + if (rc) { + console_printf("Read failed: %d\n", rc); + } + sed = databuf; + console_printf("h:%u r:%u p:%u\n", (unsigned int)sed->sed_h, + (unsigned int)sed->sed_r, (unsigned int)sed->sed_p); + } else { + rc = bno055_get_vector_data(databuf, type); + if (rc) { + console_printf("Read failed: %d\n", rc); + } + sad = databuf; + console_printf("x:%u y:%u z:%u\n", (unsigned int)sad->sad_x, + (unsigned int)sad->sad_y, (unsigned int)sad->sad_z); + } + } + + return 0; +err: + return rc; +} + +static int +bno055_shell_cmd_mode(int argc, char **argv) +{ + long val; + int rc; + + if (argc > 3) { + return bno055_shell_err_too_many_args(argv[1]); + } + + /* Display the mode */ + if (argc == 2) { + val = bno055_get_mode(); + console_printf("%u\n", val ? 16u : 1u); + } + + /* Update the mode */ + if (argc == 3) { + if (bno055_shell_stol(argv[2], 1, 16, &val)) { + return bno055_shell_err_invalid_arg(argv[2]); + } + /* Make sure mode is */ + if (val > BNO055_OPERATION_MODE_NDOF) { + return bno055_shell_err_invalid_arg(argv[2]); + } + + rc = bno055_set_mode(val); + if (rc) { + goto err; + } + } + + return 0; +err: + return rc; +} + +#if 0 +static int +bno055_shell_cmd_int(int argc, char **argv) +{ + int rc; + int pin; + long val; + uint8_t rate; + uint16_t lower; + uint16_t upper; + + if (argc > 6) { + return bno055_shell_err_too_many_args(argv[1]); + } + + if (argc == 2) { + console_printf("ToDo: Display int details\n"); + return 0; + } + + /* Enable the interrupt */ + if (argc == 3 && strcmp(argv[2], "on") == 0) { + return bno055_enable_interrupt(1); + } + + /* Disable the interrupt */ + if (argc == 3 && strcmp(argv[2], "off") == 0) { + return bno055_enable_interrupt(0); + } + + /* Clear the interrupt on 'clr' */ + if (argc == 3 && strcmp(argv[2], "clr") == 0) { + return bno055_clear_interrupt(); + } + + /* Configure the interrupt on 'set' */ + if (argc == 6 && strcmp(argv[2], "set") == 0) { + /* Get rate */ + if (bno055_shell_stol(argv[3], 0, 15, &val)) { + return bno055_shell_err_invalid_arg(argv[3]); + } + rate = (uint8_t)val; + /* Get lower threshold */ + if (bno055_shell_stol(argv[4], 0, UINT16_MAX, &val)) { + return bno055_shell_err_invalid_arg(argv[4]); + } + lower = (uint16_t)val; + /* Get upper threshold */ + if (bno055_shell_stol(argv[5], 0, UINT16_MAX, &val)) { + return bno055_shell_err_invalid_arg(argv[5]); + } + upper = (uint16_t)val; + /* Set the values */ + rc = bno055_setup_interrupt(rate, lower, upper); + console_printf("Configured interrupt as:\n"); + console_printf("\trate: %u\n", rate); + console_printf("\tlower: %u\n", lower); + console_printf("\tupper: %u\n", upper); + return rc; + } + + /* Setup INT pin on 'pin' */ + if (argc == 4 && strcmp(argv[2], "pin") == 0) { + /* Get the pin number */ + if (bno055_shell_stol(argv[3], 0, 0xFF, &val)) { + return bno055_shell_err_invalid_arg(argv[3]); + } + pin = (int)val; + /* INT is open drain, pullup is required */ + rc = hal_gpio_init_in(pin, HAL_GPIO_PULL_UP); + assert(rc == 0); + console_printf("Set pin \"%d\" to INPUT with pull up enabled\n", pin); + return 0; + } + + /* Unknown command */ + return bno055_shell_err_invalid_arg(argv[2]); +} + +static int +bno055_shell_cmd_dump(int argc, char **argv) +{ + uint8_t val; + + if (argc > 3) { + return bno055_shell_err_too_many_args(argv[1]); + } + + val = 0; + assert(0 == bno055_read8(BNO055_COMMAND_BIT | BNO055_REGISTER_CONTROL, &val)); + console_printf("0x%02X (CONTROL): 0x%02X\n", BNO055_REGISTER_CONTROL, val); + assert(0 == bno055_read8(BNO055_COMMAND_BIT | BNO055_REGISTER_TIMING, &val)); + console_printf("0x%02X (TIMING): 0x%02X\n", BNO055_REGISTER_TIMING, val); + assert(0 == bno055_read8(BNO055_COMMAND_BIT | BNO055_REGISTER_THRESHHOLDL_LOW, &val)); + console_printf("0x%02X (THRLL): 0x%02X\n", BNO055_REGISTER_THRESHHOLDL_LOW, val); + assert(0 == bno055_read8(BNO055_COMMAND_BIT | BNO055_REGISTER_THRESHHOLDL_HIGH, &val)); + console_printf("0x%02X (THRLH): 0x%02X\n", BNO055_REGISTER_THRESHHOLDL_HIGH, val); + assert(0 == bno055_read8(BNO055_COMMAND_BIT | BNO055_REGISTER_THRESHHOLDH_LOW, &val)); + console_printf("0x%02X (THRHL): 0x%02X\n", BNO055_REGISTER_THRESHHOLDH_LOW, val); + assert(0 == bno055_read8(BNO055_COMMAND_BIT | BNO055_REGISTER_THRESHHOLDH_HIGH, &val)); + console_printf("0x%02X (THRHH): 0x%02X\n", BNO055_REGISTER_THRESHHOLDH_HIGH, val); + assert(0 == bno055_read8(BNO055_COMMAND_BIT | BNO055_REGISTER_INTERRUPT, &val)); + console_printf("0x%02X (INTER): 0x%02X\n", BNO055_REGISTER_INTERRUPT, val); + assert(0 == bno055_read8(BNO055_COMMAND_BIT | BNO055_REGISTER_ID, &val)); + console_printf("0x%02X (ID): 0x%02X\n", BNO055_REGISTER_ID, val); + + return 0; +} +#endif + +static int +bno055_shell_cmd(int argc, char **argv) +{ + if (argc == 1) { + return bno055_shell_help(); + } + + /* Read command (get a new data sample) */ + if (argc > 1 && strcmp(argv[1], "r") == 0) { + return bno055_shell_cmd_read(argc, argv); + } + + /* Mode command */ + if (argc > 1 && strcmp(argv[1], "mode") == 0) { + return bno055_shell_cmd_mode(argc, argv); + } + + /* Chip ID command */ + if (argc > 1 && strcmp(argv[1], "chip_id") == 0) { + return bno055_shell_cmd_get_chip_id(argc, argv); + } + + /* Rev command */ + if (argc > 1 && strcmp(argv[1], "rev") == 0) { + return bno055_shell_cmd_get_rev_info(argc, argv); + } +#if 0 + /* Reset command */ + if (argc > 1 && strcmp(argv[1], "reset") == 0) { + return bno055_shell_cmd_reset(argc, argv); + } + + /* Power mode command */ + if (argc > 1 && strcmp(argv[1], "pmode") == 0) { + return bno055_shell_cmd_pmode(argc, argv); + } + + /* Dump Registers command */ + if (argc > 1 && strcmp(argv[1], "dumpreg") == 0) { + return bno055_shell_cmd_dumpreg(argc, argv); + } +#endif + return bno055_shell_err_unknown_arg(argv[1]); +} + +int +bno055_shell_init(void) +{ + int rc; + + rc = shell_cmd_register(&bno055_shell_cmd_struct); + SYSINIT_PANIC_ASSERT(rc == 0); + + return rc; +} + +#endif http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/blob/5a76b68c/hw/drivers/sensors/bno055/syscfg.yml ---------------------------------------------------------------------- diff --git a/hw/drivers/sensors/bno055/syscfg.yml b/hw/drivers/sensors/bno055/syscfg.yml new file mode 100644 index 0000000..f6cb228 --- /dev/null +++ b/hw/drivers/sensors/bno055/syscfg.yml @@ -0,0 +1,35 @@ +# +# Licensed to the Apache Software Foundation (ASF) under one +# or more contributor license agreements. See the NOTICE file +# distributed with this work for additional information +# regarding copyright ownership. The ASF licenses this file +# to you under the Apache License, Version 2.0 (the +# "License"); you may not use this file except in compliance +# with the License. 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. +# + +syscfg.defs: + BNO055_I2CADDR: + description: 'HW I2C address for the BNO055' + value: 0x28 + BNO055_I2CBUS: + description: 'I2C bus number for the BNO055' + value: -1 + BNO055_CLI: + description: 'Enable shell support for the BNO055' + value: 0 + BNO055_LOG: + description: 'Enable BNO055 logging' + value: 0 + BNO055_STATS: + description: 'Enable BNO055 statistics' + value: 0 http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/blob/5a76b68c/hw/drivers/sensors/tsl2561/src/tsl2561.c ---------------------------------------------------------------------- diff --git a/hw/drivers/sensors/tsl2561/src/tsl2561.c b/hw/drivers/sensors/tsl2561/src/tsl2561.c index b3c7d2d..8ea8ad3 100644 --- a/hw/drivers/sensors/tsl2561/src/tsl2561.c +++ b/hw/drivers/sensors/tsl2561/src/tsl2561.c @@ -124,7 +124,11 @@ tsl2561_write8(uint8_t reg, uint32_t value) rc = hal_i2c_master_write(MYNEWT_VAL(TSL2561_I2CBUS), &data_struct, OS_TICKS_PER_SEC / 10, 1); if (rc) { - TSL2561_ERR("Failed to write @0x%02X with value 0x%02X\n", reg, value); + TSL2561_ERR("Failed to write 0x%02X:0x%02X with value 0x%02X\n", + data_struct.address, reg, value); +#if MYNEWT_VAL(TSL2561_STATS) + STATS_INC(g_tsl2561stats, errors); +#endif } return rc; @@ -273,7 +277,7 @@ tsl2561_set_gain(uint8_t gain) if ((gain != TSL2561_LIGHT_GAIN_1X) && (gain != TSL2561_LIGHT_GAIN_16X)) { TSL2561_ERR("Invalid gain value\n"); - rc = EINVAL; + rc = SYS_EINVAL; goto err; } http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/blob/5a76b68c/hw/sensor/include/sensor/euler.h ---------------------------------------------------------------------- diff --git a/hw/sensor/include/sensor/euler.h b/hw/sensor/include/sensor/euler.h new file mode 100644 index 0000000..5ddd44f --- /dev/null +++ b/hw/sensor/include/sensor/euler.h @@ -0,0 +1,50 @@ +/* + * Licensed to the Apache Software Foundation (ASF) under one + * or more contributor license agreements. See the NOTICE file + * distributed with this work for additional information + * regarding copyright ownership. The ASF licenses this file + * to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance + * with the License. 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. + */ + +#ifndef __SENSOR_EULER_H__ +#define __SENSOR_EULER_H__ + +#include "os/os.h" +#include "os/os_dev.h" +#include "sensor/sensor.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/* Data representing Euler angles + * All values are in Degrees + * Heading, Roll and Pitch + */ +struct sensor_euler_data { + float sed_h; + float sed_r; + float sed_p; +} __attribute__((packed)); + +/** + * Accelerometer data is unused for this field. + */ +#define SENSOR_ACCEL_DATA_UNUSED (-1) + +#ifdef __cplusplus +} +#endif + +#endif /* __SENSOR_ACCEL_H__ */ http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/blob/5a76b68c/hw/sensor/include/sensor/quat.h ---------------------------------------------------------------------- diff --git a/hw/sensor/include/sensor/quat.h b/hw/sensor/include/sensor/quat.h new file mode 100644 index 0000000..3abb132 --- /dev/null +++ b/hw/sensor/include/sensor/quat.h @@ -0,0 +1,50 @@ +/* + * Licensed to the Apache Software Foundation (ASF) under one + * or more contributor license agreements. See the NOTICE file + * distributed with this work for additional information + * regarding copyright ownership. The ASF licenses this file + * to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance + * with the License. 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. + */ + +#ifndef __SENSOR_QUAT_H__ +#define __SENSOR_QUAT_H__ + +#include "os/os.h" +#include "os/os_dev.h" +#include "sensor/sensor.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/* Data representing a singular read from an accelerometer. + * All values are in MS^2 + */ +struct sensor_quat_data { + float sqd_x; + float sqd_y; + float sqd_z; + float sqd_w; +} __attribute__((packed)); + +/** + * Quaternion data is unused for this field. + */ +#define SENSOR_QUAT_DATA_UNUSED (-1) + +#ifdef __cplusplus +} +#endif + +#endif /* __SENSOR_ACCEL_H__ */ http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/blob/5a76b68c/hw/sensor/include/sensor/sensor.h ---------------------------------------------------------------------- diff --git a/hw/sensor/include/sensor/sensor.h b/hw/sensor/include/sensor/sensor.h index 2f99ea2..d707e2b 100644 --- a/hw/sensor/include/sensor/sensor.h +++ b/hw/sensor/include/sensor/sensor.h @@ -66,18 +66,18 @@ typedef enum { SENSOR_TYPE_ALTITUDE = (1 << 10), /* Weight Supported */ SENSOR_TYPE_WEIGHT = (1 << 11), + /* Linear Accelerometer (Without Gravity) */ + SENSOR_TYPE_LINEAR_ACCEL = (1 << 26), + /* Gravity Sensor */ + SENSOR_TYPE_GRAVITY = (1 << 27), + /* Euler Orientation Sensor */ + SENSOR_TYPE_EULER = (1 << 28), /* User defined sensor type 1 */ - SENSOR_TYPE_USER_DEFINED_1 = (1 << 26), + SENSOR_TYPE_USER_DEFINED_1 = (1 << 29), /* User defined sensor type 2 */ - SENSOR_TYPE_USER_DEFINED_2 = (1 << 27), + SENSOR_TYPE_USER_DEFINED_2 = (1 << 30), /* User defined sensor type 3 */ - SENSOR_TYPE_USER_DEFINED_3 = (1 << 28), - /* User defined sensor type 4 */ - SENSOR_TYPE_USER_DEFINED_4 = (1 << 29), - /* User defined sensor type 5 */ - SENSOR_TYPE_USER_DEFINED_5 = (1 << 30), - /* User defined sensor type 6 */ - SENSOR_TYPE_USER_DEFINED_6 = (1 << 31), + SENSOR_TYPE_USER_DEFINED_3 = (1 << 31), /* A selector, describes all sensors */ SENSOR_TYPE_ALL = 0xFFFFFFFF } sensor_type_t;