jh.chun/VivaMyo-firmware-test
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

initial commit

Browse Source
This commit is contained in:
jhChun
2026-04-08 16:58:54 +09:00
commit 82e33d8bf9
2578 changed files with 1590432 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

324
project/ble_peripheral/ble_app_vivaMayo/icm42670p/app_raw/app_raw.c Normal file
View File

@@ -0,0 +1,324 @@
/*******************************************************************************
* @file app_raw.c
* @author CandyPops Co.
* @version V1.0.0
* @date 2022-09-05
* @brief
******************************************************************************/
#include "sdk_config.h"
#include "app_raw.h"
#include "inv_imu_extfunc.h"
#include "inv_imu_driver.h"
#include "ble_nus.h"
#include "nrf_log.h"
#include "nrf_log_ctrl.h"
#include "nrf_log_default_backends.h"
#include "app_util_platform.h"
#include "main.h"
#include "meas_pd_48.h"
#include <cmd_parse.h>
/*
* Print raw data or scaled data
* 0 : print raw accel, gyro and temp data
* 1 : print scaled accel, gyro and temp data in g, dps and degree Celsius
*/
#define SCALED_DATA_G_DPS 0
/* --------------------------------------------------------------------------------------
* Static and extern variables
* -------------------------------------------------------------------------------------- */
/* Just a handy variable to handle the IMU object */
static struct inv_imu_device icm_driver;
uint8_t imu_bin_buffer[BLE_NUS_MAX_DATA_LEN];
/*
* ICM mounting matrix
* Coefficients are coded as Q30 integer
*/
#if (SM_BOARD_REV == SM_REVB_DB) /* when DB or EVB are used */
static int32_t icm_mounting_matrix[9] = { 0, -(1<<30), 0,
(1<<30), 0, 0,
0, 0, (1<<30) };
#else /* For SmartMotion */
static int32_t icm_mounting_matrix[9] = {(1<<30), 0, 0,
0, (1<<30), 0,
0, 0, (1<<30)};
#endif
extern bool custom_add_data;
extern bool motion_raw_data_enabled;
extern char ble_tx_buffer[BLE_NUS_MAX_DATA_LEN];
extern bool go_pdread;
extern which_cmd_t cmd_type_t;
uint16_t ssp_data[6]={0,};
extern bool info4; //cmd_parse
extern volatile uint16_t info_imu[6];
/* --------------------------------------------------------------------------------------
* static function declaration
* -------------------------------------------------------------------------------------- */
static void apply_mounting_matrix(const int32_t matrix[9], int32_t raw[3]);
/* --------------------------------------------------------------------------------------
* Functions definition
* -------------------------------------------------------------------------------------- */
int setup_imu_device(struct inv_imu_serif *icm_serif)
{
int rc = 0;
uint8_t who_am_i;
/* Init device */
rc = inv_imu_init(&icm_driver, icm_serif, imu_callback);
if (rc != INV_ERROR_SUCCESS) {
printf("!!! ERROR : Failed to initialize IMU!\r\n");
return rc;
}
/* Check WHOAMI */
rc = inv_imu_get_who_am_i(&icm_driver, &who_am_i);
if (rc != INV_ERROR_SUCCESS) {
printf("!!! ERROR : Failed to read whoami!\r\n");
return rc;
}
if (who_am_i != ICM_WHOAMI) {
printf("!!! ERROR : Bad WHOAMI value! Read 0x%02x, expected 0x%02x\r\n", who_am_i, ICM_WHOAMI);
return INV_ERROR;
}
return rc;
}
int configure_imu_device(void)
{
int rc = 0;
if (!USE_FIFO)
rc |= inv_imu_configure_fifo(&icm_driver, INV_IMU_FIFO_DISABLED);
if (USE_HIGH_RES_MODE) {
rc |= inv_imu_enable_high_resolution_fifo(&icm_driver);
} else {
rc |= inv_imu_set_accel_fsr(&icm_driver, ACCEL_CONFIG0_FS_SEL_4g);
rc |= inv_imu_set_gyro_fsr(&icm_driver, GYRO_CONFIG0_FS_SEL_2000dps);
}
if (USE_LOW_NOISE_MODE) {
rc |= inv_imu_set_accel_frequency(&icm_driver, ACCEL_CONFIG0_ODR_800_HZ);
rc |= inv_imu_set_gyro_frequency(&icm_driver, GYRO_CONFIG0_ODR_800_HZ);
rc |= inv_imu_enable_accel_low_noise_mode(&icm_driver);
} else {
rc |= inv_imu_set_accel_frequency(&icm_driver, ACCEL_CONFIG0_ODR_100_HZ);
rc |= inv_imu_set_gyro_frequency(&icm_driver, GYRO_CONFIG0_ODR_100_HZ);
rc |= inv_imu_enable_accel_low_power_mode(&icm_driver);
}
rc |= inv_imu_enable_gyro_low_noise_mode(&icm_driver);
if (!USE_FIFO)
inv_imu_sleep_us(GYR_STARTUP_TIME_US);
return rc;
}
int get_imu_data(void)
{
#if USE_FIFO
return inv_imu_get_data_from_fifo(&icm_driver);
#else
return inv_imu_get_data_from_registers(&icm_driver);
#endif
}
#if SCALED_DATA_G_DPS
static void get_accel_and_gyr_fsr(int16_t * accel_fsr_g, int16_t * gyro_fsr_dps)
{
ACCEL_CONFIG0_FS_SEL_t accel_fsr_bitfield;
GYRO_CONFIG0_FS_SEL_t gyro_fsr_bitfield;
inv_imu_get_accel_fsr(&icm_driver, &accel_fsr_bitfield);
switch(accel_fsr_bitfield) {
case ACCEL_CONFIG0_FS_SEL_2g: *accel_fsr_g = 2;
break;
case ACCEL_CONFIG0_FS_SEL_4g: *accel_fsr_g = 4;
break;
case ACCEL_CONFIG0_FS_SEL_8g: *accel_fsr_g = 8;
break;
case ACCEL_CONFIG0_FS_SEL_16g: *accel_fsr_g = 16;
break;
default: *accel_fsr_g = -1;
}
inv_imu_get_gyro_fsr(&icm_driver, &gyro_fsr_bitfield);
switch(gyro_fsr_bitfield) {
case GYRO_CONFIG0_FS_SEL_250dps: *gyro_fsr_dps = 250;
break;
case GYRO_CONFIG0_FS_SEL_500dps: *gyro_fsr_dps = 500;
break;
case GYRO_CONFIG0_FS_SEL_1000dps: *gyro_fsr_dps = 1000;
break;
case GYRO_CONFIG0_FS_SEL_2000dps: *gyro_fsr_dps = 2000;
break;
default: *gyro_fsr_dps = -1;
}
}
#endif
void imu_callback(inv_imu_sensor_event_t *event)
{
int32_t accel[3], gyro[3];
#if SCALED_DATA_G_DPS
float accel_g[3];
float gyro_dps[3];
float temp_degc;
int16_t accel_fsr_g, gyro_fsr_dps;
#endif
#if USE_FIFO
static uint64_t last_fifo_timestamp = 0;
static uint32_t rollover_num = 0;
// Handle rollover
if (last_fifo_timestamp > event->timestamp_fsync)
rollover_num++;
last_fifo_timestamp = event->timestamp_fsync;
// Compute timestamp in us
timestamp = event->timestamp_fsync + rollover_num * UINT16_MAX;
timestamp *= inv_imu_get_fifo_timestamp_resolution_us_q24(&icm_driver);
timestamp /= (1UL << 24);
if (icm_driver.fifo_highres_enabled) {
accel[0] = (((int32_t)event->accel[0] << 4)) | event->accel_high_res[0];
accel[1] = (((int32_t)event->accel[1] << 4)) | event->accel_high_res[1];
accel[2] = (((int32_t)event->accel[2] << 4)) | event->accel_high_res[2];
gyro[0] = (((int32_t)event->gyro[0] << 4)) | event->gyro_high_res[0];
gyro[1] = (((int32_t)event->gyro[1] << 4)) | event->gyro_high_res[1];
gyro[2] = (((int32_t)event->gyro[2] << 4)) | event->gyro_high_res[2];
} else {
accel[0] = event->accel[0];
accel[1] = event->accel[1];
accel[2] = event->accel[2];
gyro[0] = event->gyro[0];
gyro[1] = event->gyro[1];
gyro[2] = event->gyro[2];
}
#else
accel[0] = event->accel[0];
accel[1] = event->accel[1];
accel[2] = event->accel[2];
gyro[0] = event->gyro[0];
gyro[1] = event->gyro[1];
gyro[2] = event->gyro[2];
// Force sensor_mask so it gets displayed below
event->sensor_mask |= (1 << INV_SENSOR_TEMPERATURE);
event->sensor_mask |= (1 << INV_SENSOR_ACCEL);
event->sensor_mask |= (1 << INV_SENSOR_GYRO);
#endif
apply_mounting_matrix(icm_mounting_matrix, accel);
apply_mounting_matrix(icm_mounting_matrix, gyro);
#if SCALED_DATA_G_DPS
/*
* Convert raw data into scaled data in g and dps
*/
get_accel_and_gyr_fsr(&accel_fsr_g, &gyro_fsr_dps);
accel_g[0] = (float)(accel[0] * accel_fsr_g) / INT16_MAX;
accel_g[1] = (float)(accel[1] * accel_fsr_g) / INT16_MAX;
accel_g[2] = (float)(accel[2] * accel_fsr_g) / INT16_MAX;
gyro_dps[0] = (float)(gyro[0] * gyro_fsr_dps) / INT16_MAX;
gyro_dps[1] = (float)(gyro[1] * gyro_fsr_dps) / INT16_MAX;
gyro_dps[2] = (float)(gyro[2] * gyro_fsr_dps) / INT16_MAX;
if (USE_HIGH_RES_MODE || !USE_FIFO)
temp_degc = 25 + ((float)event->temperature / 128);
else
temp_degc = 25 + ((float)event->temperature / 2);
/*
* Output scaled data on UART link
*/
if (event->sensor_mask & (1 << INV_SENSOR_ACCEL) && event->sensor_mask & (1 << INV_SENSOR_GYRO))
printf("%u: %.3f, \t%.3f, \t%.3f, \t%.3f, \t%.3f, \t%.3f, \t%.3f\r\n",
(uint32_t)timestamp,
accel_g[0], accel_g[1], accel_g[2],
temp_degc,
gyro_dps[0], gyro_dps[1], gyro_dps[2]);
#else
/*
* Output raw data on UART link
*/
if (event->sensor_mask & (1 << INV_SENSOR_ACCEL) && event->sensor_mask & (1 << INV_SENSOR_GYRO) || motion_raw_data_enabled)
{
motion_raw_data_enabled = false;
if (info4 == true)
{
info_imu[0] = (uint16_t)accel[0];
info_imu[1] = (uint16_t)accel[1];
info_imu[2] = (uint16_t) accel[2];
info_imu[3] = (uint16_t)gyro[0];
info_imu[4] = (uint16_t) gyro[1];
info_imu[5] = (uint16_t)gyro[2];
go_pdread = true;
}
else if(cmd_type_t == CMD_UART) {
printf("Tp%d,%d,%d,%d,%d,%d\r\n\r\n", accel[0], accel[1], accel[2], gyro[0], gyro[1], gyro[2]);
} else if(cmd_type_t == CMD_BLE) {
//sprintf(ble_tx_buffer, "Tp%d,%d,%d,%d,%d,%d\r\n\r\n", accel[0], accel[1], accel[2], gyro[0], gyro[1], gyro[2]);
ssp_data[0] = (uint16_t)accel[0];
ssp_data[1] = (uint16_t)accel[1];
ssp_data[2] = (uint16_t)accel[2];
ssp_data[3] = (uint16_t)gyro[0];
ssp_data[4] = (uint16_t)gyro[1];
ssp_data[5] = (uint16_t)gyro[2];
format_data(imu_bin_buffer, "rsp:", ssp_data,12);
printf("Tp%d,%d,%d,%d,%d,%d\r\n\r\n", accel[0], accel[1], accel[2], gyro[0], gyro[1], gyro[2]);
binary_tx_handler(imu_bin_buffer,8);
if(custom_add_data==true)
{
custom_add_data = false;
}
else{
//data_tx_handler(ble_tx_buffer);
}
}
}
#endif
}
/* --------------------------------------------------------------------------------------
* Static functions definition
* -------------------------------------------------------------------------------------- */
static void apply_mounting_matrix(const int32_t matrix[9], int32_t raw[3])
{
unsigned i;
int64_t data_q30[3];
for(i = 0; i < 3; i++) {
data_q30[i] = ((int64_t)matrix[3*i+0] * raw[0]);
data_q30[i] += ((int64_t)matrix[3*i+1] * raw[1]);
data_q30[i] += ((int64_t)matrix[3*i+2] * raw[2]);
}
raw[0] = (int32_t)(data_q30[0]>>30);
raw[1] = (int32_t)(data_q30[1]>>30);
raw[2] = (int32_t)(data_q30[2]>>30);
}

