IMU direct read -> FIFO 방식 변경

- mtb? 커맨드
2026-05-18 17:54:15 +09:00
parent 8d9cb6e307
commit d439ae9b68
12 changed files with 617 additions and 10 deletions
@@ -48,6 +48,8 @@ extern void battery_level_meas(void);
 extern void pressure_all_level_meas(void);
 extern void tmp235_voltage_level_meas(void);
 extern int  imu_read_direct(void);
+extern int  imu_fifo_capture_start(void);
+extern int  imu_fifo_capture_stop_and_send_rim(void);
 extern void battery_timer_stop(void);
 extern void main_timer_start(void);
 extern void hw_i2c_init_once(void);
@@ -49,6 +49,7 @@ static const CmdEntry m_cmd_table[] = {
    { "mec?", true, Cmd_mec },
    { "maa?", true, Cmd_maa },
    { "mbb?", true, Cmd_mbb },
+    { "mtb?", true, Cmd_mtb },
    { "mcf?", true, Cmd_mcf },
    { "mcs?", true, Cmd_mcs },

@@ -17,6 +17,11 @@
 #include "dr_piezo.h"
 #include "dr_adc121s051.h"

+static void mtb_send_rim_after_piezo(void)
+{
+    send_imu_rim_fifo();
+}
+
 /*------------------------------------------------------------------------------
 * Internal clamp helpers for persisted piezo configuration
 *----------------------------------------------------------------------------*/
@@ -359,6 +364,66 @@ int Cmd_mbb(const ParsedCmd *cmd)
    return 1;
 }

+/*==============================================================================
+ * mtb? -> reb:+raa:+rim:  Piezo ADC + IMU FIFO (no rbb:)
+ *
+ * Request:  [TAG 4B "mtb?"] [CRC 2B]
+ * Response: reb: [num_samples 2B] [raw_data...]   (per channel; same maa_async as mbb?)
+ *           raa: [status 2B]
+ *           rim: [total_sample_count u16 BE] [samples: 12B each ax,ay,az,gx,gy,gz ...] (may span BLE packets)
+ * Error:    raa: + 0xFFFE       (previous capture in progress)
+ *           raa: + (0xFF00|err) (start failed)
+ *
+ * reb/raa use the same maa_async_start path as mbb?; no rbb: / all_sensors().
+ *============================================================================*/
+int Cmd_mtb(const ParsedCmd *cmd)
+{
+    dr_adc_err_t err;
+    (void)cmd;
+
+    if (maa_async_is_busy())
+    {
+        dr_ble_return_1("raa:", 0xFFFE);
+        return 1;
+    }
+
+    (void)imu_fifo_capture_start();
+
+    if (!dr_piezo_is_power_on())
+    {
+        dr_piezo_power_on();
+    }
+
+    maa_async_set_pre_capture_all(true);
+
+    err = maa_async_start(
+        m_config.piezo_freq_option,
+        m_config.piezo_delay_us,
+        m_config.piezo_num_samples,
+        m_config.piezo_cycles,
+        m_config.piezo_averaging,
+        ble_bin_buffer
+    );
+
+    if (err != DR_ADC_OK)
+    {
+        if (g_plat.log)
+        {
+            g_plat.log("[Cmd_mtb] start failed err=%d\r\n", err);
+        }
+        single_format_data(ble_bin_buffer, "raa:", (uint16_t)(0xFF00 | err));
+        dr_binary_tx_safe(ble_bin_buffer, 3);
+        dr_piezo_power_off();
+        maa_async_set_on_complete(NULL);
+        send_imu_rim_fifo();
+        return 1;
+    }
+
+    maa_async_set_on_complete(mtb_send_rim_after_piezo);
+
+    return 1;
+}
+
 /*==============================================================================
 * mcf? -> rcf:  Read piezo parameters from FDS
 *
@@ -12,6 +12,7 @@ int Cmd_mpc(const ParsedCmd *cmd);   /* mpc? -> rpc:  burst generation */
 int Cmd_mec(const ParsedCmd *cmd);   /* mec? -> reb:+raa:  single-channel capture */
 int Cmd_maa(const ParsedCmd *cmd);   /* maa? -> reb:+raa:  6-channel async capture */
 int Cmd_mbb(const ParsedCmd *cmd);   /* mbb? -> rbb:+reb:+raa:  sensors + capture */
+int Cmd_mtb(const ParsedCmd *cmd);   /* mtb? -> reb:+raa:+rim:  piezo + IMU FIFO (no rbb:) */
 int Cmd_mcf(const ParsedCmd *cmd);   /* mcf? -> rcf:  read piezo parameters */
 int Cmd_mcs(const ParsedCmd *cmd);   /* mcs? -> rcs:  write piezo parameters */

@@ -136,3 +136,57 @@ void all_sensors(void)

    dr_binary_tx_safe(buf, 10);  /* 20 bytes = 10 words */
 }
+
+/*==============================================================================
+ * all_sensors_batt_temp() - Battery + temperature only, then short rbb:
+ *
+ * Emits rbb: [batt 2B] [temp 2B] = 8 bytes = 4 words (no IMU).
+ * Not used by mtb? anymore; kept for optional host/tests.
+ *
+ * Order: battery -> (Piezo TX/RX ON) -> temperature
+ * Response: rbb: [batt 2B] [temp 2B] = 8 bytes = 4 words
+ *           TX layer appends CRC 2B, so the BLE packet is 10B total.
+ *============================================================================*/
+void all_sensors_batt_temp(void)
+{
+    uint8_t *buf;
+    uint32_t timeout_cnt;
+
+    info4 = true;
+
+    battery_saadc_done = false;
+    battery_level_meas();
+    for (timeout_cnt = 0; !battery_saadc_done && timeout_cnt < 100; timeout_cnt++)
+    {
+        dr_sd_delay_ms(1);
+    }
+
+    if (!dr_piezo_is_power_on())
+    {
+        dr_piezo_power_on();
+    }
+
+    tmp235_saadc_done = false;
+    tmp235_voltage_level_meas();
+    for (timeout_cnt = 0; !tmp235_saadc_done && timeout_cnt < 100; timeout_cnt++)
+    {
+        dr_sd_delay_ms(1);
+    }
+
+    info4 = false;
+
+    static uint8_t rbb_buf[8];
+    buf = rbb_buf;
+    buf[0] = 'r'; buf[1] = 'b'; buf[2] = 'b'; buf[3] = ':';
+    buf[4] = (uint8_t)(info_batt >> 8);
+    buf[5] = (uint8_t)(info_batt & 0xFF);
+    buf[6] = (uint8_t)(info_temp >> 8);
+    buf[7] = (uint8_t)(info_temp & 0xFF);
+
+    dr_binary_tx_safe(buf, 4);  /* 8 bytes = 4 words, CRC appended by TX layer */
+}
+
+void send_imu_rim_fifo(void)
+{
+    (void)imu_fifo_capture_stop_and_send_rim();
+}
@@ -13,4 +13,11 @@ int Cmd_msp(const ParsedCmd *cmd);   /* msp? -> rsp:  IMU 6-axis single read */
 * Called from Cmd_mbb() in cmd_piezo.c. */
 void all_sensors(void);

+/* Optional helper: battery / temperature only, then rbb: [batt 2B] [temp 2B].
+ * (mtb? no longer calls this; kept for reuse / tooling.) */
+void all_sensors_batt_temp(void);
+
+/* Test helper for mtb?: drains IMU FIFO and emits rim: packet(s). */
+void send_imu_rim_fifo(void);
+
 #endif /* CMD_SENSOR_H */