Merge pull request #538 from andrzej-kaczmarek/nimble-ll-stack-fix
nimble/ll: Increase stack for ble_ll_task
diff --git a/hw/drivers/sensors/mpu6050/include/mpu6050/mpu6050.h b/hw/drivers/sensors/mpu6050/include/mpu6050/mpu6050.h
new file mode 100644
index 0000000..63a6f70
--- /dev/null
+++ b/hw/drivers/sensors/mpu6050/include/mpu6050/mpu6050.h
@@ -0,0 +1,70 @@
+/*
+ * 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_MPU6050_H__
+#define __SENSOR_MPU6050_H__
+
+#include "os/os.h"
+#include "os/os_dev.h"
+#include "sensor/sensor.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+enum mpu6050_gyro_range {
+ MPU6050_GYRO_RANGE_250 = 0x00 << 3, /* +/- 250 Deg/s */
+ MPU6050_GYRO_RANGE_500 = 0x01 << 3, /* +/- 500 Deg/s */
+ MPU6050_GYRO_RANGE_1000 = 0x02 << 3, /* +/- 1000 Deg/s */
+ MPU6050_GYRO_RANGE_2000 = 0x03 << 3, /* +/- 2000 Deg/s */
+};
+
+enum mpu6050_accel_range {
+ MPU6050_ACCEL_RANGE_2 = 0x00 << 3, /* +/- 2g */
+ MPU6050_ACCEL_RANGE_4 = 0x01 << 3, /* +/- 4g */
+ MPU6050_ACCEL_RANGE_8 = 0x02 << 3, /* +/- 8g */
+ MPU6050_ACCEL_RANGE_16 = 0x03 << 3, /* +/- 16g */
+};
+
+#define MPU6050_I2C_ADDR (0xD0 >> 1)
+
+struct mpu6050_cfg {
+ enum mpu6050_accel_range accel_range;
+ enum mpu6050_gyro_range gyro_range;
+ uint8_t gyro_rate_div; /* Sample Rate = Gyroscope Output Rate /
+ (1 + gyro_rate_div) */
+ uint8_t lpf_cfg; /* See data sheet */
+ sensor_type_t mask;
+};
+
+struct mpu6050 {
+ struct os_dev dev;
+ struct sensor sensor;
+ struct mpu6050_cfg cfg;
+ os_time_t last_read_time;
+};
+
+int mpu6050_init(struct os_dev *, void *);
+int mpu6050_config(struct mpu6050 *, struct mpu6050_cfg *);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __SENSOR_MPU6050_H__ */
diff --git a/hw/drivers/sensors/mpu6050/pkg.yml b/hw/drivers/sensors/mpu6050/pkg.yml
new file mode 100644
index 0000000..61f4e33
--- /dev/null
+++ b/hw/drivers/sensors/mpu6050/pkg.yml
@@ -0,0 +1,33 @@
+#
+# 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/mpu6050
+pkg.description: Driver for the mpu6050 3 axis accel/gyro
+pkg.author: "Julian Ingram"
+pkg.homepage:
+pkg.keywords:
+ - accelerometer
+ - gyroscope
+ - i2c
+ - sensor
+ - invensense
+
+pkg.deps:
+ - "hw/sensor"
+ - "hw/hal"
diff --git a/hw/drivers/sensors/mpu6050/src/mpu6050.c b/hw/drivers/sensors/mpu6050/src/mpu6050.c
new file mode 100644
index 0000000..f6e1357
--- /dev/null
+++ b/hw/drivers/sensors/mpu6050/src/mpu6050.c
@@ -0,0 +1,457 @@
+/*
+ * 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/gyro.h"
+#include "mpu6050/mpu6050.h"
+#include "mpu6050_priv.h"
+
+#if MYNEWT_VAL(MPU6050_LOG)
+#include "log/log.h"
+#endif
+
+#if MYNEWT_VAL(MPU6050_STATS)
+#include "stats/stats.h"
+#endif
+
+#if MYNEWT_VAL(MPU6050_STATS)
+/* Define the stats section and records */
+STATS_SECT_START(mpu6050_stat_section)
+ STATS_SECT_ENTRY(read_errors)
+ STATS_SECT_ENTRY(write_errors)
+STATS_SECT_END
+
+/* Define stat names for querying */
+STATS_NAME_START(mpu6050_stat_section)
+ STATS_NAME(mpu6050_stat_section, read_errors)
+ STATS_NAME(mpu6050_stat_section, write_errors)
+STATS_NAME_END(mpu6050_stat_section)
+
+/* Global variable used to hold stats data */
+STATS_SECT_DECL(mpu6050_stat_section) g_mpu6050stats;
+#endif
+
+#if MYNEWT_VAL(MPU6050_LOG)
+#define LOG_MODULE_MPU6050 (6050)
+#define MPU6050_INFO(...) LOG_INFO(&_log, LOG_MODULE_MPU6050, __VA_ARGS__)
+#define MPU6050_ERR(...) LOG_ERROR(&_log, LOG_MODULE_MPU6050, __VA_ARGS__)
+static struct log _log;
+#else
+#define MPU6050_INFO(...)
+#define MPU6050_ERR(...)
+#endif
+
+/* Exports for the sensor API */
+static int mpu6050_sensor_read(struct sensor *, sensor_type_t,
+ sensor_data_func_t, void *, uint32_t);
+static int mpu6050_sensor_get_config(struct sensor *, sensor_type_t,
+ struct sensor_cfg *);
+
+static const struct sensor_driver g_mpu6050_sensor_driver = {
+ mpu6050_sensor_read,
+ mpu6050_sensor_get_config
+};
+
+/**
+ * Writes a single byte to the specified register
+ *
+ * @param The sensor interface
+ * @param The register address to write to
+ * @param The value to write
+ *
+ * @return 0 on success, non-zero error on failure.
+ */
+int
+mpu6050_write8(struct sensor_itf *itf, uint8_t reg, uint32_t value)
+{
+ int rc;
+ uint8_t payload[2] = { reg, value & 0xFF };
+
+ struct hal_i2c_master_data data_struct = {
+ .address = itf->si_addr,
+ .len = 2,
+ .buffer = payload
+ };
+
+ rc = hal_i2c_master_write(itf->si_num, &data_struct,
+ OS_TICKS_PER_SEC / 10, 1);
+
+ if (rc != 0) {
+ MPU6050_ERR("Failed to write to 0x%02X:0x%02X with value 0x%02X\n",
+ itf->si_addr, reg, value);
+#if MYNEWT_VAL(MPU6050_STATS)
+ STATS_INC(g_mpu6050stats, read_errors);
+#endif
+ }
+
+ return rc;
+}
+
+/**
+ * Reads a single byte from the specified register
+ *
+ * @param The sensor interface
+ * @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
+mpu6050_read8(struct sensor_itf *itf, uint8_t reg, uint8_t *value)
+{
+ int rc;
+
+ struct hal_i2c_master_data data_struct = {
+ .address = itf->si_addr,
+ .len = 1,
+ .buffer = ®
+ };
+
+ /* Register write */
+ rc = hal_i2c_master_write(itf->si_num, &data_struct,
+ OS_TICKS_PER_SEC / 10, 0);
+ if (rc != 0) {
+ MPU6050_ERR("I2C access failed at address 0x%02X\n", itf->si_addr);
+#if MYNEWT_VAL(MPU6050_STATS)
+ STATS_INC(g_mpu6050stats, write_errors);
+#endif
+ return rc;
+ }
+
+ /* Read one byte back */
+ data_struct.buffer = value;
+ rc = hal_i2c_master_read(itf->si_num, &data_struct,
+ OS_TICKS_PER_SEC / 10, 1);
+
+ if (rc != 0) {
+ MPU6050_ERR("Failed to read from 0x%02X:0x%02X\n", itf->si_addr, reg);
+ #if MYNEWT_VAL(MPU6050_STATS)
+ STATS_INC(g_mpu6050stats, read_errors);
+ #endif
+ }
+ return rc;
+}
+
+/**
+ * Reads a six bytes from the specified register
+ *
+ * @param The sensor interface
+ * @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
+mpu6050_read48(struct sensor_itf *itf, uint8_t reg, uint8_t *buffer)
+{
+ int rc;
+
+ struct hal_i2c_master_data data_struct = {
+ .address = itf->si_addr,
+ .len = 1,
+ .buffer = ®
+ };
+
+ /* Register write */
+ rc = hal_i2c_master_write(itf->si_num, &data_struct,
+ OS_TICKS_PER_SEC / 10, 0);
+ if (rc != 0) {
+ MPU6050_ERR("I2C access failed at address 0x%02X\n", itf->si_addr);
+#if MYNEWT_VAL(MPU6050_STATS)
+ STATS_INC(g_mpu6050stats, write_errors);
+#endif
+ return rc;
+ }
+
+ /* Read six bytes back */
+ data_struct.len = 6;
+ data_struct.buffer = buffer;
+ rc = hal_i2c_master_read(itf->si_num, &data_struct,
+ OS_TICKS_PER_SEC / 10, 1);
+
+ if (rc != 0) {
+ MPU6050_ERR("Failed to read from 0x%02X:0x%02X\n", itf->si_addr, reg);
+ #if MYNEWT_VAL(MPU6050_STATS)
+ STATS_INC(g_mpu6050stats, read_errors);
+ #endif
+ }
+ return rc;
+}
+
+/**
+ * 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
+mpu6050_init(struct os_dev *dev, void *arg)
+{
+ struct mpu6050 *mpu;
+ struct sensor *sensor;
+ int rc;
+
+ if (!arg || !dev) {
+ return SYS_ENODEV;
+ }
+
+ mpu = (struct mpu6050 *) dev;
+
+ mpu->cfg.mask = SENSOR_TYPE_ALL;
+
+#if MYNEWT_VAL(MPU6050_LOG)
+ log_register(dev->od_name, &_log, &log_console_handler, NULL, LOG_SYSLEVEL);
+#endif
+
+ sensor = &mpu->sensor;
+
+#if MYNEWT_VAL(MPU6050_STATS)
+ /* Initialise the stats entry */
+ rc = stats_init(
+ STATS_HDR(g_mpu6050stats),
+ STATS_SIZE_INIT_PARMS(g_mpu6050stats, STATS_SIZE_32),
+ STATS_NAME_INIT_PARMS(mpu6050_stat_section));
+ SYSINIT_PANIC_ASSERT(rc == 0);
+ /* Register the entry with the stats registry */
+ rc = stats_register(dev->od_name, STATS_HDR(g_mpu6050stats));
+ SYSINIT_PANIC_ASSERT(rc == 0);
+#endif
+
+ rc = sensor_init(sensor, dev);
+ if (rc != 0) {
+ return rc;
+ }
+
+ /* Add the accelerometer/gyroscope driver */
+ rc = sensor_set_driver(sensor, SENSOR_TYPE_GYROSCOPE |
+ SENSOR_TYPE_ACCELEROMETER,
+ (struct sensor_driver *) &g_mpu6050_sensor_driver);
+ if (rc != 0) {
+ return rc;
+ }
+
+ /* Set the interface */
+ rc = sensor_set_interface(sensor, arg);
+ if (rc != 0) {
+ return rc;
+ }
+
+ rc = sensor_mgr_register(sensor);
+ return rc;
+}
+
+int
+mpu6050_config(struct mpu6050 *mpu, struct mpu6050_cfg *cfg)
+{
+ int rc;
+ struct sensor_itf *itf;
+
+ itf = SENSOR_GET_ITF(&(mpu->sensor));
+
+ /* Power on */
+ rc = mpu6050_write8(itf, MPU6050_PWR_MGMT_1, 1);
+ if (rc != 0) {
+ return rc;
+ }
+
+ uint8_t val;
+ rc = mpu6050_read8(itf, MPU6050_WHO_AM_I, &val);
+ if (rc != 0) {
+ return rc;
+ }
+ if (val != MPU6050_WHO_AM_I_VAL) {
+ return SYS_EIO;
+ }
+
+ /* Set LPF */
+ rc = mpu6050_write8(itf, MPU6050_CONFIG, cfg->lpf_cfg);
+ if (rc != 0) {
+ return rc;
+ }
+
+ mpu->cfg.lpf_cfg = cfg->lpf_cfg;
+
+ /* Set gyro data rate */
+ rc = mpu6050_write8(itf, MPU6050_SMPRT_DIV, cfg->gyro_rate_div);
+ if (rc != 0) {
+ return rc;
+ }
+
+ mpu->cfg.gyro_rate_div = cfg->gyro_rate_div;
+
+ /* Set the gyroscope range */
+ rc = mpu6050_write8(itf, MPU6050_GYRO_CONFIG, cfg->gyro_range);
+ if (rc != 0) {
+ return rc;
+ }
+
+ mpu->cfg.gyro_range = cfg->gyro_range;
+
+ /* Set accelerometer range */
+ rc = mpu6050_write8(itf, MPU6050_ACCEL_CONFIG, cfg->accel_range);
+ if (rc != 0) {
+ return rc;
+ }
+
+ mpu->cfg.accel_range = cfg->accel_range;
+
+ rc = sensor_set_type_mask(&(mpu->sensor), cfg->mask);
+ if (rc != 0) {
+ return rc;
+ }
+
+ mpu->cfg.mask = cfg->mask;
+
+ return 0;
+}
+
+static int
+mpu6050_sensor_read(struct sensor *sensor, sensor_type_t type,
+ sensor_data_func_t data_func, void *data_arg, uint32_t timeout)
+{
+ (void)timeout;
+ int rc;
+ int16_t x, y, z;
+ uint8_t payload[6];
+ float lsb;
+ struct sensor_itf *itf;
+ struct mpu6050 *mpu;
+ union {
+ struct sensor_accel_data sad;
+ struct sensor_gyro_data sgd;
+ } databuf;
+
+ /* If the read isn't looking for accel or gyro, don't do anything. */
+ if (!(type & SENSOR_TYPE_ACCELEROMETER) &&
+ (!(type & SENSOR_TYPE_GYROSCOPE))) {
+ return SYS_EINVAL;
+ }
+
+ itf = SENSOR_GET_ITF(sensor);
+ mpu = (struct mpu6050 *) SENSOR_GET_DEVICE(sensor);
+
+ /* Get a new accelerometer sample */
+ if (type & SENSOR_TYPE_ACCELEROMETER) {
+ rc = mpu6050_read48(itf, MPU6050_ACCEL_XOUT_H, payload);
+ if (rc != 0) {
+ return rc;
+ }
+
+ x = (int16_t)((payload[0] << 8) | payload[1]);
+ y = (int16_t)((payload[2] << 8) | payload[3]);
+ z = (int16_t)((payload[4] << 8) | payload[5]);
+
+ switch (mpu->cfg.gyro_range) {
+ case MPU6050_ACCEL_RANGE_2: /* +/- 2g - 16384 LSB/g */
+ lsb = 16384.0F;
+ break;
+ case MPU6050_ACCEL_RANGE_4: /* +/- 4g - 8192 LSB/g */
+ lsb = 8192.0F;
+ break;
+ case MPU6050_ACCEL_RANGE_8: /* +/- 8g - 4096 LSB/g */
+ lsb = 4096.0F;
+ break;
+ case MPU6050_ACCEL_RANGE_16: /* +/- 16g - 2048 LSB/g */
+ lsb = 2048.0F;
+ break;
+ }
+
+ databuf.sad.sad_x = (x / lsb) * STANDARD_ACCEL_GRAVITY;
+ databuf.sad.sad_x_is_valid = 1;
+ databuf.sad.sad_y = (y / lsb) * STANDARD_ACCEL_GRAVITY;
+ databuf.sad.sad_y_is_valid = 1;
+ databuf.sad.sad_z = (z / lsb) * STANDARD_ACCEL_GRAVITY;
+ databuf.sad.sad_z_is_valid = 1;
+
+ rc = data_func(sensor, data_arg, &databuf.sad,
+ SENSOR_TYPE_ACCELEROMETER);
+ if (rc != 0) {
+ return rc;
+ }
+ }
+
+ /* Get a new gyroscope sample */
+ if (type & SENSOR_TYPE_GYROSCOPE) {
+ rc = mpu6050_read48(itf, MPU6050_GYRO_XOUT_H, payload);
+ if (rc != 0) {
+ return rc;
+ }
+
+ x = (int16_t)((payload[0] << 8) | payload[1]);
+ y = (int16_t)((payload[2] << 8) | payload[3]);
+ z = (int16_t)((payload[4] << 8) | payload[5]);
+
+ switch (mpu->cfg.gyro_range) {
+ case MPU6050_GYRO_RANGE_250: /* +/- 250 Deg/s - 131 LSB/Deg/s */
+ lsb = 131.0F;
+ break;
+ case MPU6050_GYRO_RANGE_500: /* +/- 500 Deg/s - 65.5 LSB/Deg/s */
+ lsb = 65.5F;
+ break;
+ case MPU6050_GYRO_RANGE_1000: /* +/- 1000 Deg/s - 32.8 LSB/Deg/s */
+ lsb = 32.8F;
+ break;
+ case MPU6050_GYRO_RANGE_2000: /* +/- 2000 Deg/s - 16.4 LSB/Deg/s */
+ lsb = 16.4F;
+ break;
+ }
+
+ databuf.sgd.sgd_x = x / lsb;
+ databuf.sgd.sgd_x_is_valid = 1;
+ databuf.sgd.sgd_y = y / lsb;
+ databuf.sgd.sgd_y_is_valid = 1;
+ databuf.sgd.sgd_z = z / lsb;
+ databuf.sgd.sgd_z_is_valid = 1;
+
+ rc = data_func(sensor, data_arg, &databuf.sgd, SENSOR_TYPE_GYROSCOPE);
+ if (rc != 0) {
+ return rc;
+ }
+ }
+
+ return rc;
+}
+
+static int
+mpu6050_sensor_get_config(struct sensor *sensor, sensor_type_t type,
+ struct sensor_cfg *cfg)
+{
+ int rc;
+
+ /* If the read isn't looking for accel or gyro, don't do anything. */
+ if (!(type & SENSOR_TYPE_ACCELEROMETER) &&
+ (!(type & SENSOR_TYPE_GYROSCOPE))) {
+ return SYS_EINVAL;
+ }
+
+ cfg->sc_valtype = SENSOR_VALUE_TYPE_FLOAT_TRIPLET;
+
+ return (rc);
+}
diff --git a/hw/drivers/sensors/mpu6050/src/mpu6050_priv.h b/hw/drivers/sensors/mpu6050/src/mpu6050_priv.h
new file mode 100644
index 0000000..7ca7b04
--- /dev/null
+++ b/hw/drivers/sensors/mpu6050/src/mpu6050_priv.h
@@ -0,0 +1,122 @@
+/*
+ * 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 __MPU6050_PRIV_H__
+#define __MPU6050_PRIV_H__
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+enum mpu6050_registers {
+ MPU6050_SELF_TEST_X = 0x0d, /* RW */
+ MPU6050_SELF_TEST_Y = 0x0e, /* RW */
+ MPU6050_SELF_TEST_Z = 0x0f, /* RW */
+ MPU6050_SELF_TEST_A = 0x10, /* RW */
+ MPU6050_SMPRT_DIV = 0x19, /* RW */
+ MPU6050_CONFIG = 0x1a, /* RW */
+ MPU6050_GYRO_CONFIG = 0x1b, /* RW */
+ MPU6050_ACCEL_CONFIG = 0x1c, /* RW */
+ MPU6050_FIFO_EN = 0x23, /* RW */
+ MPU6050_I2C_MST_CTRL = 0x24, /* RW */
+ MPU6050_I2C_SLV0_ADDR = 0x25, /* RW */
+ MPU6050_I2C_SLV0_REG = 0x26, /* RW */
+ MPU6050_I2C_SLV0_CTRL = 0x27, /* RW */
+ MPU6050_I2C_SLV1_ADDR = 0x28, /* RW */
+ MPU6050_I2C_SLV1_REG = 0x29, /* RW */
+ MPU6050_I2C_SLV1_CTRL = 0x2a, /* RW */
+ MPU6050_I2C_SLV2_ADDR = 0x2b, /* RW */
+ MPU6050_I2C_SLV2_REG = 0x2c, /* RW */
+ MPU6050_I2C_SLV2_CTRL = 0x2d, /* RW */
+ MPU6050_I2C_SLV3_ADDR = 0x2e, /* RW */
+ MPU6050_I2C_SLV3_REG = 0x2f, /* RW */
+ MPU6050_I2C_SLV3_CTRL = 0x30, /* RW */
+ MPU6050_I2C_SLV4_ADDR = 0x31, /* RW */
+ MPU6050_I2C_SLV4_REG = 0x32, /* RW */
+ MPU6050_I2C_SLV4_DO = 0x33, /* RW */
+ MPU6050_I2C_SLV4_CTRL = 0x34, /* RW */
+ MPU6050_I2C_SLV4_DI = 0x35, /* RW */
+ MPU6050_I2C_MST_STATUS = 0x36, /* RW */
+ MPU6050_INT_PIN_CFG = 0x37, /* RW */
+ MPU6050_INT_ENABLE = 0x38, /* RW */
+ MPU6050_INT_STATUS = 0x3a, /* RW */
+ MPU6050_ACCEL_XOUT_H = 0x3b, /* RW */
+ MPU6050_ACCEL_XOUT_L = 0x3c, /* RW */
+ MPU6050_ACCEL_YOUT_H = 0x3d, /* RW */
+ MPU6050_ACCEL_YOUT_L = 0x3e, /* RW */
+ MPU6050_ACCEL_ZOUT_H = 0x3f, /* RW */
+ MPU6050_ACCEL_ZOUT_L = 0x40, /* RW */
+ MPU6050_TEMP_OUT_H = 0x41, /* RW */
+ MPU6050_TEMP_OUT_L = 0x42, /* RW */
+ MPU6050_GYRO_XOUT_H = 0x43, /* RW */
+ MPU6050_GYRO_XOUT_L = 0x44, /* RW */
+ MPU6050_GYRO_YOUT_H = 0x45, /* RW */
+ MPU6050_GYRO_YOUT_L = 0x46, /* RW */
+ MPU6050_GYRO_ZOUT_H = 0x47, /* RW */
+ MPU6050_GYRO_ZOUT_L = 0x48, /* RW */
+ MPU6050_EXT_SENS_DATA_00 = 0x49, /* RW */
+ MPU6050_EXT_SENS_DATA_01 = 0x4a, /* RW */
+ MPU6050_EXT_SENS_DATA_02 = 0x4b, /* RW */
+ MPU6050_EXT_SENS_DATA_03 = 0x4c, /* RW */
+ MPU6050_EXT_SENS_DATA_04 = 0x4d, /* RW */
+ MPU6050_EXT_SENS_DATA_05 = 0x4e, /* RW */
+ MPU6050_EXT_SENS_DATA_06 = 0x4f, /* RW */
+ MPU6050_EXT_SENS_DATA_07 = 0x50, /* RW */
+ MPU6050_EXT_SENS_DATA_08 = 0x51, /* RW */
+ MPU6050_EXT_SENS_DATA_09 = 0x52, /* RW */
+ MPU6050_EXT_SENS_DATA_10 = 0x53, /* RW */
+ MPU6050_EXT_SENS_DATA_11 = 0x54, /* RW */
+ MPU6050_EXT_SENS_DATA_12 = 0x55, /* RW */
+ MPU6050_EXT_SENS_DATA_13 = 0x56, /* RW */
+ MPU6050_EXT_SENS_DATA_14 = 0x57, /* RW */
+ MPU6050_EXT_SENS_DATA_15 = 0x58, /* RW */
+ MPU6050_EXT_SENS_DATA_16 = 0x59, /* RW */
+ MPU6050_EXT_SENS_DATA_17 = 0x5a, /* RW */
+ MPU6050_EXT_SENS_DATA_18 = 0x5b, /* RW */
+ MPU6050_EXT_SENS_DATA_19 = 0x5c, /* RW */
+ MPU6050_EXT_SENS_DATA_20 = 0x5d, /* RW */
+ MPU6050_EXT_SENS_DATA_21 = 0x5e, /* RW */
+ MPU6050_EXT_SENS_DATA_22 = 0x5f, /* RW */
+ MPU6050_EXT_SENS_DATA_23 = 0x60, /* RW */
+ MPU6050_I2C_SLV0_DO = 0x63, /* RW */
+ MPU6050_I2C_SLV1_DO = 0x64, /* RW */
+ MPU6050_I2C_SLV2_DO = 0x65, /* RW */
+ MPU6050_I2C_SLV3_DO = 0x66, /* RW */
+ MPU6050_I2C_MST_DELAY_CT_RL = 0x67, /* RW */
+ MPU6050_SIGNAL_PATH_RES_ET = 0x68, /* RW */
+ MPU6050_USER_CTRL = 0x6a, /* RW */
+ MPU6050_PWR_MGMT_1 = 0x6B, /* RW */
+ MPU6050_PWR_MGMT_2 = 0x6C, /* RW */
+ MPU6050_FIFO_COUNT_H = 0x72, /* RW */
+ MPU6050_FIFO_COUNT_L = 0x73, /* RW */
+ MPU6050_FIFO_R_W = 0x74, /* RW */
+ MPU6050_WHO_AM_I = 0x75 /* RW */
+};
+
+#define MPU6050_WHO_AM_I_VAL (0x68)
+
+int mpu6050_write8(struct sensor_itf *itf, uint8_t reg, uint32_t value);
+int mpu6050_read8(struct sensor_itf *itf, uint8_t reg, uint8_t *value);
+int mpu6050_read48(struct sensor_itf *itf, uint8_t reg, uint8_t *buffer);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __MPU6050_PRIV_H__ */
diff --git a/hw/drivers/sensors/mpu6050/syscfg.yml b/hw/drivers/sensors/mpu6050/syscfg.yml
new file mode 100644
index 0000000..f142abb
--- /dev/null
+++ b/hw/drivers/sensors/mpu6050/syscfg.yml
@@ -0,0 +1,26 @@
+#
+# 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:
+ MPU6050_LOG:
+ description: 'Enable MPU6050 logging'
+ value: 0
+ MPU6050_STATS:
+ description: 'Enable MPU6050 statistics'
+ value: 0
diff --git a/hw/sensor/creator/pkg.yml b/hw/sensor/creator/pkg.yml
index 9954efc..a385b65 100644
--- a/hw/sensor/creator/pkg.yml
+++ b/hw/sensor/creator/pkg.yml
@@ -28,6 +28,8 @@
- hw/drivers/sensors/bme280
pkg.deps.LSM303DLHC_OFB:
- hw/drivers/sensors/lsm303dlhc
+pkg.deps.MPU6050_OFB:
+ - hw/drivers/sensors/mpu6050
pkg.deps.TSL2561_OFB:
- hw/drivers/sensors/tsl2561
pkg.deps.BNO055_OFB:
diff --git a/hw/sensor/creator/src/sensor_creator.c b/hw/sensor/creator/src/sensor_creator.c
index 3d24d9f..b6ec1d5 100644
--- a/hw/sensor/creator/src/sensor_creator.c
+++ b/hw/sensor/creator/src/sensor_creator.c
@@ -24,6 +24,9 @@
#if MYNEWT_VAL(LSM303DLHC_OFB)
#include <lsm303dlhc/lsm303dlhc.h>
#endif
+#if MYNEWT_VAL(MPU6050_OFB)
+#include <mpu6050/mpu6050.h>
+#endif
#if MYNEWT_VAL(BNO055_OFB)
#include <bno055/bno055.h>
#endif
@@ -42,6 +45,10 @@
static struct lsm303dlhc lsm303dlhc;
#endif
+#if MYNEWT_VAL(MPU6050_OFB)
+static struct mpu6050 mpu6050;
+#endif
+
#if MYNEWT_VAL(BNO055_OFB)
static struct bno055 bno055;
#endif
@@ -93,6 +100,14 @@
};
#endif
+#if MYNEWT_VAL(I2C_0) && MYNEWT_VAL(MPU6050_OFB)
+static struct sensor_itf i2c_0_itf_mpu = {
+ .si_type = SENSOR_ITF_I2C,
+ .si_num = 0,
+ .si_addr = MPU6050_I2C_ADDR
+};
+#endif
+
#if MYNEWT_VAL(I2C_0) && MYNEWT_VAL(BNO055_OFB)
static struct sensor_itf i2c_0_itf_bno = {
.si_type = SENSOR_ITF_I2C,
@@ -252,6 +267,38 @@
#endif
/**
+ * MPU6050 Sensor default configuration used by the creator package
+ *
+ * @return 0 on success, non-zero on failure
+ */
+#if MYNEWT_VAL(MPU6050_OFB)
+static int
+config_mpu6050_sensor(void)
+{
+ int rc;
+ struct os_dev *dev;
+ struct mpu6050_cfg mpucfg;
+
+ dev = (struct os_dev *) os_dev_open("mpu6050_0", OS_TIMEOUT_NEVER, NULL);
+ assert(dev != NULL);
+
+ mpucfg.accel_range = MPU6050_ACCEL_RANGE_4;
+ mpucfg.gyro_range = MPU6050_GYRO_RANGE_500;
+ mpucfg.gyro_rate_div = 7; /* Sample Rate = Gyroscope Output Rate /
+ (1 + gyro_rate_div */
+ mpucfg.lpf_cfg = 0; /* See data sheet */
+
+ mpucfg.mask = SENSOR_TYPE_ACCELEROMETER|
+ SENSOR_TYPE_GYROSCOPE;
+
+ rc = mpu6050_config((struct mpu6050 *) dev, &mpucfg);
+
+ os_dev_close(dev);
+ return rc;
+}
+#endif
+
+/**
* BNO055 Sensor default configuration used by the creator package
*
* @return 0 on success, non-zero on failure
@@ -313,6 +360,15 @@
assert(rc == 0);
#endif
+#if MYNEWT_VAL(MPU6050_OFB)
+ rc = os_dev_create((struct os_dev *) &mpu6050, "mpu6050_0",
+ OS_DEV_INIT_PRIMARY, 0, mpu6050_init, (void *)&i2c_0_itf_mpu);
+ assert(rc == 0);
+
+ rc = config_mpu6050_sensor();
+ assert(rc == 0);
+#endif
+
#if MYNEWT_VAL(BNO055_OFB)
rc = os_dev_create((struct os_dev *) &bno055, "bno055_0",
OS_DEV_INIT_PRIMARY, 0, bno055_init, (void *)&i2c_0_itf_bno);
diff --git a/hw/sensor/creator/syscfg.yml b/hw/sensor/creator/syscfg.yml
index 69f40ec..4fb80e3 100644
--- a/hw/sensor/creator/syscfg.yml
+++ b/hw/sensor/creator/syscfg.yml
@@ -25,6 +25,9 @@
LSM303DLHC_OFB:
description: 'LSM303 is present'
value : 0
+ MPU6050_OFB:
+ description: 'MPU6050 is present'
+ value : 0
BNO055_OFB:
description: 'BNO055 is present'
value : 0
diff --git a/mgmt/imgmgr/src/imgmgr.c b/mgmt/imgmgr/src/imgmgr.c
index 17d21c6..d59aa9c 100644
--- a/mgmt/imgmgr/src/imgmgr.c
+++ b/mgmt/imgmgr/src/imgmgr.c
@@ -81,6 +81,38 @@
struct imgr_state imgr_state;
+#if MYNEWT_VAL(BOOTUTIL_IMAGE_FORMAT_V2)
+static int
+imgr_img_tlvs(const struct flash_area *fa, struct image_header *hdr,
+ uint32_t *start_off, uint32_t *end_off)
+{
+ struct image_tlv_info tlv_info;
+ int rc;
+
+ rc = flash_area_read(fa, *start_off, &tlv_info, sizeof(tlv_info));
+ if (rc) {
+ rc = -1;
+ goto end;
+ }
+ if (tlv_info.it_magic != IMAGE_TLV_INFO_MAGIC) {
+ rc = 1;
+ goto end;
+ }
+ *start_off += sizeof(tlv_info);
+ *end_off = *start_off + tlv_info.it_tlv_tot;
+ rc = 0;
+end:
+ return rc;
+}
+#else
+static int
+imgr_img_tlvs(const struct flash_area *fa, struct image_header *hdr,
+ uint32_t *start_off, uint32_t *end_off)
+{
+ *end_off = *start_off + hdr->ih_tlv_size;
+ return 0;
+}
+#endif
/*
* Read version and build hash from image located slot "image_slot". Note:
* this is a slot index, not a flash area ID.
@@ -133,14 +165,19 @@
}
}
- if(flags) {
+ if (flags) {
*flags = hdr->ih_flags;
}
+
/*
* Build ID is in a TLV after the image.
*/
data_off = hdr->ih_hdr_size + hdr->ih_img_size;
- data_end = data_off + hdr->ih_tlv_size;
+
+ rc = imgr_img_tlvs(fa, hdr, &data_off, &data_end);
+ if (rc) {
+ goto end;
+ }
if (data_end > fa->fa_size) {
rc = 1;
@@ -387,9 +424,6 @@
if (rc) {
return MGMT_ERR_EINVAL;
}
- if (IMAGE_SIZE(hdr) > imgr_state.upload.fa->fa_size) {
- return MGMT_ERR_EINVAL;
- }
rc = flash_area_is_empty(imgr_state.upload.fa, &empty);
if (rc) {
diff --git a/net/nimble/controller/include/controller/ble_ll.h b/net/nimble/controller/include/controller/ble_ll.h
index f219dce..279a3a0 100644
--- a/net/nimble/controller/include/controller/ble_ll.h
+++ b/net/nimble/controller/include/controller/ble_ll.h
@@ -173,6 +173,7 @@
STATS_SECT_ENTRY(aux_scan_rsp_err)
STATS_SECT_ENTRY(aux_chain_cnt)
STATS_SECT_ENTRY(aux_chain_err)
+ STATS_SECT_ENTRY(adv_evt_dropped)
STATS_SECT_END
extern STATS_SECT_DECL(ble_ll_stats) ble_ll_stats;
diff --git a/net/nimble/controller/src/ble_ll.c b/net/nimble/controller/src/ble_ll.c
index 28af4a6..8f2b8e5 100644
--- a/net/nimble/controller/src/ble_ll.c
+++ b/net/nimble/controller/src/ble_ll.c
@@ -193,6 +193,7 @@
STATS_NAME(ble_ll_stats, aux_scan_rsp_err)
STATS_NAME(ble_ll_stats, aux_chain_cnt)
STATS_NAME(ble_ll_stats, aux_chain_err)
+ STATS_NAME(ble_ll_stats, adv_evt_dropped)
STATS_NAME_END(ble_ll_stats)
static void ble_ll_event_rx_pkt(struct os_event *ev);
diff --git a/net/nimble/controller/src/ble_ll_scan.c b/net/nimble/controller/src/ble_ll_scan.c
index 0b852fa..069fc55 100644
--- a/net/nimble/controller/src/ble_ll_scan.c
+++ b/net/nimble/controller/src/ble_ll_scan.c
@@ -582,6 +582,12 @@
return -1;
}
+ /* Drop packet if it does not fit into event buffer */
+ if ((sizeof(*evt) + adv_data_len) + 1 > MYNEWT_VAL(BLE_HCI_EVT_BUF_SIZE)) {
+ STATS_INC(ble_ll_stats, adv_evt_dropped);
+ return -1;
+ }
+
evt = ble_ll_scan_init_ext_adv();
if (!evt) {
return 0;
@@ -644,6 +650,12 @@
return -1;
}
+ /* Drop packet if it does not fit into event buffer */
+ if (event_len + 1 > MYNEWT_VAL(BLE_HCI_EVT_BUF_SIZE)) {
+ STATS_INC(ble_ll_stats, adv_evt_dropped);
+ return -1;
+ }
+
evbuf = ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_LO);
if (!evbuf) {
return -1;
@@ -1741,6 +1753,14 @@
if ((pdu_len - i - 1 > 0)) {
out_evt->adv_data_len = pdu_len - i - 1;
+
+ /* XXX Drop packet if it does not fit into event buffer for now.*/
+ if ((sizeof(*out_evt) + out_evt->adv_data_len) + 1 >
+ MYNEWT_VAL(BLE_HCI_EVT_BUF_SIZE)) {
+ STATS_INC(ble_ll_stats, adv_evt_dropped);
+ return -1;
+ }
+
os_mbuf_copydata(om, 0, out_evt->adv_data_len, out_evt->adv_data);
}
