VesiScan BASIC origin: Piezo + IMU firmware initial code

- nRF52840 + SoftDevice S140 BLE firmware
- Piezo ultrasound TX driver (2MHz, 8ch MUX)
- ICM42670P IMU 6-axis driver
- Echo AFE chain (ADA2200 + ADC121S051)
- BLE NUS command parser (mpa/mpc/mdc/mec/maa/msp)
- FDS flash config storage
- pc_firm parser and ADC driver included

Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>

project/ble_peripheral/ble_app_bladder_patch/icm42670p/app_raw/app_raw.c

@@ -19,6 +19,8 @@
 #include "main.h"
 #include "meas_pd_48.h"
 #include <cmd_parse.h>
+#include "debug_print.h"
+#include "nrf_delay.h"
 
 
 /*
@@ -312,7 +314,7 @@ 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]);
@@ -322,3 +324,80 @@ static void apply_mounting_matrix(const int32_t matrix[9], int32_t raw[3])
 	raw[1] = (int32_t)(data_q30[1]>>30);
 	raw[2] = (int32_t)(data_q30[2]>>30);
 }
+
+
+/* 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 */
+
+/* Direct IMU register read — raw I2C, no DRDY, sends rsp: via BLE */
+int imu_read_direct(void)
+{
+	uint8_t raw[12];  /* 6 accel + 6 gyro */
+	int32_t accel[3], gyro[3];
+	uint8_t reg;
+	uint32_t ret;
+
+	static bool twi_ready = false;
+
+	DBG_PRINTF("[IMU] enter\r\n");
+
+	/* Ensure ICM42670P TWI is initialized (once only) */
+	if (!twi_ready) {
+		inv_i2c_master_uninitialize();
+		inv_i2c_master_initialize();
+		twi_ready = true;
+	}
+
+	/* Enable accel (low-noise) + gyro (low-noise): PWR_MGMT0 = 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(2);  /* wait for sensor startup */
+	}
+
+	/* Read 12 bytes starting from ACCEL_DATA_X1 (0x0B..0x16) */
+	reg = REG_ACCEL_X1;
+	ret = icm42670_twi_tx(IMU_I2C_ADDR, &reg, 1, true);
+	if (ret) {
+		DBG_PRINTF("[IMU] tx FAIL %u\r\n", ret);
+		return -1;
+	}
+	ret = icm42670_twi_rx(IMU_I2C_ADDR, raw, 12);
+	if (ret) {
+		DBG_PRINTF("[IMU] rx FAIL %u\r\n", ret);
+		return -2;
+	}
+
+	/* Big-Endian register layout: [0..5]=accel, [6..11]=gyro */
+	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);
+
+	DBG_PRINTF("[IMU] A:%d,%d,%d G:%d,%d,%d\r\n",
+		accel[0], accel[1], accel[2], gyro[0], gyro[1], gyro[2]);
+
+	ssp_data[0] = (uint16_t)accel[0];
+	ssp_data[1] = (uint16_t)accel[1];
+	ssp_data[2] = (uint16_t)accel[2];
+	ssp_data[3] = (uint16_t)gyro[0];
+	ssp_data[4] = (uint16_t)gyro[1];
+	ssp_data[5] = (uint16_t)gyro[2];
+
+	format_data(imu_bin_buffer, "rsp:", ssp_data, 12);
+	binary_tx_handler(imu_bin_buffer, 8);
+
+	DBG_PRINTF("[IMU] sent OK\r\n");
+	return 0;
+}

project/ble_peripheral/ble_app_bladder_patch/icm42670p/app_raw/app_raw.h

@@ -71,5 +71,11 @@ int get_imu_data(void);
  */
 void imu_callback(inv_imu_sensor_event_t *event);
 
+/**
+ * \brief Direct IMU register read — bypasses DRDY, sends rsp: via BLE.
+ * \return 0 on success, negative value on error.
+ */
+int imu_read_direct(void);
+
 
 #endif /* !_APP_RAW_H_ */

project/ble_peripheral/ble_app_bladder_patch/icm42670p/system_interface.c

@@ -34,7 +34,7 @@ void inv_i2c_master_uninitialize(void){
     nrfx_twi_uninit(&m_twi_icm42670);
 }
 
-static void inv_i2c_master_initialize(void){
+void inv_i2c_master_initialize(void){
     ret_code_t err_code;
 
     const nrfx_twi_config_t twi_icm42670_config = {

project/ble_peripheral/ble_app_bladder_patch/icm42670p/system_interface.h

@@ -32,6 +32,7 @@ uint32_t icm42670_twi_rx(  uint8_t       device_id,
 uint8_t cat_read (uint8_t device_id, uint8_t address, uint8_t *data);
 void cat_write (uint8_t device_id, uint8_t address, uint8_t *data);	
 void inv_i2c_master_uninitialize(void);
+void inv_i2c_master_initialize(void);
 int inv_io_hal_init(struct inv_imu_serif *serif);
 int inv_io_hal_read_reg(struct inv_imu_serif *serif, uint8_t reg, uint8_t * rbuffer, uint32_t rlen);
 int inv_io_hal_write_reg(struct inv_imu_serif *serif, uint8_t reg, const uint8_t * wbuffer, uint32_t wlen);