i changed the code to send environment data over ble_nus service and it was worked
so i tried to send motion sensor data over ble_nus service and i changed code but no data is being transmitted.
I think code has no problem and build was success.
/*
Copyright (c) 2010 - 2017, Nordic Semiconductor ASA
All rights reserved.
Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:
1. Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.
2. Redistributions in binary form, except as embedded into a Nordic
Semiconductor ASA integrated circuit in a product or a software update for
such product, must reproduce the above copyright notice, this list of
conditions and the following disclaimer in the documentation and/or other
materials provided with the distribution.
3. Neither the name of Nordic Semiconductor ASA nor the names of its
contributors may be used to endorse or promote products derived from this
software without specific prior written permission.
4. This software, with or without modification, must only be used with a
Nordic Semiconductor ASA integrated circuit.
5. Any software provided in binary form under this license must not be reverse
engineered, decompiled, modified and/or disassembled.
THIS SOFTWARE IS PROVIDED BY NORDIC SEMICONDUCTOR ASA "AS IS" AND ANY EXPRESS
OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
OF MERCHANTABILITY, NONINFRINGEMENT, AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL NORDIC SEMICONDUCTOR ASA OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "m_motion.h"
#include "m_motion_flash.h"
//#include "ble_tms.h"
#include "drv_motion.h"
#include "sdk_errors.h"
#include "pca20020.h"
#define NRF_LOG_MODULE_NAME "m_motion "
#include "ble_nus.h"
#include "nrf_log.h"
#include "macros_common.h"
#define RAW_PARAM_NUM 9 // Number of raw parameters (3 * acc + 3 * gyro + 3 * compass).
#define RAW_Q_FORMAT_ACC_INTEGER_BITS 6 // Number of bits used for integer part of raw data.
#define RAW_Q_FORMAT_GYR_INTEGER_BITS 11 // Number of bits used for integer part of raw data.
#define RAW_Q_FORMAT_CMP_INTEGER_BITS 12 // Number of bits used for integer part of raw data.
static ble_nus_t m_nus;
static ble_tms_t m_tms;
static ble_tms_config_t * m_config;
static const ble_tms_config_t m_default_config = MOTION_DEFAULT_CONFIG;
/**@brief Function for applying the configuration.
*/
static uint32_t m_motion_configuration_apply(ble_tms_config_t * p_config)
{
uint32_t err_code;
drv_motion_cfg_t motion_cfg;
NULL_PARAM_CHECK(p_config);
motion_cfg.pedo_interval_ms = p_config->pedo_interval_ms;
motion_cfg.temp_interval_ms = p_config->temp_interval_ms;
motion_cfg.compass_interval_ms = p_config->compass_interval_ms;
motion_cfg.motion_freq_hz = p_config->motion_freq_hz;
motion_cfg.wake_on_motion = p_config->wake_on_motion;
err_code = drv_motion_config(&motion_cfg);
APP_ERROR_CHECK(err_code);
return NRF_SUCCESS;
}
static void nus_data_handler(ble_nus_t * p_nus,
//ble_tms_evt_type_t evt_type,
uint8_t * p_data,
uint16_t length)
{
uint32_t err_code;
/*
p_tms->is_tap_notif_enabled = false;
p_tms->is_orientation_notif_enabled = false;
p_tms->is_quat_notif_enabled = false;
p_tms->is_pedo_notif_enabled = false;
p_tms->is_raw_notif_enabled = true;
p_tms->is_euler_notif_enabled = false;
p_tms->is_rot_mat_notif_enabled = false;
p_tms->is_heading_notif_enabled = false;
p_tms->is_gravity_notif_enabled = false;
*/
NRF_LOG_INFO("ble_tms_evt_handler: BLE_TMS_EVT_NOTIF_RAW - %d\r\n", p_nus->is_notification_enabled);
if (p_nus->is_notification_enabled)
{
err_code = drv_motion_enable(DRV_MOTION_FEATURE_MASK_RAW);
APP_ERROR_CHECK(err_code);
}
else
{
err_code = drv_motion_disable(DRV_MOTION_FEATURE_MASK_RAW);
APP_ERROR_CHECK(err_code);
}
#if 0
switch (evt_type)
{
case BLE_TMS_EVT_NOTIF_TAP:
NRF_LOG_INFO("ble_tms_evt_handler: BLE_TMS_EVT_NOTIF_TAP - %d\r\n", p_tms->is_notification_enabled);
if (p_tms->is_notification_enabled)
{
err_code = drv_motion_enable(DRV_MOTION_FEATURE_MASK_TAP);
APP_ERROR_CHECK(err_code);
}
else
{
err_code = drv_motion_disable(DRV_MOTION_FEATURE_MASK_TAP);
APP_ERROR_CHECK(err_code);
}
break;
case BLE_TMS_EVT_NOTIF_ORIENTATION:
NRF_LOG_INFO("ble_tms_evt_handler: BLE_TMS_EVT_NOTIF_ORIENTATION - %d\r\n", p_tms->is_notification_enabled);
if (p_tms->is_notification_enabled)
{
err_code = drv_motion_enable(DRV_MOTION_FEATURE_MASK_ORIENTATION);
APP_ERROR_CHECK(err_code);
}
else
{
err_code = drv_motion_disable(DRV_MOTION_FEATURE_MASK_ORIENTATION);
APP_ERROR_CHECK(err_code);
}
break;
case BLE_TMS_EVT_NOTIF_QUAT:
NRF_LOG_INFO("ble_tms_evt_handler: BLE_TMS_EVT_NOTIF_QUAT - %d\r\n", p_tms->is_notification_enabled);
if (p_tms->is_notification_enabled)
{
err_code = drv_motion_enable(DRV_MOTION_FEATURE_MASK_QUAT);
APP_ERROR_CHECK(err_code);
}
else
{
err_code = drv_motion_disable(DRV_MOTION_FEATURE_MASK_QUAT);
APP_ERROR_CHECK(err_code);
}
break;
case BLE_TMS_EVT_NOTIF_PEDOMETER:
NRF_LOG_INFO("ble_tms_evt_handler: BLE_TMS_EVT_NOTIF_PEDOMETER - %d\r\n", p_tms->is_notification_enabled);
if (p_tms->is_notification_enabled)
{
err_code = drv_motion_enable(DRV_MOTION_FEATURE_MASK_PEDOMETER);
APP_ERROR_CHECK(err_code);
}
else
{
err_code = drv_motion_disable(DRV_MOTION_FEATURE_MASK_PEDOMETER);
APP_ERROR_CHECK(err_code);
}
break;
case BLE_TMS_EVT_NOTIF_RAW:
NRF_LOG_INFO("ble_tms_evt_handler: BLE_TMS_EVT_NOTIF_RAW - %d\r\n", p_tms->is_notification_enabled);
if (p_tms->is_notification_enabled)
{
err_code = drv_motion_enable(DRV_MOTION_FEATURE_MASK_RAW);
APP_ERROR_CHECK(err_code);
}
else
{
err_code = drv_motion_disable(DRV_MOTION_FEATURE_MASK_RAW);
APP_ERROR_CHECK(err_code);
}
break;
case BLE_TMS_EVT_NOTIF_EULER:
NRF_LOG_INFO("ble_tms_evt_handler: BLE_TMS_EVT_NOTIF_EULER - %d\r\n", p_tms->is_notification_enabled);
if (p_tms->is_notification_enabled)
{
err_code = drv_motion_enable(DRV_MOTION_FEATURE_MASK_EULER);
APP_ERROR_CHECK(err_code);
}
else
{
err_code = drv_motion_disable(DRV_MOTION_FEATURE_MASK_EULER);
APP_ERROR_CHECK(err_code);
}
break;
case BLE_TMS_EVT_NOTIF_ROT_MAT:
NRF_LOG_INFO("ble_tms_evt_handler: BLE_TMS_EVT_NOTIF_ROT_MAT - %d\r\n", p_tms->is_notification_enabled);
if (p_tms->is_notification_enabled)
{
err_code = drv_motion_enable(DRV_MOTION_FEATURE_MASK_ROT_MAT);
APP_ERROR_CHECK(err_code);
}
else
{
err_code = drv_motion_disable(DRV_MOTION_FEATURE_MASK_ROT_MAT);
APP_ERROR_CHECK(err_code);
}
break;
case BLE_TMS_EVT_NOTIF_HEADING:
NRF_LOG_INFO("ble_tms_evt_handler: BLE_TMS_EVT_NOTIF_HEADING - %d\r\n", p_tms->is_notification_enabled);
if (p_tms->is_notification_enabled)
{
err_code = drv_motion_enable(DRV_MOTION_FEATURE_MASK_HEADING);
APP_ERROR_CHECK(err_code);
}
else
{
err_code = drv_motion_disable(DRV_MOTION_FEATURE_MASK_HEADING);
APP_ERROR_CHECK(err_code);
}
break;
case BLE_TMS_EVT_NOTIF_GRAVITY:
NRF_LOG_INFO("ble_tms_evt_handler: BLE_TMS_EVT_NOTIF_GRAVITY - %d\r\n", p_tms->is_notification_enabled);
if (p_tms->is_notification_enabled)
{
err_code = drv_motion_enable(DRV_MOTION_FEATURE_MASK_GRAVITY_VECTOR);
APP_ERROR_CHECK(err_code);
}
else
{
err_code = drv_motion_disable(DRV_MOTION_FEATURE_MASK_GRAVITY_VECTOR);
APP_ERROR_CHECK(err_code);
}
break;
case BLE_TMS_EVT_CONFIG_RECEIVED:
NRF_LOG_INFO("ble_tms_evt_handler: BLE_TMS_EVT_CONFIG_RECEIVED - %d\r\n", length);
APP_ERROR_CHECK_BOOL(length == sizeof(ble_tms_config_t));
err_code = m_motion_flash_config_store((ble_tms_config_t *)p_data);
APP_ERROR_CHECK(err_code);
err_code = m_motion_configuration_apply((ble_tms_config_t *)p_data);
APP_ERROR_CHECK(err_code);
break;
default:
break;
}
#endif
}
/**@brief Function for handling the data from the Nordic UART Service.
*
* @details This function will process the data received from the Nordic UART BLE Service and send
* it to the UART module.
*
* @param[in] p_nus Nordic UART Service structure.
* @param[in] p_data Data to be send to UART module.
* @param[in] length Length of the data.
*/
/**@snippet [Handling the data received over BLE] */
/*
static void nus_data_handler(ble_nus_t * p_nus, uint8_t * p_data, uint16_t length)
{ //change ble_tms_evt_handler function
uint32_t err_code;
NRF_LOG_DEBUG("Received data from BLE NUS. Writing data on UART.\r\n");
NRF_LOG_HEXDUMP_DEBUG(p_data, length);
}
*/
/**@brief Function for initializing the Thingy Motion Service.
*
* @details This callback function will be called from the ble handling module to initialize the Weather Station service.
*
* @retval NRF_SUCCESS If initialization was successful.
*/
static uint32_t motion_service_init(bool major_minor_fw_ver_changed)
{
uint32_t err_code;
ble_nus_init_t nus_init;
//ble_tms_init_t tms_init;
/**@brief Load configuration from flash. */
err_code = m_motion_flash_init(&m_default_config, &m_config);
RETURN_IF_ERROR(err_code);
if (major_minor_fw_ver_changed)
{
err_code = m_motion_flash_config_store(&m_default_config);
APP_ERROR_CHECK(err_code);
}
err_code = m_motion_configuration_apply(m_config);
RETURN_IF_ERROR(err_code);
#if 0
memset(&tms_init, 0, sizeof(tms_init));
tms_init.p_init_config = m_config;
tms_init.evt_handler = ble_tms_evt_handler;
NRF_LOG_INFO("motion_service_init: ble_tms_init \r\n");
err_code = ble_tms_init(&m_tms, &tms_init);
#endif
memset(&nus_init, 0, sizeof(nus_init));
nus_init.data_handler = nus_data_handler;
err_code = ble_nus_init(&m_nus, &nus_init);
APP_ERROR_CHECK(err_code);
#if 0
if (err_code != NRF_SUCCESS)
{
NRF_LOG_ERROR("FAILED - %d\r\n", err_code);
return err_code;
}
#endif
return NRF_SUCCESS;
}
/**@brief Function for passing the BLE event to the Thingy Motion Service.
*
* @details This callback function will be called from the BLE handling module.
*
* @param[in] p_ble_evt Pointer to the BLE event.
*/
static void motion_on_ble_evt(ble_evt_t * p_ble_evt)
{
uint32_t err_code;
//ble_tms_on_ble_evt(&m_tms, p_ble_evt);
ble_nus_on_ble_evt(&m_nus, p_ble_evt);
if (p_ble_evt->header.evt_id == BLE_GAP_EVT_DISCONNECTED)
{
err_code = drv_motion_disable(DRV_MOTION_FEATURE_MASK);
APP_ERROR_CHECK(err_code);
}
else
{
err_code = drv_motion_enable(DRV_MOTION_FEATURE_MASK);
APP_ERROR_CHECK(err_code);
}
}
static void drv_motion_evt_handler(drv_motion_evt_t const * p_evt, void * p_data, uint32_t size)
{
switch (*p_evt)
{
case DRV_MOTION_EVT_RAW:
{
APP_ERROR_CHECK_BOOL(size == sizeof(int32_t) * RAW_PARAM_NUM);
ble_tms_raw_t data;
uint8_t raw_acc_data[7];
uint8_t raw_gyro_data[7];
uint8_t raw_compass_data[7];
uint32_t err_code;
int32_t * p_raw = (int32_t *)p_data;
/* p_raw is in 16Q16 format. This is compressed for BLE transfer. */
// Set upper and lower overflow limits.
static const int16_t overflow_limit_upper[RAW_PARAM_NUM] = {
(1 << (RAW_Q_FORMAT_ACC_INTEGER_BITS - 1)) - 1,
(1 << (RAW_Q_FORMAT_ACC_INTEGER_BITS - 1)) - 1,
(1 << (RAW_Q_FORMAT_ACC_INTEGER_BITS - 1)) - 1,
(1 << (RAW_Q_FORMAT_GYR_INTEGER_BITS - 1)) - 1,
(1 << (RAW_Q_FORMAT_GYR_INTEGER_BITS - 1)) - 1,
(1 << (RAW_Q_FORMAT_GYR_INTEGER_BITS - 1)) - 1,
(1 << (RAW_Q_FORMAT_CMP_INTEGER_BITS - 1)) - 1,
(1 << (RAW_Q_FORMAT_CMP_INTEGER_BITS - 1)) - 1,
(1 << (RAW_Q_FORMAT_CMP_INTEGER_BITS - 1)) - 1};
static const int16_t overflow_limit_lower[RAW_PARAM_NUM] = {
-(1 << (RAW_Q_FORMAT_ACC_INTEGER_BITS - 1)),
-(1 << (RAW_Q_FORMAT_ACC_INTEGER_BITS - 1)),
-(1 << (RAW_Q_FORMAT_ACC_INTEGER_BITS - 1)),
-(1 << (RAW_Q_FORMAT_GYR_INTEGER_BITS - 1)),
-(1 << (RAW_Q_FORMAT_GYR_INTEGER_BITS - 1)),
-(1 << (RAW_Q_FORMAT_GYR_INTEGER_BITS - 1)),
-(1 << (RAW_Q_FORMAT_CMP_INTEGER_BITS - 1)),
-(1 << (RAW_Q_FORMAT_CMP_INTEGER_BITS - 1)),
-(1 << (RAW_Q_FORMAT_CMP_INTEGER_BITS - 1))};
int16_t overflow_check;
for (uint8_t i = 0; i < RAW_PARAM_NUM; i++)
{
overflow_check = p_raw[i] >> 16; // Right shift 16 to remove decimal part.
if (overflow_check >= overflow_limit_upper[i])
{
NRF_LOG_WARNING("p_raw[%d] over limit. Val: %d limit: %d \r\n", i, overflow_check, overflow_limit_upper[i]);
p_raw[i] = overflow_limit_upper[i] << 16;
}
else if (overflow_check < overflow_limit_lower[i])
{
NRF_LOG_WARNING("p_raw[%d] below limit. Val: %d limit: %d \r\n", i, overflow_check, overflow_limit_lower[i]);
p_raw[i] = overflow_limit_lower[i] << 16;
}
else
{
// No overflow has occured.
}
}
data.accel.x = (int16_t)(p_raw[0] >> RAW_Q_FORMAT_ACC_INTEGER_BITS);
data.accel.y = (int16_t)(p_raw[1] >> RAW_Q_FORMAT_ACC_INTEGER_BITS);
data.accel.z = (int16_t)(p_raw[2] >> RAW_Q_FORMAT_ACC_INTEGER_BITS);
data.gyro.x = (int16_t)(p_raw[3] >> RAW_Q_FORMAT_GYR_INTEGER_BITS);
data.gyro.y = (int16_t)(p_raw[4] >> RAW_Q_FORMAT_GYR_INTEGER_BITS);
data.gyro.z = (int16_t)(p_raw[5] >> RAW_Q_FORMAT_GYR_INTEGER_BITS);
data.compass.y = -(int16_t)(p_raw[6] >> RAW_Q_FORMAT_CMP_INTEGER_BITS); // Changed axes and inverted. Corrected for rotation of axes.
data.compass.x = (int16_t)(p_raw[7] >> RAW_Q_FORMAT_CMP_INTEGER_BITS); // Changed axes. Corrected for rotation of axes.
data.compass.z = (int16_t)(p_raw[8] >> RAW_Q_FORMAT_CMP_INTEGER_BITS);
#if NRF_LOG_ENABLED
NRF_LOG_DEBUG("DRV_MOTION_EVT_RAW:\r\n");
double f_buf;
char buffer[8];
f_buf = (double)p_raw[0];
f_buf = f_buf/(1<<16);
sprintf(buffer, "%.2f", f_buf);
NRF_LOG_DEBUG(" accel.x [G's] = %s:\r\n", nrf_log_push(buffer));
f_buf = (double)p_raw[1];
f_buf = f_buf/(1<<16);
sprintf(buffer, "%.2f", f_buf);
NRF_LOG_DEBUG(" accel.y [G's] = %s:\r\n", nrf_log_push(buffer));
f_buf = (double)p_raw[2];
f_buf = f_buf/(1<<16);
sprintf(buffer, "%.2f", f_buf);
NRF_LOG_DEBUG(" accel.z [G's] = %s:\r\n", nrf_log_push(buffer));
f_buf = (double)p_raw[3];
f_buf = f_buf/(1<<16);
sprintf(buffer, "%.2f", f_buf);
NRF_LOG_DEBUG(" gyro.x [deg/s] = %s:\r\n", nrf_log_push(buffer));
f_buf = (double)p_raw[4];
f_buf = f_buf/(1<<16);
sprintf(buffer, "%.2f", f_buf);
NRF_LOG_DEBUG(" gyro.y [deg/s] = %s:\r\n", nrf_log_push(buffer));
f_buf = (double)p_raw[5];
f_buf = f_buf/(1<<16);
sprintf(buffer, "%.2f", f_buf);
NRF_LOG_DEBUG(" gyro.z [deg/s] = %s:\r\n", nrf_log_push(buffer));
f_buf = (double)p_raw[7]; // Changed axes. Corrected for rotation of axes.
f_buf = f_buf/(1<<16);
sprintf(buffer, "%.2f", f_buf);
NRF_LOG_DEBUG(" mag.x [uT] = %s:\r\n", nrf_log_push(buffer));
f_buf = -(double)p_raw[6]; // Changed axes and inverted. Corrected for rotation of axes.
f_buf = f_buf/(1<<16);
sprintf(buffer, "%.2f", f_buf);
NRF_LOG_DEBUG(" mag.y [uT] = %s:\r\n", nrf_log_push(buffer));
f_buf = (double)p_raw[8];
f_buf = f_buf/(1<<16);
sprintf(buffer, "%.2f", f_buf);
NRF_LOG_DEBUG(" mag.z [uT] = %s:\r\n", nrf_log_push(buffer));
#endif
NRF_LOG_INFO("drv_motion_evt_handler raw x:, %d, - y:, %d, - z:, %d, \r\n", (int16_t)(p_raw[0] >> RAW_Q_FORMAT_ACC_INTEGER_BITS),
(int16_t)(p_raw[1] >> RAW_Q_FORMAT_ACC_INTEGER_BITS),
(int16_t)(p_raw[2] >> RAW_Q_FORMAT_ACC_INTEGER_BITS));
raw_acc_data[0] = 0xF4;
raw_acc_data[1] = 0xFF & (data.accel.x >> 8);
raw_acc_data[2] = 0xFF & data.accel.x;
raw_acc_data[3] = 0xFF & (data.accel.y >> 8);
raw_acc_data[4] = 0xFF & data.accel.y;
raw_acc_data[5] = 0XFF & (data.accel.z >> 8);
raw_acc_data[6] = 0XFF & data.accel.z;
if((m_nus.conn_handle != BLE_CONN_HANDLE_INVALID) && (m_nus.is_notification_enabled))
{
err_code = ble_nus_string_send(&m_nus, raw_acc_data, 7);
APP_ERROR_CHECK(err_code);
}
raw_gyro_data[0] = 0xF5;
raw_gyro_data[1] = 0xFF & (data.gyro.x >> 8);
raw_gyro_data[2] = 0xFF & data.gyro.x;
raw_gyro_data[3] = 0xFF & (data.gyro.y >> 8);
raw_gyro_data[4] = 0xFF & data.gyro.y;
raw_gyro_data[5] = 0xFF & (data.gyro.z >> 8);
raw_gyro_data[6] = 0xFF & data.gyro.z;
if((m_nus.conn_handle != BLE_CONN_HANDLE_INVALID) && (m_nus.is_notification_enabled))
{
err_code = ble_nus_string_send(&m_nus, raw_gyro_data, 7);
APP_ERROR_CHECK(err_code);
}
raw_compass_data[0] = 0xF6;
raw_compass_data[1] = 0xFF & (data.compass.x >> 8);
raw_compass_data[2] = 0xFF & data.compass.x;
raw_compass_data[3] = 0xFF & (data.compass.y >> 8);
raw_compass_data[4] = 0xFF & data.compass.y;
raw_compass_data[5] = 0xFF & (data.compass.z >> 8);
raw_compass_data[6] = 0xFF & data.compass.z;
if((m_nus.conn_handle != BLE_CONN_HANDLE_INVALID) && (m_nus.is_notification_enabled))
{
err_code = ble_nus_string_send(&m_nus, raw_compass_data, 7);
APP_ERROR_CHECK(err_code);
}
//(void)ble_tms_raw_set(&m_tms, &data);
}
break;
case DRV_MOTION_EVT_QUAT:
{
APP_ERROR_CHECK_BOOL(size == sizeof(int32_t) * 4);
ble_tms_quat_t data;
int32_t * p_quat = (int32_t *)p_data;
data.w = p_quat[0];
data.x = p_quat[1];
data.y = p_quat[2];
data.z = p_quat[3];
#if NRF_LOG_ENABLED
static const uint8_t QUAT_ELEMENTS = 4;
double f_buf;
char buffer[QUAT_ELEMENTS][7];
for (uint8_t i = 0; i < QUAT_ELEMENTS; i++)
{
f_buf = (double)p_quat[i];
f_buf = f_buf/(1<<30);
sprintf(buffer[i], "% 1.3f", f_buf);
}
NRF_LOG_DEBUG("DRV_MOTION_EVT_QUAT: w:%s x:%s y:%s z:%s\r\n", nrf_log_push(buffer[0]),
nrf_log_push(buffer[1]),
nrf_log_push(buffer[2]),
nrf_log_push(buffer[3]));
#endif
//(void)ble_tms_quat_set(&m_tms, &data);
}
break;
case DRV_MOTION_EVT_EULER:
{
APP_ERROR_CHECK_BOOL(size == sizeof(long) * 3);
ble_tms_euler_t data;
int32_t * p_euler = (int32_t *)p_data;
data.roll = p_euler[0];
data.pitch = p_euler[1];
data.yaw = p_euler[2];
NRF_LOG_DEBUG("DRV_MOTION_EVT_EULER, [deg]: roll(x):%3d pitch(y):%3d yaw(z):%3d \r\n", data.roll/(1<<16), data.pitch/(1<<16), data.yaw/(1<<16));
//(void)ble_tms_euler_set(&m_tms, &data);
}
break;
case DRV_MOTION_EVT_ROT_MAT:
{
APP_ERROR_CHECK_BOOL(size == sizeof(int32_t) * 9);
ble_tms_rot_mat_t data;
int32_t * p_matrix = (int32_t *)p_data;
data.matrix[0] = (int16_t)(p_matrix[0] >> 16);
data.matrix[1] = (int16_t)(p_matrix[1] >> 16);
data.matrix[2] = (int16_t)(p_matrix[2] >> 16);
data.matrix[3] = (int16_t)(p_matrix[3] >> 16);
data.matrix[4] = (int16_t)(p_matrix[4] >> 16);
data.matrix[5] = (int16_t)(p_matrix[5] >> 16);
data.matrix[6] = (int16_t)(p_matrix[6] >> 16);
data.matrix[7] = (int16_t)(p_matrix[7] >> 16);
data.matrix[8] = (int16_t)(p_matrix[8] >> 16);
#if NRF_LOG_ENABLED
static const uint8_t ROT_MAT_ELEMENTS = 9;
char buffer[ROT_MAT_ELEMENTS][6];
double tmp;
for(uint8_t i = 0; i<ROT_MAT_ELEMENTS; i++)
{
tmp = p_matrix[i]/(double)(1<<30);
sprintf(buffer[i], "% 1.2f", tmp);
}
NRF_LOG_DEBUG("DRV_MOTION_EVT_ROT_MAT:\r\n");
NRF_LOG_DEBUG("[%s %s %s]\r\n", nrf_log_push(buffer[0]), nrf_log_push(buffer[1]), nrf_log_push(buffer[2]));
NRF_LOG_DEBUG("[%s %s %s]\r\n", nrf_log_push(buffer[3]), nrf_log_push(buffer[4]), nrf_log_push(buffer[5]));
NRF_LOG_DEBUG("[%s %s %s]\r\n", nrf_log_push(buffer[6]), nrf_log_push(buffer[7]), nrf_log_push(buffer[8]));
#endif
//(void)ble_tms_rot_mat_set(&m_tms, &data);
}
break;
case DRV_MOTION_EVT_HEADING:
{
APP_ERROR_CHECK_BOOL(size == sizeof(long));
ble_tms_heading_t heading = *(ble_tms_heading_t *)p_data;
NRF_LOG_DEBUG("DRV_MOTION_EVT_HEADING [deg]: h: %d\r\n", heading/(1<<16));
//(void)ble_tms_heading_set(&m_tms, &heading);
}
break;
case DRV_MOTION_EVT_GRAVITY:
{
APP_ERROR_CHECK_BOOL(size == sizeof(float) * 3);
ble_tms_gravity_t data;
float * p_gravity = (float *)p_data;
data.x = p_gravity[0];
data.y = p_gravity[1];
data.z = p_gravity[2];
#if NRF_LOG_ENABLED
static const uint8_t GRAVITY_ELEMENTS = 3;
char buffer[GRAVITY_ELEMENTS][8];
for (uint8_t i = 0; i<GRAVITY_ELEMENTS; i++)
{
sprintf(buffer[i], "% 2.3f", p_gravity[i]);
}
NRF_LOG_DEBUG("DRV_MOTION_EVT_GRAVITY [m/s^2]: [%s, %s, %s]\r\n", nrf_log_push(buffer[0]),
nrf_log_push(buffer[1]),
nrf_log_push(buffer[2]));
#endif
//(void)ble_tms_gravity_set(&m_tms, &data);
}
break;
case DRV_MOTION_EVT_TAP:
{
APP_ERROR_CHECK_BOOL(size == 2);
ble_tms_tap_t data;
uint8_t * p_tap = (uint8_t *)p_data;
data.dir = p_tap[0];
data.cnt = p_tap[1];
NRF_LOG_DEBUG("DRV_MOTION_EVT_TAP: [%d %d]\r\n", data.cnt,
data.dir);
//(void)ble_tms_tap_set(&m_tms, &data);
}
break;
case DRV_MOTION_EVT_ORIENTATION:
{
APP_ERROR_CHECK_BOOL(size == sizeof(uint8_t));
NRF_LOG_DEBUG("DRV_MOTION_EVT_ORIENTATION: %d\r\n", *(ble_tms_orientation_t *)p_data);
//(void)ble_tms_orientation_set(&m_tms, (ble_tms_orientation_t *)p_data);
}
break;
case DRV_MOTION_EVT_PEDOMETER:
{
APP_ERROR_CHECK_BOOL(size == sizeof(unsigned long) * 2);
ble_tms_pedo_t data;
unsigned long * p_pedo = (unsigned long *)p_data;
data.steps = p_pedo[0];
data.time_ms = p_pedo[1];
NRF_LOG_DEBUG("DRV_MOTION_EVT_PEDOMETER: %d steps %d ms\r\n", p_pedo[0],
p_pedo[1]);
//(void)ble_tms_pedo_set(&m_tms, &data);
}
break;
default:
NRF_LOG_WARNING("drv_motion_evt_handler: Unknown data!\r\n");
break;
}
}
uint32_t m_motion_sleep_prepare(bool wakeup)
{
return drv_motion_sleep_prepare(wakeup);
}
uint32_t m_motion_init(m_ble_service_handle_t * p_handle, m_motion_init_t * p_params)
{
uint32_t err_code;
drv_motion_twi_init_t motion_params_mpu9250;
drv_motion_twi_init_t motion_params_lis2dh12;
static const nrf_drv_twi_config_t twi_config_mpu9250 =
{
.scl = TWI_SCL,
.sda = TWI_SDA,
.frequency = NRF_TWI_FREQ_400K,
.interrupt_priority = APP_IRQ_PRIORITY_LOW
};
static const nrf_drv_twi_config_t twi_config_lis2dh12 =
{
#if defined(THINGY_HW_v0_7_0)
.scl = TWI_SCL,
.sda = TWI_SDA,
#elif defined(THINGY_HW_v0_8_0)
.scl = TWI_SCL,
.sda = TWI_SDA,
#elif defined(THINGY_HW_v0_9_0)
.scl = TWI_SCL,
.sda = TWI_SDA,
#else
.scl = TWI_SCL_EXT,
.sda = TWI_SDA_EXT,
#endif
.frequency = NRF_TWI_FREQ_400K,
.interrupt_priority = APP_IRQ_PRIORITY_LOW
};
NULL_PARAM_CHECK(p_handle);
NULL_PARAM_CHECK(p_params);
NRF_LOG_INFO("Init \r\n");
p_handle->ble_evt_cb = motion_on_ble_evt;
p_handle->init_cb = motion_service_init;
motion_params_mpu9250.p_twi_instance = p_params->p_twi_instance;
motion_params_mpu9250.p_twi_cfg = &twi_config_mpu9250;
motion_params_lis2dh12.p_twi_instance = p_params->p_twi_instance;
motion_params_lis2dh12.p_twi_cfg = &twi_config_lis2dh12;
err_code = drv_motion_init(drv_motion_evt_handler, &motion_params_mpu9250, &motion_params_lis2dh12);
RETURN_IF_ERROR(err_code);
return NRF_SUCCESS;
}