75
project/ble_peripheral/ble_app_vivaMayo/icm42670p/app_raw/app_raw.h Normal file
View File

@@ -0,0 +1,75 @@
/*******************************************************************************
* @file app_raw.h
* @author CandyPops Co.
* @version V1.0.0
* @date 2022-09-05
* @brief
******************************************************************************/
#ifndef _APP_RAW_H_
#define _APP_RAW_H_
#include "sdk_config.h"
#include <stdint.h>
#include "inv_imu_transport.h"
#include "inv_imu_defs.h"
#include "inv_imu_driver.h"
/*** Example configuration ***/
/*
* Select communication link between SmartMotion and IMU
*/
#define SERIF_TYPE UI_I2C
/*
* Set power mode flag
* Set this flag to run example in low-noise mode.
* Reset this flag to run example in low-power mode.
* Note: low-noise mode is not available with sensor data frequencies less than 12.5Hz.
*/
#define USE_LOW_NOISE_MODE 1
/*
* Select Fifo resolution Mode (default is low resolution mode)
* Low resolution mode: 16 bits data format
* High resolution mode: 20 bits data format
* Warning: Enabling High Res mode will force FSR to 16g and 2000dps
*/
#define USE_HIGH_RES_MODE 0
/*
* Select to use FIFO or to read data from registers
*/
#define USE_FIFO 0
/**
* \brief This function is in charge of reseting and initializing IMU device. It should
* be successfully executed before any access to IMU device.
*
* \return 0 on success, negative value on error.
*/
int setup_imu_device(struct inv_imu_serif *icm_serif);
/**
* \brief This function configures the device in order to output gyro and accelerometer.
* \return 0 on success, negative value on error.
*/
int configure_imu_device(void);
/**
* \brief This function extracts data from the IMU FIFO.
* \return 0 on success, negative value on error.
*/
int get_imu_data(void);
/**
* \brief This function is the custom handling packet function.
* \param[in] event structure containing sensor data from one packet
*/
void imu_callback(inv_imu_sensor_event_t *event);
#endif /* !_APP_RAW_H_ */

