jh.chun/VesiScan-Basic_imu
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

주석 정리

Browse Source
This commit is contained in:
jhchun
2026-05-19 16:39:53 +09:00
parent c86be4112c
commit 06a6f5687b
1 changed files with 6 additions and 88 deletions
Download Patch File Download Diff File Expand all files Collapse all files
project/ble_peripheral/ble_app_bladder_patch/measurement/imu/app_raw/app_raw.c
+6 -88
View File
@@ -50,7 +50,7 @@
/*
* Data output format selection
* SCALED_DATA_G_DPS : Data output format selection
* 0 : Raw data output (raw accel, gyro, temp)
* 1 : Scaled data output (g, dps, Celsius)
*/
@@ -353,15 +353,10 @@ void imu_callback(inv_imu_sensor_event_t *event)
else
temp_degc = 25 + ((float)event->temperature / 2);
/*
* Output scaled data via 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]);
(void)timestamp;
(void)accel_g;
(void)gyro_dps;
(void)temp_degc;
#else
/*
@@ -382,11 +377,6 @@ void imu_callback(inv_imu_sensor_event_t *event)
info_imu[5] = (uint16_t)gyro[2];
}
/* UART mode: output 6-axis data in text format with "Tp" prefix */
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 mode: send 6-axis data as binary packet via BLE
* ssp_data[0..2] = accel XYZ, ssp_data[3..5] = gyro XYZ
@@ -402,7 +392,6 @@ void imu_callback(inv_imu_sensor_event_t *event)
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) {
@@ -623,19 +612,16 @@ static int imu_fifo_driver_prepare(void)
struct inv_imu_serif icm_serif;
int rc;
DBG_PRINTF("[IMU FIFO] prepare: TWI reinit + setup_imu_device\r\n");
inv_i2c_master_uninitialize();
inv_i2c_master_initialize();
imu_serif_make(&icm_serif);
rc = setup_imu_device(&icm_serif);
DBG_PRINTF("[IMU FIFO] prepare: setup_imu_device rc=%d\r\n", rc);
return rc;
}
static void imu_fifo_power_off(void)
{
DBG_PRINTF("[IMU FIFO] power_off: disable gyro/accel, FIFO off, active=0\r\n");
(void)inv_imu_disable_gyro(&icm_driver);
(void)inv_imu_disable_accel(&icm_driver);
(void)inv_imu_configure_fifo(&icm_driver, INV_IMU_FIFO_DISABLED);
@@ -646,11 +632,8 @@ int imu_fifo_capture_start(void)
{
int rc;
DBG_PRINTF("[IMU FIFO] capture_start: enter active=%u\r\n", (unsigned)s_fifo_capture_active);
if (s_fifo_capture_active)
{
DBG_PRINTF("[IMU FIFO] capture_start: skip (already active)\r\n");
return 0;
}
@@ -661,8 +644,6 @@ int imu_fifo_capture_start(void)
return rc;
}
DBG_PRINTF("[IMU FIFO] capture_start: FSR/ODR/FIFO stream, settle %ums\r\n",
(unsigned)IMU_FIFO_ENABLE_SETTLE_MS);
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_500dps);
rc |= inv_imu_set_accel_frequency(&icm_driver, ACCEL_CONFIG0_ODR_25_HZ);
@@ -681,7 +662,6 @@ int imu_fifo_capture_start(void)
rc |= inv_imu_reset_fifo(&icm_driver);
rc |= inv_imu_enable_accel_low_noise_mode(&icm_driver);
rc |= inv_imu_enable_gyro_low_noise_mode(&icm_driver);
DBG_PRINTF("[IMU FIFO] capture_start: delay %ums\r\n", (unsigned)IMU_FIFO_ENABLE_SETTLE_MS);
dr_sd_delay_ms(IMU_FIFO_ENABLE_SETTLE_MS);
rc |= inv_imu_reset_fifo(&icm_driver);
@@ -693,7 +673,6 @@ int imu_fifo_capture_start(void)
}
s_fifo_capture_active = true;
DBG_PRINTF("[IMU FIFO] capture_start: OK active=1 rc_accum=%d\r\n", rc);
return 0;
}
@@ -701,10 +680,6 @@ static void imu_fifo_send_rim_packets(uint16_t total_sample_count)
{
static uint8_t rim_buf[BLE_NUS_MAX_DATA_LEN];
uint16_t record_idx = 0;
uint16_t pkt = 0;
DBG_PRINTF("[IMU FIFO] rim_send: total_samples=%u max_per_pkt=%u\r\n",
(unsigned)total_sample_count, (unsigned)RIM_SAMPLES_PER_PACKET);
do
{
@@ -730,28 +705,19 @@ static void imu_fifo_send_rim_packets(uint16_t total_sample_count)
dst_idx += 6;
}
DBG_PRINTF("[IMU FIFO] rim_send: pkt=%u samples=%u bytes=%u words=%u\r\n",
(unsigned)pkt, (unsigned)sample_count, (unsigned)dst_idx, (unsigned)(dst_idx / 2));
dr_binary_tx_safe(rim_buf, dst_idx / 2);
record_idx += sample_count;
pkt++;
if (record_idx < total_sample_count)
{
dr_sd_delay_ms(IMU_FIFO_RIM_POST_TX_MS);
}
} while (record_idx < total_sample_count);
DBG_PRINTF("[IMU FIFO] rim_send: done packets=%u\r\n", (unsigned)pkt);
}
static int imu_fifo_read_records(uint16_t record_count)
{
int rc = 0;
uint16_t record_idx = 0;
uint16_t burst_n = 0;
DBG_PRINTF("[IMU FIFO] fifo_read: records=%u burst_max=%u\r\n",
(unsigned)record_count, (unsigned)IMU_FIFO_READ_RECORDS_PER_BURST);
while ((record_idx < record_count) && (rc == 0))
{
@@ -764,18 +730,14 @@ static int imu_fifo_read_records(uint16_t record_count)
}
burst_bytes = burst_records * IMU_FIFO_PACKET_SIZE_BYTES;
DBG_PRINTF("[IMU FIFO] fifo_read: burst=%u idx=%u n=%u bytes=%u\r\n",
(unsigned)burst_n, (unsigned)record_idx, (unsigned)burst_records, (unsigned)burst_bytes);
rc |= inv_imu_read_reg(&icm_driver,
FIFO_DATA,
burst_bytes,
&icm_driver.fifo_data[record_idx * IMU_FIFO_PACKET_SIZE_BYTES]);
record_idx += burst_records;
burst_n++;
}
DBG_PRINTF("[IMU FIFO] fifo_read: done bursts=%u rc=%d\r\n", (unsigned)burst_n, rc);
return rc;
}
@@ -881,8 +843,6 @@ static uint16_t imu_fifo_normalize_for_rim(uint8_t *fifo, uint16_t n)
memmove(fifo, fifo + ((uint32_t)drop * sz), (uint32_t)IMU_FIFO_RIM_TARGET_SAMPLES * sz);
n = (uint16_t)IMU_FIFO_RIM_TARGET_SAMPLES;
DBG_PRINTF("[IMU FIFO] normalize: capped to newest %u samples\r\n",
(unsigned)IMU_FIFO_RIM_TARGET_SAMPLES);
}
(void)sz;
#elif (IMU_FIFO_RIM_MIN_SAMPLES > 0)
@@ -893,11 +853,6 @@ static uint16_t imu_fifo_normalize_for_rim(uint8_t *fifo, uint16_t n)
memset(fifo + ((uint32_t)n * sz), 0, sz);
n++;
}
if (n < (uint16_t)IMU_FIFO_RIM_MIN_SAMPLES)
{
DBG_PRINTF("[IMU FIFO] normalize: MIN=%u unreachable, using %u\r\n",
(unsigned)IMU_FIFO_RIM_MIN_SAMPLES, (unsigned)n);
}
}
#else
(void)sz;
@@ -911,56 +866,31 @@ int imu_fifo_capture_stop_and_send_rim(void)
uint8_t count_raw[2] = {0};
uint16_t packet_count;
DBG_PRINTF("[IMU FIFO] stop_send: enter active=%u\r\n", (unsigned)s_fifo_capture_active);
if (!s_fifo_capture_active)
{
DBG_PRINTF("[IMU FIFO] stop_send: not active -> empty rim\r\n");
imu_fifo_send_rim_packets(0);
return -1;
}
rc |= inv_imu_switch_on_mclk(&icm_driver);
DBG_PRINTF("[IMU FIFO] stop_send: mclk on rc=%d\r\n", rc);
rc |= inv_imu_read_reg(&icm_driver, FIFO_COUNTH, 2, count_raw);
packet_count = (uint16_t)count_raw[0] | ((uint16_t)count_raw[1] << 8);
DBG_PRINTF("[IMU FIFO] stop_send: FIFO_COUNT raw[0]=0x%02X [1]=0x%02X -> records=%u\r\n",
count_raw[0], count_raw[1], (unsigned)packet_count);
if (packet_count > IMU_FIFO_MAX_PACKET_COUNT)
{
DBG_PRINTF("[IMU FIFO] stop_send: clamp %u -> %u\r\n",
(unsigned)packet_count, (unsigned)IMU_FIFO_MAX_PACKET_COUNT);
packet_count = IMU_FIFO_MAX_PACKET_COUNT;
}
if ((rc == 0) && (packet_count > 0))
{
uint16_t before_count = packet_count;
rc |= imu_fifo_read_records(packet_count);
if (rc == 0)
{
packet_count = imu_fifo_compact_placeholder_records(icm_driver.fifo_data, packet_count);
if (before_count != packet_count)
{
DBG_PRINTF("[IMU FIFO] stop_send: dropped %u invalid FIFO placeholder record(s)\r\n",
(unsigned)(before_count - packet_count));
}
before_count = packet_count;
packet_count = imu_fifo_compact_invalid_gyro_records(icm_driver.fifo_data, packet_count);
if (before_count != packet_count)
{
DBG_PRINTF("[IMU FIFO] stop_send: dropped %u invalid-gyro FIFO record(s)\r\n",
(unsigned)(before_count - packet_count));
}
}
}
else
{
DBG_PRINTF("[IMU FIFO] stop_send: skip read (rc=%d count=%u)\r\n", rc, (unsigned)packet_count);
}
if ((rc == 0) && (IMU_FIFO_RIM_DROP_LEADING_SAMPLES > 0U) && (packet_count > 0U))
{
@@ -971,24 +901,14 @@ int imu_fifo_capture_stop_and_send_rim(void)
packet_count = (uint16_t)(packet_count - drop);
memmove(fifo, fifo + ((uint32_t)drop * sz), (uint32_t)packet_count * sz);
DBG_PRINTF("[IMU FIFO] stop_send: dropped %u leading sample(s), remain=%u\r\n",
(unsigned)drop, (unsigned)packet_count);
}
if (rc == 0)
{
const uint16_t after_norm = imu_fifo_normalize_for_rim(icm_driver.fifo_data, packet_count);
if (after_norm != packet_count)
{
DBG_PRINTF("[IMU FIFO] stop_send: normalize %u -> %u samples (TARGET/MIN)\r\n",
(unsigned)packet_count, (unsigned)after_norm);
}
packet_count = after_norm;
packet_count = imu_fifo_normalize_for_rim(icm_driver.fifo_data, packet_count);
}
rc |= inv_imu_switch_off_mclk(&icm_driver);
DBG_PRINTF("[IMU FIFO] stop_send: mclk off rc=%d\r\n", rc);
if (rc != 0)
{
@@ -997,8 +917,6 @@ int imu_fifo_capture_stop_and_send_rim(void)
return rc;
}
DBG_PRINTF("[IMU FIFO] stop_send: rim TX records=%u\r\n", (unsigned)packet_count);
imu_fifo_send_rim_packets(packet_count);
DBG_PRINTF("[IMU FIFO] stop_send: OK\r\n");
return 0;
}