IMU FIFO 커맨드 mim? 추가
This commit is contained in:
@@ -50,6 +50,8 @@ extern int imu_read_direct(void);
|
|||||||
extern int imu_read_temperature_x100(uint16_t *temp_x100, float *temp_c);
|
extern int imu_read_temperature_x100(uint16_t *temp_x100, float *temp_c);
|
||||||
extern int imu_fifo_capture_start(void);
|
extern int imu_fifo_capture_start(void);
|
||||||
extern int imu_fifo_capture_stop_and_send_rim(void);
|
extern int imu_fifo_capture_stop_and_send_rim(void);
|
||||||
|
extern int imu_fifo_capture_wait_samples_and_send_rim(uint16_t target_samples, uint32_t timeout_ms);
|
||||||
|
extern void imu_fifo_capture_disable(void);
|
||||||
extern void battery_timer_stop(void);
|
extern void battery_timer_stop(void);
|
||||||
extern void main_timer_start(void);
|
extern void main_timer_start(void);
|
||||||
extern void hw_i2c_init_once(void);
|
extern void hw_i2c_init_once(void);
|
||||||
|
|||||||
@@ -41,6 +41,7 @@ static const CmdEntry m_cmd_table[] = {
|
|||||||
{ "msn?", true, Cmd_msn },
|
{ "msn?", true, Cmd_msn },
|
||||||
{ "mst?", true, Cmd_mst },
|
{ "mst?", true, Cmd_mst },
|
||||||
{ "msp?", true, Cmd_msp },
|
{ "msp?", true, Cmd_msp },
|
||||||
|
{ "mim?", true, Cmd_mim },
|
||||||
|
|
||||||
/* D. Piezo ultrasound */
|
/* D. Piezo ultrasound */
|
||||||
{ "mpa?", true, Cmd_mpa },
|
{ "mpa?", true, Cmd_mpa },
|
||||||
|
|||||||
@@ -4,6 +4,7 @@
|
|||||||
* msn? -> rsn: battery ADC measurement
|
* msn? -> rsn: battery ADC measurement
|
||||||
* mst? -> rso: IMU die temperature reading
|
* mst? -> rso: IMU die temperature reading
|
||||||
* msp? -> rsp: IMU 6-axis single read
|
* msp? -> rsp: IMU 6-axis single read
|
||||||
|
* mim? -> rim: IMU FIFO 15-sample read
|
||||||
* all_sensors() bulk-measurement helper used by the mbb? handler
|
* all_sensors() bulk-measurement helper used by the mbb? handler
|
||||||
*============================================================================*/
|
*============================================================================*/
|
||||||
|
|
||||||
@@ -47,6 +48,7 @@ int Cmd_mst(const ParsedCmd *cmd)
|
|||||||
|
|
||||||
/*==============================================================================
|
/*==============================================================================
|
||||||
* msp? -> rsp: IMU 6-axis single read
|
* msp? -> rsp: IMU 6-axis single read
|
||||||
|
* mim? -> rim: IMU FIFO 15-sample read
|
||||||
*
|
*
|
||||||
* Request: [TAG 4B "msp?"] [CRC 2B]
|
* Request: [TAG 4B "msp?"] [CRC 2B]
|
||||||
* Response: rsp: + accel(xyz) + gyro(xyz) (transmitted inside imu_read_direct)
|
* Response: rsp: + accel(xyz) + gyro(xyz) (transmitted inside imu_read_direct)
|
||||||
@@ -61,6 +63,33 @@ int Cmd_msp(const ParsedCmd *cmd)
|
|||||||
return 1;
|
return 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/*==============================================================================
|
||||||
|
* mim? -> rim: IMU FIFO-only capture
|
||||||
|
*
|
||||||
|
* Request: [TAG 4B "mim?"] [CRC 2B]
|
||||||
|
* Response: rim: [total_sample_count u16 BE]
|
||||||
|
* [samples: 12B each ax,ay,az,gx,gy,gz ...]
|
||||||
|
*
|
||||||
|
* Uses the same FIFO/rim path as mtb? but without piezo reb:/raa: packets.
|
||||||
|
* At 50 Hz, 15 samples are about 300 ms; timeout leaves margin for startup.
|
||||||
|
*============================================================================*/
|
||||||
|
int Cmd_mim(const ParsedCmd *cmd)
|
||||||
|
{
|
||||||
|
int rc;
|
||||||
|
(void)cmd;
|
||||||
|
|
||||||
|
rc = imu_fifo_capture_start();
|
||||||
|
if (rc != 0)
|
||||||
|
{
|
||||||
|
(void)imu_fifo_capture_stop_and_send_rim();
|
||||||
|
imu_fifo_capture_disable();
|
||||||
|
return 1;
|
||||||
|
}
|
||||||
|
|
||||||
|
(void)imu_fifo_capture_wait_samples_and_send_rim(15, 500);
|
||||||
|
return 1;
|
||||||
|
}
|
||||||
|
|
||||||
/*==============================================================================
|
/*==============================================================================
|
||||||
* all_sensors() - Bulk-measurement helper for the mbb? handler
|
* all_sensors() - Bulk-measurement helper for the mbb? handler
|
||||||
*
|
*
|
||||||
|
|||||||
@@ -9,6 +9,7 @@
|
|||||||
int Cmd_msn(const ParsedCmd *cmd); /* msn? -> rsn: battery ADC measurement */
|
int Cmd_msn(const ParsedCmd *cmd); /* msn? -> rsn: battery ADC measurement */
|
||||||
int Cmd_mst(const ParsedCmd *cmd); /* mst? -> rso: IMU die temperature */
|
int Cmd_mst(const ParsedCmd *cmd); /* mst? -> rso: IMU die temperature */
|
||||||
int Cmd_msp(const ParsedCmd *cmd); /* msp? -> rsp: IMU 6-axis single read */
|
int Cmd_msp(const ParsedCmd *cmd); /* msp? -> rsp: IMU 6-axis single read */
|
||||||
|
int Cmd_mim(const ParsedCmd *cmd); /* mim? -> rim: IMU FIFO 15 samples */
|
||||||
/* Helper for the mbb? handler: sequentially measures battery / IMU / temperature, then emits a single rbb: response.
|
/* Helper for the mbb? handler: sequentially measures battery / IMU / temperature, then emits a single rbb: response.
|
||||||
* Called from Cmd_mbb() in cmd_piezo.c. */
|
* Called from Cmd_mbb() in cmd_piezo.c. */
|
||||||
void all_sensors(void);
|
void all_sensors(void);
|
||||||
|
|||||||
@@ -51,6 +51,7 @@
|
|||||||
* - VBTFW0119 260615 jhChun
|
* - VBTFW0119 260615 jhChun
|
||||||
* : Replaced all TMP235 temperature paths with IMU register direct-read temperature and removed legacy TMP235 build references.
|
* : Replaced all TMP235 temperature paths with IMU register direct-read temperature and removed legacy TMP235 build references.
|
||||||
* : Unify both IMU direct-read and FIFO outputs as raw data in datasheet axis convention.
|
* : Unify both IMU direct-read and FIFO outputs as raw data in datasheet axis convention.
|
||||||
|
* : Added mim? command (rim:, IMU FIFO-only 15 samples at 50 Hz).
|
||||||
* - VBTFW0120 260615 jhChun : Add EMC mitigation logic for BLE link stability: extended supervision timeout and dynamic TX power control based on RSSI, RSSI silence, HVN latency, and TX queue congestion.
|
* - VBTFW0120 260615 jhChun : Add EMC mitigation logic for BLE link stability: extended supervision timeout and dynamic TX power control based on RSSI, RSSI silence, HVN latency, and TX queue congestion.
|
||||||
------------------------------------------------------------------------- */
|
------------------------------------------------------------------------- */
|
||||||
#define FIRMWARE_VERSION "VBTFW0120"
|
#define FIRMWARE_VERSION "VBTFW0120"
|
||||||
|
|||||||
@@ -986,3 +986,48 @@ int imu_fifo_capture_stop_and_send_rim(void)
|
|||||||
imu_fifo_send_rim_packets(packet_count);
|
imu_fifo_send_rim_packets(packet_count);
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
int imu_fifo_capture_wait_samples_and_send_rim(uint16_t target_samples, uint32_t timeout_ms)
|
||||||
|
{
|
||||||
|
int rc = 0;
|
||||||
|
uint8_t count_raw[2] = {0};
|
||||||
|
uint16_t packet_count = 0;
|
||||||
|
uint32_t waited_ms = 0;
|
||||||
|
const uint32_t poll_ms = 10U;
|
||||||
|
|
||||||
|
if (target_samples == 0U)
|
||||||
|
{
|
||||||
|
return imu_fifo_capture_stop_and_send_rim();
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!s_fifo_capture_active)
|
||||||
|
{
|
||||||
|
imu_fifo_send_rim_packets(0);
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
|
||||||
|
while (waited_ms <= timeout_ms)
|
||||||
|
{
|
||||||
|
rc = inv_imu_read_reg(&icm_driver, FIFO_COUNTH, 2, count_raw);
|
||||||
|
if (rc != 0)
|
||||||
|
{
|
||||||
|
DBG_PRINTF("[IMU FIFO] count read fail %d\r\n", rc);
|
||||||
|
imu_fifo_send_rim_packets(0);
|
||||||
|
imu_fifo_power_off();
|
||||||
|
return rc;
|
||||||
|
}
|
||||||
|
|
||||||
|
packet_count = (uint16_t)count_raw[0] | ((uint16_t)count_raw[1] << 8);
|
||||||
|
if (packet_count > target_samples)
|
||||||
|
{
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
dr_sd_delay_ms(poll_ms);
|
||||||
|
waited_ms += poll_ms;
|
||||||
|
}
|
||||||
|
|
||||||
|
rc = imu_fifo_capture_stop_and_send_rim();
|
||||||
|
imu_fifo_capture_disable();
|
||||||
|
return rc;
|
||||||
|
}
|
||||||
|
|||||||
@@ -122,6 +122,13 @@ int imu_fifo_capture_start(void);
|
|||||||
*/
|
*/
|
||||||
int imu_fifo_capture_stop_and_send_rim(void);
|
int imu_fifo_capture_stop_and_send_rim(void);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* \brief Wait until at least target_samples FIFO records are available, then drain
|
||||||
|
* FIFO and send rim: packets.
|
||||||
|
* \return 0=success, negative=error
|
||||||
|
*/
|
||||||
|
int imu_fifo_capture_wait_samples_and_send_rim(uint16_t target_samples, uint32_t timeout_ms);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* \brief Stop IMU FIFO capture without sending rim: packets.
|
* \brief Stop IMU FIFO capture without sending rim: packets.
|
||||||
*/
|
*/
|
||||||
|
|||||||
Reference in New Issue
Block a user