189
project/ble_peripheral/ble_app_vivaMayo/icm42670p/app_raw/app_raw_main.c Normal file
View File

@@ -0,0 +1,189 @@
/*******************************************************************************
* @file app_raw_main.c
* @author CandyPops Co.
* @version V1.0.0
* @date 2022-09-05
* @brief
******************************************************************************/
#include "sdk_config.h"
#include "app_raw.h"
#include "app_raw_main.h"
#include "RingBuffer.h"
#include "inv_imu_driver.h"
#include "system_interface.h"
/* std */
#include <stdio.h>
#include "nrf.h"
#include "app_error.h"
#include "boards.h"
#include "nrfx_gpiote.h"
#include "nrf_delay.h"
#include "app_util_platform.h"
#include "meas_pd_48.h"
#include <cmd_parse.h>
#include "i2c_manager.h"
/* --------------------------------------------------------------------------------------
* Global variables
* -------------------------------------------------------------------------------------- */
/* --------------------------------------------------------------------------------------
* Static variables
* -------------------------------------------------------------------------------------- */
/* Flag set from IMU device irq handler */
static volatile int irq_from_device;
/* --------------------------------------------------------------------------------------
* Forward declaration
* -------------------------------------------------------------------------------------- */
static int setup_mcu(struct inv_imu_serif *icm_serif);
/*!
* @brief Sensor general interrupt handler, calls specific handlers.
*
* This function is called when an external interrupt is triggered by the sensor,
* checks interrupt registers of InvenSense Sensor to determine the source and type of interrupt
* and calls the specific interrupt handler accordingly.
*
* @param[in] NULL
*
* @param[out] NULL
*
* @return NULL
*
*/
static void inv_gpio_sensor_interrupt_handler(nrfx_gpiote_pin_t pin, nrf_gpiote_polarity_t action)
{
irq_from_device = 1;
}
void inv_gpio_sensor_irq_init(void)
{
ret_code_t err_code;
/* Initialize int pin */
if (!nrfx_gpiote_is_init())
{
err_code = nrfx_gpiote_init();
APP_ERROR_CHECK(err_code);
}
nrfx_gpiote_in_config_t in_config = NRFX_GPIOTE_CONFIG_IN_SENSE_HITOLO(true);
in_config.pull = NRF_GPIO_PIN_PULLUP;
err_code = nrfx_gpiote_in_init(ICM42670_INT1_PIN, &in_config, inv_gpio_sensor_interrupt_handler);
APP_ERROR_CHECK(err_code);
nrfx_gpiote_in_event_enable(ICM42670_INT1_PIN, true);
}
void inv_gpio_sensor_irq_uninit(void)
{
nrfx_gpiote_in_event_disable(ICM42670_INT1_PIN);
nrfx_gpiote_in_uninit(ICM42670_INT1_PIN);
/* Initialize int pin */
if (nrfx_gpiote_is_init())
{
nrfx_gpiote_uninit();
}
}
/* --------------------------------------------------------------------------------------
* Main
* -------------------------------------------------------------------------------------- */
int icm42670_init(void)
{
int rc = 0;
struct inv_imu_serif icm_serif;
rc |= setup_mcu(&icm_serif);
rc |= setup_imu_device(&icm_serif);
rc |= configure_imu_device();
if(rc != 0){
printf("!!!error during initialization\r\n");
return -1;
}
inv_gpio_sensor_irq_init();
return rc;
}
void icm42670_main(void)
{
int rc = 0;
hw_i2c_init_once();
/* Poll device for data */
if (irq_from_device) {
rc = get_imu_data();
if(rc < 0) {
printf("error while getting data\r\n");
}
irq_from_device = 0;
}
}
/* --------------------------------------------------------------------------------------
* Functions definitions
* -------------------------------------------------------------------------------------- */
/*
* This function initializes MCU on which this software is running.
* It configures:
* - a UART link used to print some messages
* - interrupt priority group and GPIO so that MCU can receive interrupts from IMU
* - a microsecond timer requested by IMU driver to compute some delay
* - a microsecond timer used to get some timestamps
* - a serial link to communicate from MCU to IMU
*/
static int setup_mcu(struct inv_imu_serif *icm_serif)
{
int rc = 0;
/* Initialize serial interface between MCU and IMU */
icm_serif->context = 0; /* no need */
icm_serif->read_reg = inv_io_hal_read_reg;
icm_serif->write_reg = inv_io_hal_write_reg;
icm_serif->max_read = 1024*32; /* maximum number of bytes allowed per serial read */
icm_serif->max_write = 1024*32; /* maximum number of bytes allowed per serial write */
icm_serif->serif_type = SERIF_TYPE;
rc |= inv_io_hal_init(icm_serif);
return rc;
}
/* --------------------------------------------------------------------------------------
* Extern functions definition
* -------------------------------------------------------------------------------------- */
/* Sleep implementation */
void inv_imu_sleep_us(uint32_t us)
{
nrf_delay_us(us);
}
/* Get time implementation */
uint64_t inv_imu_get_time_us(void)
{
return NRF_RTC1->COUNTER;
}

18
project/ble_peripheral/ble_app_vivaMayo/icm42670p/app_raw/app_raw_main.h Normal file
View File

@@ -0,0 +1,18 @@
/*******************************************************************************
* @file app_raw_main.h
* @author CandyPops Co.
* @version V1.0.0
* @date 2022-09-05
* @brief
******************************************************************************/
#ifndef _APP_RAW_MAIN_H_
#define _APP_RAW_MAIN_H_
#include "sdk_config.h"
int icm42670_init(void);
void icm42670_main(void);
int icm42670_uninit(void);
#endif /* !_APP_RAW_MAIN_H_ */