프로젝트 정리: 앱 내부 구조 수정
- measurement/hal/system/command
This commit is contained in:
@@ -0,0 +1,297 @@
|
||||
/*******************************************************************************
|
||||
* @file battery_saadc.c
|
||||
* @author CandyPops Co.
|
||||
* @version V1.0.0
|
||||
* @date 2022-09-05
|
||||
* @brief
|
||||
******************************************************************************/
|
||||
|
||||
/*******************************************************************************
|
||||
* [모듈 개요] 배터리 전압 ADC 측정 모듈
|
||||
*
|
||||
* nRF52840의 SAADC(Successive Approximation ADC)를 사용하여 배터리 전압을 측정:
|
||||
* - AIN2 채널, 10bit 해상도
|
||||
* - 5초 주기 타이머(battery_loop)로 반복 측정
|
||||
* - 저전압(3500mV 이하) 10회 연속 감지 시 자동 전원 OFF
|
||||
* - info4 모드(전체 센서 수집)에서는 info_batt에 저장 후 온도 측정으로 전환
|
||||
*
|
||||
* 배터리 전압 변환 공식:
|
||||
* 전압(mV) = ADC값 x (600mV / 1023) x 6 x 1.42 (분압 저항 보정 계수)
|
||||
*
|
||||
* info4 모드 순서: 배터리 -> 온도(go_temp) -> IMU(motion_raw_data_enabled)
|
||||
******************************************************************************/
|
||||
|
||||
#include "sdk_common.h"
|
||||
|
||||
#include <stdint.h>
|
||||
#include <string.h>
|
||||
#include "nrf.h"
|
||||
#include "boards.h"
|
||||
#include "app_error.h"
|
||||
#include "nrf_drv_saadc.h"
|
||||
#include "nrf_drv_timer.h"
|
||||
#include "ble_nus.h"
|
||||
#include "nrf_log.h"
|
||||
#include "main.h"
|
||||
#include "app_timer.h"
|
||||
//#include "fstorage.h"
|
||||
#include "battery_saadc.h"
|
||||
#include "main_timer.h"
|
||||
#include "main.h"
|
||||
#include "debug_print.h"
|
||||
|
||||
/* SAADC 내부 기준전압 600mV (부동소수점) */
|
||||
#define BATTERY_REF_VOLTAGE_IN_MILLIVOLTS 600.0f /**< Reference voltage (in milli volts) used by ADC while doing conversion. */
|
||||
|
||||
/* 1/3 프리스케일링 보상 계수 (입력 전압을 1/3로 분압하므로 x3, 추가 x2 = 총 x6) (부동소수점) */
|
||||
#define BATTERY_PRE_SCALING_COMPENSATION 6.0f /**< The ADC is configured to use VDD with 1/3 prescaling as input. And hence the result of conversion is to be multiplied by 3 to get the actual value of the battery voltage.*/
|
||||
|
||||
/* 12비트 ADC 최대 디지털 값 (부동소수점) */
|
||||
#define BATTERY_ADC_RES_12BITS 4095.0f /**< Maximum digital value for 12-bit ADC conversion. */
|
||||
|
||||
/**@brief Macro to convert the result of ADC conversion in millivolts.
|
||||
*
|
||||
* @param[in] ADC_VALUE ADC result.
|
||||
*
|
||||
* @retval Result converted to millivolts.
|
||||
*/
|
||||
#define BATTERY_RESULT_IN_MILLI_VOLTS(ADC_VALUE)\
|
||||
((((ADC_VALUE) * BATTERY_REF_VOLTAGE_IN_MILLIVOLTS) / BATTERY_ADC_RES_12BITS) * BATTERY_PRE_SCALING_COMPENSATION)
|
||||
|
||||
/* 배터리 측정용 싱글 버퍼 (1회 측정 후 uninit하므로 더블 버퍼 불필요) */
|
||||
static nrf_saadc_value_t adc_buf;
|
||||
//static nrf_saadc_value_t adc_bufs[2]; // 이전: 더블 버퍼 (연속 측정용)
|
||||
|
||||
|
||||
/* 배터리 모니터링 반복 타이머 정의 */
|
||||
APP_TIMER_DEF(m_battery_loop_timer_id);
|
||||
|
||||
/* 배터리 측정 주기: 5초 (밀리초 단위) */
|
||||
#define BATTERY_LOOP_INTERVAL 60000
|
||||
|
||||
/* 저전압 체크 플래그 — battery_loop에서 true로 설정, 핸들러에서 소비 */
|
||||
bool low_battery_check = false;
|
||||
|
||||
/* SAADC 콜백 완료 플래그 — all_sensors()에서 배터리 측정 완료 대기용 */
|
||||
volatile bool battery_saadc_done = false;
|
||||
|
||||
/* info4: 전체 센서 데이터 수집 모드 플래그 */
|
||||
extern bool info4; // main.c
|
||||
|
||||
|
||||
extern char ble_tx_buffer[BLE_NUS_MAX_DATA_LEN];
|
||||
|
||||
/* true가 되면 main_timer에서 전원 OFF 시퀀스 실행 */
|
||||
extern bool go_device_power_off;
|
||||
|
||||
/* 다른 작업(IMU 등) 처리 중이면 true — 배터리 측정 스킵용 */
|
||||
extern volatile bool processing;
|
||||
|
||||
/* 현재 명령 소스: CMD_UART 또는 CMD_BLE */
|
||||
extern which_cmd_t cmd_type_t;
|
||||
|
||||
extern uint8_t ble_bin_buffer[BLE_NUS_MAX_DATA_LEN] ;
|
||||
|
||||
/* info4 모드에서 배터리 전압을 임시 저장 (mV 단위) */
|
||||
volatile uint16_t info_batt; //48_c
|
||||
|
||||
/* info4 순차 측정 제어 플래그: go_batt→ go_temp → motion */
|
||||
extern bool go_temp; //
|
||||
extern bool go_batt; //cmd_parse
|
||||
|
||||
extern bool motion_raw_data_enabled ;
|
||||
extern bool ble_got_new_data;
|
||||
extern bool motion_data_once ;
|
||||
/**@brief Function for handling the ADC interrupt.
|
||||
*
|
||||
* @details This function will fetch the conversion result from the ADC, convert the value into
|
||||
* percentage and send it to peer.
|
||||
*/
|
||||
|
||||
|
||||
/**
|
||||
* @brief 배터리 전압 ADC 완료 콜백
|
||||
*
|
||||
* SAADC 변환 완료 시 호출된다.
|
||||
* ADC 값을 실제 배터리 전압(mV)으로 변환하고, 동작 모드에 따라:
|
||||
* - 저전압 체크 모드: 3500mV 이하 10회 연속이면 자동 전원 OFF
|
||||
* - info4 모드: info_batt에 저장 후 온도 측정(go_temp)으로 전환
|
||||
* - 일반 모드: BLE 또는 UART로 즉시 전송
|
||||
*
|
||||
* 전압 변환: ADC값 x (600/1023) x 6 = 기본 전압, x 1.42 = 분압 보정 후 실제 전압
|
||||
*/
|
||||
void battery_event_handler( nrf_drv_saadc_evt_t const * p_event )
|
||||
{
|
||||
/* 저전압 연속 감지 카운터 (static으로 호출 간 유지) */
|
||||
static uint8_t low_battery_cnt = 0;
|
||||
|
||||
if (p_event->type == NRF_DRV_SAADC_EVT_DONE)
|
||||
{
|
||||
nrf_saadc_value_t register_val = 0;
|
||||
float batt_lvl_in_milli_volt_0 = 0; /* 보정 전 전압 (부동소수점) */
|
||||
float batt_lvl_in_milli_volt_1 = 0; /* 분압 보정 후 최종 전압 (부동소수점) */
|
||||
|
||||
/* ADC 변환 결과 읽기 */
|
||||
register_val = p_event->data.done.p_buffer[0];
|
||||
|
||||
//err_code = nrf_drv_saadc_buffer_convert(p_event->data.done.p_buffer, 1); // 이전: 다음 변환을 위해 버퍼 재등록
|
||||
//APP_ERROR_CHECK(err_code);
|
||||
|
||||
/* SAADC 해제 — 다른 ADC 측정(온도, 압력)과 하드웨어 공유 */
|
||||
/* 1회 측정 후 해제이므로 buffer_convert(다음 버퍼 등록) 불필요 */
|
||||
nrf_drv_saadc_channel_uninit(0);
|
||||
nrf_drv_saadc_uninit();
|
||||
|
||||
/* 콜백 완료 알림 (all_sensors 대기 해제용) */
|
||||
if (info4) DBG_PRINTF("2");
|
||||
battery_saadc_done = true;
|
||||
|
||||
/* ADC값 → mV 변환 (매크로: ADC x 600/1023 x 6) */
|
||||
batt_lvl_in_milli_volt_0 = BATTERY_RESULT_IN_MILLI_VOLTS(register_val);
|
||||
|
||||
/* 분압 저항 보정 계수 1.42 적용 → 실제 배터리 전압 */
|
||||
batt_lvl_in_milli_volt_1 = (batt_lvl_in_milli_volt_0) *1.42f;
|
||||
|
||||
/* === 저전압 체크 모드 (battery_loop 타이머에서 설정) === */
|
||||
if(low_battery_check == true)
|
||||
{
|
||||
low_battery_check = false;
|
||||
|
||||
/* 배터리 전압이 LOW_BATTERY_VOLTAGE(3500mV) 이하인지 확인 */
|
||||
if(batt_lvl_in_milli_volt_1 <= LOW_BATTERY_VOLTAGE)
|
||||
{
|
||||
/* 10회 연속 저전압 감지 시 전원 OFF 시퀀스 시작 */
|
||||
if(low_battery_cnt >= 10)
|
||||
{
|
||||
low_battery_cnt = 0;
|
||||
/*go to power off and fds save */
|
||||
DBG_PRINTF("Save FDS parameters and then Power OFF\r\n");
|
||||
go_device_power_off = true;
|
||||
main_timer_start();
|
||||
}
|
||||
else
|
||||
{
|
||||
/* 아직 10회 미만 — 카운터 증가 후 경고 출력 */
|
||||
low_battery_cnt++;
|
||||
DBG_PRINTF("WARNING!!! low_battery cnt = %d, Batt = %d(mV)\r\n", low_battery_cnt, (int)batt_lvl_in_milli_volt_1);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* === info4 모드: 전체 센서 수집(mbb) 중 배터리 값 저장 === */
|
||||
else if (info4 == true)
|
||||
{
|
||||
info_batt = batt_lvl_in_milli_volt_1;
|
||||
}
|
||||
|
||||
/* === 일반 모드: 단독 배터리 측정 요청(msn)에 대한 응답 전송 === */
|
||||
else
|
||||
{
|
||||
if (cmd_type_t == CMD_UART)
|
||||
{
|
||||
DBG_PRINTF("Tn%d\r\n\r\n", (int)batt_lvl_in_milli_volt_1);
|
||||
}
|
||||
else if (cmd_type_t == CMD_BLE)
|
||||
{
|
||||
/* "rsn:" 헤더와 함께 배터리 전압을 바이너리로 BLE 전송 */
|
||||
single_format_data(ble_bin_buffer, "rsn:", batt_lvl_in_milli_volt_1);
|
||||
dr_binary_tx_safe(ble_bin_buffer,3);
|
||||
//data_tx_handler(ble_tx_buffer);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief SAADC를 배터리 전압 측정용으로 설정
|
||||
*
|
||||
* AIN2 채널을 싱글엔드(SE) 모드, 1/3 프리스케일링으로 초기화한다.
|
||||
* 더블 버퍼(adc_bufs[0], [1])를 등록하여 연속 측정이 가능하도록 한다.
|
||||
* 콜백: battery_event_handler
|
||||
*/
|
||||
static void battery_configure(void)
|
||||
{
|
||||
/* SAADC 드라이버 초기화 (4x 오버샘플링으로 노이즈 저감) */
|
||||
nrf_drv_saadc_config_t saadc_config = NRF_DRV_SAADC_DEFAULT_CONFIG;
|
||||
saadc_config.resolution = NRF_SAADC_RESOLUTION_12BIT; // 10 -> 12bit
|
||||
saadc_config.oversample = NRF_SAADC_OVERSAMPLE_4X;
|
||||
ret_code_t err_code = nrf_drv_saadc_init(&saadc_config, battery_event_handler);
|
||||
if (err_code != NRF_SUCCESS) {
|
||||
return; /* SAADC 사용 중 → 이번 측정 스킵, 다음 주기에 재시도 */
|
||||
}
|
||||
|
||||
/* AIN2 채널 설정: 싱글엔드 입력, 1/6 gain, burst + TACQ 20μs */
|
||||
nrf_saadc_channel_config_t config = NRF_DRV_SAADC_DEFAULT_CHANNEL_CONFIG_SE(NRF_SAADC_INPUT_AIN2);
|
||||
config.burst = NRF_SAADC_BURST_ENABLED;
|
||||
config.acq_time = NRF_SAADC_ACQTIME_10US;
|
||||
err_code = nrf_drv_saadc_channel_init(0, &config);
|
||||
APP_ERROR_CHECK(err_code);
|
||||
|
||||
/* 싱글 버퍼 등록 (1회 측정 후 uninit) */
|
||||
err_code = nrf_drv_saadc_buffer_convert(&adc_buf, 1);
|
||||
APP_ERROR_CHECK(err_code);
|
||||
//err_code = nrf_drv_saadc_buffer_convert(&adc_bufs[0], 1); // 이전: 더블 버퍼
|
||||
//APP_ERROR_CHECK(err_code);
|
||||
//err_code = nrf_drv_saadc_buffer_convert(&adc_bufs[1], 1);
|
||||
//APP_ERROR_CHECK(err_code);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 배터리 전압 1회 측정 시작
|
||||
*
|
||||
* SAADC를 배터리용으로 설정 후 샘플링을 트리거한다.
|
||||
* 결과는 battery_event_handler 콜백에서 비동기로 처리된다.
|
||||
*/
|
||||
void battery_level_meas(void)
|
||||
{
|
||||
ret_code_t err_code;
|
||||
|
||||
battery_configure(); /* SAADC 배터리용 초기화 */
|
||||
err_code = nrf_drv_saadc_sample(); /* ADC 샘플링 트리거 (비동기) */
|
||||
APP_ERROR_CHECK(err_code);
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief 배터리 모니터링 타이머 콜백 (5초 주기) - 주기 확인 필요(너무 짧음)
|
||||
*
|
||||
* 저전압 체크 플래그를 설정하고 배터리 측정을 시작한다.
|
||||
* 다른 작업(IMU 등) 처리 중이면 측정을 건너뛴다.
|
||||
*/
|
||||
void battery_loop(void * p_context) /* For 1sec */
|
||||
{
|
||||
UNUSED_PARAMETER(p_context);
|
||||
|
||||
/* 다른 센서 처리 중 또는 MBB 센서 수집 중이면 배터리 측정 스킵 (SAADC 충돌 방지) */
|
||||
if (processing == true || info4 == true)
|
||||
{
|
||||
processing = false ; // add 20241218
|
||||
//low_battery_check = true;
|
||||
return;
|
||||
}
|
||||
else
|
||||
{
|
||||
low_battery_check = true; /* 저전압 감지 모드로 측정 */
|
||||
battery_level_meas(); /* 배터리 ADC 1회 측정 시작 */
|
||||
}
|
||||
}
|
||||
|
||||
/** @brief 배터리 모니터링 타이머 시작 (5초 반복) */
|
||||
void battery_timer_start(void)
|
||||
{
|
||||
APP_ERROR_CHECK(app_timer_start(m_battery_loop_timer_id, APP_TIMER_TICKS(BATTERY_LOOP_INTERVAL), NULL));
|
||||
}
|
||||
|
||||
/** @brief 배터리 모니터링 타이머 정지 */
|
||||
void battery_timer_stop(void)
|
||||
{
|
||||
APP_ERROR_CHECK(app_timer_stop(m_battery_loop_timer_id));
|
||||
}
|
||||
|
||||
/** @brief 배터리 모니터링 타이머 초기화 (반복 모드, 콜백: battery_loop) */
|
||||
void battery_timer_init(void)
|
||||
{
|
||||
APP_ERROR_CHECK(app_timer_create(&m_battery_loop_timer_id, APP_TIMER_MODE_REPEATED, battery_loop));
|
||||
}
|
||||
|
||||
@@ -0,0 +1,41 @@
|
||||
/*******************************************************************************
|
||||
* @file battery_saadc.h
|
||||
* @author CandyPops Co.
|
||||
* @version V1.0.0
|
||||
* @date 2022-09-05
|
||||
* @brief
|
||||
******************************************************************************/
|
||||
|
||||
/*******************************************************************************
|
||||
* [헤더 개요] 배터리 전압 SAADC 측정 인터페이스
|
||||
*
|
||||
* nRF52840 SAADC를 이용한 배터리 전압 측정 API를 선언한다.
|
||||
*
|
||||
* 주요 API:
|
||||
* - battery_level_meas() : 배터리 전압 1회 측정 (AIN2)
|
||||
* - battery_timer_init/start/stop() : 5초 주기 배터리 모니터링 타이머 제어
|
||||
*
|
||||
* LOW_BATTERY_VOLTAGE(3500mV) 이하가 10회 연속 감지되면 자동 전원 OFF
|
||||
******************************************************************************/
|
||||
|
||||
#ifndef _BATTERY_SAADC_H_
|
||||
#define _BATTERY_SAADC_H_
|
||||
|
||||
/* 저전압 판정 임계값 (mV) — 이 값 이하가 10회 연속이면 자동 전원 OFF */
|
||||
#define LOW_BATTERY_VOLTAGE 3500 /* Low Battery 임계값 */
|
||||
|
||||
|
||||
/** @brief 배터리 SAADC 콜백 완료 플래그 (all_sensors 대기용) */
|
||||
extern volatile bool battery_saadc_done;
|
||||
|
||||
/** @brief 배터리 전압 1회 측정 시작 (비동기, 결과는 콜백에서 처리) */
|
||||
void battery_level_meas(void);
|
||||
/** @brief 배터리 모니터링 5초 반복 타이머 시작 */
|
||||
void battery_timer_start(void);
|
||||
/** @brief 배터리 모니터링 타이머 정지 */
|
||||
void battery_timer_stop(void);
|
||||
/** @brief 배터리 모니터링 타이머 초기화 (앱 시작 시 1회 호출) */
|
||||
void battery_timer_init(void);
|
||||
|
||||
#endif //_BATTERY_SAADC_H_
|
||||
|
||||
@@ -0,0 +1,103 @@
|
||||
/*
|
||||
* ________________________________________________________________________________________________________
|
||||
* Copyright (c) 2015-2015 InvenSense Inc. All rights reserved.
|
||||
*
|
||||
* This software, related documentation and any modifications thereto (collectively “Software”) is subject
|
||||
* to InvenSense and its licensors' intellectual property rights under U.S. and international copyright
|
||||
* and other intellectual property rights laws.
|
||||
*
|
||||
* InvenSense and its licensors retain all intellectual property and proprietary rights in and to the Software
|
||||
* and any use, reproduction, disclosure or distribution of the Software without an express license agreement
|
||||
* from InvenSense is strictly prohibited.
|
||||
*
|
||||
* EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, THE SOFTWARE IS
|
||||
* PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED
|
||||
* TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT.
|
||||
* EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, IN NO EVENT SHALL
|
||||
* INVENSENSE BE LIABLE FOR ANY DIRECT, SPECIAL, INDIRECT, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, OR ANY
|
||||
* DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT,
|
||||
* NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE
|
||||
* OF THE SOFTWARE.
|
||||
* ________________________________________________________________________________________________________
|
||||
*/
|
||||
|
||||
#include "DataConverter.h"
|
||||
|
||||
uint8_t * inv_dc_int32_to_little8(int32_t x, uint8_t * little8)
|
||||
{
|
||||
little8[3] = (uint8_t)((x >> 24) & 0xff);
|
||||
little8[2] = (uint8_t)((x >> 16) & 0xff);
|
||||
little8[1] = (uint8_t)((x >> 8) & 0xff);
|
||||
little8[0] = (uint8_t)(x & 0xff);
|
||||
|
||||
return little8;
|
||||
}
|
||||
|
||||
uint8_t * inv_dc_int16_to_little8(int16_t x, uint8_t * little8)
|
||||
{
|
||||
little8[0] = (uint8_t)(x & 0xff);
|
||||
little8[1] = (uint8_t)((x >> 8) & 0xff);
|
||||
|
||||
return little8;
|
||||
}
|
||||
|
||||
uint8_t * inv_dc_int32_to_big8(int32_t x, uint8_t * big8)
|
||||
{
|
||||
big8[0] = (uint8_t)((x >> 24) & 0xff);
|
||||
big8[1] = (uint8_t)((x >> 16) & 0xff);
|
||||
big8[2] = (uint8_t)((x >> 8) & 0xff);
|
||||
big8[3] = (uint8_t)(x & 0xff);
|
||||
|
||||
return big8;
|
||||
}
|
||||
|
||||
int32_t inv_dc_little8_to_int32(const uint8_t * little8)
|
||||
{
|
||||
int32_t x = 0;
|
||||
|
||||
x |= ((int32_t)little8[3] << 24);
|
||||
x |= ((int32_t)little8[2] << 16);
|
||||
x |= ((int32_t)little8[1] << 8);
|
||||
x |= ((int32_t)little8[0]);
|
||||
|
||||
return x;
|
||||
}
|
||||
|
||||
int16_t inv_dc_big16_to_int16(uint8_t * data)
|
||||
{
|
||||
int16_t result;
|
||||
|
||||
result = (*data << 8);
|
||||
data++;
|
||||
result |= *data;
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
int16_t inv_dc_le_to_int16(const uint8_t * little8)
|
||||
{
|
||||
uint16_t x = 0;
|
||||
|
||||
x |= ((uint16_t)little8[0]);
|
||||
x |= ((uint16_t)little8[1] << 8);
|
||||
|
||||
return (int16_t)x;
|
||||
}
|
||||
|
||||
void inv_dc_sfix32_to_float(const int32_t * in, uint32_t len, uint8_t qx, float * out)
|
||||
{
|
||||
uint8_t i;
|
||||
|
||||
for(i = 0; i < len; ++i) {
|
||||
out[i] = (float)in[i] / (1 << qx);
|
||||
}
|
||||
}
|
||||
|
||||
void inv_dc_float_to_sfix32(const float * in, uint32_t len, uint8_t qx, int32_t * out)
|
||||
{
|
||||
uint8_t i;
|
||||
|
||||
for(i = 0; i < len; ++i) {
|
||||
out[i] = (int32_t)((in[i] * (1 << qx)) + ((in[i] >= 0) - 0.5f));
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,87 @@
|
||||
/*
|
||||
* ________________________________________________________________________________________________________
|
||||
* Copyright (c) 2015-2015 InvenSense Inc. All rights reserved.
|
||||
*
|
||||
* This software, related documentation and any modifications thereto (collectively “Software”) is subject
|
||||
* to InvenSense and its licensors' intellectual property rights under U.S. and international copyright
|
||||
* and other intellectual property rights laws.
|
||||
*
|
||||
* InvenSense and its licensors retain all intellectual property and proprietary rights in and to the Software
|
||||
* and any use, reproduction, disclosure or distribution of the Software without an express license agreement
|
||||
* from InvenSense is strictly prohibited.
|
||||
*
|
||||
* EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, THE SOFTWARE IS
|
||||
* PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED
|
||||
* TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT.
|
||||
* EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, IN NO EVENT SHALL
|
||||
* INVENSENSE BE LIABLE FOR ANY DIRECT, SPECIAL, INDIRECT, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, OR ANY
|
||||
* DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT,
|
||||
* NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE
|
||||
* OF THE SOFTWARE.
|
||||
* ________________________________________________________________________________________________________
|
||||
*/
|
||||
|
||||
/** @defgroup DataConverter Data Converter
|
||||
* @brief Helper functions to convert integer
|
||||
* @ingroup EmbUtils
|
||||
* @{
|
||||
*/
|
||||
|
||||
#ifndef _INV_DATA_CONVERTER_H_
|
||||
#define _INV_DATA_CONVERTER_H_
|
||||
|
||||
#include "InvExport.h"
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
/** @brief Converts a 32-bit long to a little endian byte stream
|
||||
*/
|
||||
uint8_t INV_EXPORT * inv_dc_int32_to_little8(int32_t x, uint8_t * little8);
|
||||
|
||||
/** @brief Converts a 16-bit integer to a little endian byte stream
|
||||
*/
|
||||
uint8_t INV_EXPORT * inv_dc_int16_to_little8(int16_t x, uint8_t * little8);
|
||||
|
||||
/** @brief Converts a 32-bit long to a big endian byte stream
|
||||
*/
|
||||
uint8_t INV_EXPORT * inv_dc_int32_to_big8(int32_t x, uint8_t *big8);
|
||||
|
||||
/** @brief Converts a little endian byte stream into a 32-bit integer
|
||||
*/
|
||||
int32_t INV_EXPORT inv_dc_little8_to_int32(const uint8_t * little8);
|
||||
|
||||
/** @brief Converts a little endian byte stream into a 16-bit integer
|
||||
*/
|
||||
int16_t INV_EXPORT inv_dc_le_to_int16(const uint8_t * little8);
|
||||
|
||||
/** @brief Converts big endian on 16 bits into an unsigned short
|
||||
*/
|
||||
int16_t INV_EXPORT inv_dc_big16_to_int16(uint8_t * data);
|
||||
|
||||
/** @brief Converts an array of 32-bit signed fixed-point integers to an array of floats
|
||||
* @param[in] in Pointer to the first element of the array of 32-bit signed fixed-point integers
|
||||
* @param[in] len Length of the array
|
||||
* @param[in] qx Number of bits used to represent the decimal part of the fixed-point integers
|
||||
* @param[out] out Pointer to the memory area where the output will be stored
|
||||
*/
|
||||
void INV_EXPORT inv_dc_sfix32_to_float(const int32_t * in, uint32_t len, uint8_t qx, float * out);
|
||||
|
||||
/** @brief Converts an array of floats to an array of 32-bit signed fixed-point integers
|
||||
* @param[in] in Pointer to the first element of the array of floats
|
||||
* @param[in] len Length of the array
|
||||
* @param[in] qx Number of bits used to represent the decimal part of the fixed-point integers
|
||||
* @param[out] out Pointer to the memory area where the output will be stored
|
||||
*/
|
||||
void INV_EXPORT inv_dc_float_to_sfix32(const float * in, uint32_t len, uint8_t qx, int32_t * out);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif /* _INV_DATA_CONVERTER_H_ */
|
||||
|
||||
/** @} */
|
||||
@@ -0,0 +1,47 @@
|
||||
/*
|
||||
* ________________________________________________________________________________________________________
|
||||
* Copyright (c) 2015-2015 InvenSense Inc. All rights reserved.
|
||||
*
|
||||
* This software, related documentation and any modifications thereto (collectively “Software”) is subject
|
||||
* to InvenSense and its licensors' intellectual property rights under U.S. and international copyright
|
||||
* and other intellectual property rights laws.
|
||||
*
|
||||
* InvenSense and its licensors retain all intellectual property and proprietary rights in and to the Software
|
||||
* and any use, reproduction, disclosure or distribution of the Software without an express license agreement
|
||||
* from InvenSense is strictly prohibited.
|
||||
*
|
||||
* EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, THE SOFTWARE IS
|
||||
* PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED
|
||||
* TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT.
|
||||
* EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, IN NO EVENT SHALL
|
||||
* INVENSENSE BE LIABLE FOR ANY DIRECT, SPECIAL, INDIRECT, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, OR ANY
|
||||
* DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT,
|
||||
* NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE
|
||||
* OF THE SOFTWARE.
|
||||
* ________________________________________________________________________________________________________
|
||||
*/
|
||||
|
||||
#include "ErrorHelper.h"
|
||||
|
||||
const char * inv_error_str(int error)
|
||||
{
|
||||
switch(error) {
|
||||
case INV_ERROR_SUCCESS: return "Success";
|
||||
case INV_ERROR: return "Unspecified error";
|
||||
case INV_ERROR_NIMPL: return "Not implemented";
|
||||
case INV_ERROR_TRANSPORT: return "Transport error";
|
||||
case INV_ERROR_TIMEOUT: return "Timeout, action did not complete in time";
|
||||
case INV_ERROR_SIZE: return "Wrong size error";
|
||||
case INV_ERROR_OS: return "Operating system failure";
|
||||
case INV_ERROR_IO: return "Input/Output error";
|
||||
case INV_ERROR_MEM: return "Bad allocation";
|
||||
case INV_ERROR_HW: return "Hardware error";
|
||||
case INV_ERROR_BAD_ARG: return "Invalid arguments";
|
||||
case INV_ERROR_UNEXPECTED: return "Unexpected error";
|
||||
case INV_ERROR_FILE: return "Invalid file format";
|
||||
case INV_ERROR_PATH: return "Invalid file path";
|
||||
case INV_ERROR_IMAGE_TYPE: return "Unknown image type";
|
||||
case INV_ERROR_WATCHDOG: return "Watchdog error";
|
||||
default: return "Unknown error";
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,51 @@
|
||||
/*
|
||||
* ________________________________________________________________________________________________________
|
||||
* Copyright (c) 2015-2015 InvenSense Inc. All rights reserved.
|
||||
*
|
||||
* This software, related documentation and any modifications thereto (collectively “Software”) is subject
|
||||
* to InvenSense and its licensors' intellectual property rights under U.S. and international copyright
|
||||
* and other intellectual property rights laws.
|
||||
*
|
||||
* InvenSense and its licensors retain all intellectual property and proprietary rights in and to the Software
|
||||
* and any use, reproduction, disclosure or distribution of the Software without an express license agreement
|
||||
* from InvenSense is strictly prohibited.
|
||||
*
|
||||
* EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, THE SOFTWARE IS
|
||||
* PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED
|
||||
* TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT.
|
||||
* EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, IN NO EVENT SHALL
|
||||
* INVENSENSE BE LIABLE FOR ANY DIRECT, SPECIAL, INDIRECT, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, OR ANY
|
||||
* DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT,
|
||||
* NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE
|
||||
* OF THE SOFTWARE.
|
||||
* ________________________________________________________________________________________________________
|
||||
*/
|
||||
|
||||
/** @defgroup ErrorHelper Error Helper
|
||||
* @brief Helper functions related to error code
|
||||
* @ingroup EmbUtils
|
||||
* @{
|
||||
*/
|
||||
|
||||
#ifndef _INV_ERROR_HELPER_H_
|
||||
#define _INV_ERROR_HELPER_H_
|
||||
|
||||
#include "InvExport.h"
|
||||
#include "InvError.h"
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/** @brief Returns string describing error number
|
||||
* @sa enum inv_error
|
||||
*/
|
||||
const char INV_EXPORT * inv_error_str(int error);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif /* _INV_ERROR_HELPER_H_ */
|
||||
|
||||
/** @} */
|
||||
@@ -0,0 +1,82 @@
|
||||
/*
|
||||
Copyright (c) 2014-2015 InvenSense Inc. Portions Copyright (c) 2014-2015 Movea. All rights reserved.
|
||||
|
||||
This software, related documentation and any modifications thereto (collectively "Software") is subject
|
||||
to InvenSense and its licensors' intellectual property rights under U.S. and international copyright and
|
||||
other intellectual property rights laws.
|
||||
|
||||
InvenSense and its licensors retain all intellectual property and proprietary rights in and to the Software
|
||||
and any use, reproduction, disclosure or distribution of the Software without an express license
|
||||
agreement from InvenSense is strictly prohibited.
|
||||
*/
|
||||
|
||||
#include "InvBasicMath.h"
|
||||
|
||||
#include <limits.h>
|
||||
|
||||
unsigned int InvBasicMath_log2u(unsigned int val)
|
||||
{
|
||||
unsigned int ret = UINT_MAX;
|
||||
|
||||
while (val != 0) {
|
||||
val >>= 1;
|
||||
ret++;
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
int InvBasicMath_isAnOrthonormalMatrix(const float matrix[9])
|
||||
{
|
||||
// Check if matrix is orthogonal
|
||||
// Matrix is orthogonal if transpose(Matrix) x Matrix = Identity
|
||||
|
||||
float transpose[9];
|
||||
float mult[9];
|
||||
int i, j;
|
||||
|
||||
// Compute Transpose(matrix)
|
||||
for (i = 0; i < 3; i++) {
|
||||
for(j = 0; j < 3; j++) {
|
||||
transpose[i*3+j] = matrix[i+j*3];
|
||||
}
|
||||
}
|
||||
|
||||
// Multiply transpose x matrix
|
||||
mult[0] = transpose[0]*matrix[0] + transpose[1]*matrix[3] + transpose[2]*matrix[6];
|
||||
mult[1] = transpose[0]*matrix[1] + transpose[1]*matrix[4] + transpose[2]*matrix[7];
|
||||
mult[2] = transpose[0]*matrix[2] + transpose[1]*matrix[5] + transpose[2]*matrix[8];
|
||||
|
||||
mult[3] = transpose[3]*matrix[0] + transpose[4]*matrix[3] + transpose[5]*matrix[6];
|
||||
mult[4] = transpose[3]*matrix[1] + transpose[4]*matrix[4] + transpose[5]*matrix[7];
|
||||
mult[5] = transpose[3]*matrix[2] + transpose[4]*matrix[5] + transpose[5]*matrix[8];
|
||||
|
||||
mult[6] = transpose[6]*matrix[0] + transpose[7]*matrix[3] + transpose[8]*matrix[6];
|
||||
mult[7] = transpose[6]*matrix[1] + transpose[7]*matrix[4] + transpose[8]*matrix[7];
|
||||
mult[8] = transpose[6]*matrix[2] + transpose[7]*matrix[5] + transpose[8]*matrix[8];
|
||||
|
||||
// Check that mult is identity
|
||||
for (i = 0; i < 3; i++) {
|
||||
for(j = 0; j < 3; j++) {
|
||||
if (i == j) {
|
||||
if (mult[i+j*3] != 1)
|
||||
return 0;
|
||||
}
|
||||
else {
|
||||
if (mult[i+j*3] != 0)
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
float InvBasicMath_computeMatrixDeterminant(const float matrix[9])
|
||||
{
|
||||
return matrix[0] * (matrix[4]*matrix[8] - matrix[7]*matrix[5])
|
||||
-matrix[1] * (matrix[3]*matrix[8] - matrix[6]*matrix[5])
|
||||
+matrix[2] * (matrix[3]*matrix[7] - matrix[4]*matrix[6]);
|
||||
}
|
||||
|
||||
|
||||
@@ -0,0 +1,94 @@
|
||||
/*
|
||||
Copyright (c) 2014-2015 InvenSense Inc. Portions Copyright (c) 2014-2015 Movea. All rights reserved.
|
||||
|
||||
This software, related documentation and any modifications thereto (collectively "Software") is subject
|
||||
to InvenSense and its licensors' intellectual property rights under U.S. and international copyright and
|
||||
other intellectual property rights laws.
|
||||
|
||||
InvenSense and its licensors retain all intellectual property and proprietary rights in and to the Software
|
||||
and any use, reproduction, disclosure or distribution of the Software without an express license
|
||||
agreement from InvenSense is strictly prohibited.
|
||||
*/
|
||||
|
||||
/** @defgroup InvBasicMath InvBasicMath
|
||||
@brief This file contains basic (overloadable) math functions and macros
|
||||
@ingroup EmbUtils
|
||||
@{
|
||||
*/
|
||||
|
||||
#ifndef _INV_BASIC_MATH_H_
|
||||
#define _INV_BASIC_MATH_H_
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/** @brief Return absolute value of argument
|
||||
*/
|
||||
#ifndef INV_ABS
|
||||
# define INV_ABS(a) ((a) < 0 ? -(a) : (a))
|
||||
#endif
|
||||
|
||||
/** @brief Return minimum of two arguments
|
||||
*/
|
||||
#ifndef INV_MIN
|
||||
# define INV_MIN(a, b) ((a) < (b) ? (a) : (b))
|
||||
#endif
|
||||
|
||||
/** @brief Return maximum of two arguments
|
||||
*/
|
||||
#ifndef INV_MAX
|
||||
# define INV_MAX(a, b) ((a) > (b) ? (a) : (b))
|
||||
#endif
|
||||
|
||||
/** @brief Define value for pi
|
||||
*/
|
||||
#ifndef INV_PI
|
||||
# define INV_PI 3.14159265358979
|
||||
#endif
|
||||
#ifndef M_PI
|
||||
# define M_PI INV_PI
|
||||
#endif
|
||||
|
||||
/** @brief Return saturated integer
|
||||
*/
|
||||
#ifndef INV_SATURATE
|
||||
static inline long InvBasicMath_saturatel(long in, long min, long max)
|
||||
{
|
||||
if (in > max)
|
||||
return max;
|
||||
else if (in < min)
|
||||
return min;
|
||||
else
|
||||
return in;
|
||||
}
|
||||
# define INV_SATURATE(a, min, max) InvBasicMath_saturatel(a, min, max)
|
||||
#endif
|
||||
|
||||
/** @brief Compute log2 from integer
|
||||
*/
|
||||
#ifndef INV_LOG2
|
||||
unsigned int InvBasicMath_log2u(unsigned int val);
|
||||
# define INV_LOG2(a) InvBasicMath_log2u(a)
|
||||
#endif
|
||||
|
||||
/** @brief Check if matrix is orthonormal
|
||||
* @param [in] matrix 3x3 Matrix to be checked
|
||||
* @return 1 if it is an orthonormal matrix, 0 otherwise
|
||||
*/
|
||||
int InvBasicMath_isAnOrthonormalMatrix(const float matrix[9]);
|
||||
|
||||
/** @brief Compute the determinant of the matrix
|
||||
* @param [in] matrix 3x3 Matrix to be checked
|
||||
* @return the determinant value
|
||||
*/
|
||||
float InvBasicMath_computeMatrixDeterminant(const float matrix[9]);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif /* _INV_BASIC_MATH_H_ */
|
||||
|
||||
/** @} */
|
||||
|
||||
@@ -0,0 +1,389 @@
|
||||
/*
|
||||
Copyright (c) 2014-2015 InvenSense Inc. Portions Copyright (c) 2014-2015 Movea. All rights reserved.
|
||||
|
||||
This software, related documentation and any modifications thereto (collectively "Software") is subject
|
||||
to InvenSense and its licensors' intellectual property rights under U.S. and international copyright and
|
||||
other intellectual property rights laws.
|
||||
|
||||
InvenSense and its licensors retain all intellectual property and proprietary rights in and to the Software
|
||||
and any use, reproduction, disclosure or distribution of the Software without an express license
|
||||
agreement from InvenSense is strictly prohibited.
|
||||
*/
|
||||
|
||||
/** \defgroup RingBuffer RingBuffer
|
||||
\brief Macros to manage static circular buffer of any data type
|
||||
\ingroup EmbUtils
|
||||
\{
|
||||
*/
|
||||
|
||||
#ifndef _RING_BUFFER_H_
|
||||
#define _RING_BUFFER_H_
|
||||
|
||||
#include <stdint.h>
|
||||
#include <string.h>
|
||||
|
||||
|
||||
/** \brief Macro to declare a ring buffer
|
||||
\param[in] type type of item contained in the ring buffer
|
||||
\param[in] size number of items that can contain the ring buffer
|
||||
To improve speed, size should be a power of 2
|
||||
*/
|
||||
#define RINGBUFFER_DECLARE(type, size) \
|
||||
struct { \
|
||||
uint16_t read, write; \
|
||||
type buffer[size]; \
|
||||
}
|
||||
|
||||
/** \brief Macro to declare a volatile ring buffer, i.e. modified within an interrupt context
|
||||
\param[in] type type of item contained in the ring buffer
|
||||
\param[in] size number of items that can contain the ring buffer
|
||||
To improve speed, size should be a power of 2
|
||||
*/
|
||||
#define RINGBUFFER_VOLATILE_DECLARE(type, size) \
|
||||
struct { \
|
||||
volatile uint16_t read, write; \
|
||||
volatile type buffer[size]; \
|
||||
}
|
||||
|
||||
/** \brief Macro to declare a ring buffer
|
||||
\param[in] name name of the circular buffer
|
||||
\param[in] size number of items that can contain the ring buffer
|
||||
To improve speed, size should be a power of 2
|
||||
\param[in] type type of item contained in the ring buffer
|
||||
*/
|
||||
#define RINGBUFFER(name, size, type) RINGBUFFER_DECLARE(type, size) name
|
||||
|
||||
/** \brief Macro to declare a volatile ring buffer, i.e. modified within an interrupt context
|
||||
\param[in] name name of the circular buffer
|
||||
\param[in] size number of items that can contain the ring buffer
|
||||
To improve speed, size should be a power of 2
|
||||
\param[in] type type of item contained in the ring buffer
|
||||
*/
|
||||
#define RINGBUFFER_VOLATILE(name, size, type) RINGBUFFER_VOLATILE_DECLARE(type, size) name
|
||||
|
||||
/** \brief Macro to get maximum size of a ring buffer
|
||||
\param[in] rb pointer to the ring buffer
|
||||
\return maximum number of items that can contain the ringbuffer
|
||||
*/
|
||||
#define RINGBUFFER_MAXSIZE(rb) (sizeof((rb)->buffer)/sizeof((rb)->buffer[0]))
|
||||
|
||||
/** \brief Macro to get maximum size of a volatile ring buffer, i.e. modified within an interrupt context
|
||||
\param[in] rb pointer to the ring buffer
|
||||
\return maximum number of items that can contain the ringbuffer
|
||||
*/
|
||||
#define RINGBUFFER_VOLATILE_MAXSIZE(rb) RINGBUFFER_MAXSIZE(rb)
|
||||
|
||||
/** \brief Macro to get current size of a ring buffer
|
||||
\param[in] rb pointer to the ring buffer
|
||||
\return current number of items hold in the ringbuffer
|
||||
*/
|
||||
#define RINGBUFFER_SIZE(rb) ((uint16_t)((rb)->write - (rb)->read))
|
||||
|
||||
static inline uint16_t get_ringbuffer_volatile_size(void * rb)
|
||||
{
|
||||
struct { uint16_t read, write; } rb_var;
|
||||
memcpy(&rb_var, rb, sizeof(rb_var));
|
||||
return (rb_var.write - rb_var.read);
|
||||
}
|
||||
|
||||
/** \brief Macro to get current size of a volatile ring buffer, i.e. modified within an interrupt context
|
||||
\param[in] rb pointer to the ring buffer
|
||||
\return current number of items hold in the ringbuffer
|
||||
\warning it is advised to put this in a critical section
|
||||
*/
|
||||
#define RINGBUFFER_VOLATILE_SIZE(rb) get_ringbuffer_volatile_size(rb)
|
||||
|
||||
/** \brief Macro to check if a ring buffer is full
|
||||
\param[in] rb pointer to the ring buffer
|
||||
\return 1 if there is no slot left in the ring buffer, 0 otherwise
|
||||
*/
|
||||
#define RINGBUFFER_FULL(rb) (RINGBUFFER_SIZE(rb) == RINGBUFFER_MAXSIZE(rb))
|
||||
|
||||
/** \brief Macro to check if a volatile ring buffer, i.e. modified within an interrupt context, is full
|
||||
\param[in] rb pointer to the ring buffer
|
||||
\return 1 if there is no slot left in the ring buffer, 0 otherwise
|
||||
\warning it is advised to put this in a critical section
|
||||
*/
|
||||
#define RINGBUFFER_VOLATILE_FULL(rb) (RINGBUFFER_VOLATILE_SIZE(rb) == RINGBUFFER_VOLATILE_MAXSIZE(rb))
|
||||
|
||||
/** \brief Macro to check if a ring buffer is empty
|
||||
\param[in] rb pointer to the ring buffer
|
||||
\return 1 if there is no item in the ring buffer, 0 otherwise
|
||||
*/
|
||||
#define RINGBUFFER_EMPTY(rb) (RINGBUFFER_SIZE(rb) == 0)
|
||||
|
||||
/** \brief Macro to check if a volatile ring buffer, i.e. modified within an interrupt context, is empty
|
||||
\param[in] rb pointer to the ring buffer
|
||||
\return 1 if there is no item in the ring buffer, 0 otherwise
|
||||
\warning it is advised to put this in a critical section
|
||||
*/
|
||||
#define RINGBUFFER_VOLATILE_EMPTY(rb) (RINGBUFFER_VOLATILE_SIZE(rb) == 0)
|
||||
|
||||
/** \brief Macro to get number of available slot in a ring buffer
|
||||
\param[in] rb pointer to the ring buffer
|
||||
\return number of empty slot in the ring buffer
|
||||
*/
|
||||
#define RINGBUFFER_AVAILABLE(rb) (RINGBUFFER_MAXSIZE(rb) - RINGBUFFER_SIZE(rb))
|
||||
|
||||
/** \brief Macro to get number of available slot in a volatile ring buffer, i.e. modified within an interrupt context
|
||||
\param[in] rb pointer to the ring buffer
|
||||
\return number of empty slot in the ring buffer
|
||||
\warning it is advised to put this in a critical section
|
||||
*/
|
||||
#define RINGBUFFER_VOLATILE_AVAILABLE(rb) (RINGBUFFER_VOLATILE_MAXSIZE(rb) - RINGBUFFER_VOLATILE_SIZE(rb))
|
||||
|
||||
/** \brief Macro to clear a ring buffer
|
||||
\param[in] rb pointer to the ring buffer
|
||||
*/
|
||||
#define RINGBUFFER_CLEAR(rb) \
|
||||
do { \
|
||||
(rb)->read = 0, (rb)->write = 0; \
|
||||
} while(0)
|
||||
|
||||
/** \brief Macro to clear a volatile ring buffer, i.e. modified within an interrupt context
|
||||
\param[in] rb pointer to the ring buffer
|
||||
\warning it is advised to put this in a critical section
|
||||
*/
|
||||
#define RINGBUFFER_VOLATILE_CLEAR(rb) RINGBUFFER_CLEAR(rb)
|
||||
|
||||
/** \brief Push item by reference
|
||||
\param[in] rb pointer to the ring buffer
|
||||
\param[out] refData to available item slot
|
||||
\warning There is no error checking done.
|
||||
*/
|
||||
#define RINGBUFFER_PUSHREF(rb, refData) \
|
||||
do { \
|
||||
refData = &(rb)->buffer[(rb)->write % RINGBUFFER_MAXSIZE(rb)]; \
|
||||
++(rb)->write; \
|
||||
} while(0)
|
||||
|
||||
/** \brief Push item by reference
|
||||
\param[in] rb pointer to the ring buffer
|
||||
\param[out] refData to available item slot
|
||||
\warning There is no error checking done.
|
||||
\warning it is advised to put this in a critical section
|
||||
*/
|
||||
#define RINGBUFFER_VOLATILE_PUSHREF(rb, refData) \
|
||||
do { \
|
||||
uint16_t wr_ptr = (rb)->write; \
|
||||
refData = &(rb)->buffer[wr_ptr % RINGBUFFER_VOLATILE_MAXSIZE(rb)]; \
|
||||
++(rb)->write; \
|
||||
} while(0)
|
||||
|
||||
/** \brief Return reference to next available slot
|
||||
No push is performed
|
||||
\param[in] rb pointer to the ring buffer
|
||||
\param[out] refData to available item slot
|
||||
\warning There is no error checking done.
|
||||
*/
|
||||
#define RINGBUFFER_GETREFNEXT(rb, refData) \
|
||||
do { \
|
||||
refData = &(rb)->buffer[(rb)->write % RINGBUFFER_MAXSIZE(rb)]; \
|
||||
} while(0)
|
||||
|
||||
/** \brief Return reference to next available slot
|
||||
No push is performed
|
||||
\param[in] rb pointer to the ring buffer
|
||||
\param[out] refData to available item slot
|
||||
\warning There is no error checking done.
|
||||
\warning it is advised to put this in a critical section
|
||||
*/
|
||||
#define RINGBUFFER_VOLATILE_GETREFNEXT(rb, refData) \
|
||||
do { \
|
||||
uint16_t wr_ptr = (rb)->write; \
|
||||
refData = &(rb)->buffer[wr_ptr % RINGBUFFER_VOLATILE_MAXSIZE(rb)]; \
|
||||
} while(0)
|
||||
|
||||
/** \brief Increment write counter
|
||||
Actually performed a push (assuming data were already copied)
|
||||
\param[in] rb pointer to the ring buffer
|
||||
\warning There is no error checking done.
|
||||
*/
|
||||
#define RINGBUFFER_INCREMENT(rb, refData) \
|
||||
do { \
|
||||
++(rb)->write; \
|
||||
} while(0)
|
||||
|
||||
/** \brief Increment write counter
|
||||
Actually performed a push (assuming data were already copied)
|
||||
\param[in] rb pointer to the ring buffer
|
||||
\warning There is no error checking done.
|
||||
\warning it is advised to put this in a critical section
|
||||
*/
|
||||
#define RINGBUFFER_VOLATILE_INCREMENT(rb, refData) \
|
||||
do { \
|
||||
++(rb)->write; \
|
||||
} while(0)
|
||||
|
||||
/** \brief Return reference to youngest item
|
||||
\param[in] rb pointer to the ring buffer
|
||||
\param[out] refData reference to youngest item
|
||||
\warning There is no error checking done.
|
||||
*/
|
||||
#define RINGBUFFER_BACK(rb, refData) \
|
||||
do { \
|
||||
refData = &(rb)->buffer[((rb)->write-1) % RINGBUFFER_MAXSIZE(rb)]; \
|
||||
} while(0)
|
||||
|
||||
/** \brief Return reference to youngest item
|
||||
\param[in] rb pointer to the ring buffer
|
||||
\param[out] refData reference to youngest item
|
||||
\warning There is no error checking done.
|
||||
\warning it is advised to put this in a critical section
|
||||
*/
|
||||
#define RINGBUFFER_VOLATILE_BACK(rb, refData) \
|
||||
do { \
|
||||
uint16_t wr_ptr = (rb)->write; \
|
||||
refData = &(rb)->buffer[(wr_ptr-1) % RINGBUFFER_VOLATILE_MAXSIZE(rb)]; \
|
||||
} while(0)
|
||||
|
||||
/** \brief Macro to push an item to a ring buffer
|
||||
\param[in] rb pointer to the ring buffer
|
||||
\param[in] ptrData pointer to the item to push.
|
||||
\warning There is no error checking done.
|
||||
You must check for fullness before pushing data
|
||||
*/
|
||||
#define RINGBUFFER_PUSH(rb, ptrData) \
|
||||
do { \
|
||||
(rb)->buffer[(rb)->write % RINGBUFFER_MAXSIZE(rb)] = *ptrData; \
|
||||
++(rb)->write; \
|
||||
} while(0)
|
||||
|
||||
/** \brief Macro to push an item to a volatile ring buffer
|
||||
\param[in] rb pointer to the ring buffer
|
||||
\param[in] ptrData pointer to the item to push.
|
||||
\warning There is no error checking done.
|
||||
You must check for fullness before pushing data
|
||||
\warning it is advised to put this in a critical section
|
||||
*/
|
||||
#define RINGBUFFER_VOLATILE_PUSH(rb, ptrData) \
|
||||
do { \
|
||||
uint16_t wr_ptr = (rb)->write; \
|
||||
(rb)->buffer[wr_ptr % RINGBUFFER_VOLATILE_MAXSIZE(rb)] = *ptrData; \
|
||||
++(rb)->write; \
|
||||
} while(0)
|
||||
|
||||
/** \brief Macro to unpush an item to a ring buffer
|
||||
\param[in] rb pointer to the ring buffer
|
||||
\param[in] ptrData pointer to placeholder to hold unpushed item
|
||||
\warning There is no error checking done.
|
||||
You must check for emptiness before pushing data
|
||||
*/
|
||||
#define RINGBUFFER_UNPUSH(rb, ptrData) \
|
||||
do { \
|
||||
--(rb)->write; \
|
||||
*ptrData = (rb)->buffer[(rb)->write % RINGBUFFER_MAXSIZE(rb)]; \
|
||||
} while(0)
|
||||
|
||||
/** \brief Macro to unpush an item to a volatile ring buffer
|
||||
\param[in] rb pointer to the ring buffer
|
||||
\param[in] ptrData pointer to placeholder to hold unpushed item
|
||||
\warning There is no error checking done.
|
||||
You must check for emptiness before pushing data
|
||||
\warning it is advised to put this in a critical section
|
||||
*/
|
||||
#define RINGBUFFER_VOLATILE_UNPUSH(rb, ptrData) \
|
||||
do { \
|
||||
--(rb)->write; \
|
||||
uint16_t wr_ptr = (rb)->write; \
|
||||
*ptrData = (rb)->buffer[wr_ptr % RINGBUFFER_VOLATILE_MAXSIZE(rb)]; \
|
||||
} while(0)
|
||||
|
||||
/** \brief Return reference to oldest item
|
||||
\param[in] rb pointer to the ring buffer
|
||||
\param[out] refData reference to oldest item
|
||||
\warning There is no error checking done.
|
||||
*/
|
||||
#define RINGBUFFER_FRONT(rb, refData) \
|
||||
do { \
|
||||
refData = &(rb)->buffer[(rb)->read % RINGBUFFER_MAXSIZE(rb)]; \
|
||||
} while(0)
|
||||
|
||||
/** \brief Return reference to oldest item
|
||||
\param[in] rb pointer to the ring buffer
|
||||
\param[out] refData reference to oldest item
|
||||
\warning There is no error checking done.
|
||||
\warning it is advised to put this in a critical section
|
||||
*/
|
||||
#define RINGBUFFER_VOLATILE_FRONT(rb, refData) \
|
||||
do { \
|
||||
uint16_t rd_ptr = (rb)->read; \
|
||||
refData = &(rb)->buffer[rd_ptr % RINGBUFFER_VOLATILE_MAXSIZE(rb)]; \
|
||||
} while(0)
|
||||
|
||||
/** \brief Macro to pop an item from a ring buffer
|
||||
\param[in] rb pointer to the ring buffer
|
||||
\param[out] ptrData pointer to placeholder to hold popped item
|
||||
\warning There is no error checking done.
|
||||
You must check for emptiness before popping data
|
||||
*/
|
||||
#define RINGBUFFER_POP(rb, ptrData) \
|
||||
do { \
|
||||
*ptrData = (rb)->buffer[(rb)->read % RINGBUFFER_MAXSIZE(rb)]; \
|
||||
++(rb)->read; \
|
||||
} while(0)
|
||||
|
||||
/** \brief Macro to pop an item from a volatile ring buffer
|
||||
\param[in] rb pointer to the ring buffer
|
||||
\param[out] ptrData pointer to placeholder to hold popped item
|
||||
\warning There is no error checking done.
|
||||
You must check for emptiness before popping data
|
||||
\warning it is advised to put this in a critical section
|
||||
*/
|
||||
#define RINGBUFFER_VOLATILE_POP(rb, ptrData) \
|
||||
do { \
|
||||
uint16_t rd_ptr = (rb)->read; \
|
||||
*ptrData = (rb)->buffer[rd_ptr % RINGBUFFER_VOLATILE_MAXSIZE(rb)]; \
|
||||
++(rb)->read; \
|
||||
} while(0)
|
||||
|
||||
/** \brief Macro to pop an item from a ring buffer (data is not copied)
|
||||
\param[in] rb pointer to the ring buffer
|
||||
\warning There is no error checking done.
|
||||
You must check for emptiness before popping data
|
||||
*/
|
||||
#define RINGBUFFER_POPNLOSE(rb) \
|
||||
do { \
|
||||
++(rb)->read; \
|
||||
} while(0)
|
||||
|
||||
/** \brief Macro to pop an item from a volatile ring buffer (data is not copied)
|
||||
\param[in] rb pointer to the ring buffer
|
||||
\warning There is no error checking done.
|
||||
You must check for emptiness before popping data
|
||||
\warning it is advised to put this in a critical section
|
||||
*/
|
||||
#define RINGBUFFER_VOLATILE_POPNLOSE(rb) \
|
||||
do { \
|
||||
++(rb)->read; \
|
||||
} while(0)
|
||||
|
||||
/** \brief Macro to unpop an item to a ring buffer
|
||||
\param[in] rb pointer to the ring buffer
|
||||
\param[out] ptrData pointer to to the item to unpop.
|
||||
\warning There is no error checking done.
|
||||
You must check for fullness before unpopping data
|
||||
*/
|
||||
#define RINGBUFFER_UNPOP(rb, ptrData) \
|
||||
do { \
|
||||
--(rb)->read; \
|
||||
(rb)->buffer[(rb)->read % RINGBUFFER_MAXSIZE(rb)] = *ptrData; \
|
||||
} while(0)
|
||||
|
||||
/** \brief Macro to unpop an item to a volatile ring buffer
|
||||
\param[in] rb pointer to the ring buffer
|
||||
\param[out] ptrData pointer to to the item to unpop.
|
||||
\warning There is no error checking done.
|
||||
You must check for fullness before unpopping data
|
||||
\warning it is advised to put this in a critical section
|
||||
*/
|
||||
#define RINGBUFFER_VOLATILE_UNPOP(rb, ptrData) \
|
||||
do { \
|
||||
--(rb)->read; \
|
||||
uint16_t rd_ptr = (rb)->read; \
|
||||
(rb)->buffer[rd_ptr % RINGBUFFER_VOLATILE_MAXSIZE(rb)] = *ptrData; \
|
||||
} while(0)
|
||||
|
||||
#endif
|
||||
|
||||
/** \} */
|
||||
@@ -0,0 +1,47 @@
|
||||
/*
|
||||
* ________________________________________________________________________________________________________
|
||||
* Copyright (c) 2015-2015 InvenSense Inc. All rights reserved.
|
||||
*
|
||||
* This software, related documentation and any modifications thereto (collectively “Software”) is subject
|
||||
* to InvenSense and its licensors' intellectual property rights under U.S. and international copyright
|
||||
* and other intellectual property rights laws.
|
||||
*
|
||||
* InvenSense and its licensors retain all intellectual property and proprietary rights in and to the Software
|
||||
* and any use, reproduction, disclosure or distribution of the Software without an express license agreement
|
||||
* from InvenSense is strictly prohibited.
|
||||
*
|
||||
* EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, THE SOFTWARE IS
|
||||
* PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED
|
||||
* TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT.
|
||||
* EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, IN NO EVENT SHALL
|
||||
* INVENSENSE BE LIABLE FOR ANY DIRECT, SPECIAL, INDIRECT, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, OR ANY
|
||||
* DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT,
|
||||
* NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE
|
||||
* OF THE SOFTWARE.
|
||||
* ________________________________________________________________________________________________________
|
||||
*/
|
||||
|
||||
/** @brief Custom definition for boolean type to avoid compiler discrepancies
|
||||
* @{
|
||||
*/
|
||||
|
||||
#ifndef _INV_BOOL_H_
|
||||
#define _INV_BOOL_H_
|
||||
|
||||
typedef int inv_bool_t;
|
||||
|
||||
#ifndef __cplusplus
|
||||
|
||||
#ifndef true
|
||||
#define true 1
|
||||
#endif
|
||||
|
||||
#ifndef false
|
||||
#define false 0
|
||||
#endif
|
||||
|
||||
#endif /* __cplusplus */
|
||||
|
||||
#endif /* _INV_BOOL_H_ */
|
||||
|
||||
/** @} */
|
||||
@@ -0,0 +1,64 @@
|
||||
/*
|
||||
* ________________________________________________________________________________________________________
|
||||
* Copyright (c) 2015-2015 InvenSense Inc. All rights reserved.
|
||||
*
|
||||
* This software, related documentation and any modifications thereto (collectively “Software”) is subject
|
||||
* to InvenSense and its licensors' intellectual property rights under U.S. and international copyright
|
||||
* and other intellectual property rights laws.
|
||||
*
|
||||
* InvenSense and its licensors retain all intellectual property and proprietary rights in and to the Software
|
||||
* and any use, reproduction, disclosure or distribution of the Software without an express license agreement
|
||||
* from InvenSense is strictly prohibited.
|
||||
*
|
||||
* EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, THE SOFTWARE IS
|
||||
* PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED
|
||||
* TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT.
|
||||
* EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, IN NO EVENT SHALL
|
||||
* INVENSENSE BE LIABLE FOR ANY DIRECT, SPECIAL, INDIRECT, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, OR ANY
|
||||
* DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT,
|
||||
* NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE
|
||||
* OF THE SOFTWARE.
|
||||
* ________________________________________________________________________________________________________
|
||||
*/
|
||||
|
||||
/** @defgroup InvError Error code
|
||||
* @brief Common error code
|
||||
*
|
||||
* @ingroup EmbUtils
|
||||
* @{
|
||||
*/
|
||||
|
||||
#ifndef _INV_ERROR_H_
|
||||
#define _INV_ERROR_H_
|
||||
|
||||
/** @brief Common error code definition
|
||||
*/
|
||||
enum inv_error
|
||||
{
|
||||
INV_ERROR_SUCCESS = 0, /**< no error */
|
||||
INV_ERROR = -1, /**< unspecified error */
|
||||
INV_ERROR_NIMPL = -2, /**< function not implemented for given
|
||||
arguments */
|
||||
INV_ERROR_TRANSPORT = -3, /**< error occurred at transport level */
|
||||
INV_ERROR_TIMEOUT = -4, /**< action did not complete in the expected
|
||||
time window */
|
||||
INV_ERROR_SIZE = -5, /**< size/length of given arguments is not
|
||||
suitable to complete requested action */
|
||||
INV_ERROR_OS = -6, /**< error related to OS */
|
||||
INV_ERROR_IO = -7, /**< error related to IO operation */
|
||||
INV_ERROR_MEM = -9, /**< not enough memory to complete requested
|
||||
action */
|
||||
INV_ERROR_HW = -10, /**< error at HW level */
|
||||
INV_ERROR_BAD_ARG = -11, /**< provided arguments are not good to
|
||||
perform requested action */
|
||||
INV_ERROR_UNEXPECTED = -12, /**< something unexpected happened */
|
||||
INV_ERROR_FILE = -13, /**< cannot access file or unexpected format */
|
||||
INV_ERROR_PATH = -14, /**< invalid file path */
|
||||
INV_ERROR_IMAGE_TYPE = -15, /**< error when image type is not managed */
|
||||
INV_ERROR_WATCHDOG = -16, /**< error when device doesn't respond
|
||||
to ping */
|
||||
};
|
||||
|
||||
#endif /* _INV_ERROR_H_ */
|
||||
|
||||
/** @} */
|
||||
@@ -0,0 +1,39 @@
|
||||
/*
|
||||
* ________________________________________________________________________________________________________
|
||||
* Copyright (c) 2015-2015 InvenSense Inc. All rights reserved.
|
||||
*
|
||||
* This software, related documentation and any modifications thereto (collectively “Software”) is subject
|
||||
* to InvenSense and its licensors' intellectual property rights under U.S. and international copyright
|
||||
* and other intellectual property rights laws.
|
||||
*
|
||||
* InvenSense and its licensors retain all intellectual property and proprietary rights in and to the Software
|
||||
* and any use, reproduction, disclosure or distribution of the Software without an express license agreement
|
||||
* from InvenSense is strictly prohibited.
|
||||
*
|
||||
* EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, THE SOFTWARE IS
|
||||
* PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED
|
||||
* TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT.
|
||||
* EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, IN NO EVENT SHALL
|
||||
* INVENSENSE BE LIABLE FOR ANY DIRECT, SPECIAL, INDIRECT, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, OR ANY
|
||||
* DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT,
|
||||
* NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE
|
||||
* OF THE SOFTWARE.
|
||||
* ________________________________________________________________________________________________________
|
||||
*/
|
||||
|
||||
#ifndef _INV_IDD_EXPORT_H_
|
||||
#define _INV_IDD_EXPORT_H_
|
||||
|
||||
#if defined(_WIN32)
|
||||
#if !defined(INV_EXPORT) && defined(INV_DO_DLL_EXPORT)
|
||||
#define INV_EXPORT __declspec(dllexport)
|
||||
#elif !defined(INV_EXPORT) && defined(INV_DO_DLL_IMPORT)
|
||||
#define INV_EXPORT __declspec(dllimport)
|
||||
#endif
|
||||
#endif
|
||||
|
||||
#if !defined(INV_EXPORT)
|
||||
#define INV_EXPORT
|
||||
#endif
|
||||
|
||||
#endif /* _INV_IDD_EXPORT_H_ */
|
||||
@@ -0,0 +1,370 @@
|
||||
/*
|
||||
* ________________________________________________________________________________________________________
|
||||
* Copyright (c) 2017 InvenSense Inc. All rights reserved.
|
||||
*
|
||||
* This software, related documentation and any modifications thereto (collectively "Software") is subject
|
||||
* to InvenSense and its licensors' intellectual property rights under U.S. and international copyright
|
||||
* and other intellectual property rights laws.
|
||||
*
|
||||
* InvenSense and its licensors retain all intellectual property and proprietary rights in and to the Software
|
||||
* and any use, reproduction, disclosure or distribution of the Software without an express license agreement
|
||||
* from InvenSense is strictly prohibited.
|
||||
*
|
||||
* EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, THE SOFTWARE IS
|
||||
* PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED
|
||||
* TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT.
|
||||
* EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, IN NO EVENT SHALL
|
||||
* INVENSENSE BE LIABLE FOR ANY DIRECT, SPECIAL, INDIRECT, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, OR ANY
|
||||
* DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT,
|
||||
* NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE
|
||||
* OF THE SOFTWARE.
|
||||
* ________________________________________________________________________________________________________
|
||||
*/
|
||||
|
||||
#include "inv_imu_defs.h"
|
||||
#include "inv_imu_extfunc.h"
|
||||
#include "inv_imu_driver.h"
|
||||
#include "inv_imu_apex.h"
|
||||
|
||||
int inv_imu_apex_enable_ff(struct inv_imu_device *s)
|
||||
{
|
||||
int status = 0;
|
||||
uint8_t value;
|
||||
|
||||
status |= inv_imu_start_dmp(s);
|
||||
|
||||
status |= inv_imu_read_reg(s, APEX_CONFIG1, 1, &value);
|
||||
value &= ~APEX_CONFIG1_FF_ENABLE_MASK;
|
||||
value |= (uint8_t)APEX_CONFIG1_FF_ENABLE_EN;
|
||||
status |= inv_imu_write_reg(s, APEX_CONFIG1, 1, &value);
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
int inv_imu_apex_disable_ff(struct inv_imu_device *s)
|
||||
{
|
||||
int status = 0;
|
||||
uint8_t value;
|
||||
|
||||
status |= inv_imu_read_reg(s, APEX_CONFIG1, 1, &value);
|
||||
value &= ~APEX_CONFIG1_FF_ENABLE_MASK;
|
||||
value |= (uint8_t)APEX_CONFIG1_FF_ENABLE_DIS;
|
||||
status |= inv_imu_write_reg(s, APEX_CONFIG1, 1, &value);
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
int inv_imu_apex_enable_smd(struct inv_imu_device *s)
|
||||
{
|
||||
int status = 0;
|
||||
uint8_t value;
|
||||
|
||||
status |= inv_imu_start_dmp(s);
|
||||
|
||||
status |= inv_imu_read_reg(s, APEX_CONFIG1, 1, &value);
|
||||
value &= ~APEX_CONFIG1_SMD_ENABLE_MASK;
|
||||
value |= (uint8_t)APEX_CONFIG1_SMD_ENABLE_EN;
|
||||
status |= inv_imu_write_reg(s, APEX_CONFIG1, 1, &value);
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
int inv_imu_apex_disable_smd(struct inv_imu_device *s)
|
||||
{
|
||||
int status = 0;
|
||||
uint8_t value;
|
||||
|
||||
status |= inv_imu_read_reg(s, APEX_CONFIG1, 1, &value);
|
||||
value &= ~APEX_CONFIG1_SMD_ENABLE_MASK;
|
||||
value |= (uint8_t)APEX_CONFIG1_SMD_ENABLE_DIS;
|
||||
status |= inv_imu_write_reg(s, APEX_CONFIG1, 1, &value);
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
int inv_imu_apex_init_parameters_struct(struct inv_imu_device *s, inv_imu_apex_parameters_t *apex_inputs)
|
||||
{
|
||||
int status = 0;
|
||||
(void)s;
|
||||
|
||||
/* Default parameters at POR */
|
||||
apex_inputs->pedo_amp_th = APEX_CONFIG3_PEDO_AMP_TH_62_MG;
|
||||
apex_inputs->pedo_step_cnt_th = 0x5;
|
||||
apex_inputs->pedo_step_det_th = 0x2;
|
||||
apex_inputs->pedo_sb_timer_th = APEX_CONFIG4_PEDO_SB_TIMER_TH_150_SAMPLES;
|
||||
apex_inputs->pedo_hi_enrgy_th = APEX_CONFIG4_PEDO_HI_ENRGY_TH_104_MG;
|
||||
apex_inputs->tilt_wait_time = APEX_CONFIG5_TILT_WAIT_TIME_4_S;
|
||||
apex_inputs->power_save_time = APEX_CONFIG2_DMP_POWER_SAVE_TIME_SEL_8_S;
|
||||
apex_inputs->power_save = APEX_CONFIG0_DMP_POWER_SAVE_EN;
|
||||
apex_inputs->sensitivity_mode = APEX_CONFIG9_SENSITIVITY_MODE_NORMAL;
|
||||
apex_inputs->low_energy_amp_th = APEX_CONFIG2_LOW_ENERGY_AMP_TH_SEL_80_MG;
|
||||
apex_inputs->smd_sensitivity = APEX_CONFIG9_SMD_SENSITIVITY_0;
|
||||
apex_inputs->ff_debounce_duration = APEX_CONFIG9_FF_DEBOUNCE_DURATION_2000_MS;
|
||||
apex_inputs->ff_max_duration_cm = APEX_CONFIG12_FF_MAX_DURATION_204_CM;
|
||||
apex_inputs->ff_min_duration_cm = APEX_CONFIG12_FF_MIN_DURATION_10_CM;
|
||||
apex_inputs->lowg_peak_th = APEX_CONFIG10_LOWG_PEAK_TH_563_MG;
|
||||
apex_inputs->lowg_peak_hyst = APEX_CONFIG5_LOWG_PEAK_TH_HYST_156_MG;
|
||||
apex_inputs->lowg_samples_th = APEX_CONFIG10_LOWG_TIME_TH_1_SAMPLE;
|
||||
apex_inputs->highg_peak_th = APEX_CONFIG11_HIGHG_PEAK_TH_2500_MG;
|
||||
apex_inputs->highg_peak_hyst = APEX_CONFIG5_HIGHG_PEAK_TH_HYST_156_MG;
|
||||
apex_inputs->highg_samples_th = APEX_CONFIG11_HIGHG_TIME_TH_1_SAMPLE;
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
int inv_imu_apex_configure_parameters(struct inv_imu_device *s, const inv_imu_apex_parameters_t *apex_inputs)
|
||||
{
|
||||
int status = 0;
|
||||
uint8_t data;
|
||||
uint8_t apexConfig[7];
|
||||
APEX_CONFIG1_PED_ENABLE_t pedo_state;
|
||||
APEX_CONFIG1_TILT_ENABLE_t tilt_state;
|
||||
APEX_CONFIG1_FF_ENABLE_t ff_state;
|
||||
APEX_CONFIG1_SMD_ENABLE_t smd_state;
|
||||
|
||||
/* DMP cannot be configured if it is running, hence make sure all APEX algorithms are off */
|
||||
status |= inv_imu_read_reg(s, APEX_CONFIG1, 1, &data);
|
||||
pedo_state = (APEX_CONFIG1_PED_ENABLE_t)(data & APEX_CONFIG1_PED_ENABLE_MASK);
|
||||
tilt_state = (APEX_CONFIG1_TILT_ENABLE_t)(data & APEX_CONFIG1_TILT_ENABLE_MASK);
|
||||
ff_state = (APEX_CONFIG1_FF_ENABLE_t)(data & APEX_CONFIG1_FF_ENABLE_MASK);
|
||||
smd_state = (APEX_CONFIG1_SMD_ENABLE_t)(data & APEX_CONFIG1_SMD_ENABLE_MASK);
|
||||
if (pedo_state == APEX_CONFIG1_PED_ENABLE_EN)
|
||||
return INV_ERROR;
|
||||
if (tilt_state == APEX_CONFIG1_TILT_ENABLE_EN)
|
||||
return INV_ERROR;
|
||||
if (ff_state == APEX_CONFIG1_FF_ENABLE_EN)
|
||||
return INV_ERROR;
|
||||
if (smd_state == APEX_CONFIG1_SMD_ENABLE_EN)
|
||||
return INV_ERROR;
|
||||
|
||||
|
||||
status |= inv_imu_switch_on_mclk(s);
|
||||
|
||||
/* Power Save mode and low energy amplitude threshold (for Pedometer in Slow Walk mode) */
|
||||
/* APEX_CONFIG2_MREG1 */
|
||||
apexConfig[0] = (uint8_t)apex_inputs->power_save_time
|
||||
| (uint8_t)apex_inputs->low_energy_amp_th;
|
||||
|
||||
/* Pedometer parameters */
|
||||
/* APEX_CONFIG3_MREG1 */
|
||||
apexConfig[1] = (uint8_t)apex_inputs->pedo_amp_th
|
||||
| (apex_inputs->pedo_step_cnt_th & APEX_CONFIG3_PED_STEP_CNT_TH_SEL_MASK);
|
||||
|
||||
/* APEX_CONFIG4_MREG1 */
|
||||
apexConfig[2] = ((apex_inputs->pedo_step_det_th << APEX_CONFIG4_PED_STEP_DET_TH_SEL_POS)
|
||||
& APEX_CONFIG4_PED_STEP_DET_TH_SEL_MASK)
|
||||
| (uint8_t)apex_inputs->pedo_sb_timer_th
|
||||
| (uint8_t)apex_inputs->pedo_hi_enrgy_th;
|
||||
|
||||
/* Tilt, Lowg and highg parameters */
|
||||
/* APEX_CONFIG5_MREG1 */
|
||||
apexConfig[3] = (uint8_t)apex_inputs->tilt_wait_time
|
||||
| (uint8_t)apex_inputs->lowg_peak_hyst
|
||||
| (uint8_t)apex_inputs->highg_peak_hyst;
|
||||
|
||||
status |= inv_imu_write_reg(s, APEX_CONFIG2_MREG1, 4, &apexConfig[0]);
|
||||
|
||||
|
||||
/* APEX_CONFIG0 */
|
||||
status |= inv_imu_read_reg(s, APEX_CONFIG0, 1, &apexConfig[0]);
|
||||
apexConfig[0] &= ~APEX_CONFIG0_DMP_POWER_SAVE_EN_MASK;
|
||||
apexConfig[0] |= apex_inputs->power_save;
|
||||
status |= inv_imu_write_reg(s, APEX_CONFIG0, 1, &apexConfig[0]);
|
||||
|
||||
/* free fall parameter, SMD parameter and parameters for Pedometer in Slow Walk mode */
|
||||
/* APEX_CONFIG9_MREG1 */
|
||||
apexConfig[0] = (uint8_t)apex_inputs->ff_debounce_duration
|
||||
| (uint8_t)apex_inputs->smd_sensitivity
|
||||
| (uint8_t)apex_inputs->sensitivity_mode;
|
||||
|
||||
/* Lowg and highg parameters and free fall parameters */
|
||||
/* APEX_CONFIG10_MREG1 */
|
||||
apexConfig[1] = (uint8_t)apex_inputs->lowg_peak_th
|
||||
| (uint8_t)apex_inputs->lowg_samples_th;
|
||||
|
||||
/* APEX_CONFIG11_MREG1 */
|
||||
apexConfig[2] = (uint8_t)apex_inputs->highg_peak_th
|
||||
| (uint8_t)apex_inputs->highg_samples_th;
|
||||
|
||||
status |= inv_imu_write_reg(s, APEX_CONFIG9_MREG1, 3, &apexConfig[0]);
|
||||
|
||||
|
||||
/* APEX_CONFIG12_MREG1 */
|
||||
apexConfig[0] = (uint8_t)apex_inputs->ff_max_duration_cm
|
||||
| (uint8_t)apex_inputs->ff_min_duration_cm;
|
||||
|
||||
status |= inv_imu_write_reg(s, APEX_CONFIG12_MREG1, 1, &apexConfig[0]);
|
||||
|
||||
status |= inv_imu_switch_off_mclk(s);
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
int inv_imu_apex_get_parameters(struct inv_imu_device *s, inv_imu_apex_parameters_t *apex_params)
|
||||
{
|
||||
int status = 0;
|
||||
uint8_t data[7];
|
||||
uint8_t value;
|
||||
|
||||
status |= inv_imu_read_reg(s, APEX_CONFIG0, 1, &value);
|
||||
apex_params->power_save = (APEX_CONFIG0_DMP_POWER_SAVE_t)(value & APEX_CONFIG0_DMP_POWER_SAVE_EN_MASK);
|
||||
|
||||
/* Access continuous config registers (CONFIG2-CONFIG11) */
|
||||
status |= inv_imu_read_reg(s, APEX_CONFIG2_MREG1, sizeof(data), &data[0]);
|
||||
|
||||
/* Get params from apex_config2 : dmp_power_save_time and low_energy_amp_th */
|
||||
apex_params->power_save_time = (APEX_CONFIG2_DMP_POWER_SAVE_TIME_t)
|
||||
(data[0] & APEX_CONFIG2_DMP_POWER_SAVE_TIME_SEL_MASK);
|
||||
apex_params->low_energy_amp_th = (APEX_CONFIG2_LOW_ENERGY_AMP_TH_t)
|
||||
(data[0] & APEX_CONFIG2_LOW_ENERGY_AMP_TH_SEL_MASK);
|
||||
|
||||
/* Get params from apex_config3 : pedo_amp_th and pedo_step_cnt_th */
|
||||
apex_params->pedo_amp_th = (APEX_CONFIG3_PEDO_AMP_TH_t)
|
||||
(data[1] & APEX_CONFIG3_PED_AMP_TH_SEL_MASK);
|
||||
apex_params->pedo_step_cnt_th = (data[1] & APEX_CONFIG3_PED_STEP_CNT_TH_SEL_MASK)
|
||||
>> APEX_CONFIG3_PED_STEP_CNT_TH_SEL_POS;
|
||||
|
||||
/* Get params from apex_config4 : pedo_step_det_th, pedo_sb_timer_th and pedo_hi_enrgy_th */
|
||||
apex_params->pedo_step_det_th = (data[2] & APEX_CONFIG4_PED_STEP_DET_TH_SEL_MASK)
|
||||
>> APEX_CONFIG4_PED_STEP_DET_TH_SEL_POS;
|
||||
apex_params->pedo_sb_timer_th = (APEX_CONFIG4_PEDO_SB_TIMER_TH_t)
|
||||
(data[2] & APEX_CONFIG4_PED_SB_TIMER_TH_SEL_MASK);
|
||||
apex_params->pedo_hi_enrgy_th = (APEX_CONFIG4_PEDO_HI_ENRGY_TH_t)
|
||||
(data[2] & APEX_CONFIG4_PED_HI_EN_TH_SEL_MASK);
|
||||
|
||||
/* Get params from apex_config5 : tilt_wait_time, lowg_peak_hyst and highg_peak_hyst */
|
||||
apex_params->tilt_wait_time = (APEX_CONFIG5_TILT_WAIT_TIME_t)
|
||||
(data[3] & APEX_CONFIG5_TILT_WAIT_TIME_SEL_MASK);
|
||||
apex_params->lowg_peak_hyst = (APEX_CONFIG5_LOWG_PEAK_TH_HYST_t)
|
||||
(data[3] & APEX_CONFIG5_LOWG_PEAK_TH_HYST_SEL_MASK);
|
||||
apex_params->highg_peak_hyst = (APEX_CONFIG5_HIGHG_PEAK_TH_HYST_t)
|
||||
(data[3] & APEX_CONFIG5_HIGHG_PEAK_TH_HYST_SEL_MASK);
|
||||
|
||||
/* Get params from apex_config9 : ff_debounce_duration, smd_sensitivity and sensitivity_mode */
|
||||
apex_params->ff_debounce_duration = (APEX_CONFIG9_FF_DEBOUNCE_DURATION_t)
|
||||
(data[4] & APEX_CONFIG9_FF_DEBOUNCE_DURATION_SEL_MASK);
|
||||
apex_params->smd_sensitivity = (APEX_CONFIG9_SMD_SENSITIVITY_t)
|
||||
(data[4] & APEX_CONFIG9_SMD_SENSITIVITY_SEL_MASK);
|
||||
apex_params->sensitivity_mode = (APEX_CONFIG9_SENSITIVITY_MODE_t)
|
||||
(data[4] & APEX_CONFIG9_SENSITIVITY_MODE_MASK);
|
||||
|
||||
/* Get params from apex_config10 : lowg_peak_th and lowg_samples_th */
|
||||
apex_params->lowg_peak_th = (APEX_CONFIG10_LOWG_PEAK_TH_t)
|
||||
(data[5] & APEX_CONFIG10_LOWG_PEAK_TH_SEL_MASK);
|
||||
apex_params->lowg_samples_th = (APEX_CONFIG10_LOWG_TIME_TH_SAMPLES_t)
|
||||
(data[5] & APEX_CONFIG10_LOWG_TIME_TH_SEL_MASK);
|
||||
|
||||
/* Get params from apex_config11 : highg_peak_th and highg_samples_th */
|
||||
apex_params->highg_peak_th = (APEX_CONFIG11_HIGHG_PEAK_TH_t)
|
||||
(data[6] & APEX_CONFIG11_HIGHG_PEAK_TH_SEL_MASK);
|
||||
apex_params->highg_samples_th = (APEX_CONFIG11_HIGHG_TIME_TH_SAMPLES_t)
|
||||
(data[6] & APEX_CONFIG11_HIGHG_TIME_TH_SEL_MASK);
|
||||
|
||||
/* Access apex reg 12 */
|
||||
status |= inv_imu_read_reg(s, APEX_CONFIG12_MREG1, 1, &data[0]);
|
||||
|
||||
/* Get params from apex_config12 : ff_max_duration_cm and ff_min_duration_cm */
|
||||
apex_params->ff_max_duration_cm = (APEX_CONFIG12_FF_MAX_DURATION_t)
|
||||
(data[0] & APEX_CONFIG12_FF_MAX_DURATION_SEL_MASK);
|
||||
apex_params->ff_min_duration_cm = (APEX_CONFIG12_FF_MIN_DURATION_t)
|
||||
(data[0] & APEX_CONFIG12_FF_MIN_DURATION_SEL_MASK);
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
int inv_imu_apex_set_frequency(struct inv_imu_device *s, const APEX_CONFIG1_DMP_ODR_t frequency)
|
||||
{
|
||||
uint8_t value;
|
||||
int status = 0;
|
||||
|
||||
status |= inv_imu_read_reg(s, APEX_CONFIG1, 1, &value);
|
||||
|
||||
value &= ~APEX_CONFIG1_DMP_ODR_MASK;
|
||||
value |= frequency;
|
||||
|
||||
status |= inv_imu_write_reg(s, APEX_CONFIG1, 1, &value);
|
||||
return status;
|
||||
}
|
||||
|
||||
int inv_imu_apex_enable_pedometer(struct inv_imu_device *s)
|
||||
{
|
||||
int status = 0;
|
||||
uint8_t value;
|
||||
|
||||
status |= inv_imu_start_dmp(s);
|
||||
|
||||
/* Enable Pedometer */
|
||||
status |= inv_imu_read_reg(s, APEX_CONFIG1, 1, &value);
|
||||
value &= ~APEX_CONFIG1_PED_ENABLE_MASK;
|
||||
value |= (uint8_t)APEX_CONFIG1_PED_ENABLE_EN;
|
||||
status |= inv_imu_write_reg(s, APEX_CONFIG1, 1, &value);
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
int inv_imu_apex_disable_pedometer(struct inv_imu_device *s)
|
||||
{
|
||||
int status = 0;
|
||||
uint8_t value;
|
||||
|
||||
/* Disable Pedometer */
|
||||
status |= inv_imu_read_reg(s, APEX_CONFIG1, 1, &value);
|
||||
value &= ~APEX_CONFIG1_PED_ENABLE_MASK;
|
||||
value |= (uint8_t)APEX_CONFIG1_PED_ENABLE_DIS;
|
||||
status |= inv_imu_write_reg(s, APEX_CONFIG1, 1, &value);
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
int inv_imu_apex_enable_tilt(struct inv_imu_device *s)
|
||||
{
|
||||
int status = 0;
|
||||
uint8_t value;
|
||||
|
||||
status |= inv_imu_start_dmp(s);
|
||||
|
||||
/* Enable Tilt */
|
||||
status |= inv_imu_read_reg(s, APEX_CONFIG1, 1, &value);
|
||||
value &= ~APEX_CONFIG1_TILT_ENABLE_MASK;
|
||||
value |= (uint8_t)APEX_CONFIG1_TILT_ENABLE_EN;
|
||||
status |= inv_imu_write_reg(s, APEX_CONFIG1, 1, &value);
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
int inv_imu_apex_disable_tilt(struct inv_imu_device *s)
|
||||
{
|
||||
int status = 0;
|
||||
uint8_t value;
|
||||
|
||||
/* Disable Tilt */
|
||||
status |= inv_imu_read_reg(s, APEX_CONFIG1, 1, &value);
|
||||
value &= ~APEX_CONFIG1_TILT_ENABLE_MASK;
|
||||
value |= (uint8_t)APEX_CONFIG1_TILT_ENABLE_DIS;
|
||||
status |= inv_imu_write_reg(s, APEX_CONFIG1, 1, &value);
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
int inv_imu_apex_get_data_activity(struct inv_imu_device *s, inv_imu_apex_step_activity_t *apex_activity)
|
||||
{
|
||||
uint8_t data[4];
|
||||
int status = inv_imu_read_reg(s, APEX_DATA0, 4, data);
|
||||
|
||||
apex_activity->step_cnt = data[1] << 8 | data[0];
|
||||
apex_activity->step_cadence = data[2];
|
||||
apex_activity->activity_class = data[3] & APEX_DATA3_ACTIVITY_CLASS_MASK;
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
int inv_imu_apex_get_data_free_fall(struct inv_imu_device *s, uint16_t *freefall_duration)
|
||||
{
|
||||
uint8_t data[2];
|
||||
int status = inv_imu_read_reg(s, APEX_DATA4, 2, &data[0]);
|
||||
|
||||
*freefall_duration = (data[1] << 8) | data[0];
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
@@ -0,0 +1,185 @@
|
||||
/*
|
||||
* ________________________________________________________________________________________________________
|
||||
* Copyright (c) 2017 InvenSense Inc. All rights reserved.
|
||||
*
|
||||
* This software, related documentation and any modifications thereto (collectively "Software") is subject
|
||||
* to InvenSense and its licensors' intellectual property rights under U.S. and international copyright
|
||||
* and other intellectual property rights laws.
|
||||
*
|
||||
* InvenSense and its licensors retain all intellectual property and proprietary rights in and to the Software
|
||||
* and any use, reproduction, disclosure or distribution of the Software without an express license agreement
|
||||
* from InvenSense is strictly prohibited.
|
||||
*
|
||||
* EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, THE SOFTWARE IS
|
||||
* PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED
|
||||
* TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT.
|
||||
* EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, IN NO EVENT SHALL
|
||||
* INVENSENSE BE LIABLE FOR ANY DIRECT, SPECIAL, INDIRECT, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, OR ANY
|
||||
* DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT,
|
||||
* NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE
|
||||
* OF THE SOFTWARE.
|
||||
* ________________________________________________________________________________________________________
|
||||
*/
|
||||
|
||||
/** @defgroup DriverApex IMU driver high level functions related to APEX and the DMP
|
||||
* @brief High-level function to setup an IMU device
|
||||
* @ingroup Driver
|
||||
* @{
|
||||
*/
|
||||
|
||||
/** @file inv_imu_apex.h
|
||||
* High-level function to setup an IMU device
|
||||
*/
|
||||
|
||||
#ifndef _INV_IMU_APEX_H_
|
||||
#define _INV_IMU_APEX_H_
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#include "inv_imu_defs.h"
|
||||
|
||||
#include "InvError.h"
|
||||
|
||||
#include <stdint.h>
|
||||
#include <string.h>
|
||||
|
||||
/* Forward declarations */
|
||||
struct inv_imu_device;
|
||||
|
||||
/** @brief IMU APEX inputs parameters definition
|
||||
*/
|
||||
typedef struct {
|
||||
APEX_CONFIG3_PEDO_AMP_TH_t pedo_amp_th;
|
||||
uint8_t pedo_step_cnt_th;
|
||||
uint8_t pedo_step_det_th;
|
||||
APEX_CONFIG4_PEDO_SB_TIMER_TH_t pedo_sb_timer_th;
|
||||
APEX_CONFIG4_PEDO_HI_ENRGY_TH_t pedo_hi_enrgy_th;
|
||||
APEX_CONFIG5_TILT_WAIT_TIME_t tilt_wait_time;
|
||||
APEX_CONFIG2_DMP_POWER_SAVE_TIME_t power_save_time;
|
||||
APEX_CONFIG0_DMP_POWER_SAVE_t power_save;
|
||||
APEX_CONFIG9_SENSITIVITY_MODE_t sensitivity_mode;
|
||||
APEX_CONFIG2_LOW_ENERGY_AMP_TH_t low_energy_amp_th;
|
||||
APEX_CONFIG9_SMD_SENSITIVITY_t smd_sensitivity;
|
||||
APEX_CONFIG9_FF_DEBOUNCE_DURATION_t ff_debounce_duration;
|
||||
APEX_CONFIG12_FF_MAX_DURATION_t ff_max_duration_cm;
|
||||
APEX_CONFIG12_FF_MIN_DURATION_t ff_min_duration_cm;
|
||||
APEX_CONFIG10_LOWG_PEAK_TH_t lowg_peak_th;
|
||||
APEX_CONFIG5_LOWG_PEAK_TH_HYST_t lowg_peak_hyst;
|
||||
APEX_CONFIG10_LOWG_TIME_TH_SAMPLES_t lowg_samples_th;
|
||||
APEX_CONFIG11_HIGHG_PEAK_TH_t highg_peak_th;
|
||||
APEX_CONFIG5_HIGHG_PEAK_TH_HYST_t highg_peak_hyst;
|
||||
APEX_CONFIG11_HIGHG_TIME_TH_SAMPLES_t highg_samples_th;
|
||||
} inv_imu_apex_parameters_t;
|
||||
|
||||
/** @brief APEX pedometer outputs
|
||||
*/
|
||||
typedef struct inv_imu_apex_step_activity {
|
||||
uint16_t step_cnt; /**< Number of steps taken */
|
||||
uint8_t step_cadence; /**< Walk/run cadence in number of samples.
|
||||
Format is u6.2. E.g, At 50Hz and 2Hz walk frequency, if the cadency is 25 samples.
|
||||
The register will output 100. */
|
||||
uint8_t activity_class; /**< Detected activity unknown (0), walk (1) or run (2) */
|
||||
} inv_imu_apex_step_activity_t;
|
||||
|
||||
/** @brief Enable Free Fall.
|
||||
* @return 0 on success, negative value on error.
|
||||
*/
|
||||
int inv_imu_apex_enable_ff(struct inv_imu_device *s);
|
||||
|
||||
/** @brief Disable Free Fall.
|
||||
* @return 0 on success, negative value on error.
|
||||
*/
|
||||
int inv_imu_apex_disable_ff(struct inv_imu_device *s);
|
||||
|
||||
/** @brief Enable Significant Motion Detection.
|
||||
* note : SMD requests to have the accelerometer enabled to work.
|
||||
* To have good performance, it's recommended to set accelerometer ODR (Output Data Rate) to 20ms
|
||||
* and the accelerometer in Low Power Mode.
|
||||
* @return 0 on success, negative value on error.
|
||||
*/
|
||||
int inv_imu_apex_enable_smd(struct inv_imu_device *s);
|
||||
|
||||
/** @brief Disable Significant Motion Detection.
|
||||
* @return 0 on success, negative value on error.
|
||||
*/
|
||||
int inv_imu_apex_disable_smd(struct inv_imu_device *s);
|
||||
|
||||
/** @brief Fill the APEX parameters structure with all the default parameters for APEX algorithms (pedometer, tilt)
|
||||
* @param[out] apex_inputs Default input parameters. See @sa inv_imu_apex_parameters_t
|
||||
* @return 0 on success, negative value on error.
|
||||
*/
|
||||
int inv_imu_apex_init_parameters_struct(struct inv_imu_device *s, inv_imu_apex_parameters_t *apex_inputs);
|
||||
|
||||
/** @brief Configures DMP parameters for APEX algorithms (pedometer, tilt, lowg, highg).
|
||||
* This programmable parameters will be decoded and propagate to the SRAM to be executed at DMP start.
|
||||
* @param[in] apex_inputs The requested input parameters. See @sa inv_imu_apex_parameters_t
|
||||
* @warning APEX inputs can't change on the fly, this API should be called before enabling any APEX features.
|
||||
* @warning APEX configuration can't be done too frequently, but only once every 10ms.
|
||||
* Otherwise it can create unknown behavior.
|
||||
* @return 0 on success, negative value on error.
|
||||
*/
|
||||
int inv_imu_apex_configure_parameters(struct inv_imu_device *s, const inv_imu_apex_parameters_t *apex_inputs);
|
||||
|
||||
/** @brief Returns current DMP parameters for APEX algorithms (pedometer, tilt).
|
||||
* @param[out] apex_params The current parameter, fetched from registers. See @sa inv_imu_apex_parameters_t
|
||||
* @return 0 on success, negative value on error.
|
||||
*/
|
||||
int inv_imu_apex_get_parameters(struct inv_imu_device *s, inv_imu_apex_parameters_t *apex_params);
|
||||
|
||||
/** @brief Configure DMP Output Data Rate for APEX algorithms (pedometer, tilt)
|
||||
* @param[in] frequency The requested frequency.
|
||||
* @sa APEX_CONFIG1_DMP_ODR_t
|
||||
* @warning DMP_ODR can change on the fly, and the DMP code will accommodate necessary modifications
|
||||
* @warning The user needs to take care to set Accel frequency >= DMP frequency. This is a hard constraint
|
||||
since HW will not handle incorrect setting.
|
||||
* @return 0 on success, negative value on error.
|
||||
*/
|
||||
int inv_imu_apex_set_frequency(struct inv_imu_device *s, const APEX_CONFIG1_DMP_ODR_t frequency);
|
||||
|
||||
/** @brief Enable APEX algorithm Pedometer.
|
||||
* note : Pedometer request to have the accelerometer enabled to works
|
||||
* with accelerometer frequency less than dmp frequency.
|
||||
* @return 0 on success, negative value on error.
|
||||
* @warning Pedometer must be turned OFF to reconfigure it
|
||||
*/
|
||||
int inv_imu_apex_enable_pedometer(struct inv_imu_device *s);
|
||||
|
||||
/** @brief Disable APEX algorithm Pedometer.
|
||||
* @return 0 on success, negative value on error.
|
||||
*/
|
||||
int inv_imu_apex_disable_pedometer(struct inv_imu_device *s);
|
||||
|
||||
/** @brief Enable APEX algorithm Tilt.
|
||||
* note : Tilt request to have the accelerometer enabled to works
|
||||
* with accelerometer frequency less than dmp frequency.
|
||||
* @return 0 on success, negative value on error.
|
||||
*/
|
||||
int inv_imu_apex_enable_tilt(struct inv_imu_device *s);
|
||||
|
||||
/** @brief Disable APEX algorithm Tilt.
|
||||
* @return 0 on success, negative value on error.
|
||||
*/
|
||||
int inv_imu_apex_disable_tilt(struct inv_imu_device *s);
|
||||
|
||||
/** @brief Retrieve APEX pedometer outputs and format them
|
||||
* @param[out] apex_activity Apex step and activity data value.
|
||||
* @return 0 in case of success, negative value on error. See enum inv_error
|
||||
*/
|
||||
int inv_imu_apex_get_data_activity(struct inv_imu_device *s, inv_imu_apex_step_activity_t *apex_activity);
|
||||
|
||||
/** @brief Retrieve APEX free fall outputs and format them
|
||||
* @param[out] Free fall duration in number of sample.
|
||||
* @return 0 in case of success, negative value on error. See enum inv_error
|
||||
*/
|
||||
int inv_imu_apex_get_data_free_fall(struct inv_imu_device *s, uint16_t *freefall_duration);
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif /* _INV_IMU_APEX_H_ */
|
||||
|
||||
/** @} */
|
||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,520 @@
|
||||
/*
|
||||
* ________________________________________________________________________________________________________
|
||||
* Copyright (c) 2017 InvenSense Inc. All rights reserved.
|
||||
*
|
||||
* This software, related documentation and any modifications thereto (collectively "Software") is subject
|
||||
* to InvenSense and its licensors' intellectual property rights under U.S. and international copyright
|
||||
* and other intellectual property rights laws.
|
||||
*
|
||||
* InvenSense and its licensors retain all intellectual property and proprietary rights in and to the Software
|
||||
* and any use, reproduction, disclosure or distribution of the Software without an express license agreement
|
||||
* from InvenSense is strictly prohibited.
|
||||
*
|
||||
* EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, THE SOFTWARE IS
|
||||
* PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED
|
||||
* TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT.
|
||||
* EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, IN NO EVENT SHALL
|
||||
* INVENSENSE BE LIABLE FOR ANY DIRECT, SPECIAL, INDIRECT, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, OR ANY
|
||||
* DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT,
|
||||
* NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE
|
||||
* OF THE SOFTWARE.
|
||||
* ________________________________________________________________________________________________________
|
||||
*/
|
||||
|
||||
/** @defgroup Driver IMU driver high level functions
|
||||
* @brief High-level function to setup an IMU device
|
||||
* @ingroup DriverIcm
|
||||
* @{
|
||||
*/
|
||||
|
||||
/** @file inv_imu_driver.h
|
||||
* High-level function to setup an IMU device
|
||||
*/
|
||||
|
||||
#ifndef _INV_IMU_DRIVER_H_
|
||||
#define _INV_IMU_DRIVER_H_
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#include "inv_imu_defs.h"
|
||||
#include "inv_imu_transport.h"
|
||||
|
||||
#include "InvError.h"
|
||||
|
||||
#include <stdint.h>
|
||||
#include <string.h>
|
||||
|
||||
|
||||
/** @brief IMU max FSR values for accel and gyro
|
||||
* Dependent on chip
|
||||
*/
|
||||
#define ACCEL_CONFIG0_FS_SEL_MAX ACCEL_CONFIG0_FS_SEL_16g
|
||||
#define GYRO_CONFIG0_FS_SEL_MAX GYRO_CONFIG0_FS_SEL_2000dps
|
||||
|
||||
#define ACCEL_OFFUSER_MAX_MG 1000
|
||||
#define GYRO_OFFUSER_MAX_DPS 64
|
||||
|
||||
/** @brief IMU maximum buffer size mirrored from FIFO at polling time
|
||||
* @warning fifo_idx type variable must be large enough to parse the FIFO_MIRRORING_SIZE
|
||||
*/
|
||||
#define FIFO_MIRRORING_SIZE 16 * 258 // packet size * max_count = 4kB
|
||||
|
||||
/** @brief IMU Accelerometer start-up time before having correct data
|
||||
*/
|
||||
#define ACC_STARTUP_TIME_US 10000
|
||||
|
||||
/** @brief IMU Gyroscope start-up time before having correct data
|
||||
*/
|
||||
#define GYR_STARTUP_TIME_US 70000
|
||||
|
||||
/** @brief IMU Gyroscope power off to power on duration
|
||||
*/
|
||||
#define GYR_POWER_OFF_DUR_US 20000
|
||||
|
||||
/** @brief Sensor identifier for UI control function
|
||||
*/
|
||||
enum inv_imu_sensor {
|
||||
INV_SENSOR_ACCEL, /**< Accelerometer */
|
||||
INV_SENSOR_GYRO, /**< Gyroscope */
|
||||
INV_SENSOR_FSYNC_EVENT, /**< FSYNC */
|
||||
INV_SENSOR_TEMPERATURE, /**< Chip temperature */
|
||||
INV_SENSOR_DMP_PEDOMETER_EVENT, /**< Pedometer: step detected */
|
||||
INV_SENSOR_DMP_PEDOMETER_COUNT, /**< Pedometer: step counter */
|
||||
INV_SENSOR_DMP_TILT, /**< Tilt */
|
||||
INV_SENSOR_DMP_FF, /**< FreeFall */
|
||||
INV_SENSOR_DMP_LOWG, /**< Low G */
|
||||
INV_SENSOR_DMP_SMD, /**< Significant Motion Detection */
|
||||
INV_SENSOR_MAX
|
||||
};
|
||||
|
||||
/** @brief Configure Fifo usage
|
||||
*/
|
||||
typedef enum {
|
||||
INV_IMU_FIFO_DISABLED = 0, /**< Fifo is disabled and data source is sensors registers */
|
||||
INV_IMU_FIFO_ENABLED = 1, /**< Fifo is used as data source */
|
||||
}INV_IMU_FIFO_CONFIG_t;
|
||||
|
||||
/** @brief Sensor event structure definition
|
||||
*/
|
||||
typedef struct {
|
||||
int sensor_mask;
|
||||
uint16_t timestamp_fsync;
|
||||
int16_t accel[3];
|
||||
int16_t gyro[3];
|
||||
int16_t temperature;
|
||||
int8_t accel_high_res[3];
|
||||
int8_t gyro_high_res[3];
|
||||
} inv_imu_sensor_event_t;
|
||||
|
||||
/** @brief IMU driver states definition
|
||||
*/
|
||||
struct inv_imu_device {
|
||||
struct inv_imu_transport transport; /**< Transport layer
|
||||
Must be the first one of struct inv_imu_device */
|
||||
void (*sensor_event_cb)(inv_imu_sensor_event_t *event); /**< callback executed by:
|
||||
inv_imu_get_data_from_fifo (if FIFO is used)
|
||||
inv_imu_get_data_from_registers (if FIFO isn't used)
|
||||
May be NULL if above API are not used by application */
|
||||
uint8_t fifo_data[FIFO_MIRRORING_SIZE]; /**< FIFO mirroring memory area */
|
||||
uint8_t dmp_is_on; /**< DMP started status */
|
||||
uint8_t endianness_data; /**< Data endianness configuration */
|
||||
uint8_t fifo_highres_enabled; /**< Highres mode configuration */
|
||||
INV_IMU_FIFO_CONFIG_t fifo_is_used; /**< FIFO configuration */
|
||||
uint64_t gyro_start_time_us; /**< Gyro start time used to discard first samples */
|
||||
uint64_t accel_start_time_us; /**< Accel start time used to discard first samples */
|
||||
uint64_t gyro_power_off_tmst; /**< Gyro power off time */
|
||||
};
|
||||
|
||||
|
||||
/* Interrupt enum state for INT1, INT2, and IBI */
|
||||
typedef enum {
|
||||
INV_IMU_DISABLE = 0,
|
||||
INV_IMU_ENABLE
|
||||
} inv_imu_interrupt_value;
|
||||
|
||||
/** @brief Interrupt definition
|
||||
*/
|
||||
typedef struct {
|
||||
inv_imu_interrupt_value INV_UI_FSYNC;
|
||||
inv_imu_interrupt_value INV_UI_DRDY;
|
||||
inv_imu_interrupt_value INV_FIFO_THS;
|
||||
inv_imu_interrupt_value INV_FIFO_FULL;
|
||||
inv_imu_interrupt_value INV_SMD;
|
||||
inv_imu_interrupt_value INV_WOM_X;
|
||||
inv_imu_interrupt_value INV_WOM_Y;
|
||||
inv_imu_interrupt_value INV_WOM_Z;
|
||||
inv_imu_interrupt_value INV_FF;
|
||||
inv_imu_interrupt_value INV_LOWG;
|
||||
inv_imu_interrupt_value INV_STEP_DET;
|
||||
inv_imu_interrupt_value INV_STEP_CNT_OVFL;
|
||||
inv_imu_interrupt_value INV_TILT_DET;
|
||||
} inv_imu_interrupt_parameter_t;
|
||||
|
||||
|
||||
/** @brief Configure the serial interface used to access the device and execute hardware initialization.
|
||||
*
|
||||
* This functions first configures serial interface passed in parameter to make sure device
|
||||
* is accessible both in read and write. Thus no serial access should be done before
|
||||
* successfully executing the present function.
|
||||
*
|
||||
* Then if requested serial interface is a primary interface (aka UI interface or AP
|
||||
* interface), this function initializes the device using the following hardware settings:
|
||||
* - set timestamp resolution to 16us
|
||||
* - enable FIFO mechanism with the following configuration:
|
||||
* - FIFO record mode i.e FIFO count unit is packet
|
||||
* - FIFO snapshot mode i.e drop the data when the FIFO overflows
|
||||
* - Timestamp is logged in FIFO
|
||||
* - Little Endian fifo_count and fifo_data
|
||||
* - generate FIFO threshold interrupt when packet count reaches FIFO watermark
|
||||
* - set FIFO watermark to 1 packet
|
||||
* - enable temperature and timestamp data to go to FIFO
|
||||
*
|
||||
*
|
||||
* @param[in] s driver structure. Note that first field of this structure MUST be a struct
|
||||
* inv_imu_serif.
|
||||
*
|
||||
* @param[in] serif pointer on serial interface structure to be used to access inv_device.
|
||||
*
|
||||
* @param[in] sensor_event_cb callback executed by inv_imu_get_data_from_fifo function
|
||||
* each time it extracts some valid data from fifo. Or inv_imu_get_data_from_registers read data
|
||||
* from register. Thus this parameter is optional as long
|
||||
* as inv_imu_get_data_from_fifo/inv_imu_get_data_from_registers function is not used.
|
||||
*
|
||||
* @return 0 on success, negative value on error.
|
||||
*/
|
||||
int inv_imu_init(struct inv_imu_device *s,
|
||||
struct inv_imu_serif *serif,
|
||||
void (*sensor_event_cb)(inv_imu_sensor_event_t *event));
|
||||
|
||||
/** @brief Perform a soft reset of the device
|
||||
* @return 0 on success, negative value on error.
|
||||
*/
|
||||
int inv_imu_device_reset(struct inv_imu_device *s);
|
||||
|
||||
/** @brief return WHOAMI value
|
||||
* @param[out] who_am_i WHOAMI for device
|
||||
* @return 0 on success, negative value on error
|
||||
*/
|
||||
int inv_imu_get_who_am_i(struct inv_imu_device *s, uint8_t *who_am_i);
|
||||
|
||||
/** @brief Enable/put accel in low power mode
|
||||
* @return 0 on success, negative value on error.
|
||||
* @details
|
||||
* It enables accel and gyro data in the FIFO (so
|
||||
* the packet format is 16 bytes). If called first,
|
||||
* the configuration will be applied, otherwise it
|
||||
* will be ignored if the FIFO is not empty (but since
|
||||
* the new configuration is identical it is not a issue).
|
||||
* @warning inv_device::register_cache::pwr_mgmt0_reg is modified by this function
|
||||
*/
|
||||
int inv_imu_enable_accel_low_power_mode(struct inv_imu_device *s);
|
||||
|
||||
/** @brief Enable/put accel in low noise mode
|
||||
* @return 0 on success, negative value on error.
|
||||
* @details
|
||||
* It enables accel and gyro data in the FIFO (so
|
||||
* the packet format is 16 bytes). If called first,
|
||||
* the configuration will be applied, otherwise it
|
||||
* will be ignored if the FIFO is not empty (but since
|
||||
* the new configuration is identical it is not a issue).
|
||||
* @warning inv_device::register_cache::pwr_mgmt0_reg is modified by this function
|
||||
*/
|
||||
int inv_imu_enable_accel_low_noise_mode(struct inv_imu_device *s);
|
||||
|
||||
/** @brief Disable all 3 axes of accel
|
||||
* @return 0 on success, negative value on error.
|
||||
* @details
|
||||
* If both accel and gyro are turned off as a result of this
|
||||
* function, they will also be removed from the FIFO and a
|
||||
* FIFO reset will be performed (to guarantee no side effects
|
||||
* until the next enable sensor call)
|
||||
* @warning inv_device::register_cache::pwr_mgmt0_reg is modified by this function
|
||||
*/
|
||||
int inv_imu_disable_accel(struct inv_imu_device *s);
|
||||
|
||||
/** @brief Enable/put gyro in low noise mode
|
||||
* @return 0 on success, negative value on error.
|
||||
* @details
|
||||
* It enables gyro and accel data in the FIFO (so
|
||||
* the packet format is 16 bytes). If called first,
|
||||
* the configuration will be applied, otherwise it
|
||||
* will be ignored if the FIFO is not empty (but since
|
||||
* the new configuration is identical it is not a issue).
|
||||
* @warning inv_device::register_cache::pwr_mgmt0_reg is modified by this function
|
||||
*/
|
||||
int inv_imu_enable_gyro_low_noise_mode(struct inv_imu_device *s);
|
||||
|
||||
/** @brief Disable all 3 axes of gyro
|
||||
* @return 0 on success, negative value on error.
|
||||
* @details
|
||||
* If both accel and gyro are turned off as a result of this
|
||||
* function, they will also be removed from the FIFO and a
|
||||
* FIFO reset will be performed (to guarantee no side effects
|
||||
* until the next enable sensor call)
|
||||
* @warning inv_device::register_cache::pwr_mgmt0_reg is modified by this function
|
||||
*/
|
||||
int inv_imu_disable_gyro(struct inv_imu_device *s);
|
||||
|
||||
/** @brief Enable fsync tagging functionality.
|
||||
* In details it:
|
||||
* - enables fsync
|
||||
* - enables timestamp to registers. Once fsync is enabled fsync counter is pushed to
|
||||
* fifo instead of timestamp. So timestamp is made available in registers. Note that
|
||||
* this increase power consumption.
|
||||
* - enables fsync related interrupt
|
||||
* @return 0 on success, negative value on error.
|
||||
*/
|
||||
int inv_imu_enable_fsync(struct inv_imu_device *s);
|
||||
|
||||
/** @brief Disable fsync tagging functionality.
|
||||
* In details it:
|
||||
* - disables fsync
|
||||
* - disables timestamp to registers. Once fsync is disabled timestamp is pushed to fifo
|
||||
* instead of fsync counter. So in order to decrease power consumption, timestamp is no
|
||||
* more available in registers.
|
||||
* - disables fsync related interrupt
|
||||
* @return 0 on success, negative value on error.
|
||||
*/
|
||||
int inv_imu_disable_fsync(struct inv_imu_device *s);
|
||||
|
||||
/** @brief Configure which interrupt source can trigger INT1.
|
||||
* @param[in] interrupt_to_configure structure with the corresponding state to manage INT1.
|
||||
* @return 0 on success, negative value on error.
|
||||
*/
|
||||
int inv_imu_set_config_int1(struct inv_imu_device *s,
|
||||
inv_imu_interrupt_parameter_t *interrupt_to_configure);
|
||||
|
||||
/** @brief Retrieve interrupts configuration.
|
||||
* @param[in] interrupt_to_configure structure with the corresponding state to manage INT1.
|
||||
* @return 0 on success, negative value on error.
|
||||
*/
|
||||
int inv_imu_get_config_int1(struct inv_imu_device *s,
|
||||
inv_imu_interrupt_parameter_t *interrupt_to_configure);
|
||||
|
||||
/** @brief Configure which interrupt source can trigger INT2.
|
||||
* @param[in] interrupt_to_configure structure with the corresponding state to INT2.
|
||||
* @return 0 on success, negative value on error.
|
||||
*/
|
||||
int inv_imu_set_config_int2(struct inv_imu_device *s,
|
||||
inv_imu_interrupt_parameter_t *interrupt_to_configure);
|
||||
|
||||
/** @brief Retrieve interrupts configuration.
|
||||
* @param[in] interrupt_to_configure structure with the corresponding state to manage INT2.
|
||||
* @return 0 on success, negative value on error.
|
||||
*/
|
||||
int inv_imu_get_config_int2(struct inv_imu_device *s,
|
||||
inv_imu_interrupt_parameter_t *interrupt_to_configure);
|
||||
|
||||
/** @brief Read all registers containing data (temperature, accelerometer and gyroscope). Then it calls
|
||||
* sensor_event_cb function passed in parameter of inv_imu_init function for each packet
|
||||
* @return 0 on success, negative value on error.
|
||||
*/
|
||||
int inv_imu_get_data_from_registers(struct inv_imu_device *s);
|
||||
|
||||
/** @brief Read all available packets from the FIFO. For each packet function builds a
|
||||
* sensor event containing packet data and validity information. Then it calls
|
||||
* sensor_event_cb funtion passed in parameter of inv_imu_init function for each
|
||||
* packet.
|
||||
* @return number of valid packets read on success, negative value on error.
|
||||
*/
|
||||
int inv_imu_get_data_from_fifo(struct inv_imu_device *s);
|
||||
|
||||
/** @brief Converts ACCEL_CONFIG0_ODR_t or GYRO_CONFIG0_ODR_t enums to period expressed in us
|
||||
* @param[in] odr_bitfield An ACCEL_CONFIG0_ODR_t or GYRO_CONFIG0_ODR_t enum
|
||||
* @return The corresponding period expressed in us
|
||||
*/
|
||||
uint32_t inv_imu_convert_odr_bitfield_to_us(uint32_t odr_bitfield);
|
||||
|
||||
/** @brief Configure accel Output Data Rate
|
||||
* @param[in] frequency The requested frequency.
|
||||
* @sa ACCEL_CONFIG0_ODR_t
|
||||
* @return 0 on success, negative value on error.
|
||||
* @warning inv_device::register_cache::accel_config0_reg is modified by this function
|
||||
*/
|
||||
int inv_imu_set_accel_frequency(struct inv_imu_device *s,
|
||||
const ACCEL_CONFIG0_ODR_t frequency);
|
||||
|
||||
/** @brief Configure gyro Output Data Rate
|
||||
* @param[in] frequency The requested frequency.
|
||||
* @sa GYRO_CONFIG0_ODR_t
|
||||
* @return 0 on success, negative value on error.
|
||||
* @warning inv_device::register_cache::gyro_config0_reg is modified by this function
|
||||
*/
|
||||
int inv_imu_set_gyro_frequency(struct inv_imu_device *s,
|
||||
const GYRO_CONFIG0_ODR_t frequency);
|
||||
|
||||
/** @brief Set accel full scale range
|
||||
* @param[in] accel_fsr_g requested full scale range.
|
||||
* @sa ACCEL_CONFIG0_FS_SEL_t.
|
||||
* @return 0 on success, negative value on error.
|
||||
* @warning inv_device::register_cache::accel_config0_reg is modified by this function
|
||||
*/
|
||||
int inv_imu_set_accel_fsr(struct inv_imu_device *s,
|
||||
ACCEL_CONFIG0_FS_SEL_t accel_fsr_g);
|
||||
|
||||
/** @brief Access accel full scale range
|
||||
* @param[out] accel_fsr_g current full scale range.
|
||||
* @sa ACCEL_CONFIG0_FS_SEL_t.
|
||||
* @return 0 on success, negative value on error.
|
||||
* @warning inv_device::register_cache::accel_config0_reg is relied upon by this function
|
||||
*/
|
||||
int inv_imu_get_accel_fsr(struct inv_imu_device *s,
|
||||
ACCEL_CONFIG0_FS_SEL_t *accel_fsr_g);
|
||||
|
||||
/** @brief Set gyro full scale range
|
||||
* @param[in] gyro_fsr_dps requested full scale range.
|
||||
* @sa GYRO_CONFIG0_FS_SEL_t.
|
||||
* @return 0 on success, negative value on error.
|
||||
* @warning inv_device::register_cache::gyro_config0_reg is modified by this function
|
||||
*/
|
||||
int inv_imu_set_gyro_fsr(struct inv_imu_device *s,
|
||||
GYRO_CONFIG0_FS_SEL_t gyro_fsr_dps);
|
||||
|
||||
/** @brief Access gyro full scale range
|
||||
* @param[out] gyro_fsr_dps current full scale range.
|
||||
* @sa GYRO_CONFIG0_FS_SEL_t.
|
||||
* @return 0 on success, negative value on error.
|
||||
* @warning inv_device::register_cache::gyro_config0_reg is relied upon by this function
|
||||
*/
|
||||
int inv_imu_get_gyro_fsr(struct inv_imu_device *s,
|
||||
GYRO_CONFIG0_FS_SEL_t *gyro_fsr_dps);
|
||||
|
||||
/** @brief Set accel Low-Power averaging value
|
||||
* @param[in] acc_avg requested averaging value
|
||||
* @sa ACCEL_CONFIG1_ACCEL_FILT_AVG_t
|
||||
* @return 0 on success, negative value on error.
|
||||
*/
|
||||
int inv_imu_set_accel_lp_avg(struct inv_imu_device *s,
|
||||
ACCEL_CONFIG1_ACCEL_FILT_AVG_t acc_avg);
|
||||
|
||||
/** @brief Set accel Low-Noise bandwidth value
|
||||
* @param[in] acc_bw requested averaging value
|
||||
* @sa ACCEL_CONFIG1_ACCEL_FILT_BW_t
|
||||
* @return 0 on success, negative value on error.
|
||||
*/
|
||||
int inv_imu_set_accel_ln_bw(struct inv_imu_device *s,
|
||||
ACCEL_CONFIG1_ACCEL_FILT_BW_t acc_bw);
|
||||
|
||||
/** @brief Set gyro Low-Noise bandwidth value
|
||||
* @param[in] gyr_bw requested averaging value
|
||||
* @sa GYRO_CONFIG1_GYRO_FILT_BW_t
|
||||
* @return 0 on success, negative value on error.
|
||||
*/
|
||||
int inv_imu_set_gyro_ln_bw(struct inv_imu_device *s,
|
||||
GYRO_CONFIG1_GYRO_FILT_BW_t gyr_bw);
|
||||
|
||||
/** @brief Set timestamp resolution
|
||||
* @param[in] timestamp_resol requested timestamp resolution
|
||||
* @sa TMST_CONFIG1_RESOL_t
|
||||
* @return 0 on success, negative value on error.
|
||||
*/
|
||||
int inv_imu_set_timestamp_resolution(struct inv_imu_device *s,
|
||||
const TMST_CONFIG1_RESOL_t timestamp_resol);
|
||||
|
||||
/** @brief reset IMU fifo
|
||||
* @return 0 on success, negative value on error.
|
||||
*/
|
||||
int inv_imu_reset_fifo(struct inv_imu_device *s);
|
||||
|
||||
/** @brief Enable 20 bits raw acc and raw gyr data in fifo.
|
||||
* @return 0 on success, negative return code otherwise
|
||||
*/
|
||||
int inv_imu_enable_high_resolution_fifo(struct inv_imu_device *s);
|
||||
|
||||
/** @brief Disable 20 bits raw acc and raw gyr data in fifo.
|
||||
* @return 0 on success, negative return code otherwise
|
||||
*/
|
||||
int inv_imu_disable_high_resolution_fifo(struct inv_imu_device *s);
|
||||
|
||||
/** @brief Configure Fifo
|
||||
* @param[in] fifo_config Fifo configuration method :
|
||||
* if FIFO is enabled, data are pushed to FIFO and FIFO THS interrupt is set
|
||||
* if FIFO is disabled, data are not pused to FIFO and DRDY interrupt is set
|
||||
* @sa INV_IMU_FIFO_CONFIG_t
|
||||
*/
|
||||
int inv_imu_configure_fifo(struct inv_imu_device *s,
|
||||
INV_IMU_FIFO_CONFIG_t fifo_config);
|
||||
|
||||
/** @brief Get FIFO timestamp resolution
|
||||
* @return the timestamp resolution in us as a q24 or 0 in case of error
|
||||
*/
|
||||
int32_t inv_imu_get_fifo_timestamp_resolution_us_q24(struct inv_imu_device *s);
|
||||
|
||||
/** @brief Get register timestamp resolution
|
||||
* @return the timestamp resolution in us as a q24 or 0 in case of error
|
||||
*/
|
||||
uint32_t inv_imu_get_reg_timestamp_resolution_us_q24(struct inv_imu_device *s);
|
||||
|
||||
/** @brief Enable Wake On Motion.
|
||||
* @param[in] wom_x_th threshold value for the Wake on Motion Interrupt for X-axis accelerometer.
|
||||
* @param[in] wom_y_th threshold value for the Wake on Motion Interrupt for Y-axis accelerometer.
|
||||
* @param[in] wom_z_th threshold value for the Wake on Motion Interrupt for Z-axis accelerometer.
|
||||
* @param[in] wom_int select which mode between AND/OR is used to generate interrupt.
|
||||
* @param[in] wom_dur select the number of overthreshold event to wait before generating interrupt.
|
||||
* @return 0 on success, negative value on error.
|
||||
*/
|
||||
int inv_imu_configure_wom(struct inv_imu_device *s,
|
||||
const uint8_t wom_x_th,
|
||||
const uint8_t wom_y_th,
|
||||
const uint8_t wom_z_th,
|
||||
WOM_CONFIG_WOM_INT_MODE_t wom_int,
|
||||
WOM_CONFIG_WOM_INT_DUR_t wom_dur);
|
||||
|
||||
/** @brief Enable Wake On Motion.
|
||||
* note : WoM requests to have the accelerometer enabled to work.
|
||||
* As a consequence Fifo water-mark interrupt is disabled to only trigger WoM interrupts.
|
||||
* To have good performance, it's recommended to set accelerometer ODR (Output Data Rate) to 20ms
|
||||
* and the accelerometer in Low Power Mode.
|
||||
* @return 0 on success, negative value on error.
|
||||
*/
|
||||
int inv_imu_enable_wom(struct inv_imu_device *s);
|
||||
|
||||
/** @brief Disable Wake On Motion.
|
||||
* note : Fifo water-mark interrupt is re-enabled when WoM is disabled.
|
||||
* @return 0 on success, negative value on error.
|
||||
*/
|
||||
int inv_imu_disable_wom(struct inv_imu_device *s);
|
||||
|
||||
/** @brief Start DMP for APEX algorithms and selftest
|
||||
* @return 0 on success, negative value on error.
|
||||
*/
|
||||
int inv_imu_start_dmp(struct inv_imu_device *s);
|
||||
|
||||
/** @brief Reset DMP for APEX algorithms and selftest
|
||||
* @return 0 on success, negative value on error.
|
||||
*/
|
||||
int inv_imu_reset_dmp(struct inv_imu_device *s,
|
||||
const APEX_CONFIG0_DMP_MEM_RESET_t sram_reset);
|
||||
|
||||
/** @breif Set the UI endianness and set the inv_device endianness field
|
||||
* @return 0 on success, negative value on error.
|
||||
*/
|
||||
int inv_imu_set_endianness(struct inv_imu_device *s,
|
||||
INTF_CONFIG0_DATA_ENDIAN_t endianness);
|
||||
|
||||
/** @breif Read the UI endianness and set the inv_device endianness field
|
||||
* @return 0 on success, negative value on error.
|
||||
*/
|
||||
int inv_imu_get_endianness(struct inv_imu_device *s);
|
||||
|
||||
/** @brief Configure Fifo decimation
|
||||
* @param[in] requested decimation factor value from 2 to 256
|
||||
* @return 0 on success, negative value on error.
|
||||
*/
|
||||
int inv_imu_configure_fifo_data_rate(struct inv_imu_device *s,
|
||||
FDR_CONFIG_FDR_SEL_t dec_factor);
|
||||
|
||||
/** @brief Return driver version x.y.z-suffix as a char array
|
||||
* @retval driver version a char array "x.y.z-suffix"
|
||||
*/
|
||||
const char * inv_imu_get_version(void);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif /* _INV_IMU_DRIVER_H_ */
|
||||
|
||||
/** @} */
|
||||
@@ -0,0 +1,64 @@
|
||||
/*
|
||||
* ________________________________________________________________________________________________________
|
||||
* Copyright (c) 2017 InvenSense Inc. All rights reserved.
|
||||
*
|
||||
* This software, related documentation and any modifications thereto (collectively "Software") is subject
|
||||
* to InvenSense and its licensors' intellectual property rights under U.S. and international copyright
|
||||
* and other intellectual property rights laws.
|
||||
*
|
||||
* InvenSense and its licensors retain all intellectual property and proprietary rights in and to the Software
|
||||
* and any use, reproduction, disclosure or distribution of the Software without an express license agreement
|
||||
* from InvenSense is strictly prohibited.
|
||||
*
|
||||
* EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, THE SOFTWARE IS
|
||||
* PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED
|
||||
* TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT.
|
||||
* EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, IN NO EVENT SHALL
|
||||
* INVENSENSE BE LIABLE FOR ANY DIRECT, SPECIAL, INDIRECT, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, OR ANY
|
||||
* DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT,
|
||||
* NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE
|
||||
* OF THE SOFTWARE.
|
||||
* ________________________________________________________________________________________________________
|
||||
*/
|
||||
|
||||
/** @defgroup DriverExt IMU driver extern functions
|
||||
* @brief Extern functions for IMU devices
|
||||
* @ingroup Driver
|
||||
* @{
|
||||
*/
|
||||
|
||||
/** @file inv_imu_extfunc.h
|
||||
* Extern functions for IMU devices
|
||||
*/
|
||||
|
||||
#ifndef _INV_IMU_EXTFUNC_H_
|
||||
#define _INV_IMU_EXTFUNC_H_
|
||||
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
|
||||
/** @brief Hook for low-level high res system sleep() function to be implemented by upper layer
|
||||
* ~100us resolution is sufficient
|
||||
* @param[in] us number of us the calling thread should sleep
|
||||
*/
|
||||
extern void inv_imu_sleep_us(uint32_t us);
|
||||
|
||||
/** @brief Hook for low-level high res system get_time() function to be implemented by upper layer
|
||||
* Value shall be on 64bit with a 1 us resolution
|
||||
* @return The current time in us
|
||||
*/
|
||||
extern uint64_t inv_imu_get_time_us(void);
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif /* _INV_IMU_EXTFUNC_H_ */
|
||||
|
||||
/** @} */
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,179 @@
|
||||
/*
|
||||
* ________________________________________________________________________________________________________
|
||||
* Copyright (c) 2015-2015 InvenSense Inc. All rights reserved.
|
||||
*
|
||||
* This software, related documentation and any modifications thereto (collectively "Software") is subject
|
||||
* to InvenSense and its licensors' intellectual property rights under U.S. and international copyright
|
||||
* and other intellectual property rights laws.
|
||||
*
|
||||
* InvenSense and its licensors retain all intellectual property and proprietary rights in and to the Software
|
||||
* and any use, reproduction, disclosure or distribution of the Software without an express license agreement
|
||||
* from InvenSense is strictly prohibited.
|
||||
*
|
||||
* EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, THE SOFTWARE IS
|
||||
* PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED
|
||||
* TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT.
|
||||
* EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, IN NO EVENT SHALL
|
||||
* INVENSENSE BE LIABLE FOR ANY DIRECT, SPECIAL, INDIRECT, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, OR ANY
|
||||
* DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT,
|
||||
* NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE
|
||||
* OF THE SOFTWARE.
|
||||
* ________________________________________________________________________________________________________
|
||||
*/
|
||||
|
||||
#include "inv_imu_selftest.h"
|
||||
#include "inv_imu_extfunc.h"
|
||||
#include "inv_imu_transport.h"
|
||||
|
||||
static int configure_selftest_parameters(struct inv_imu_device *s,
|
||||
const inv_imu_selftest_parameters_t st_params);
|
||||
|
||||
int inv_imu_run_selftest(struct inv_imu_device *s,
|
||||
const inv_imu_selftest_parameters_t st_params,
|
||||
inv_imu_selftest_output_t *st_output)
|
||||
{
|
||||
int status = 0;
|
||||
uint8_t value;
|
||||
uint8_t data[2] = {0};
|
||||
uint8_t st_done = 0;
|
||||
int polling_timeout_ms = 1000;
|
||||
|
||||
/* Disables Gyro/Accel sensors */
|
||||
status |= inv_imu_read_reg(s, PWR_MGMT0, 1, &value);
|
||||
value &= ~(PWR_MGMT0_ACCEL_MODE_MASK | PWR_MGMT0_GYRO_MODE_MASK);
|
||||
status |= inv_imu_write_reg(s, PWR_MGMT0, 1, &value);
|
||||
|
||||
/* Enable RC oscillator */
|
||||
status |= inv_imu_switch_on_mclk(s);
|
||||
|
||||
/* Clear DMP SRAM (1 ms wait included in `inv_imu_reset_dmp()`) */
|
||||
status |= inv_imu_reset_dmp(s, APEX_CONFIG0_DMP_MEM_RESET_APEX_ST_EN);
|
||||
/* Update `dmp_is_on` since APEX features will have to restart from scratch */
|
||||
s->dmp_is_on = 0;
|
||||
|
||||
/* Load self-test data */
|
||||
status |= inv_imu_load_selftest_data(s);
|
||||
|
||||
/* Set self-test parameters */
|
||||
status |= configure_selftest_parameters(s, st_params);
|
||||
|
||||
/*
|
||||
* Enable accel and/or gyro self-test.
|
||||
* If both accel and gyro self-test are enabled,
|
||||
* they should be set simultaneously in the same write access
|
||||
*/
|
||||
status |= inv_imu_read_reg(s, SELFTEST_MREG1, 1, &value);
|
||||
value &= ~SELFTEST_EN;
|
||||
value |= (uint8_t)st_params.st_control;
|
||||
status |= inv_imu_write_reg(s, SELFTEST_MREG1, 1, &value);
|
||||
|
||||
/* Poll int_status_st_done bit */
|
||||
do {
|
||||
inv_imu_sleep_us(1000);
|
||||
status |= inv_imu_read_reg(s, INT_STATUS, 1, &st_done);
|
||||
st_done &= INT_STATUS_ST_INT_MASK;
|
||||
|
||||
if (0 == --polling_timeout_ms)
|
||||
return (status | -1); /* Return error if timeout is reached */
|
||||
|
||||
} while ( !st_done /* Exit if ST_DONE */
|
||||
&& !status /* Or if error is detected */);
|
||||
|
||||
/* Read self-test results */
|
||||
status |= inv_imu_read_reg(s, ST_STATUS1_MREG1, 2, &data[0]);
|
||||
st_output->accel_status = (data[0] & ST_STATUS1_ACCEL_ST_PASS_MASK) >> ST_STATUS1_ACCEL_ST_PASS_POS;
|
||||
st_output->ax_status = (data[0] & ST_STATUS1_AX_ST_PASS_MASK) >> ST_STATUS1_AX_ST_PASS_POS;
|
||||
st_output->ay_status = (data[0] & ST_STATUS1_AY_ST_PASS_MASK) >> ST_STATUS1_AY_ST_PASS_POS;
|
||||
st_output->az_status = (data[0] & ST_STATUS1_AZ_ST_PASS_MASK) >> ST_STATUS1_AZ_ST_PASS_POS;
|
||||
st_output->gyro_status = (data[1] & ST_STATUS2_GYRO_ST_PASS_MASK) >> ST_STATUS2_GYRO_ST_PASS_POS;
|
||||
st_output->gyro_status |= ((data[1] & ST_STATUS2_ST_INCOMPLETE_MASK) >> ST_STATUS2_ST_INCOMPLETE_POS) << 1;
|
||||
st_output->gx_status = (data[1] & ST_STATUS2_GX_ST_PASS_MASK) >> ST_STATUS2_GX_ST_PASS_POS;
|
||||
st_output->gy_status = (data[1] & ST_STATUS2_GY_ST_PASS_MASK) >> ST_STATUS2_GY_ST_PASS_POS;
|
||||
st_output->gz_status = (data[1] & ST_STATUS2_GZ_ST_PASS_MASK) >> ST_STATUS2_GZ_ST_PASS_POS;
|
||||
|
||||
/* Disable self-test */
|
||||
status |= inv_imu_read_reg(s, SELFTEST_MREG1, 1, &value);
|
||||
value &= ~SELFTEST_EN;
|
||||
value |= (uint8_t)SELFTEST_DIS;
|
||||
status |= inv_imu_write_reg(s, SELFTEST_MREG1, 1, &value);
|
||||
|
||||
/* Reset FIFO because ST data may have been pushed to it */
|
||||
status |= inv_imu_reset_fifo(s);
|
||||
|
||||
/* Restore idle bit */
|
||||
status |= inv_imu_switch_off_mclk(s);
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
int inv_imu_init_selftest_parameters_struct(struct inv_imu_device *s,
|
||||
inv_imu_selftest_parameters_t *st_params)
|
||||
{
|
||||
(void)s;
|
||||
st_params->st_num_samples = ST_CONFIG_16_SAMPLES;
|
||||
st_params->st_control = (SELFTEST_ACCEL_GYRO_ST_EN_t)SELFTEST_EN;
|
||||
return 0;
|
||||
}
|
||||
|
||||
int inv_imu_load_selftest_data(struct inv_imu_device *s)
|
||||
{
|
||||
int status = 0;
|
||||
uint8_t value;
|
||||
|
||||
/* Enable RC oscillator */
|
||||
status |= inv_imu_switch_on_mclk(s);
|
||||
|
||||
/* Set up OTP controller to reload factory-trimmed self-test response into SRAM */
|
||||
status |= inv_imu_read_reg(s, OTP_CONFIG_MREG1, 1, &value);
|
||||
value &= ~OTP_CONFIG_OTP_COPY_MODE_MASK;
|
||||
value |= (uint8_t)OTP_CONFIG_OTP_COPY_ST_DATA;
|
||||
status |= inv_imu_write_reg(s, OTP_CONFIG_MREG1, 1, &value);
|
||||
|
||||
/* Take the OTP macro out of power-down mode */
|
||||
status |= inv_imu_read_reg(s, OTP_CTRL7_MREG2, 1, &value);
|
||||
value &= ~OTP_CTRL7_OTP_PWR_DOWN_MASK;
|
||||
value |= (uint8_t)OTP_CTRL7_PWR_DOWN_DIS;
|
||||
status |= inv_imu_write_reg(s, OTP_CTRL7_MREG2, 1, &value);
|
||||
|
||||
/* Wait for voltage generator to power on */
|
||||
inv_imu_sleep_us(100);
|
||||
|
||||
/* Host should disable INT function first before kicking off OTP copy operation */
|
||||
|
||||
/* Trigger OTP to reload data (this time in self-test mode) */
|
||||
status |= inv_imu_read_reg(s, OTP_CTRL7_MREG2, 1, &value);
|
||||
value &= ~OTP_CTRL7_OTP_RELOAD_MASK;
|
||||
value |= (uint8_t)OTP_CTRL7_OTP_RELOAD_EN;
|
||||
status |= inv_imu_write_reg(s, OTP_CTRL7_MREG2, 1, &value);
|
||||
|
||||
/* Wait for OTP reload */
|
||||
inv_imu_sleep_us(20);
|
||||
|
||||
/* Disable RC oscillator */
|
||||
status |= inv_imu_switch_off_mclk(s);
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
static int configure_selftest_parameters(struct inv_imu_device *s,
|
||||
const inv_imu_selftest_parameters_t st_params)
|
||||
{
|
||||
int status = 0;
|
||||
uint8_t value;
|
||||
|
||||
/* Self-test configuration cannot be updated if it already running */
|
||||
status |= inv_imu_read_reg(s, SELFTEST_MREG1, 1, &value);
|
||||
if ((value & SELFTEST_EN) != SELFTEST_DIS)
|
||||
return INV_ERROR_UNEXPECTED;
|
||||
|
||||
status |= inv_imu_read_reg(s, ST_CONFIG_MREG1, 1, &value);
|
||||
value &= ~((uint8_t)ST_CONFIG_ST_NUMBER_SAMPLE_MASK
|
||||
| (uint8_t)ST_CONFIG_ACCEL_ST_LIM_MASK
|
||||
| (uint8_t)ST_CONFIG_GYRO_ST_LIM_MASK);
|
||||
value |= (uint8_t)st_params.st_num_samples
|
||||
| (uint8_t)ST_CONFIG_ACCEL_ST_LIM_50
|
||||
| (uint8_t)ST_CONFIG_GYRO_ST_LIM_50;
|
||||
status |= inv_imu_write_reg(s, ST_CONFIG_MREG1, 1, &value);
|
||||
|
||||
return status;
|
||||
}
|
||||
@@ -0,0 +1,101 @@
|
||||
/*
|
||||
* ________________________________________________________________________________________________________
|
||||
* Copyright (c) 2015-2015 InvenSense Inc. All rights reserved.
|
||||
*
|
||||
* This software, related documentation and any modifications thereto (collectively "Software") is subject
|
||||
* to InvenSense and its licensors' intellectual property rights under U.S. and international copyright
|
||||
* and other intellectual property rights laws.
|
||||
*
|
||||
* InvenSense and its licensors retain all intellectual property and proprietary rights in and to the Software
|
||||
* and any use, reproduction, disclosure or distribution of the Software without an express license agreement
|
||||
* from InvenSense is strictly prohibited.
|
||||
*
|
||||
* EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, THE SOFTWARE IS
|
||||
* PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED
|
||||
* TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT.
|
||||
* EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, IN NO EVENT SHALL
|
||||
* INVENSENSE BE LIABLE FOR ANY DIRECT, SPECIAL, INDIRECT, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, OR ANY
|
||||
* DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT,
|
||||
* NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE
|
||||
* OF THE SOFTWARE.
|
||||
* ________________________________________________________________________________________________________
|
||||
*/
|
||||
|
||||
/** @defgroup DriverST SelfTest IMU selftest
|
||||
* @brief Low-level function to run selftest on a IMU device
|
||||
* @ingroup Driver
|
||||
* @{
|
||||
*/
|
||||
|
||||
/** @file inv_imu_selftest.h
|
||||
* Low-level function to run selftest on a IMU device
|
||||
*/
|
||||
|
||||
#ifndef _INV_IMU_SELFTEST_H_
|
||||
#define _INV_IMU_SELFTEST_H_
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#include "InvExport.h"
|
||||
#include "inv_imu_defs.h"
|
||||
#include "inv_imu_driver.h"
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/* forward declaration */
|
||||
struct inv_imu_device;
|
||||
|
||||
|
||||
/** @brief Self-test input parameters
|
||||
*/
|
||||
typedef struct {
|
||||
ST_CONFIG_NUM_SAMPLES_t st_num_samples; /**< Number of samples used to perform self-test */
|
||||
SELFTEST_ACCEL_GYRO_ST_EN_t st_control; /**< Define which sensor is under self-test */
|
||||
} inv_imu_selftest_parameters_t;
|
||||
|
||||
/** @brief Self-test routine outputs
|
||||
*/
|
||||
typedef struct {
|
||||
int8_t accel_status; /**< global accelerometer self-test passed */
|
||||
int8_t gyro_status; /**< global gyroscope self-test status: st_pass (bit0), st_incomplete (bit1) */
|
||||
int8_t ax_status; /**< AX self-test status */
|
||||
int8_t ay_status; /**< AY self-test status */
|
||||
int8_t az_status; /**< AZ self-test status */
|
||||
int8_t gx_status; /**< GX self-test status */
|
||||
int8_t gy_status; /**< GY self-test status */
|
||||
int8_t gz_status; /**< GZ self-test status */
|
||||
} inv_imu_selftest_output_t;
|
||||
|
||||
|
||||
/**
|
||||
* @brief Perform hardware self-test for Accel and Gyro
|
||||
* @param[in] Self-test parameters (see inv_imu_selftest_parameters_t)
|
||||
* @param[out] Self-test results (see inv_imu_selftest_output_t)
|
||||
* @return 0 on completion, negative number if intermediate errors occurred
|
||||
*/
|
||||
int inv_imu_run_selftest(struct inv_imu_device *s,
|
||||
const inv_imu_selftest_parameters_t st_params,
|
||||
inv_imu_selftest_output_t *st_output);
|
||||
|
||||
/** @brief Fill the self-test configuration structure with default configuration
|
||||
* @param[in] selftest_params self-test parameters to be initialized
|
||||
* @return 0 on success, negative return code otherwise
|
||||
*/
|
||||
int inv_imu_init_selftest_parameters_struct(struct inv_imu_device *s,
|
||||
inv_imu_selftest_parameters_t *selftest_params);
|
||||
|
||||
/** @brief Load self-test data
|
||||
* @return 0 on success, negative return code otherwise
|
||||
*/
|
||||
int inv_imu_load_selftest_data(struct inv_imu_device *s);
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif /* _INV_IMU_SELFTEST_H_ */
|
||||
|
||||
/** @} */
|
||||
@@ -0,0 +1,257 @@
|
||||
/*
|
||||
* ________________________________________________________________________________________________________
|
||||
* Copyright (c) 2015-2015 InvenSense Inc. All rights reserved.
|
||||
*
|
||||
* This software, related documentation and any modifications thereto (collectively "Software") is subject
|
||||
* to InvenSense and its licensors' intellectual property rights under U.S. and international copyright
|
||||
* and other intellectual property rights laws.
|
||||
*
|
||||
* InvenSense and its licensors retain all intellectual property and proprietary rights in and to the Software
|
||||
* and any use, reproduction, disclosure or distribution of the Software without an express license agreement
|
||||
* from InvenSense is strictly prohibited.
|
||||
*
|
||||
* EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, THE SOFTWARE IS
|
||||
* PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED
|
||||
* TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT.
|
||||
* EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, IN NO EVENT SHALL
|
||||
* INVENSENSE BE LIABLE FOR ANY DIRECT, SPECIAL, INDIRECT, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, OR ANY
|
||||
* DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT,
|
||||
* NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE
|
||||
* OF THE SOFTWARE.
|
||||
* ________________________________________________________________________________________________________
|
||||
*/
|
||||
|
||||
#include "inv_imu_extfunc.h"
|
||||
#include "inv_imu_transport.h"
|
||||
#include "inv_imu_regmap.h"
|
||||
|
||||
#include "InvError.h"
|
||||
|
||||
|
||||
/* Function definition */
|
||||
static uint8_t *get_register_cache_addr(struct inv_imu_device *s, uint32_t reg);
|
||||
static int write_sreg(struct inv_imu_device *s, uint8_t reg, uint32_t len, const uint8_t *buf);
|
||||
static int read_sreg(struct inv_imu_device *s, uint8_t reg, uint32_t len, uint8_t *buf);
|
||||
static int write_mclk_reg(struct inv_imu_device *s, uint16_t regaddr, uint8_t wr_cnt, const uint8_t *buf);
|
||||
static int read_mclk_reg(struct inv_imu_device *s, uint16_t regaddr, uint8_t rd_cnt, uint8_t *buf);
|
||||
|
||||
|
||||
int inv_imu_init_transport(struct inv_imu_device *s)
|
||||
{
|
||||
int status = 0;
|
||||
struct inv_imu_transport *t = (struct inv_imu_transport *)s;
|
||||
|
||||
status |= read_sreg(s, (uint8_t)PWR_MGMT0, 1, &(t->register_cache.pwr_mgmt0_reg));
|
||||
status |= read_sreg(s, (uint8_t)GYRO_CONFIG0, 1, &(t->register_cache.gyro_config0_reg));
|
||||
status |= read_sreg(s, (uint8_t)ACCEL_CONFIG0, 1, &(t->register_cache.accel_config0_reg));
|
||||
|
||||
status |= read_mclk_reg(s, (TMST_CONFIG1_MREG1 & 0xFFFF), 1, &(t->register_cache.tmst_config1_reg));
|
||||
|
||||
t->need_mclk_cnt = 0;
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
int inv_imu_read_reg(struct inv_imu_device *s, uint32_t reg, uint32_t len, uint8_t *buf)
|
||||
{
|
||||
uint32_t i;
|
||||
int rc = 0;
|
||||
|
||||
for (i = 0; i < len; i++) {
|
||||
uint8_t *cache_addr = get_register_cache_addr(s, reg + i);
|
||||
|
||||
if (cache_addr) {
|
||||
buf[i] = *cache_addr;
|
||||
} else {
|
||||
if (!(reg & 0x10000)) {
|
||||
rc |= read_mclk_reg(s, ((reg + i) & 0xFFFF), 1, &buf[i]);
|
||||
} else {
|
||||
rc |= read_sreg(s, (uint8_t)reg + i, len - i, &buf[i]);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return rc;
|
||||
}
|
||||
|
||||
int inv_imu_write_reg(struct inv_imu_device *s, uint32_t reg, uint32_t len, const uint8_t *buf)
|
||||
{
|
||||
uint32_t i;
|
||||
int rc = 0;
|
||||
|
||||
for (i = 0; i < len; i++) {
|
||||
uint8_t *cache_addr = get_register_cache_addr(s, reg + i);
|
||||
|
||||
if (cache_addr)
|
||||
*cache_addr = buf[i];
|
||||
|
||||
if (!(reg & 0x10000))
|
||||
rc |= write_mclk_reg(s, ((reg + i) & 0xFFFF), 1, &buf[i]);
|
||||
}
|
||||
|
||||
if (reg & 0x10000)
|
||||
rc |= write_sreg(s, (uint8_t)reg, len, buf);
|
||||
|
||||
return rc;
|
||||
}
|
||||
|
||||
int inv_imu_switch_on_mclk(struct inv_imu_device *s)
|
||||
{
|
||||
int status = 0;
|
||||
uint8_t data;
|
||||
struct inv_imu_transport *t = (struct inv_imu_transport *)s;
|
||||
|
||||
/* set IDLE bit only if it is not set yet */
|
||||
if (t->need_mclk_cnt == 0) {
|
||||
|
||||
status |= inv_imu_read_reg(s, PWR_MGMT0, 1, &data);
|
||||
data |= PWR_MGMT0_IDLE_MASK;
|
||||
status |= inv_imu_write_reg(s, PWR_MGMT0, 1, &data);
|
||||
|
||||
if (status)
|
||||
return status;
|
||||
|
||||
/* Check if MCLK is ready */
|
||||
do {
|
||||
status = inv_imu_read_reg(s, MCLK_RDY, 1, &data);
|
||||
} while ((status != 0) || !(data & MCLK_RDY_MCLK_RDY_MASK));
|
||||
} else {
|
||||
|
||||
/* Make sure it is already on */
|
||||
status |= inv_imu_read_reg(s, PWR_MGMT0, 1, &data);
|
||||
if (0 == (data &= PWR_MGMT0_IDLE_MASK))
|
||||
status |= INV_ERROR;
|
||||
}
|
||||
|
||||
/* Increment the counter to keep track of number of MCLK requesters */
|
||||
t->need_mclk_cnt++;
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
int inv_imu_switch_off_mclk(struct inv_imu_device *s)
|
||||
{
|
||||
int status = 0;
|
||||
uint8_t data;
|
||||
struct inv_imu_transport *t = (struct inv_imu_transport *)s;
|
||||
|
||||
/* Reset the IDLE but only if there is one requester left */
|
||||
if (t->need_mclk_cnt == 1) {
|
||||
status |= inv_imu_read_reg(s, PWR_MGMT0, 1, &data);
|
||||
data &= ~PWR_MGMT0_IDLE_MASK;
|
||||
status |= inv_imu_write_reg(s, PWR_MGMT0, 1, &data);
|
||||
} else {
|
||||
/* Make sure it is still on */
|
||||
status |= inv_imu_read_reg(s, PWR_MGMT0, 1, &data);
|
||||
if (0 == (data &= PWR_MGMT0_IDLE_MASK))
|
||||
status |= INV_ERROR;
|
||||
}
|
||||
|
||||
/* Decrement the counter */
|
||||
t->need_mclk_cnt--;
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
|
||||
/* Static function */
|
||||
|
||||
static uint8_t *get_register_cache_addr(struct inv_imu_device *s, uint32_t reg)
|
||||
{
|
||||
struct inv_imu_transport *t = (struct inv_imu_transport *)s;
|
||||
|
||||
switch(reg) {
|
||||
case PWR_MGMT0: return &(t->register_cache.pwr_mgmt0_reg);
|
||||
case GYRO_CONFIG0: return &(t->register_cache.gyro_config0_reg);
|
||||
case ACCEL_CONFIG0: return &(t->register_cache.accel_config0_reg);
|
||||
case TMST_CONFIG1_MREG1: return &(t->register_cache.tmst_config1_reg);
|
||||
default: return (uint8_t *)0; // Not found
|
||||
}
|
||||
}
|
||||
|
||||
static int read_sreg(struct inv_imu_device *s, uint8_t reg, uint32_t len, uint8_t *buf)
|
||||
{
|
||||
struct inv_imu_serif *serif = (struct inv_imu_serif *)s;
|
||||
|
||||
if (len > serif->max_read)
|
||||
return INV_ERROR_SIZE;
|
||||
|
||||
if (serif->read_reg(serif, reg, buf, len) != 0)
|
||||
return INV_ERROR_TRANSPORT;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int write_sreg(struct inv_imu_device *s, uint8_t reg, uint32_t len, const uint8_t *buf)
|
||||
{
|
||||
struct inv_imu_serif *serif = (struct inv_imu_serif *)s;
|
||||
|
||||
if (len > serif->max_write)
|
||||
return INV_ERROR_SIZE;
|
||||
|
||||
if (serif->write_reg(serif, reg, buf, len) != 0)
|
||||
return INV_ERROR_TRANSPORT;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int read_mclk_reg(struct inv_imu_device *s, uint16_t regaddr, uint8_t rd_cnt, uint8_t *buf)
|
||||
{
|
||||
uint8_t data;
|
||||
uint8_t blk_sel = (regaddr & 0xFF00) >> 8;
|
||||
int status = 0;
|
||||
|
||||
// Have IMU not in IDLE mode to access MCLK domain
|
||||
status |= inv_imu_switch_on_mclk(s);
|
||||
|
||||
// optimize by changing BLK_SEL only if not NULL
|
||||
if (blk_sel)
|
||||
status |= write_sreg(s, (uint8_t)BLK_SEL_R & 0xff, 1, &blk_sel);
|
||||
|
||||
data = (regaddr & 0x00FF);
|
||||
status |= write_sreg(s, (uint8_t)MADDR_R, 1, &data);
|
||||
inv_imu_sleep_us(10);
|
||||
status |= read_sreg(s, (uint8_t)M_R, rd_cnt, buf);
|
||||
inv_imu_sleep_us(10);
|
||||
|
||||
if (blk_sel) {
|
||||
data = 0;
|
||||
status |= write_sreg(s, (uint8_t)BLK_SEL_R, 1, &data);
|
||||
}
|
||||
|
||||
// switch OFF MCLK if needed
|
||||
status |= inv_imu_switch_off_mclk(s);
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
static int write_mclk_reg(struct inv_imu_device *s, uint16_t regaddr, uint8_t wr_cnt, const uint8_t *buf)
|
||||
{
|
||||
uint8_t data;
|
||||
uint8_t blk_sel = (regaddr & 0xFF00) >> 8;
|
||||
int status = 0;
|
||||
|
||||
// Have IMU not in IDLE mode to access MCLK domain
|
||||
status |= inv_imu_switch_on_mclk(s);
|
||||
|
||||
// optimize by changing BLK_SEL only if not NULL
|
||||
if (blk_sel)
|
||||
status |= write_sreg(s, (uint8_t)BLK_SEL_W, 1, &blk_sel);
|
||||
|
||||
data = (regaddr & 0x00FF);
|
||||
status |= write_sreg(s, (uint8_t)MADDR_W, 1, &data);
|
||||
for (uint8_t i = 0; i < wr_cnt; i++) {
|
||||
status |= write_sreg(s, (uint8_t)M_W, 1, &buf[i]);
|
||||
inv_imu_sleep_us(10);
|
||||
}
|
||||
|
||||
if (blk_sel) {
|
||||
data = 0;
|
||||
status = write_sreg(s, (uint8_t)BLK_SEL_W, 1, &data);
|
||||
}
|
||||
|
||||
status |= inv_imu_switch_off_mclk(s);
|
||||
|
||||
return status;
|
||||
}
|
||||
@@ -0,0 +1,122 @@
|
||||
/*
|
||||
* ________________________________________________________________________________________________________
|
||||
* Copyright (c) 2015-2015 InvenSense Inc. All rights reserved.
|
||||
*
|
||||
* This software, related documentation and any modifications thereto (collectively "Software") is subject
|
||||
* to InvenSense and its licensors' intellectual property rights under U.S. and international copyright
|
||||
* and other intellectual property rights laws.
|
||||
*
|
||||
* InvenSense and its licensors retain all intellectual property and proprietary rights in and to the Software
|
||||
* and any use, reproduction, disclosure or distribution of the Software without an express license agreement
|
||||
* from InvenSense is strictly prohibited.
|
||||
*
|
||||
* EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, THE SOFTWARE IS
|
||||
* PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED
|
||||
* TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT.
|
||||
* EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, IN NO EVENT SHALL
|
||||
* INVENSENSE BE LIABLE FOR ANY DIRECT, SPECIAL, INDIRECT, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, OR ANY
|
||||
* DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT,
|
||||
* NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE
|
||||
* OF THE SOFTWARE.
|
||||
* ________________________________________________________________________________________________________
|
||||
*/
|
||||
|
||||
/** @defgroup Transport IMU transport
|
||||
* @brief Low-level IMU SCLK register access
|
||||
* @ingroup Driver
|
||||
* @{
|
||||
*/
|
||||
|
||||
/** @file inv_imu_transport.h
|
||||
* Low-level IMU SCLK register access
|
||||
*/
|
||||
|
||||
#ifndef _INV_IMU_TRANSPORT_H_
|
||||
#define _INV_IMU_TRANSPORT_H_
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
/* forward declaration */
|
||||
struct inv_imu_device;
|
||||
|
||||
|
||||
/** @brief enumeration of serial interfaces available on IMU */
|
||||
typedef enum
|
||||
{
|
||||
UI_I2C,
|
||||
UI_SPI4,
|
||||
UI_SPI3
|
||||
} SERIAL_IF_TYPE_t;
|
||||
|
||||
/** @brief basesensor serial interface
|
||||
*/
|
||||
struct inv_imu_serif {
|
||||
void *context;
|
||||
int (*read_reg)(struct inv_imu_serif *serif, uint8_t reg, uint8_t *buf, uint32_t len);
|
||||
int (*write_reg)(struct inv_imu_serif *serif, uint8_t reg, const uint8_t *buf, uint32_t len);
|
||||
int (*configure)(struct inv_imu_serif *serif);
|
||||
uint32_t max_read;
|
||||
uint32_t max_write;
|
||||
SERIAL_IF_TYPE_t serif_type;
|
||||
};
|
||||
|
||||
/** @brief transport interface
|
||||
*/
|
||||
struct inv_imu_transport {
|
||||
struct inv_imu_serif serif; /**< Warning : this field MUST be the first one of struct inv_imu_transport */
|
||||
|
||||
/** @brief Contains mirrored values of some IP registers */
|
||||
struct register_cache {
|
||||
uint8_t pwr_mgmt0_reg; /**< PWR_MGMT0, Bank: 0 */
|
||||
uint8_t gyro_config0_reg; /**< GYRO_CONFIG0, Bank: 0 */
|
||||
uint8_t accel_config0_reg; /**< ACCEL_CONFIG0, Bank: 0 */
|
||||
uint8_t tmst_config1_reg; /**< TMST_CONFIG1, Bank: MREG_TOP1 */
|
||||
} register_cache; /**< Store mostly used register values on SRAM */
|
||||
|
||||
uint8_t need_mclk_cnt; /**< internal counter to keep track of everyone that needs MCLK */
|
||||
|
||||
};
|
||||
|
||||
/** @brief Init cache variable.
|
||||
* @return 0 in case of success, -1 for any error
|
||||
*/
|
||||
int inv_imu_init_transport(struct inv_imu_device *s);
|
||||
|
||||
/** @brief Reads data from a register on IMU.
|
||||
* @param[in] reg register address to be read
|
||||
* @param[in] len number of byte to be read
|
||||
* @param[out] buf output data from the register
|
||||
* @return 0 in case of success, -1 for any error
|
||||
*/
|
||||
int inv_imu_read_reg(struct inv_imu_device *s, uint32_t reg, uint32_t len, uint8_t *buf);
|
||||
|
||||
/** @brief Writes data to a register on IMU.
|
||||
* @param[in] reg register address to be written
|
||||
* @param[in] len number of byte to be written
|
||||
* @param[in] buf input data to write
|
||||
* @return 0 in case of success, -1 for any error
|
||||
*/
|
||||
int inv_imu_write_reg(struct inv_imu_device *s, uint32_t reg, uint32_t len, const uint8_t *buf);
|
||||
|
||||
/** @brief Enable MCLK so that MREG are clocked and system beyond SOI can be safely accessed
|
||||
* @return 0 in case of success, -1 for any error
|
||||
*/
|
||||
int inv_imu_switch_on_mclk(struct inv_imu_device *s);
|
||||
|
||||
/** @brief Disable MCLK so that MREG are not clocked anymore, hence reducing power consumption
|
||||
* @return 0 in case of success, -1 for any error
|
||||
*/
|
||||
int inv_imu_switch_off_mclk(struct inv_imu_device *s);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif /* _INV_IMU_TRANSPORT_H_ */
|
||||
|
||||
/** @} */
|
||||
@@ -0,0 +1,37 @@
|
||||
/*
|
||||
* ________________________________________________________________________________________________________
|
||||
* Copyright (c) 2019 InvenSense Inc. All rights reserved.
|
||||
*
|
||||
* This software, related documentation and any modifications thereto (collectively “Software”) is subject
|
||||
* to InvenSense and its licensors' intellectual property rights under U.S. and international copyright
|
||||
* and other intellectual property rights laws.
|
||||
*
|
||||
* InvenSense and its licensors retain all intellectual property and proprietary rights in and to the Software
|
||||
* and any use, reproduction, disclosure or distribution of the Software without an express license agreement
|
||||
* from InvenSense is strictly prohibited.
|
||||
*
|
||||
* EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, THE SOFTWARE IS
|
||||
* PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED
|
||||
* TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT.
|
||||
* EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, IN NO EVENT SHALL
|
||||
* INVENSENSE BE LIABLE FOR ANY DIRECT, SPECIAL, INDIRECT, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, OR ANY
|
||||
* DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT,
|
||||
* NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE
|
||||
* OF THE SOFTWARE.
|
||||
* ________________________________________________________________________________________________________
|
||||
*/
|
||||
|
||||
#ifndef _INV_IMU_VERSION_H_
|
||||
#define _INV_IMU_VERSION_H_
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#define INV_IMU_VERSION_STRING "2.0.4"
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif /* _INV_IMU_VERSION_H_ */
|
||||
@@ -0,0 +1,153 @@
|
||||
//-----------------------------------------------------------------------------
|
||||
/*
|
||||
Copyright © 2014-2015 InvenSense Inc. Portions Copyright © 2014-2015 Movea. All rights reserved.
|
||||
|
||||
This software, related documentation and any modifications thereto (collectively “Software”) is subject
|
||||
to InvenSense and its licensors' intellectual property rights under U.S. and international copyright
|
||||
and other intellectual property rights laws.
|
||||
|
||||
InvenSense and its licensors retain all intellectual property and proprietary rights in and to the Software
|
||||
and any use, reproduction, disclosure or distribution of the Software without an express license agreement
|
||||
from InvenSense is strictly prohibited.
|
||||
*/
|
||||
//-----------------------------------------------------------------------------
|
||||
|
||||
#ifndef INVN_COMMON_INVN_TYPES_H_
|
||||
#define INVN_COMMON_INVN_TYPES_H_
|
||||
|
||||
/**
|
||||
* @defgroup invn_types Types
|
||||
* @brief Motion Library - Type definitions.
|
||||
* \details Definition of codes and error codes used within the MPL and
|
||||
* returned to the user.
|
||||
* Every function tries to return a meaningful error code basing
|
||||
* on the occuring error condition. The error code is numeric.
|
||||
*
|
||||
* The available error codes and their associated values are:
|
||||
* - (0) INV_SUCCESS
|
||||
* - (32) INV_ERROR
|
||||
* - (22 / EINVAL) INV_ERROR_INVALID_PARAMETER
|
||||
* - (1 / EPERM) INV_ERROR_FEATURE_NOT_ENABLED
|
||||
* - (36) INV_ERROR_FEATURE_NOT_IMPLEMENTED
|
||||
* - (64) INV_ERROR_FIFO_READ_COUNT
|
||||
* \todo Clean up the details documentation in order to use only the \\def one.
|
||||
* \todo Add documentation to all the definitions
|
||||
* \ingroup Common
|
||||
* @file invn_types.h
|
||||
*/
|
||||
|
||||
//=======================================//
|
||||
//========= Integer Definition =========//
|
||||
//=======================================//
|
||||
#ifdef _MSC_VER
|
||||
# include "inttypes.h"
|
||||
#else
|
||||
# include <stdint.h>
|
||||
#endif
|
||||
|
||||
//=======================================//
|
||||
//======= Fixed Point Conversion =======//
|
||||
//=======================================//
|
||||
|
||||
//! \def INVN_FLT_TO_FXP
|
||||
//! Convert the \a value from float to QN value. \ingroup invn_macro
|
||||
#define INVN_FLT_TO_FXP(value, shift) ( (int32_t) ((float)(value)*(1ULL << (shift)) + ( (value>=0)-0.5f )) )
|
||||
//! \def INVN_DBL_TO_FXP
|
||||
//! Convert the \a value from double to QN value. \ingroup invn_macro
|
||||
#define INVN_DBL_TO_FXP(value, shift) ( (int32_t) ((double)(value)*(1ULL << (shift)) + ( (value>=0)-0.5 )) )
|
||||
//! \def INVN_FLT_TO_UFXP
|
||||
//! Convert the \a value from float to unsigned QN value. \ingroup invn_macro
|
||||
#define INVN_FLT_TO_UFXP(value, shift) ( (uint32_t) ((float)(value)*(1ULL << (shift)) + 0.5f) )
|
||||
//! \def INVN_DBL_TO_UFXP
|
||||
//! Convert the \a value from double to unsigned QN value. \ingroup invn_macro
|
||||
#define INVN_DBL_TO_UFXP(value, shift) ( (uint32_t) ((double)(value)*(1ULL << (shift)) + 0.5) )
|
||||
//! \def INVN_FXP_TO_FLT
|
||||
//! Convert the \a value from QN value to float. \ingroup invn_macro
|
||||
#define INVN_FXP_TO_FLT(value, shift) ( (float) (int32_t)(value) / (float)(1ULL << (shift)) )
|
||||
//! \def INVN_FXP_TO_DBL
|
||||
//! Convert the \a value from QN value to double. \ingroup invn_macro
|
||||
#define INVN_FXP_TO_DBL(value, shift) ( (double) (int32_t)(value) / (double)(1ULL << (shift)) )
|
||||
//! \def INVN_UFXP_TO_FLT
|
||||
//! Convert the \a value from unsigned QN value to float. \ingroup invn_macro
|
||||
#define INVN_UFXP_TO_FLT(value, shift) ( (float) (uint32_t)(value) / (float)(1ULL << (shift)) )
|
||||
//! \def INVN_UFXP_TO_DBL
|
||||
//! Convert the \a value from unsigned QN value to double. \ingroup invn_macro
|
||||
#define INVN_UFXP_TO_DBL(value, shift) ( (double) (uint32_t)(value) / (double)(1ULL << (shift)) )
|
||||
//! \def INVN_CONVERT_FLT_TO_FXP
|
||||
//! Macro to convert float values from an address into QN values, and copy them to another address. \ingroup invn_macro
|
||||
#define INVN_CONVERT_FLT_TO_FXP(fltptr, fixptr, length, shift) { int32_t i; for(i=0; i<(length); ++i) (fixptr)[i] = INVN_FLT_TO_FXP((fltptr)[i], shift); }
|
||||
//! \def INVN_CONVERT_FLT_TO_UFXP
|
||||
//! Macro to convert float values from an address into unsigned QN values, and copy them to another address. \ingroup invn_macro
|
||||
#define INVN_CONVERT_FLT_TO_UFXP(fltptr, fixptr, length, shift) { int32_t i; for(i=0; i<(length); ++i) (fixptr)[i] = INVN_FLT_TO_UFXP((fltptr)[i], shift); }
|
||||
//! \def INVN_CONVERT_DBL_TO_FXP
|
||||
//! Macro to convert double values from an address into QN values, and copy them to another address. \ingroup invn_macro
|
||||
#define INVN_CONVERT_DBL_TO_FXP(fltptr, fixptr, length, shift) { int32_t i; for(i=0; i<(length); ++i) (fixptr)[i] = INVN_DBL_TO_FXP((fltptr)[i], shift); }
|
||||
//! \def INVN_CONVERT_DBL_TO_UFXP
|
||||
//! Macro to convert double values from an address into unsigned QN values, and copy them to another address. \ingroup invn_macro
|
||||
#define INVN_CONVERT_DBL_TO_UFXP(fltptr, fixptr, length, shift) { int32_t i; for(i=0; i<(length); ++i) (fixptr)[i] = INVN_DBL_TO_UFXP((fltptr)[i], shift); }
|
||||
//! \def INVN_CONVERT_FXP_TO_FLT
|
||||
//! Macro to convert QN values from an address into float values, and copy them to another address. \ingroup invn_macro
|
||||
#define INVN_CONVERT_FXP_TO_FLT(fixptr, fltptr, length, shift) { int32_t i; for(i=0; i<(length); ++i) (fltptr)[i] = INVN_FXP_TO_FLT((fixptr)[i], shift); }
|
||||
//! \def INVN_CONVERT_UFXP_TO_FLT
|
||||
//! Macro to convert unsigned QN values from an address into float values, and copy them to another address. \ingroup invn_macro
|
||||
#define INVN_CONVERT_UFXP_TO_FLT(fixptr, fltptr, length, shift) { int32_t i; for(i=0; i<(length); ++i) (fltptr)[i] = INVN_UFXP_TO_FLT((fixptr)[i], shift); }
|
||||
//! \def INVN_CONVERT_FXP_TO_DBL
|
||||
//! Macro to convert QN values from an address into double values, and copy them to another address. \ingroup invn_macro
|
||||
#define INVN_CONVERT_FXP_TO_DBL(fixptr, fltptr, length, shift) { int32_t i; for(i=0; i<(length); ++i) (fltptr)[i] = INVN_FXP_TO_DBL((fixptr)[i], shift); }
|
||||
//! \def INVN_CONVERT_UFXP_TO_DBL
|
||||
//! \brief Macro to convert unsigned QN values from an address into double values, and copy them to another address. \ingroup invn_macro
|
||||
#define INVN_CONVERT_UFXP_TO_DBL(fixptr, fltptr, length, shift) { int32_t i; for(i=0; i<(length); ++i) (fltptr)[i] = INVN_UFXP_TO_DBL((fixptr)[i], shift); }
|
||||
|
||||
|
||||
//=====================================//
|
||||
//========= Error Definition =========//
|
||||
//=====================================//
|
||||
|
||||
#ifndef REMOVE_INV_ERROR_T
|
||||
typedef int32_t inv_error_t; /*!< Type used for error definitions. \ingroup invn_types */
|
||||
#endif
|
||||
|
||||
//typedef int32_t mpu_error_t;
|
||||
typedef int64_t mpu_time_t; /*!< Type used for mpu time. \ingroup invn_types */
|
||||
|
||||
// Typically I2C addresses are 8-bit, but some specifications allow for a 10-bit address
|
||||
// This definition allows the length to be optimally defined for the platform
|
||||
typedef uint8_t inv_i2c_addr_t; /*!< Type used for I2C addresses. \ingroup invn_types */
|
||||
|
||||
#ifdef __IAR_SYSTEMS_ICC__
|
||||
// These are defined in standard C errno.h
|
||||
#define EINVAL (22)
|
||||
#define EPERM (1)
|
||||
#define ENOMEM (12)
|
||||
#else
|
||||
#include "errno.h"
|
||||
#endif
|
||||
|
||||
#define INVN_SUCCESS (0) /*!< Constant definition for success. \ingroup invn_types */
|
||||
#define INVN_ERROR_BASE (0x20) /*!< Constant definition for basic error. Value is \b 32 \ingroup invn_types */
|
||||
#define INVN_ERROR (INVN_ERROR_BASE) /*!< Constant definition for error. Value is \b 32 \ingroup invn_types */
|
||||
#define INVN_ERROR_FEATURE_NOT_ENABLED (EPERM) /*!< Constant definition for feature not enabled error. \ingroup invn_types */
|
||||
#define INVN_ERROR_FEATURE_NOT_IMPLEMENTED (INVN_ERROR_BASE + 4) /*!< Constant definition for feature not implemented error. \ingroup invn_types */
|
||||
#define INVN_ERROR_INVALID_PARAMETER (EINVAL) /*!< Constant definition for invalid parameter error. \ingroup invn_types */
|
||||
#define INVN_ERROR_FILE_OPEN (INVN_ERROR_BASE + 14) /*!< Constant definition for opening file error. \ingroup invn_types */
|
||||
#define INVN_ERROR_FILE_READ (INVN_ERROR_BASE + 15) /*!< Constant definition for reading file error. \ingroup invn_types */
|
||||
#define INVN_ERROR_FILE_WRITE (INVN_ERROR_BASE + 16) /*!< Constant definition for writing file error. \ingroup invn_types */
|
||||
#define INVN_ERROR_INVALID_CONFIGURATION (INVN_ERROR_BASE + 17) /*!< Constant definition for invalid configuration error. \ingroup invn_types */
|
||||
/* Serial Communication */
|
||||
#define INVN_ERROR_SERIAL_OPEN_ERROR (INVN_ERROR_BASE + 21) /*!< Constant definition for serial open error. \ingroup invn_types */
|
||||
#define INVN_ERROR_SERIAL_READ (INVN_ERROR_BASE + 22) /*!< Constant definition for serial read error. \ingroup invn_types */
|
||||
#define INVN_ERROR_SERIAL_WRITE (INVN_ERROR_BASE + 23) /*!< Constant definition for serial write error. \ingroup invn_types */
|
||||
/* Fifo */
|
||||
#define INVN_ERROR_FIFO_OVERFLOW (INVN_ERROR_BASE + 30) /*!< Constant definition for fifo overflow error. \ingroup invn_types */
|
||||
#define INVN_ERROR_FIFO_FOOTER (INVN_ERROR_BASE + 31) /*!< Constant definition for fifo footer error. \ingroup invn_types */
|
||||
#define INVN_ERROR_FIFO_READ_COUNT (INVN_ERROR_BASE + 32) /*!< Constant definition for fifo read count error. \ingroup invn_types */
|
||||
#define INVN_ERROR_FIFO_READ_DATA (INVN_ERROR_BASE + 33) /*!< Constant definition for fifo read data error. \ingroup invn_types */
|
||||
/* OS interface errors */
|
||||
#define INVN_ERROR_OS_BAD_HANDLE (INVN_ERROR_BASE + 61) /*!< Constant definition for OS bad handle error. \ingroup invn_types */
|
||||
#define INVN_ERROR_OS_CREATE_FAILED (INVN_ERROR_BASE + 62) /*!< Constant definition for OS create failed error. \ingroup invn_types */
|
||||
#define INVN_ERROR_OS_LOCK_FAILED (INVN_ERROR_BASE + 63) /*!< Constant definition for OS lock failed error. \ingroup invn_types */
|
||||
/* Warning */
|
||||
#define INVN_WARNING_SEMAPHORE_TIMEOUT (INVN_ERROR_BASE + 86) /*!< Constant definition for semaphore timeout warning. \ingroup invn_types */
|
||||
|
||||
|
||||
#endif // INVN_COMMON_INVN_TYPES_H_
|
||||
@@ -0,0 +1,156 @@
|
||||
/*
|
||||
$License:
|
||||
Copyright (C) 2018 InvenSense Corporation, All Rights Reserved.
|
||||
$
|
||||
*/
|
||||
|
||||
|
||||
#ifndef _INVN_ALGO_AGM_H_
|
||||
#define _INVN_ALGO_AGM_H_
|
||||
|
||||
#include "invn_types.h"
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/** \defgroup AGM AGM
|
||||
* \brief Algorithm that provides device orientation. Algorithm inputs are raw Accelerometer, Gyroscope and Magnetometer data.
|
||||
* Algorithm outputs: calibrated sensor and 9-axis sensor fusion.
|
||||
* \warning supported sampling frequency [50 Hz-1000 Hz]
|
||||
* \warning supported gyroscope FSR [250 dps, 500 dps, 1000 dps, 2000 dps, 4000 dps]
|
||||
* \warning supported accelerometer FSR [1 g, 2 g, 4 g, 8 g, 16 g]
|
||||
*/
|
||||
|
||||
#define INVN_ALGO_AGM_INPUT_MASK_ACC 1 ///< Raw Accel update mask
|
||||
#define INVN_ALGO_AGM_INPUT_MASK_GYR 2 ///< Raw Gyro update mask
|
||||
#define INVN_ALGO_AGM_INPUT_MASK_MAG 4 ///< Raw Mag update mask
|
||||
|
||||
#define INVN_ALGO_AGM_OUTPUT_MASK_ACCEL_CAL 1 ///< Accel cal output update mask
|
||||
#define INVN_ALGO_AGM_OUTPUT_MASK_GYRO_CAL 2 ///< Gyro cal output update mask
|
||||
#define INVN_ALGO_AGM_OUTPUT_MASK_MAG_CAL 4 ///< Mag cal output update mask
|
||||
#define INVN_ALGO_AGM_OUTPUT_MASK_QUAT_AG 8 ///< Game Rotation Vector (Accel and Gyro Fusion) output update mask
|
||||
#define INVN_ALGO_AGM_OUTPUT_MASK_QUAT_AGM 16 ///< Rotation Vector (Accel, Gyro and Magnetometer Fusion) output update mask
|
||||
#define INVN_ALGO_AGM_OUTPUT_MASK_GRAVITY 32 ///< Gravity vector output update mask
|
||||
#define INVN_ALGO_AGM_OUTPUT_MASK_LINEARACC 64 ///< Linear acceleration vector output update mask
|
||||
|
||||
|
||||
/* Forward declarations */
|
||||
struct inv_icm426xx;
|
||||
|
||||
/*! \struct InvnAlgoAGMInput
|
||||
* AGM input structure (raw data) \ingroup AGM
|
||||
*/
|
||||
typedef struct
|
||||
{
|
||||
int32_t mask; /*!< mask to specify updated inputs. */
|
||||
int64_t sRimu_time_us; /*!< timestamp \f$ [\mu s]\f$ of raw accel and gyro */
|
||||
int32_t sRacc_data[3]; /*!< raw accelerometer in high resolution mode. Expect Full Scale Value coded on 20 bit (i.e. +/- FSR g = 1<<19 LSB) */
|
||||
int32_t sRgyr_data[3]; /*!< raw gyroscope in high resolution mode. Expect Full Scale Value coded on 20 bit (i.e. +/- FSR dps = 1<<19 LSB) */
|
||||
int16_t sRtemp_data; /*!< raw temperature */
|
||||
int64_t sRmag_time_us; /*!< timestamp of raw mag */
|
||||
int32_t sRmag_data[3]; /*!< raw mag */
|
||||
} InvnAlgoAGMInput;
|
||||
|
||||
|
||||
/*! \struct InvnAlgoAGMOutput
|
||||
* AGM output structure (calibrated sensors and fusion output) \ingroup AGM
|
||||
*/
|
||||
typedef struct
|
||||
{
|
||||
int32_t mask; /*!< mask to specify updated outputs */
|
||||
int32_t acc_uncal_q16[3]; /*!< uncalibrated accelerometer (1 g = 1<<16) */
|
||||
int32_t acc_cal_q16[3]; /*!< calibrated accelerometer (1 g = 1<<16) */
|
||||
int32_t acc_bias_q16[3]; /*!< accelerometer bias (1 g = 1<<16)*/
|
||||
int8_t acc_accuracy_flag; /*!< accelerometer accuracy from 0(non calibrated) to 3(well calibrated) */
|
||||
|
||||
int32_t gyr_uncal_q16[3]; /*!< uncalibrated gyroscope (1 dps = 1<<16) */
|
||||
int32_t gyr_cal_q16[3]; /*!< calibrated gyroscope (1 dps = 1<<16) */
|
||||
int32_t gyr_bias_q16[3]; /*!< gyro bias (1 dps = 1<<16)*/
|
||||
int8_t gyr_accuracy_flag; /*!< gyro accuracy, from 0(non calibrated) to 3(well calibrated) */
|
||||
|
||||
int32_t mag_uncal_q16[3]; /*!< uncalibrated magnetometer (1uT = 1<<16) */
|
||||
int32_t mag_cal_q16[3]; /*!< calibrated magnetometer (1uT = 1<<16) */
|
||||
int32_t mag_bias_q16[3]; /*!< magnetometer bias (1uT = 1<<16) */
|
||||
int8_t mag_accuracy_flag; /*!< magnetometer accuracy, from 0(non calibrated) to 3(well calibrated) */
|
||||
|
||||
int32_t grv_quat_q30[4]; /*!< 6-axis (accel and gyro fusion) quaternion */
|
||||
int32_t rv_quat_q30[4]; /*!< 9-axis (accel, gyro and magnetometer fusion) quaternion */
|
||||
int32_t rv_accuracy_q27; /*!< 9-axis (accel, gyro and magnetometer fusion) 3\sigma accuracy in rad */
|
||||
int32_t gravity_q16[3]; /*!< gravity estimation in sensor frame */
|
||||
int32_t linear_acc_q16[3]; /*!< linear acceleration estimation in sensor frame */
|
||||
|
||||
int32_t temp_degC_q16; /*!< temperature (1 \f$ [^{\circ}C]\f$ = 1<<16)*/
|
||||
} InvnAlgoAGMOutput;
|
||||
|
||||
|
||||
/*! \struct InvnAlgoAGMConfig
|
||||
* AGM configuration structure (sensor related settings) \ingroup AGM
|
||||
*/
|
||||
typedef struct
|
||||
{
|
||||
int32_t * acc_bias_q16; /*!< Previously stored accel bias pointer. If pointer is NULL or 0, offset will be set to { 0, 0, 0} */
|
||||
int32_t * gyr_bias_q16; /*!< Previously stored gyro bias pointer. If pointer is NULL or 0, offset will be set to { 0, 0, 0} */
|
||||
int32_t * mag_bias_q16; /*!< mag_bias_q16 Previously stored mag bias pointer If pointer is NULL or 0, offset will be set to { 0, 0, 0} */
|
||||
int8_t acc_accuracy; /*!< Previously stored accelerometer bias accuracy (0 to 3) */
|
||||
int8_t gyr_accuracy; /*!< Previously stored gyroscope bias accuracy (0 to 3) */
|
||||
int8_t mag_accuracy; /*!< Previously stored magnetometer bias accuracy (0 to 3) */
|
||||
|
||||
int32_t acc_fsr; /*!< accelerometer full scale range [g] */
|
||||
int32_t gyr_fsr; /*!< gyroscope full scale range [dps] */
|
||||
|
||||
uint32_t acc_odr_us; /*!< accelerometer output data rate in \f$ [\mu s]\f$ */
|
||||
uint32_t gyr_odr_us; /*!< gyroscope output data rate \f$ [\mu s]\f$ */
|
||||
|
||||
int32_t mag_sc_q16; /*!< magnetometer sensitivity (uT/LSB, e.g. mag_uT = (mag_sc_q16 * raw_mag_LSB)/65536) */
|
||||
uint32_t mag_odr_us; /*!< magnetometer output data rate \f$ [\mu s]\f$ */
|
||||
|
||||
int32_t temp_sensitivity; /*!< temperature sensitivity in q30 (if temperature(\f$ ^{\circ}C \f$) = LSB * k + z, then temp_sensitivity = k) */
|
||||
int32_t temp_offset; /*!< temperature offset in q16 (if temperature(\f$ ^{\circ}C \f$) = LSB * k + z, then temp_offset = z) */
|
||||
} InvnAlgoAGMConfig;
|
||||
|
||||
|
||||
/*!
|
||||
* \brief Return library version x.y.z-suffix as a char array
|
||||
* \retval library version a char array "x.y.z-suffix"
|
||||
* \ingroup AGM
|
||||
*/
|
||||
const char * invn_algo_agm_version(void);
|
||||
|
||||
/*!
|
||||
* \brief Initializes algorithms with default parameters and reset states.
|
||||
* (\icm_device[in] Invensense ICM426XX device pointer. Only when gyro assisted is enabled.)
|
||||
* \config[in] algo init parameters structure.
|
||||
* \return initialization success indicator.
|
||||
* \retval 0 Success
|
||||
* \retval 1 Fail
|
||||
* \ingroup AGM
|
||||
*/
|
||||
#ifdef WITH_GYRO_ASSIST
|
||||
uint8_t invn_algo_agm_init_a(struct inv_icm426xx * icm_device, const InvnAlgoAGMConfig * config);
|
||||
#else
|
||||
uint8_t invn_algo_agm_init(const InvnAlgoAGMConfig * config);
|
||||
#endif
|
||||
|
||||
/*!
|
||||
* \brief Sets algo config structure.
|
||||
* \config[in] config structure of the algo.
|
||||
* \ingroup AGM
|
||||
*/
|
||||
void invn_algo_agm_set_config(const InvnAlgoAGMConfig * config);
|
||||
|
||||
|
||||
/*!
|
||||
* \brief Performs algorithm computation.
|
||||
* \in inputs algorithm input. Input mask (inputs->mask) should be set with respect to new sensor data in InvnAlgoAGMInput.
|
||||
* \out outputs algorithm output. Output mask (outputs->mask) reports updated outputs in InvnAlgoAGMOutput.
|
||||
* \ingroup AGM
|
||||
*/
|
||||
void invn_algo_agm_process(const InvnAlgoAGMInput *inputs, InvnAlgoAGMOutput *outputs);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
#endif
|
||||
Binary file not shown.
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,582 @@
|
||||
/*******************************************************************************
|
||||
* @file app_raw.c
|
||||
* @author CandyPops Co.
|
||||
* @version V1.0.0
|
||||
* @date 2022-09-05
|
||||
* @brief
|
||||
******************************************************************************/
|
||||
|
||||
/*******************************************************************************
|
||||
* [모듈 개요] ICM42670P IMU 드라이버 상위 레이어
|
||||
*
|
||||
* ICM42670P IMU 센서의 초기화, 설정, 데이터 읽기를 담당하는 애플리케이션 레이어 모듈
|
||||
* InvenSense 드라이버 API를 래핑하여 사용
|
||||
*
|
||||
* 주요 기능:
|
||||
* 1) setup_imu_device() - IMU 초기화 및 WHOAMI 확인 (0x67 = ICM42670P)
|
||||
* 2) configure_imu_device() - 센서 파라미터 설정
|
||||
* - 가속도계: ±4g FSR, 100Hz(저전력) 또는 800Hz(저잡음)
|
||||
* - 자이로: ±2000dps FSR, 100Hz 또는 800Hz
|
||||
* - FIFO 비활성화 (레지스터 직접 읽기 모드)
|
||||
* 3) get_imu_data() - FIFO 또는 레지스터에서 센서 데이터 읽기
|
||||
* 4) imu_callback() - 센서 데이터 수신 콜백
|
||||
* - 마운팅 매트릭스 적용 (보드 방향 보정)
|
||||
* - info4 모드: info_imu[6]에 데이터 저장
|
||||
* - BLE 모드: "rsp:" 태그로 6축 데이터 BLE 전송
|
||||
* - UART 모드: 텍스트 형식으로 시리얼 출력
|
||||
* 5) imu_read_direct() - 드라이버 API를 우회한 직접 I2C 레지스터 읽기
|
||||
* - 센서 설정 → 전원 ON → 80ms 대기 → 12바이트 읽기 → 슬립
|
||||
*
|
||||
* 마운팅 매트릭스:
|
||||
* Q30 고정소수점 형식의 3x3 회전 매트릭스로, 보드에 장착된 센서의물리적 방향을 소프트웨어 좌표계에 맞춰 보정
|
||||
******************************************************************************/
|
||||
|
||||
#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 "debug_print.h"
|
||||
#include "nrf_delay.h"
|
||||
|
||||
|
||||
/*
|
||||
* 데이터 출력 형식 선택
|
||||
* 0 : 원시 데이터 (raw accel, gyro, temp) 출력
|
||||
* 1 : 스케일링된 데이터 (g, dps, 섭씨) 출력
|
||||
*/
|
||||
#define SCALED_DATA_G_DPS 0
|
||||
|
||||
|
||||
/* --------------------------------------------------------------------------------------
|
||||
* Static and extern variables
|
||||
* -------------------------------------------------------------------------------------- */
|
||||
|
||||
/* IMU 드라이버 객체 — 드라이버 API 호출 시 항상 이 구조체 전달 */
|
||||
static struct inv_imu_device icm_driver;
|
||||
|
||||
/* BLE 전송용 바이너리 버퍼 */
|
||||
uint8_t imu_bin_buffer[BLE_NUS_MAX_DATA_LEN];
|
||||
|
||||
/*
|
||||
* ICM42670P 마운팅 매트릭스 (Q30 고정소수점)
|
||||
*
|
||||
* 센서가 보드에 장착된 물리적 방향에 따라 좌표 변환
|
||||
* Q30 형식: 1.0 = (1 << 30) = 0x40000000
|
||||
*
|
||||
* SM_REVB_DB (개발보드): X→-Y, Y→X 변환 (90도 회전)
|
||||
* 기본 (SmartMotion): 단위 행렬 (변환 없음)
|
||||
*/
|
||||
#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
|
||||
|
||||
bool custom_add_data; /* 커스텀 데이터 추가 플래그 (BLE 전송 제어) */
|
||||
extern bool motion_raw_data_enabled; /* 외부에서 원시 데이터 읽기를 요청하는 플래그 */
|
||||
extern char ble_tx_buffer[BLE_NUS_MAX_DATA_LEN]; /* BLE 텍스트 전송 버퍼 */
|
||||
extern which_cmd_t cmd_type_t; /* 현재 명령 소스 (BLE 또는 UART) */
|
||||
uint16_t ssp_data[6]={0,}; /* BLE 전송용 6축 데이터 배열 (accel XYZ + gyro XYZ) */
|
||||
extern bool info4; /* info4 모드 플래그 (cmd_parse에서 설정) */
|
||||
volatile uint16_t info_imu[6]; /* info4 모드에서 IMU 데이터를 저장하는 전역 배열 */
|
||||
|
||||
/* --------------------------------------------------------------------------------------
|
||||
* static function declaration
|
||||
* -------------------------------------------------------------------------------------- */
|
||||
static void apply_mounting_matrix(const int32_t matrix[9], int32_t raw[3]);
|
||||
|
||||
/* --------------------------------------------------------------------------------------
|
||||
* Functions definition
|
||||
* -------------------------------------------------------------------------------------- */
|
||||
|
||||
/*
|
||||
* setup_imu_device()
|
||||
* IMU 디바이스 초기화 및 식별 확인
|
||||
*
|
||||
* 처리 흐름:
|
||||
* 1) inv_imu_init()으로 드라이버 초기화 (시리얼 인터페이스 + 콜백 등록)
|
||||
* 2) WHOAMI 레지스터 읽기로 디바이스 확인
|
||||
* 3) WHOAMI 값이 ICM_WHOAMI(0x67)와 일치하는지 검증
|
||||
*
|
||||
* 반환값: 0=성공, 음수=에러
|
||||
*/
|
||||
int setup_imu_device(struct inv_imu_serif *icm_serif)
|
||||
{
|
||||
int rc = 0;
|
||||
uint8_t who_am_i;
|
||||
|
||||
/* IMU 드라이버 초기화 — 시리얼 인터페이스 연결 및 콜백 함수 등록 */
|
||||
rc = inv_imu_init(&icm_driver, icm_serif, imu_callback);
|
||||
if (rc != INV_ERROR_SUCCESS) {
|
||||
DBG_PRINTF("!!! ERROR : Failed to initialize IMU!\r\n");
|
||||
return rc;
|
||||
}
|
||||
|
||||
/* WHOAMI 레지스터 읽기 — 디바이스 존재 및 통신 확인 */
|
||||
rc = inv_imu_get_who_am_i(&icm_driver, &who_am_i);
|
||||
if (rc != INV_ERROR_SUCCESS) {
|
||||
DBG_PRINTF("!!! ERROR : Failed to read whoami!\r\n");
|
||||
return rc;
|
||||
}
|
||||
|
||||
/* WHOAMI 값 검증 — ICM42670P의 경우 0x67이어야 함 */
|
||||
if (who_am_i != ICM_WHOAMI) {
|
||||
DBG_PRINTF("!!! ERROR : Bad WHOAMI value! Read 0x%02x, expected 0x%02x\r\n", who_am_i, ICM_WHOAMI);
|
||||
return INV_ERROR;
|
||||
}
|
||||
|
||||
return rc;
|
||||
}
|
||||
|
||||
/*
|
||||
* configure_imu_device()
|
||||
* IMU 센서 동작 파라미터 설정
|
||||
*
|
||||
* 설정 항목:
|
||||
* - FIFO: 비활성화 (USE_FIFO=0일 때, 레지스터 직접 읽기 모드)
|
||||
* - 가속도계 FSR: ±4g (USE_HIGH_RES_MODE=0일 때)
|
||||
* - 자이로 FSR: ±2000dps
|
||||
* - ODR(출력 데이터율):
|
||||
* - 저잡음 모드(USE_LOW_NOISE_MODE=1): 800Hz
|
||||
* - 저전력 모드(USE_LOW_NOISE_MODE=0): 100Hz
|
||||
* - 자이로는 항상 저잡음 모드로 동작
|
||||
* - FIFO 미사용 시 자이로 스타트업 시간만큼 대기
|
||||
*
|
||||
* 반환값: 0=성공, 음수=에러
|
||||
*/
|
||||
int configure_imu_device(void)
|
||||
{
|
||||
int rc = 0;
|
||||
|
||||
/* FIFO 비활성화 — 레지스터에서 직접 데이터를 읽기 */
|
||||
if (!USE_FIFO)
|
||||
rc |= inv_imu_configure_fifo(&icm_driver, INV_IMU_FIFO_DISABLED);
|
||||
|
||||
if (USE_HIGH_RES_MODE) {
|
||||
/* 고해상도 FIFO 모드: 20비트 데이터, FSR은 16g/2000dps로 고정됨 */
|
||||
rc |= inv_imu_enable_high_resolution_fifo(&icm_driver);
|
||||
} else {
|
||||
/* 표준 모드: 가속도계 ±4g, 자이로 ±2000dps FSR 설정 */
|
||||
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) {
|
||||
/* 저잡음 모드: 800Hz ODR, 가속도계 저잡음 모드 활성화 */
|
||||
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 {
|
||||
/* 저전력 모드: 100Hz ODR, 가속도계 저전력 모드 활성화 */
|
||||
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);
|
||||
|
||||
/* FIFO 미사용 시 자이로 스타트업 시간만큼 대기 (첫 유효 데이터까지의 지연) */
|
||||
if (!USE_FIFO)
|
||||
inv_imu_sleep_us(GYR_STARTUP_TIME_US);
|
||||
|
||||
return rc;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* get_imu_data()
|
||||
* IMU에서 센서 데이터 읽기
|
||||
* USE_FIFO 설정에 따라 FIFO 또는 레지스터에서 데이터를 가져옴
|
||||
* 읽은 데이터는 imu_callback()을 통해 처리
|
||||
*/
|
||||
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
|
||||
/*
|
||||
* get_accel_and_gyr_fsr()
|
||||
* 현재 설정된 가속도계와 자이로의 FSR(Full Scale Range) 값을 가져온다.
|
||||
* 스케일링된 데이터(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
|
||||
|
||||
|
||||
/*
|
||||
* imu_callback()
|
||||
* IMU 드라이버가 새 센서 데이터를 읽을 때마다 호출되는 콜백 함수
|
||||
*
|
||||
* 처리 흐름:
|
||||
* 1) 이벤트에서 가속도/자이로 원시 데이터 추출
|
||||
* - FIFO 모드: 타임스탬프 롤오버 처리, 고해상도(20비트) 지원
|
||||
* - 레지스터 모드: 16비트 데이터 직접 사용
|
||||
* 2) 마운팅 매트릭스 적용 (보드 장착 방향 보정)
|
||||
* 3) 데이터 출력 (모드에 따라 분기):
|
||||
* - info4 모드: info_imu[6] 전역 배열에 저장 (외부에서 폴링)
|
||||
* - UART 모드: "Tp" 접두사로 6축 데이터 텍스트 출력
|
||||
* - BLE 모드: "rsp:" 태그로 바이너리 패킷 전송 + UART 텍스트 동시 출력
|
||||
*/
|
||||
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;
|
||||
|
||||
/* FIFO 타임스탬프 롤오버 처리 (16비트 → 64비트 확장) */
|
||||
if (last_fifo_timestamp > event->timestamp_fsync)
|
||||
rollover_num++;
|
||||
last_fifo_timestamp = event->timestamp_fsync;
|
||||
|
||||
/* 타임스탬프를 마이크로초 단위로 변환 (Q24 해상도 적용) */
|
||||
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) {
|
||||
/* 고해상도 모드: 16비트 데이터를 4비트 좌측 시프트 + 하위 4비트 추가 → 20비트 */
|
||||
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 {
|
||||
/* 표준 해상도: 16비트 데이터 그대로 사용 */
|
||||
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
|
||||
|
||||
/* 레지스터 직접 읽기 모드: 16비트 원시 데이터 추출 */
|
||||
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];
|
||||
|
||||
/* 레지스터 모드에서는 센서 마스크를 강제 설정하여 아래 출력 로직이 동작하도록 함 */
|
||||
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
|
||||
/*
|
||||
* 원시 데이터를 물리 단위(g, dps)로 변환
|
||||
* 변환 공식: 물리값 = 원시값 * FSR / INT16_MAX
|
||||
*/
|
||||
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;
|
||||
|
||||
/* 온도 변환: 고해상도/레지스터 모드는 /128, FIFO 표준 모드는 /2 */
|
||||
if (USE_HIGH_RES_MODE || !USE_FIFO)
|
||||
temp_degc = 25 + ((float)event->temperature / 128);
|
||||
else
|
||||
temp_degc = 25 + ((float)event->temperature / 2);
|
||||
|
||||
/*
|
||||
* 스케일링된 데이터를 UART로 출력
|
||||
*/
|
||||
if (event->sensor_mask & (1 << INV_SENSOR_ACCEL) && event->sensor_mask & (1 << INV_SENSOR_GYRO))
|
||||
DBG_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
|
||||
|
||||
/*
|
||||
* 원시 데이터 출력 — 명령 소스(info4/UART/BLE)에 따라 분기
|
||||
*/
|
||||
if (event->sensor_mask & (1 << INV_SENSOR_ACCEL) && event->sensor_mask & (1 << INV_SENSOR_GYRO) || motion_raw_data_enabled)
|
||||
{
|
||||
motion_raw_data_enabled = false;
|
||||
|
||||
/* info4 모드: 전역 배열 info_imu[6]에 데이터 저장, 외부 모듈에서 이 배열을 폴링하여 데이터 사용 */
|
||||
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];
|
||||
}
|
||||
|
||||
/* UART 모드: "Tp" 접두사로 6축 데이터를 텍스트 형식으로 출력 */
|
||||
else if(cmd_type_t == CMD_UART) {
|
||||
//DBG_PRINTF("Tp%d,%d,%d,%d,%d,%d\r\n\r\n", accel[0], accel[1], accel[2], gyro[0], gyro[1], gyro[2]);
|
||||
}
|
||||
|
||||
/*
|
||||
* BLE 모드: 6축 데이터를 바이너리 패킷으로 BLE 전송
|
||||
* ssp_data[0~2] = 가속도 XYZ, ssp_data[3~5] = 자이로 XYZ
|
||||
* format_data()로 "rsp:" 태그 + 12바이트 데이터를 패킷화
|
||||
* dr_binary_tx_safe()로 8바이트 BLE 전송
|
||||
*/
|
||||
else if(cmd_type_t == CMD_BLE) {
|
||||
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);
|
||||
//DBG_PRINTF("Tp%d,%d,%d,%d,%d,%d\r\n\r\n", accel[0], accel[1], accel[2], gyro[0], gyro[1], gyro[2]);
|
||||
dr_binary_tx_safe(imu_bin_buffer,8);
|
||||
|
||||
if(custom_add_data==true) {
|
||||
custom_add_data = false;
|
||||
}
|
||||
else {
|
||||
//data_tx_handler(ble_tx_buffer);
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
/* --------------------------------------------------------------------------------------
|
||||
* Static functions definition
|
||||
* -------------------------------------------------------------------------------------- */
|
||||
|
||||
/*
|
||||
* apply_mounting_matrix()
|
||||
* Q30 고정소수점 회전 매트릭스를 3축 벡터에 적용한다.
|
||||
*
|
||||
* 계산 방식:
|
||||
* result[i] = matrix[i*3+0]*raw[0] + matrix[i*3+1]*raw[1] + matrix[i*3+2]*raw[2]
|
||||
* 결과를 30비트 우측 시프트하여 Q30 → 정수 변환
|
||||
*
|
||||
* 이를 통해 센서의 물리적 장착 방향에 관계없이 일관된 좌표계를 사용할 수 있다.
|
||||
*/
|
||||
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]);
|
||||
}
|
||||
/* Q30 → 정수 변환: 30비트 우측 시프트 */
|
||||
raw[0] = (int32_t)(data_q30[0]>>30);
|
||||
raw[1] = (int32_t)(data_q30[1]>>30);
|
||||
raw[2] = (int32_t)(data_q30[2]>>30);
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* imu_read_direct()
|
||||
* 드라이버 API를 우회하여 직접 I2C 레지스터를 읽는 함수.
|
||||
* DRDY(데이터 준비) 인터럽트를 기다리지 않고 즉시 데이터를 읽는다.
|
||||
*
|
||||
* 처리 흐름:
|
||||
* 1) TWI 초기화 확인 (최초 1회만)
|
||||
* 2) 자이로 설정: ±2000dps, 100Hz ODR (GYRO_CONFIG0 = 0x09)
|
||||
* 3) 가속도 설정: ±4g, 100Hz ODR (ACCEL_CONFIG0 = 0x29)
|
||||
* 4) 전원 ON: 가속도+자이로 저잡음 모드 (PWR_MGMT0 = 0x0F)
|
||||
* 5) 80ms 대기 (자이로 스타트업: 최소 45ms + 여유)
|
||||
* 6) ACCEL_DATA_X1(0x0B)부터 12바이트 연속 읽기 (accel 6 + gyro 6)
|
||||
* 7) 빅엔디안 → int16_t 변환
|
||||
* 8) 마운팅 매트릭스 적용
|
||||
* 9) "rsp:" 태그로 BLE 전송
|
||||
* 10) IMU 슬립 모드로 전환 (전력 절감)
|
||||
*
|
||||
* 반환값: 0=성공, -1=TX 실패, -2=RX 실패
|
||||
*/
|
||||
/* Raw I2C read from ICM42670P — bypasses driver API entirely */
|
||||
#include "system_interface.h"
|
||||
#include "nrfx_twi.h"
|
||||
|
||||
extern const nrfx_twi_t m_twi_icm42670;
|
||||
|
||||
#define IMU_I2C_ADDR 0x68
|
||||
#define REG_ACCEL_X1 0x0B /* ACCEL_DATA_X1 — 가속도 X축 상위 바이트 레지스터 */
|
||||
|
||||
/* --------------------------------------------------------------------------------------
|
||||
* Direct IMU register read — raw I2C, no DRDY, sends rsp: via BLE
|
||||
* 직접 I2C로 레지스터 읽는 방식 (인터럽트 X, IMU 드라이버 API X)
|
||||
* -------------------------------------------------------------------------------------- */
|
||||
int imu_read_direct(void)
|
||||
{
|
||||
uint8_t raw[12]; /* 가속도 6바이트 + 자이로 6바이트 */
|
||||
int32_t accel[3], gyro[3];
|
||||
uint8_t reg;
|
||||
uint32_t ret;
|
||||
|
||||
static bool twi_ready = false;
|
||||
|
||||
/* TWI(I2C) 초기화 — 최초 1회만 수행 (재초기화로 클린 상태 보장) */
|
||||
if (!twi_ready) {
|
||||
inv_i2c_master_uninitialize();
|
||||
inv_i2c_master_initialize();
|
||||
twi_ready = true;
|
||||
}
|
||||
|
||||
/* 자이로 설정: GYRO_CONFIG0(0x20) = 0x09 → ±2000dps FSR, 100Hz ODR */
|
||||
{
|
||||
uint8_t gyro_cfg[2] = { 0x20, 0x09 };
|
||||
icm42670_twi_tx(IMU_I2C_ADDR, gyro_cfg, 2, false);
|
||||
}
|
||||
|
||||
/* 가속도 설정: ACCEL_CONFIG0(0x21) = 0x29 → ±4g FSR, 100Hz ODR */
|
||||
{
|
||||
uint8_t accel_cfg[2] = { 0x21, 0x29 };
|
||||
icm42670_twi_tx(IMU_I2C_ADDR, accel_cfg, 2, false);
|
||||
}
|
||||
|
||||
/* 전원 ON: PWR_MGMT0(0x1F) = 0x0F → 가속도(저잡음) + 자이로(저잡음) 활성화 */
|
||||
{
|
||||
uint8_t pwr_cmd[2] = { 0x1F, 0x0F }; /* reg=0x1F, val=0x0F */
|
||||
icm42670_twi_tx(IMU_I2C_ADDR, pwr_cmd, 2, false);
|
||||
//nrf_delay_ms(80); /* 자이로 스타트업: 최소 45ms + 안전 마진 */
|
||||
dr_sd_delay_ms(80);
|
||||
}
|
||||
|
||||
/* ACCEL_DATA_X1(0x0B)부터 12바이트 연속 읽기 (0x0B~0x16) */
|
||||
reg = REG_ACCEL_X1;
|
||||
ret = icm42670_twi_tx(IMU_I2C_ADDR, ®, 1, true); /* 레지스터 주소 전송 (STOP 없음) */
|
||||
|
||||
if (ret)
|
||||
{
|
||||
DBG_PRINTF("[IMU] tx FAIL %u\r\n", ret);
|
||||
return -1;
|
||||
}
|
||||
|
||||
ret = icm42670_twi_rx(IMU_I2C_ADDR, raw, 12); /* 12바이트 데이터 수신 */
|
||||
|
||||
if (ret)
|
||||
{
|
||||
DBG_PRINTF("[IMU] rx FAIL %u\r\n", ret);
|
||||
return -2;
|
||||
}
|
||||
|
||||
/*
|
||||
* 빅엔디안 레지스터 레이아웃을 int16_t로 변환
|
||||
* raw[0..5] = 가속도 X,Y,Z (각 2바이트, MSB first)
|
||||
* raw[6..11] = 자이로 X,Y,Z (각 2바이트, MSB first)
|
||||
*/
|
||||
accel[0] = (int16_t)((raw[0] << 8) | raw[1]);
|
||||
accel[1] = (int16_t)((raw[2] << 8) | raw[3]);
|
||||
accel[2] = (int16_t)((raw[4] << 8) | raw[5]);
|
||||
gyro[0] = (int16_t)((raw[6] << 8) | raw[7]);
|
||||
gyro[1] = (int16_t)((raw[8] << 8) | raw[9]);
|
||||
gyro[2] = (int16_t)((raw[10] << 8) | raw[11]);
|
||||
|
||||
/* 마운팅 매트릭스 적용 — 보드 장착 방향 보정 */
|
||||
apply_mounting_matrix(icm_mounting_matrix, accel);
|
||||
apply_mounting_matrix(icm_mounting_matrix, gyro);
|
||||
|
||||
/* 데이터 패킹 */
|
||||
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];
|
||||
|
||||
if (info4 == true)
|
||||
{
|
||||
/* info4 모드: 전역 배열에 저장 (mbb?에서 rbb: 패킷으로 일괄 전송) */
|
||||
info_imu[0] = ssp_data[0];
|
||||
info_imu[1] = ssp_data[1];
|
||||
info_imu[2] = ssp_data[2];
|
||||
info_imu[3] = ssp_data[3];
|
||||
info_imu[4] = ssp_data[4];
|
||||
info_imu[5] = ssp_data[5];
|
||||
}
|
||||
else
|
||||
{
|
||||
/* 일반 모드: "rsp:" 태그로 BLE 즉시 전송 */
|
||||
format_data(imu_bin_buffer, "rsp:", ssp_data, 12);
|
||||
dr_binary_tx_safe(imu_bin_buffer, 8);
|
||||
DBG_PRINTF("0");
|
||||
}
|
||||
|
||||
/* IMU 슬립 모드: PWR_MGMT0 = 0x00 → 가속도/자이로 모두 OFF (전력 절감) */
|
||||
{
|
||||
uint8_t pwr_off[2] = { 0x1F, 0x00 }; /* reg=PWR_MGMT0, val=0x00 */
|
||||
icm42670_twi_tx(IMU_I2C_ADDR, pwr_off, 2, false);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,101 @@
|
||||
/*******************************************************************************
|
||||
* @file app_raw.h
|
||||
* @author CandyPops Co.
|
||||
* @version V1.0.0
|
||||
* @date 2022-09-05
|
||||
* @brief
|
||||
******************************************************************************/
|
||||
|
||||
/*******************************************************************************
|
||||
* [헤더 개요] ICM42670P IMU 드라이버 상위 레이어 선언
|
||||
*
|
||||
* IMU 센서의 초기화, 설정, 데이터 읽기를 위한 함수 프로토타입과
|
||||
* 동작 모드 설정 매크로를 정의한다.
|
||||
*
|
||||
* 주요 설정 매크로:
|
||||
* SERIF_TYPE - 통신 인터페이스 (UI_I2C)
|
||||
* USE_LOW_NOISE_MODE - 1:저잡음(800Hz), 0:저전력(100Hz)
|
||||
* USE_HIGH_RES_MODE - 1:20비트 고해상도, 0:16비트 표준
|
||||
* USE_FIFO - 1:FIFO 사용, 0:레지스터 직접 읽기
|
||||
******************************************************************************/
|
||||
|
||||
#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"
|
||||
|
||||
|
||||
/*** 설정 매크로 ***/
|
||||
|
||||
/*
|
||||
* MCU와 IMU 간 통신 인터페이스 선택
|
||||
* UI_I2C: I2C 통신 사용 (기본)
|
||||
*/
|
||||
#define SERIF_TYPE UI_I2C
|
||||
|
||||
/*
|
||||
* 전원 모드 설정
|
||||
* 1: 저잡음 모드 — 800Hz ODR, 높은 정밀도, 높은 전력 소모
|
||||
* 0: 저전력 모드 — 100Hz ODR, 낮은 전력 소모
|
||||
* 주의: 12.5Hz 미만 ODR에서는 저잡음 모드 사용 불가
|
||||
*/
|
||||
#define USE_LOW_NOISE_MODE 1
|
||||
|
||||
/*
|
||||
* FIFO 해상도 모드 선택
|
||||
* 0: 저해상도 — 16비트 데이터 (기본)
|
||||
* 1: 고해상도 — 20비트 데이터 (FSR이 16g/2000dps로 강제 고정됨)
|
||||
*/
|
||||
#define USE_HIGH_RES_MODE 0
|
||||
|
||||
/*
|
||||
* 데이터 읽기 방식 선택
|
||||
* 0: 레지스터 직접 읽기 (현재 사용 중)
|
||||
* 1: FIFO에서 읽기
|
||||
*/
|
||||
#define USE_FIFO 0
|
||||
|
||||
|
||||
/**
|
||||
* \brief IMU 디바이스를 리셋하고 초기화한다. WHOAMI 확인 포함.
|
||||
* 다른 IMU 접근 함수 호출 전에 반드시 성공적으로 실행되어야 한다.
|
||||
*
|
||||
* \return 0=성공, 음수=에러
|
||||
*/
|
||||
int setup_imu_device(struct inv_imu_serif *icm_serif);
|
||||
|
||||
/**
|
||||
* \brief 자이로 및 가속도계 출력을 위한 디바이스 설정을 수행한다.
|
||||
* FSR, ODR, 전원 모드, FIFO 설정 등을 적용한다.
|
||||
* \return 0=성공, 음수=에러
|
||||
*/
|
||||
int configure_imu_device(void);
|
||||
|
||||
/**
|
||||
* \brief FIFO 또는 레지스터에서 IMU 데이터를 추출한다.
|
||||
* 내부적으로 imu_callback()이 호출되어 데이터를 처리한다.
|
||||
* \return 0=성공, 음수=에러
|
||||
*/
|
||||
int get_imu_data(void);
|
||||
|
||||
/**
|
||||
* \brief 센서 데이터 수신 콜백. 마운팅 매트릭스 적용 후
|
||||
* info4/BLE/UART 모드에 따라 데이터를 출력한다.
|
||||
* \param[in] event 하나의 센서 데이터 패킷을 담은 구조체
|
||||
*/
|
||||
void imu_callback(inv_imu_sensor_event_t *event);
|
||||
|
||||
/**
|
||||
* \brief 드라이버 API를 우회한 직접 I2C 레지스터 읽기.
|
||||
* DRDY 인터럽트 없이 즉시 센서 데이터를 읽어 BLE로 전송한다.
|
||||
* 읽기 후 IMU를 슬립 모드로 전환하여 전력을 절감한다.
|
||||
* \return 0=성공, 음수=에러
|
||||
*/
|
||||
int imu_read_direct(void);
|
||||
|
||||
|
||||
#endif /* !_APP_RAW_H_ */
|
||||
@@ -0,0 +1,290 @@
|
||||
/*******************************************************************************
|
||||
* @file app_raw_main.c
|
||||
* @author CandyPops Co.
|
||||
* @version V1.0.0
|
||||
* @date 2022-09-05
|
||||
* @brief
|
||||
******************************************************************************/
|
||||
|
||||
/*******************************************************************************
|
||||
* 2026.03.26 jhChun
|
||||
* 현재 이 파일은 실제 런타임에 실행되지 않고 있음
|
||||
* 인터럽트 방식 대신 app_raw.c imu_read_direct()에서 직접 레지스터 읽는 방식 사용 중
|
||||
* 추후 필요 여부에 따라 정리 예정
|
||||
******************************************************************************/
|
||||
|
||||
/*******************************************************************************
|
||||
* [모듈 개요] ICM42670P 메인 초기화 및 폴링 루프
|
||||
*
|
||||
* ICM42670P IMU 센서의 전체 초기화 시퀀스와 메인 루프를 담당한다.
|
||||
*
|
||||
* 초기화 흐름 (icm42670_init):
|
||||
* 1) setup_mcu() - I2C 시리얼 인터페이스 구조체 설정 및 TWI 초기화
|
||||
* 2) setup_imu_device() - IMU 드라이버 초기화 + WHOAMI 확인
|
||||
* 3) configure_imu_device() - 센서 파라미터 설정 (FSR, ODR, 전원 모드)
|
||||
* 4) inv_gpio_sensor_irq_init() - INT1(P1.13) GPIO 인터럽트 설정
|
||||
*
|
||||
* 메인 루프 (icm42670_main):
|
||||
* - INT1 인터럽트 발생 시 irq_from_device 플래그가 세팅됨
|
||||
* - 메인 루프에서 플래그를 확인하고, 세팅되어 있으면 센서 데이터를 읽음
|
||||
* - 인터럽트는 하강 에지(HITOLO)에서 발생 (INT1 핀 풀업 설정)
|
||||
*
|
||||
* 보조 함수:
|
||||
* - inv_imu_sleep_us() - nrf_delay_us 래퍼 (IMU 드라이버가 사용)
|
||||
* - inv_imu_get_time_us() - RTC1 카운터로 타임스탬프 제공
|
||||
******************************************************************************/
|
||||
#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 "main.h" /* 2026-03-17: cmd_parse.h 삭제 → main.h */
|
||||
#include "i2c_manager.h"
|
||||
/* --------------------------------------------------------------------------------------
|
||||
* Global variables
|
||||
* -------------------------------------------------------------------------------------- */
|
||||
|
||||
/* --------------------------------------------------------------------------------------
|
||||
* Static variables
|
||||
* -------------------------------------------------------------------------------------- */
|
||||
|
||||
/*
|
||||
* IMU 인터럽트 플래그
|
||||
* INT1 핀의 하강 에지 인터럽트 발생 시 1로 세팅된다.
|
||||
* 메인 루프에서 이 플래그를 확인 후 데이터를 읽고 0으로 클리어한다.
|
||||
* volatile: ISR에서 변경되므로 컴파일러 최적화 방지
|
||||
*/
|
||||
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
|
||||
*
|
||||
*/
|
||||
/*
|
||||
* inv_gpio_sensor_interrupt_handler()
|
||||
* INT1 핀 인터럽트 핸들러 (ISR).
|
||||
* 센서가 새 데이터를 준비했을 때 호출되며, 플래그만 세팅하고 즉시 반환한다.
|
||||
* 실제 데이터 처리는 메인 루프(icm42670_main)에서 수행한다.
|
||||
*/
|
||||
static void inv_gpio_sensor_interrupt_handler(nrfx_gpiote_pin_t pin, nrf_gpiote_polarity_t action)
|
||||
{
|
||||
irq_from_device = 1;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* inv_gpio_sensor_irq_init()
|
||||
* INT1(P1.13) GPIO 인터럽트를 초기화한다.
|
||||
*
|
||||
* 설정:
|
||||
* - 트리거: 하강 에지 (HITOLO) — 센서가 INT를 Low로 끌어내릴 때
|
||||
* - 풀업 저항: 내부 풀업 활성화
|
||||
* - 핸들러: inv_gpio_sensor_interrupt_handler
|
||||
* - GPIOTE 모듈이 미초기화 상태이면 먼저 초기화
|
||||
*/
|
||||
void inv_gpio_sensor_irq_init(void)
|
||||
{
|
||||
ret_code_t err_code;
|
||||
|
||||
/* GPIOTE 모듈 초기화 (이미 초기화되어 있으면 건너뜀) */
|
||||
if (!nrfx_gpiote_is_init())
|
||||
{
|
||||
err_code = nrfx_gpiote_init();
|
||||
APP_ERROR_CHECK(err_code);
|
||||
}
|
||||
|
||||
/* 하강 에지 인터럽트 설정: High→Low 전환 시 트리거, 내부 풀업 사용 */
|
||||
nrfx_gpiote_in_config_t in_config = NRFX_GPIOTE_CONFIG_IN_SENSE_HITOLO(true);
|
||||
in_config.pull = NRF_GPIO_PIN_PULLUP;
|
||||
|
||||
/* INT1 핀에 인터럽트 핸들러 등록 */
|
||||
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);
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* inv_gpio_sensor_irq_uninit()
|
||||
* INT1 GPIO 인터럽트를 비활성화하고 해제한다.
|
||||
* 센서 비활성화 시 또는 재초기화 전에 호출된다.
|
||||
*/
|
||||
void inv_gpio_sensor_irq_uninit(void)
|
||||
{
|
||||
/* 인터럽트 이벤트 비활성화 */
|
||||
nrfx_gpiote_in_event_disable(ICM42670_INT1_PIN);
|
||||
|
||||
/* INT1 핀 인터럽트 설정 해제 */
|
||||
nrfx_gpiote_in_uninit(ICM42670_INT1_PIN);
|
||||
|
||||
/* GPIOTE 모듈 해제 (초기화된 경우에만) */
|
||||
if (nrfx_gpiote_is_init())
|
||||
{
|
||||
nrfx_gpiote_uninit();
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
/* --------------------------------------------------------------------------------------
|
||||
* Main
|
||||
* -------------------------------------------------------------------------------------- */
|
||||
|
||||
/*
|
||||
* icm42670_init()
|
||||
* ICM42670P 전체 초기화 시퀀스를 수행한다.
|
||||
*
|
||||
* 초기화 순서:
|
||||
* 1) setup_mcu() - I2C 인터페이스 구조체 설정 및 TWI 하드웨어 초기화
|
||||
* 2) setup_imu_device() - IMU 드라이버 초기화, WHOAMI(0x67) 확인
|
||||
* 3) configure_imu_device() - FSR, ODR, 전원 모드 설정
|
||||
* 4) inv_gpio_sensor_irq_init() - INT1 인터럽트 활성화 (데이터 준비 알림)
|
||||
*
|
||||
* 반환값: 0=성공, -1=초기화 실패
|
||||
*/
|
||||
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;
|
||||
}
|
||||
|
||||
/* 초기화 성공 후 INT1 인터럽트 활성화 — 이후 데이터 준비 시 ISR이 호출됨 */
|
||||
inv_gpio_sensor_irq_init();
|
||||
|
||||
return rc;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* icm42670_main()
|
||||
* ICM42670P 메인 폴링 루프.
|
||||
* 메인 애플리케이션 루프에서 주기적으로 호출되어야 한다.
|
||||
*
|
||||
* 동작:
|
||||
* 1) I2C 하드웨어가 초기화되었는지 확인 (hw_i2c_init_once)
|
||||
* 2) irq_from_device 플래그 확인 (ISR에서 세팅됨)
|
||||
* 3) 플래그가 세팅되어 있으면 센서 데이터 읽기 (get_imu_data)
|
||||
* 4) 데이터 읽기 완료 후 플래그 클리어
|
||||
*
|
||||
* 참고: 인터럽트 기반 폴링 방식으로, ISR에서는 플래그만 세팅하고 실제 I2C 통신은 메인 컨텍스트에서 수행한다.
|
||||
*/
|
||||
void icm42670_main(void)
|
||||
{
|
||||
int rc = 0;
|
||||
hw_i2c_init_once();
|
||||
/* 인터럽트 발생 여부 확인 후 데이터 읽기 */
|
||||
|
||||
if (irq_from_device) {
|
||||
rc = get_imu_data();
|
||||
|
||||
if(rc < 0) {
|
||||
printf("error while getting data\r\n");
|
||||
}
|
||||
|
||||
/* 플래그 클리어 — 다음 인터럽트까지 대기 */
|
||||
irq_from_device = 0;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
/* --------------------------------------------------------------------------------------
|
||||
* Functions definitions
|
||||
* -------------------------------------------------------------------------------------- */
|
||||
|
||||
/*
|
||||
* setup_mcu()
|
||||
* MCU 측 시리얼 인터페이스를 설정한다.
|
||||
*
|
||||
* inv_imu_serif 구조체에 다음을 등록:
|
||||
* - read_reg / write_reg : I2C 읽기/쓰기 콜백 함수 (system_interface.c에서 구현)
|
||||
* - max_read / max_write : 최대 전송 크기 (32KB)
|
||||
* - serif_type : 통신 타입 (UI_I2C)
|
||||
*
|
||||
* 설정 후 inv_io_hal_init()을 호출하여 실제 TWI 하드웨어를 초기화한다.
|
||||
*/
|
||||
static int setup_mcu(struct inv_imu_serif *icm_serif)
|
||||
{
|
||||
int rc = 0;
|
||||
|
||||
/* IMU 드라이버용 시리얼 인터페이스 구조체 설정 */
|
||||
icm_serif->context = 0; /* 컨텍스트 미사용 */
|
||||
icm_serif->read_reg = inv_io_hal_read_reg; /* 레지스터 읽기 콜백 */
|
||||
icm_serif->write_reg = inv_io_hal_write_reg; /* 레지스터 쓰기 콜백 */
|
||||
icm_serif->max_read = 1024*32; /* 1회 읽기 최대 바이트 수 */
|
||||
icm_serif->max_write = 1024*32; /* 1회 쓰기 최대 바이트 수 */
|
||||
icm_serif->serif_type = SERIF_TYPE; /* UI_I2C (app_raw.h에서 정의) */
|
||||
|
||||
/* TWI 하드웨어 초기화 */
|
||||
rc |= inv_io_hal_init(icm_serif);
|
||||
|
||||
return rc;
|
||||
}
|
||||
|
||||
|
||||
/* --------------------------------------------------------------------------------------
|
||||
* Extern functions definition
|
||||
* -------------------------------------------------------------------------------------- */
|
||||
|
||||
/*
|
||||
* inv_imu_sleep_us()
|
||||
* IMU 드라이버가 사용하는 마이크로초 단위 슬립 함수.
|
||||
* nrf_delay_us()를 래핑하여 플랫폼 독립적 인터페이스를 제공한다.
|
||||
* 예: 자이로 스타트업 대기(GYR_STARTUP_TIME_US) 시 사용
|
||||
*/
|
||||
void inv_imu_sleep_us(uint32_t us)
|
||||
{
|
||||
nrf_delay_us(us);
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* inv_imu_get_time_us()
|
||||
* IMU 드라이버가 사용하는 타임스탬프 함수.
|
||||
* nRF52840의 RTC1 카운터 값을 반환한다.
|
||||
*
|
||||
* 주의: RTC1은 32.768kHz로 동작하므로, 반환값의 단위는 엄밀히
|
||||
* 마이크로초가 아닌 RTC 틱(약 30.5us/tick)이다.
|
||||
* 드라이버 내부에서 상대적 시간 비교 용도로 사용된다.
|
||||
*/
|
||||
uint64_t inv_imu_get_time_us(void)
|
||||
{
|
||||
return NRF_RTC1->COUNTER;
|
||||
}
|
||||
@@ -0,0 +1,31 @@
|
||||
/*******************************************************************************
|
||||
* @file app_raw_main.h
|
||||
* @author CandyPops Co.
|
||||
* @version V1.0.0
|
||||
* @date 2022-09-05
|
||||
* @brief
|
||||
******************************************************************************/
|
||||
|
||||
/*******************************************************************************
|
||||
* [헤더 개요] ICM42670P 메인 초기화/폴링 루프 선언
|
||||
*
|
||||
* ICM42670P IMU 센서의 전체 초기화 및 메인 루프 함수를 선언한다.
|
||||
* - icm42670_init() : 전체 초기화 (MCU설정 → IMU초기화 → 센서설정 → 인터럽트 활성화)
|
||||
* - icm42670_main() : 메인 폴링 루프 (INT1 인터럽트 확인 → 데이터 읽기)
|
||||
* - icm42670_uninit() : 해제 (프로토타입만 선언, 구현은 별도)
|
||||
******************************************************************************/
|
||||
|
||||
#ifndef _APP_RAW_MAIN_H_
|
||||
#define _APP_RAW_MAIN_H_
|
||||
#include "sdk_config.h"
|
||||
|
||||
/* ICM42670P 전체 초기화 — MCU I2C 설정 → IMU 드라이버 초기화 → 센서 설정 → 인터럽트 활성화 */
|
||||
int icm42670_init(void);
|
||||
|
||||
/* ICM42670P 메인 폴링 루프 — INT1 인터럽트 플래그 확인 후 센서 데이터 읽기 */
|
||||
void icm42670_main(void);
|
||||
|
||||
/* ICM42670P 해제 (프로토타입 선언) */
|
||||
int icm42670_uninit(void);
|
||||
|
||||
#endif /* !_APP_RAW_MAIN_H_ */
|
||||
@@ -0,0 +1,334 @@
|
||||
/*******************************************************************************
|
||||
* @file system_interface.c
|
||||
* @author CandyPops Co.
|
||||
* @version V1.0.0
|
||||
* @date 2022-09-05
|
||||
* @brief
|
||||
******************************************************************************/
|
||||
|
||||
/*******************************************************************************
|
||||
* [모듈 개요] ICM42670P IMU 센서 I2C 통신 인터페이스
|
||||
*
|
||||
* nRF52840의 TWI(I2C) 하드웨어를 사용하여 ICM42670P IMU 센서와 통신하는
|
||||
* 저수준 인터페이스 모듈이다.
|
||||
*
|
||||
* - I2C 슬레이브 주소: 0x68 (ICM42670P 기본 주소)
|
||||
* - I2C 핀 설정: SCL=P1.14, SDA=P1.15 (system_interface.h에서 정의)
|
||||
* - TWI 인스턴스: NRFX_TWI_INSTANCE(0) 사용
|
||||
* - 통신 속도: 100kHz (NRF_TWI_FREQ_100K)
|
||||
*
|
||||
* 주요 함수 흐름:
|
||||
* inv_io_hal_init() → I2C 또는 SPI 초기화 (현재 I2C만 구현)
|
||||
* inv_io_hal_read_reg() → 레지스터 읽기 (TX로 주소 전송 → RX로 데이터 수신)
|
||||
* inv_io_hal_write_reg() → 레지스터 쓰기 (주소+데이터를 한번에 TX)
|
||||
*
|
||||
* 에러 처리: 모든 I2C 읽기/쓰기에서 실패 시 1회 재시도 후 에러 출력
|
||||
*
|
||||
* 참고: SPI4 인터페이스 코드 경로도 존재하지만 현재 미구현 상태이며,
|
||||
* 실제 운용에서는 I2C(UI_I2C)만 사용한다.
|
||||
******************************************************************************/
|
||||
|
||||
/* board driver */
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <stdarg.h>
|
||||
|
||||
#include <stdbool.h>
|
||||
#include "nrf.h"
|
||||
#include "app_error.h"
|
||||
#include "boards.h"
|
||||
#include "nrfx_gpiote.h"
|
||||
#include "nrfx_twi.h"
|
||||
|
||||
#include "system_interface.h"
|
||||
#include "nrf_delay.h"
|
||||
|
||||
/* ICM42670P I2C 슬레이브 주소 및 직렬 쓰기 최대 바이트 수 */
|
||||
#define ICM_I2C_ADDR 0x68
|
||||
#define INV_MAX_SERIAL_WRITE 16
|
||||
|
||||
/* TWI(I2C) 인스턴스 생성 — system_interface.h의 ICM42670_I2C_INSTANCE(0)을 사용 */
|
||||
const nrfx_twi_t m_twi_icm42670 = NRFX_TWI_INSTANCE(ICM42670_I2C_INSTANCE);
|
||||
|
||||
/*
|
||||
* inv_i2c_master_uninitialize()
|
||||
* I2C 버스를 비활성화하고 TWI 인스턴스를 해제한다.
|
||||
* 슬립 모드 진입 전 또는 재초기화 전에 호출된다.
|
||||
*/
|
||||
void inv_i2c_master_uninitialize(void){
|
||||
nrfx_twi_disable(&m_twi_icm42670);
|
||||
nrfx_twi_uninit(&m_twi_icm42670);
|
||||
}
|
||||
|
||||
/*
|
||||
* inv_i2c_master_initialize()
|
||||
* nRF52840 TWI 하드웨어를 초기화하고 활성화한다.
|
||||
* - SCL: P1.14, SDA: P1.15
|
||||
* - 속도: 100kHz
|
||||
* - 인터럽트 우선순위: 최고 (APP_IRQ_PRIORITY_HIGH)
|
||||
* - 이벤트 핸들러 없음 (블로킹 모드로 동작)
|
||||
*/
|
||||
void inv_i2c_master_initialize(void){
|
||||
ret_code_t err_code;
|
||||
|
||||
const nrfx_twi_config_t twi_icm42670_config = {
|
||||
.scl = ICM42670_I2C_SCL_PIN,
|
||||
.sda = ICM42670_I2C_SDA_PIN,
|
||||
.frequency = NRF_TWI_FREQ_100K,
|
||||
.interrupt_priority = APP_IRQ_PRIORITY_HIGH,
|
||||
};
|
||||
|
||||
/* TWI 드라이버 초기화 (이벤트 핸들러=NULL → 블로킹 모드) */
|
||||
err_code = nrfx_twi_init(&m_twi_icm42670, &twi_icm42670_config, NULL, NULL);
|
||||
APP_ERROR_CHECK(err_code);
|
||||
|
||||
/* TWI 하드웨어 활성화 — 이후 tx/rx 가능 */
|
||||
nrfx_twi_enable(&m_twi_icm42670);
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* icm42670_twi_tx()
|
||||
* I2C 전송 래퍼 함수. nrfx_twi_tx를 호출하여 데이터를 송신한다.
|
||||
* no_stop=true이면 STOP 컨디션을 보내지 않음 (Repeated START를 위해 사용)
|
||||
*/
|
||||
uint32_t icm42670_twi_tx( uint8_t device_id,
|
||||
uint8_t const * p_data,
|
||||
uint8_t length,
|
||||
bool no_stop)
|
||||
{
|
||||
ret_code_t ret;
|
||||
ret = nrfx_twi_tx(&m_twi_icm42670, device_id, p_data, length, no_stop);
|
||||
return ret;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* icm42670_twi_rx()
|
||||
* I2C 수신 래퍼 함수. nrfx_twi_rx를 호출하여 데이터를 수신한다.
|
||||
*/
|
||||
uint32_t icm42670_twi_rx( uint8_t device_id,
|
||||
uint8_t * p_data,
|
||||
uint8_t length)
|
||||
{
|
||||
ret_code_t ret;
|
||||
ret = nrfx_twi_rx(&m_twi_icm42670, device_id, p_data, length);
|
||||
return ret;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* inv_i2c_master_read_register()
|
||||
* ICM42670P의 특정 레지스터에서 데이터를 읽는다.
|
||||
*
|
||||
* 동작 순서:
|
||||
* 1) TX: 레지스터 주소 1바이트 전송 (no_stop=true → Repeated START 준비)
|
||||
* 2) RX: 지정된 길이만큼 데이터 수신
|
||||
*
|
||||
* 에러 처리: TX, RX 각각 실패 시 1회 재시도한다.
|
||||
*/
|
||||
static unsigned long inv_i2c_master_read_register(unsigned char Address, unsigned char RegisterAddr, unsigned short RegisterLen, unsigned char *RegisterValue){
|
||||
//ret_code_t ret;
|
||||
uint32_t ret;
|
||||
uint8_t addr8 = (uint8_t)RegisterAddr;
|
||||
|
||||
/* 1단계: 읽을 레지스터 주소를 전송 (STOP 없이 → Repeated START 사용) */
|
||||
ret = icm42670_twi_tx(Address, &addr8, 1, true);
|
||||
if(ret != NRF_SUCCESS) {
|
||||
/* 실패 시 1회 재시도 */
|
||||
ret = icm42670_twi_tx(Address, &addr8, 1, true);
|
||||
if(ret != NRF_SUCCESS) {
|
||||
printf("ERR! i2c read-1\r\n");
|
||||
}
|
||||
}
|
||||
|
||||
/* 2단계: 해당 레지스터에서 데이터 수신 */
|
||||
ret = icm42670_twi_rx(Address, RegisterValue, RegisterLen);
|
||||
if(ret != NRF_SUCCESS) {
|
||||
/* 실패 시 1회 재시도 */
|
||||
ret = icm42670_twi_rx(Address, RegisterValue, RegisterLen);
|
||||
if(ret != NRF_SUCCESS) {
|
||||
printf("ERR! i2c read-2\r\n");
|
||||
}
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
/*
|
||||
* inv_i2c_master_write_register()
|
||||
* ICM42670P의 특정 레지스터에 데이터를 쓴다.
|
||||
*
|
||||
* 동작 순서:
|
||||
* 1) 버퍼[0]에 레지스터 주소, 버퍼[1~N]에 쓸 데이터를 배치
|
||||
* 2) TX: 주소+데이터를 한번에 전송 (no_stop=false → STOP 컨디션 포함)
|
||||
*
|
||||
* 에러 처리: 실패 시 1회 재시도한다.
|
||||
*/
|
||||
static unsigned long inv_i2c_master_write_register(unsigned char Address, unsigned char RegisterAddr, unsigned short RegisterLen, const unsigned char *RegisterValue){
|
||||
uint32_t ret;
|
||||
uint8_t buffer[1 + INV_MAX_SERIAL_WRITE]; /* 레지스터 주소(1) + 데이터(최대 16바이트) */
|
||||
|
||||
/* 버퍼 구성: [레지스터 주소][데이터 바이트들] */
|
||||
buffer[0] = (uint8_t)RegisterAddr;
|
||||
memcpy(buffer+1, RegisterValue, RegisterLen);
|
||||
|
||||
/* 주소+데이터를 한번에 전송 */
|
||||
ret = icm42670_twi_tx(Address, buffer, RegisterLen+1, false);
|
||||
if(ret != NRF_SUCCESS) {
|
||||
/* 실패 시 1회 재시도 */
|
||||
ret = icm42670_twi_tx(Address, buffer, RegisterLen+1, false);
|
||||
if(ret != NRF_SUCCESS) {
|
||||
printf("ERR! i2c write\r\n");
|
||||
}
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
|
||||
|
||||
/*
|
||||
* inv_io_hal_init()
|
||||
* IMU 드라이버가 사용하는 시리얼 인터페이스(I2C 또는 SPI)를 초기화한다.
|
||||
* serif->serif_type에 따라 분기하며, 현재는 I2C만 구현되어 있다.
|
||||
* 반환값: 0=성공, -1=지원하지 않는 인터페이스 타입
|
||||
*/
|
||||
|
||||
int inv_io_hal_init(struct inv_imu_serif *serif)
|
||||
{
|
||||
|
||||
switch (serif->serif_type) {
|
||||
case UI_SPI4:
|
||||
{
|
||||
/* SPI4 초기화 — 현재 미구현 (I2C만 사용) */
|
||||
break;
|
||||
}
|
||||
|
||||
case UI_I2C:
|
||||
inv_i2c_master_initialize();
|
||||
break;
|
||||
|
||||
default:
|
||||
return -1;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* inv_io_hal_read_reg()
|
||||
* IMU 드라이버 콜백: 지정된 레지스터에서 데이터를 읽는다.
|
||||
* 시리얼 타입에 따라 I2C 또는 SPI 읽기를 수행한다.
|
||||
*/
|
||||
int inv_io_hal_read_reg(struct inv_imu_serif *serif, uint8_t reg, uint8_t * rbuffer, uint32_t rlen)
|
||||
{
|
||||
switch (serif->serif_type) {
|
||||
case UI_SPI4:
|
||||
return 0;
|
||||
|
||||
case UI_I2C:
|
||||
return inv_i2c_master_read_register(ICM_I2C_ADDR, reg, rlen, rbuffer);
|
||||
|
||||
default:
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* inv_io_hal_write_reg()
|
||||
* IMU 드라이버 콜백: 지정된 레지스터에 데이터를 쓴다.
|
||||
* 시리얼 타입에 따라 I2C 또는 SPI 쓰기를 수행한다.
|
||||
*/
|
||||
int inv_io_hal_write_reg(struct inv_imu_serif *serif, uint8_t reg, const uint8_t * wbuffer, uint32_t wlen)
|
||||
{
|
||||
switch (serif->serif_type) {
|
||||
case UI_SPI4:
|
||||
return 0;
|
||||
|
||||
case UI_I2C:
|
||||
return inv_i2c_master_write_register(ICM_I2C_ADDR, reg, wlen, wbuffer);
|
||||
|
||||
default:
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* cat_read()
|
||||
* 범용 I2C 읽기 함수 (디버그/레거시용).
|
||||
* 8바이트를 읽어 첫 번째 바이트를 반환하고, 읽은 데이터를 콘솔에 출력한다.
|
||||
* 참고: 현재 실제 운용에서는 사용되지 않으며, 디버그 목적으로 남아 있다.
|
||||
*/
|
||||
uint8_t cat_read(uint8_t device_id, uint8_t address, uint8_t *data)
|
||||
{
|
||||
|
||||
uint8_t read_data = 0;
|
||||
char adata[8];
|
||||
ret_code_t err_code;
|
||||
//address = 1|(address<<1);
|
||||
address = (address & 0xFF);
|
||||
|
||||
/* 레지스터 주소 전송 (STOP 없이, Repeated START 준비) */
|
||||
err_code = nrfx_twi_tx(&m_twi_icm42670, device_id, &address, 1, true);
|
||||
if (err_code != NRF_SUCCESS) {
|
||||
// Handle error
|
||||
// return;
|
||||
}
|
||||
|
||||
/* 8바이트 데이터 수신 */
|
||||
err_code = nrfx_twi_rx(&m_twi_icm42670, device_id, data, 8);
|
||||
if (err_code != NRF_SUCCESS) {
|
||||
// Handle error
|
||||
return 0;
|
||||
}
|
||||
read_data = data[0];
|
||||
memcpy(adata,data,8);
|
||||
printf("Data %s . \r\n", adata);
|
||||
return read_data;
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
/*
|
||||
* cat_write()
|
||||
* 범용 I2C 쓰기 함수 (디버그/레거시용).
|
||||
* 주소 1바이트 + 데이터 1바이트를 전송한다.
|
||||
* 참고: buffer에 6바이트를 복사하지만, 실제 전송은 2바이트만 수행한다.
|
||||
*/
|
||||
void cat_write(uint8_t device_id, uint8_t address, uint8_t *data){
|
||||
|
||||
uint8_t buffer[7]={0x00,0x00,0x00,0x00,0x00,0x00,0x00};
|
||||
|
||||
address = (address & 0xFF);
|
||||
|
||||
buffer[0] = (address);
|
||||
//buffer[1] =(data & 0xFF);
|
||||
memcpy(buffer+1,data,6);
|
||||
ret_code_t err_code;
|
||||
//err_code = nrf_drv_twi_tx(&m_twi_ir, device_id, 0x00, 1, false);
|
||||
|
||||
/* 주소(1바이트) + 데이터(1바이트) = 2바이트 전송 */
|
||||
err_code = nrfx_twi_tx(&m_twi_icm42670, device_id, buffer, 2, false);
|
||||
// err_code = nrf_drv_twi_tx(&m_twi_ir, device_id, buffer, 2, false);
|
||||
// nrfx_twi_rx(&m_twi_icm42670, device_id, p_data, length);
|
||||
// nrfx_twi_tx(&m_twi_icm42670, device_id, p_data, length, no_stop);
|
||||
printf("Data %x %x %x %x. \r\n", buffer[0], buffer[1], buffer[2], buffer[3]);
|
||||
|
||||
//err_code = nrf_drv_twi_tx(&m_twi_ir, device_id, buffer, 6, false);
|
||||
if (err_code != NRF_SUCCESS) {
|
||||
|
||||
printf("TWI Error.");
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@@ -0,0 +1,72 @@
|
||||
/*******************************************************************************
|
||||
* @file system_interface.h
|
||||
* @author CandyPops Co.
|
||||
* @version V1.0.0
|
||||
* @date 2022-09-05
|
||||
* @brief
|
||||
******************************************************************************/
|
||||
|
||||
/*******************************************************************************
|
||||
* [헤더 개요] ICM42670P I2C 통신 인터페이스 선언
|
||||
*
|
||||
* nRF52840 TWI 하드웨어를 통해 ICM42670P IMU 센서와 통신하기 위한 핀 정의, 함수 프로토타입 선언
|
||||
*
|
||||
* 핀 배치:
|
||||
* - I2C SCL : P1.14
|
||||
* - I2C SDA : P1.15
|
||||
* - INT1 : P1.13 (데이터 준비 인터럽트)
|
||||
* - INT2 : P0.26 (보조 인터럽트, 현재 미사용)
|
||||
*
|
||||
* TWI 인스턴스: 0번 사용
|
||||
******************************************************************************/
|
||||
|
||||
#ifndef _SYSTEM_INTERFACE_H_
|
||||
#define _SYSTEM_INTERFACE_H_
|
||||
|
||||
#include "inv_imu_transport.h"
|
||||
#include <stdbool.h>
|
||||
/* TODO: Move that somewhere else */
|
||||
#ifndef TO_MASK
|
||||
#define TO_MASK(a) (1U << (unsigned)(a))
|
||||
#endif
|
||||
|
||||
|
||||
#define ICM42670_I2C_INSTANCE 0 /**< I2C(TWI) 인스턴스 인덱스 */
|
||||
#define ICM42670_I2C_SDA_PIN NRF_GPIO_PIN_MAP(1,15) /**< SDA 핀: P1.15 */
|
||||
#define ICM42670_I2C_SCL_PIN NRF_GPIO_PIN_MAP(1,14) /**< SCL 핀: P1.14 */
|
||||
#define ICM42670_INT1_PIN NRF_GPIO_PIN_MAP(1,13) /**< INT1 핀: P1.13 (데이터 준비 인터럽트) */
|
||||
#define ICM42670_INT2_PIN NRF_GPIO_PIN_MAP(0,26) /**< INT2 핀: P0.26 (보조, 현재 미사용) */
|
||||
|
||||
/* I2C 전송 래퍼 — no_stop=true이면 Repeated START를 위해 STOP 컨디션 생략 */
|
||||
uint32_t icm42670_twi_tx( uint8_t device_id,
|
||||
uint8_t const * p_data,
|
||||
uint8_t length,
|
||||
bool no_stop);
|
||||
|
||||
/* I2C 수신 래퍼 */
|
||||
uint32_t icm42670_twi_rx( uint8_t device_id,
|
||||
uint8_t * p_data,
|
||||
uint8_t length);
|
||||
|
||||
/* 범용 I2C 읽기 (디버그/레거시용) — 8바이트를 읽어 첫 바이트 반환 */
|
||||
uint8_t cat_read (uint8_t device_id, uint8_t address, uint8_t *data);
|
||||
|
||||
/* 범용 I2C 쓰기 (디버그/레거시용) — 주소+데이터 2바이트 전송 */
|
||||
void cat_write (uint8_t device_id, uint8_t address, uint8_t *data);
|
||||
|
||||
/* I2C 하드웨어 해제 (슬립 또는 재초기화 전 호출) */
|
||||
void inv_i2c_master_uninitialize(void);
|
||||
|
||||
/* I2C 하드웨어 초기화 (100kHz, 블로킹 모드) */
|
||||
void inv_i2c_master_initialize(void);
|
||||
|
||||
/* IMU 드라이버용 시리얼 인터페이스 초기화 (I2C/SPI 분기) */
|
||||
int inv_io_hal_init(struct inv_imu_serif *serif);
|
||||
|
||||
/* IMU 드라이버 콜백: 레지스터 읽기 */
|
||||
int inv_io_hal_read_reg(struct inv_imu_serif *serif, uint8_t reg, uint8_t * rbuffer, uint32_t rlen);
|
||||
|
||||
/* IMU 드라이버 콜백: 레지스터 쓰기 */
|
||||
int inv_io_hal_write_reg(struct inv_imu_serif *serif, uint8_t reg, const uint8_t * wbuffer, uint32_t wlen);
|
||||
|
||||
#endif /* !_SYSTEM_INTERFACE_H_ */
|
||||
@@ -0,0 +1,217 @@
|
||||
/*******************************************************************************
|
||||
* @file tmp235_q1.c
|
||||
* @author CandyPops Co.
|
||||
* @version V1.0.0
|
||||
* @date 2022-09-05
|
||||
* @brief
|
||||
******************************************************************************/
|
||||
|
||||
/*******************************************************************************
|
||||
* [모듈 개요] TMP235-Q1 아날로그 온도센서 드라이버
|
||||
*
|
||||
* TMP235-Q1은 온도에 비례하는 아날로그 전압(Vout)을 출력하는 센서
|
||||
* nRF52840 SAADC의 AIN3 채널로 Vout을 읽고, mV로 변환한 뒤 온도(°C)로 계산
|
||||
*
|
||||
* 온도 계산 공식 (구간별 선형 보간):
|
||||
* - Vout <= 1500mV (0~100°C): Ta = (Vout - 500) / 10.0
|
||||
* - Vout <= 1750mV (100~125°C): Ta = (Vout - 1500) / 10.1 + 100
|
||||
* - Vout <= 2000mV (125~150°C): Ta = (Vout - 1752.5) / 10.6 + 125
|
||||
* - Vout > 2000mV: 오류 (150°C 초과)
|
||||
*
|
||||
* info4 모드(전체 센서 수집) 동작 순서:
|
||||
* 배터리(go_batt) → 온도(go_temp) → IMU(motion_raw_data_enabled)
|
||||
* 온도 측정 완료 시 go_temp=false, motion_raw_data_enabled=true 로 전환
|
||||
******************************************************************************/
|
||||
|
||||
#include "sdk_common.h"
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include "nrf.h"
|
||||
#include "boards.h"
|
||||
#include "app_error.h"
|
||||
#include "nrf_drv_saadc.h"
|
||||
#include "ble_nus.h"
|
||||
#include "tmp235_q1.h"
|
||||
#include "main.h"
|
||||
/* 2026-03-17: cmd_parse.h 삭제 — main.h는 이미 포함됨 */
|
||||
#include "main_timer.h"
|
||||
#include "debug_print.h"
|
||||
/* SAADC 내부 기준전압 600mV (부동소수점) */
|
||||
#define TMP235_REF_VOLTAGE_IN_MILLIVOLTS 600.0f /**< Reference voltage (in milli volts) used by ADC while doing conversion. */
|
||||
/* 1/3 프리스케일링 보상 계수 x6 (부동소수점) */
|
||||
#define TMP235_PRE_SCALING_COMPENSATION 6.0f /**< The ADC is configured to use VDD with 1/3 prescaling as input. And hence the result of conversion is to be multiplied by 3 to get the actual value of the battery voltage.*/
|
||||
/* 12비트 ADC 최대값 4096 (부동소수점, 분해능 기준) */
|
||||
#define TMP235_ADC_RES_12BITS 4096.0f /**< Maximum digital value for 12-bit ADC conversion. */
|
||||
|
||||
/**@brief Macro to convert the result of ADC conversion in millivolts.
|
||||
*
|
||||
* @param[in] ADC_VALUE ADC result.
|
||||
*
|
||||
* @retval Result converted to millivolts.
|
||||
*/
|
||||
/* ADC 원시값 → TMP235 출력전압(mV) 변환 매크로: ADC x (600/4096) x 6 */
|
||||
#define TMP235_VOUT_IN_MILLI_VOLTS(ADC_VALUE)\
|
||||
((((ADC_VALUE) * TMP235_REF_VOLTAGE_IN_MILLIVOLTS) / TMP235_ADC_RES_12BITS) * TMP235_PRE_SCALING_COMPENSATION)
|
||||
|
||||
/* SAADC 변환 결과 저장 버퍼 (1채널) */
|
||||
static nrf_saadc_value_t adc_buf;
|
||||
extern char ble_tx_buffer[BLE_NUS_MAX_DATA_LEN];
|
||||
extern uint8_t ble_bin_buffer[BLE_NUS_MAX_DATA_LEN] ;
|
||||
|
||||
/* 현재 명령 소스: CMD_UART 또는 CMD_BLE */
|
||||
extern which_cmd_t cmd_type_t;
|
||||
|
||||
/* info4: 전체 센서 데이터 수집 모드 플래그 */
|
||||
extern bool info4; // main.c
|
||||
|
||||
/* 온도 측정 순서 제어 플래그 */
|
||||
extern bool go_temp; // main_timer.c
|
||||
|
||||
/* info4 모드에서 온도값 임시 저장 (°C x 100, 정수 표현) */
|
||||
volatile uint16_t info_temp; //48_C
|
||||
extern bool motion_raw_data_enabled;
|
||||
|
||||
/* SAADC 완료 플래그 — all_sensors()에서 콜백 완료 대기용 */
|
||||
volatile bool tmp235_saadc_done = false;
|
||||
|
||||
/**@brief Function for handling the ADC interrupt.
|
||||
*
|
||||
* @details This function will fetch the conversion result from the ADC, convert the value into
|
||||
* percentage and send it to peer.
|
||||
*/
|
||||
|
||||
/**
|
||||
* @brief TMP235 온도센서 ADC 완료 콜백
|
||||
*
|
||||
* SAADC 변환 완료 시 호출된다.
|
||||
* ADC값 → Vout(mV) → 온도(°C) 순으로 변환하고, 동작 모드에 따라:
|
||||
* - info4 모드: info_temp에 저장 (°C x 100 정수), 이후 IMU 측정으로 전환
|
||||
* - 일반 모드: BLE("rso:" 바이너리) 또는 UART로 온도값 전송
|
||||
*/
|
||||
void tmp235_voltage_handler(nrf_drv_saadc_evt_t const * p_event) /* TMP325 Vout reading */
|
||||
{
|
||||
float led_temp; /* 계산된 온도 (°C, 부동소수점) */
|
||||
float led_temp_16; /* BLE 전송용 온도 (°C x 100, 부동소수점) */
|
||||
|
||||
if (p_event->type == NRF_DRV_SAADC_EVT_DONE)
|
||||
{
|
||||
nrf_saadc_value_t adc_result;
|
||||
float tmp235_voltage_in_milli_volts = 0;
|
||||
|
||||
/* ADC 변환 결과 읽기 */
|
||||
adc_result = p_event->data.done.p_buffer[0];
|
||||
//DBG_PRINTF("[TMP] adc=%d\r\n", adc_result);
|
||||
|
||||
/* SAADC 해제 — 배터리/압력센서 측정과 하드웨어 공유 */
|
||||
nrf_drv_saadc_channel_uninit(0); // 채널 먼저 해제
|
||||
nrf_drv_saadc_uninit();
|
||||
//nrf_drv_saadc_uninit(); // 이전: 드라이버 먼저 해제
|
||||
//nrf_drv_saadc_channel_uninit(0);
|
||||
|
||||
/* ADC값 → TMP235 출력전압(mV) 변환 */
|
||||
tmp235_voltage_in_milli_volts = TMP235_VOUT_IN_MILLI_VOLTS(adc_result);
|
||||
|
||||
/*
|
||||
* Vout → 온도(°C) 변환 (구간별 선형 보간)
|
||||
* TMP235 데이터시트 기반:
|
||||
* 0~100°C 구간: 기울기 10.0 mV/°C, 오프셋 500mV
|
||||
* 100~125°C 구간: 기울기 10.1 mV/°C
|
||||
* 125~150°C 구간: 기울기 10.6 mV/°C
|
||||
*/
|
||||
if(tmp235_voltage_in_milli_volts <= 1500)
|
||||
{
|
||||
/* 0~100°C: Ta = (Vout - 500mV) / 10.0 mV/°C */
|
||||
led_temp = (tmp235_voltage_in_milli_volts - 500.0f) / 10.0f + 0.0f;
|
||||
}
|
||||
else if(tmp235_voltage_in_milli_volts <= 1750)
|
||||
{
|
||||
/* 100~125°C: 기울기가 10.1로 약간 증가 */
|
||||
led_temp = (tmp235_voltage_in_milli_volts - 1500.0f) / 10.1f + 100.0f;
|
||||
}
|
||||
else if(tmp235_voltage_in_milli_volts <= 2000)
|
||||
{
|
||||
/* 125~150°C: 기울기가 10.6으로 더 증가 */
|
||||
led_temp = (tmp235_voltage_in_milli_volts - 1752.5f) / 10.6f + 125.0f;
|
||||
}
|
||||
else
|
||||
{
|
||||
/* 150°C 초과 — 센서 측정 범위 벗어남 */
|
||||
DBG_PRINTF("ERR!!! Temprature is over 150c\r\n");
|
||||
}
|
||||
|
||||
/* info4 모드: 온도값을 정수(°C x 100)로 저장 (예: 36.50°C → 3650) */
|
||||
if (info4 == true)
|
||||
{
|
||||
info_temp = (uint16_t)(led_temp * 100);
|
||||
}
|
||||
/* UART 모드: 소수점 2자리까지 텍스트로 출력 */
|
||||
else if(cmd_type_t == CMD_UART)
|
||||
{
|
||||
DBG_PRINTF("To%.2f\r\n\r\n",led_temp);
|
||||
}
|
||||
/* BLE 모드: °C x 100 정수를 "rso:" 헤더로 바이너리 전송 */
|
||||
else if(cmd_type_t == CMD_BLE)
|
||||
{
|
||||
led_temp_16 = led_temp * 100;
|
||||
single_format_data(ble_bin_buffer, "rso:", (uint16_t)led_temp_16);
|
||||
|
||||
dr_binary_tx_safe(ble_bin_buffer,3);
|
||||
|
||||
// sprintf(ble_tx_buffer, "To%.2f\r\n",led_temp);
|
||||
// data_tx_handler(ble_tx_buffer);
|
||||
}
|
||||
|
||||
DBG_PRINTF("7");
|
||||
tmp235_saadc_done = true;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief TMP235 온도센서 SAADC 초기화 및 측정 시작
|
||||
*
|
||||
* AIN3 채널을 싱글엔드 모드로 설정하고 즉시 샘플링을 트리거한다.
|
||||
* 결과는 tmp235_voltage_handler 콜백에서 비동기로 처리된다.
|
||||
*/
|
||||
void tmp235_init(void)
|
||||
{
|
||||
/* SAADC 드라이버 초기화 (4x 오버샘플링으로 노이즈 저감) */
|
||||
nrf_drv_saadc_config_t saadc_config = NRF_DRV_SAADC_DEFAULT_CONFIG;
|
||||
saadc_config.resolution = NRF_SAADC_RESOLUTION_12BIT;
|
||||
saadc_config.oversample = NRF_SAADC_OVERSAMPLE_4X;
|
||||
ret_code_t err_code = nrf_drv_saadc_init(&saadc_config, tmp235_voltage_handler);
|
||||
APP_ERROR_CHECK(err_code);
|
||||
|
||||
/* AIN3 채널 설정: TMP235-Q1 Vout 핀 (싱글엔드 입력, burst 활성화) */
|
||||
nrf_saadc_channel_config_t config = NRF_DRV_SAADC_DEFAULT_CHANNEL_CONFIG_SE(NRF_SAADC_INPUT_AIN3);
|
||||
config.burst = NRF_SAADC_BURST_ENABLED;
|
||||
err_code = nrf_drv_saadc_channel_init(0, &config);
|
||||
APP_ERROR_CHECK(err_code);
|
||||
|
||||
/* ADC 버퍼 등록 (1채널, 1샘플) */
|
||||
err_code = nrf_drv_saadc_buffer_convert(&adc_buf, 1);
|
||||
APP_ERROR_CHECK(err_code);
|
||||
|
||||
/* 즉시 ADC 샘플링 시작 (비동기) */
|
||||
err_code = nrf_drv_saadc_sample();
|
||||
APP_ERROR_CHECK(err_code);
|
||||
}
|
||||
|
||||
|
||||
/* Ta = (Vout – Voffs ) / Tc + Tinfl */
|
||||
/**
|
||||
* @brief 온도 측정 외부 호출 함수
|
||||
*
|
||||
* tmp235_init()을 호출하여 SAADC 초기화 + 측정을 일괄 수행한다.
|
||||
* 내부적으로 init 시 바로 샘플링이 시작되므로 별도 sample 호출 불필요.
|
||||
*/
|
||||
void tmp235_voltage_level_meas(void)
|
||||
{
|
||||
tmp235_init(); // init 함수에 있는 걸 그냥 여기 넣어도
|
||||
//tmp235_uninit();
|
||||
}
|
||||
|
||||
@@ -0,0 +1,31 @@
|
||||
/*******************************************************************************
|
||||
* @file tmp235_q1.h
|
||||
* @author CandyPops Co.
|
||||
* @version V1.0.0
|
||||
* @date 2022-09-05
|
||||
* @brief
|
||||
******************************************************************************/
|
||||
|
||||
/*******************************************************************************
|
||||
* [헤더 개요] TMP235-Q1 아날로그 온도센서 드라이버 인터페이스
|
||||
*
|
||||
* TMP235-Q1의 아날로그 전압 출력을 SAADC(AIN3)로 읽어
|
||||
* 온도(°C)로 변환하는 기능의 외부 호출용 API를 선언한다.
|
||||
*
|
||||
* 주요 API:
|
||||
* - tmp235_init() : SAADC 초기화 + 즉시 측정 시작 (내부 사용)
|
||||
* - tmp235_voltage_level_meas() : 온도 1회 측정 (외부 호출용 래퍼)
|
||||
*
|
||||
* 온도 변환: Vout(mV) → Ta(°C) = (Vout - 500) / 10.0 (0~100°C 구간)
|
||||
******************************************************************************/
|
||||
|
||||
#ifndef _TMP235_Q1_H_
|
||||
#define _TMP235_Q1_H_
|
||||
|
||||
/** @brief TMP235 SAADC 초기화 및 측정 시작 (AIN3 채널) */
|
||||
void tmp235_init(void);
|
||||
/** @brief 온도 1회 측정 외부 호출 함수 (내부적으로 tmp235_init 호출) */
|
||||
void tmp235_voltage_level_meas(void);
|
||||
|
||||
#endif /* !_TMP235_Q1_H_ */
|
||||
|
||||
Reference in New Issue
Block a user