Compare commits
2 Commits
48a0a7c221
...
ba964a9301
| Author | SHA1 | Date | |
|---|---|---|---|
| ba964a9301 | |||
| aa8d8f698f |
@@ -48,6 +48,7 @@ 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_read_temperature_x100(uint16_t *temp_x100, float *temp_c);
|
||||
extern int imu_fifo_capture_start(void);
|
||||
extern int imu_fifo_capture_stop_and_send_rim(void);
|
||||
extern void battery_timer_stop(void);
|
||||
|
||||
@@ -80,7 +80,7 @@ int Cmd_msp(const ParsedCmd *cmd)
|
||||
* single rbb: packet. SAADC measurements run asynchronously (callback), so
|
||||
* dr_sd_delay_ms() is used to wait for completion.
|
||||
*
|
||||
* Order: battery -> IMU -> (Piezo TX/RX ON) -> temperature
|
||||
* Order: battery -> IMU 6-axis + IMU temperature
|
||||
* Response: rbb: [batt 2B] [IMU 6x2B] [temp 2B] = 20 bytes = 10 words
|
||||
*============================================================================*/
|
||||
void all_sensors(void)
|
||||
@@ -98,23 +98,11 @@ void all_sensors(void)
|
||||
dr_sd_delay_ms(1);
|
||||
}
|
||||
|
||||
/* 2. IMU 6-axis single read -> info_imu[6] */
|
||||
/* 2. IMU 6-axis single read + IMU die temperature -> info_imu[6], info_temp */
|
||||
info_temp = 0xFFFF;
|
||||
hw_i2c_init_once();
|
||||
imu_read_direct();
|
||||
|
||||
/* 3. Temperature -> info_temp (TMP235 needs Piezo TX/RX power) */
|
||||
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;
|
||||
|
||||
/* Assemble and transmit the rbb: packet (dedicated buffer to avoid
|
||||
@@ -134,6 +122,11 @@ void all_sensors(void)
|
||||
buf[18] = (uint8_t)(info_temp >> 8);
|
||||
buf[19] = (uint8_t)(info_temp & 0xFF);
|
||||
|
||||
DBG_PRINTF("[RBB] IMU temp_x100=%u (%u.%02u C)\r\n",
|
||||
info_temp,
|
||||
info_temp / 100,
|
||||
info_temp % 100);
|
||||
|
||||
dr_binary_tx_safe(buf, 10); /* 20 bytes = 10 words */
|
||||
}
|
||||
|
||||
@@ -143,7 +136,7 @@ void all_sensors(void)
|
||||
* 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
|
||||
* Order: battery -> IMU temperature
|
||||
* Response: rbb: [batt 2B] [temp 2B] = 8 bytes = 4 words
|
||||
* TX layer appends CRC 2B, so the BLE packet is 10B total.
|
||||
*============================================================================*/
|
||||
@@ -151,6 +144,7 @@ void all_sensors_batt_temp(void)
|
||||
{
|
||||
uint8_t *buf;
|
||||
uint32_t timeout_cnt;
|
||||
uint16_t imu_temp_x100;
|
||||
|
||||
info4 = true;
|
||||
|
||||
@@ -161,16 +155,13 @@ void all_sensors_batt_temp(void)
|
||||
dr_sd_delay_ms(1);
|
||||
}
|
||||
|
||||
if (!dr_piezo_is_power_on())
|
||||
if (imu_read_temperature_x100(&imu_temp_x100, NULL) == 0)
|
||||
{
|
||||
dr_piezo_power_on();
|
||||
info_temp = imu_temp_x100;
|
||||
}
|
||||
|
||||
tmp235_saadc_done = false;
|
||||
tmp235_voltage_level_meas();
|
||||
for (timeout_cnt = 0; !tmp235_saadc_done && timeout_cnt < 100; timeout_cnt++)
|
||||
else
|
||||
{
|
||||
dr_sd_delay_ms(1);
|
||||
info_temp = 0xFFFF;
|
||||
}
|
||||
|
||||
info4 = false;
|
||||
@@ -183,6 +174,11 @@ void all_sensors_batt_temp(void)
|
||||
buf[6] = (uint8_t)(info_temp >> 8);
|
||||
buf[7] = (uint8_t)(info_temp & 0xFF);
|
||||
|
||||
DBG_PRINTF("[RBB] IMU temp_x100=%u (%u.%02u C)\r\n",
|
||||
info_temp,
|
||||
info_temp / 100,
|
||||
info_temp % 100);
|
||||
|
||||
dr_binary_tx_safe(buf, 4); /* 8 bytes = 4 words, CRC appended by TX layer */
|
||||
}
|
||||
|
||||
|
||||
+19
-7
@@ -152,6 +152,8 @@ static uint16_t clamp_measure_delay_us(uint16_t delay_us)
|
||||
#define BLE_RDB_HEADER_LEN 8 /* "rdb:" tag(4) + num_samples(2) + compressed_size(2) */
|
||||
#define BLE_REB_DATA_LEN (BLE_MTU_SIZE - BLE_REB_HEADER_LEN) /* 238 bytes = 119 samples */
|
||||
#define BLE_RED_DATA_LEN (BLE_MTU_SIZE - BLE_RED_HEADER_LEN) /* 238 bytes = 119 samples */
|
||||
#define MAA_REB_HEADER_LEN 8 /* "reb:" tag(4) + ch_info(2) + num_samples(2) */
|
||||
#define MAA_REB_DATA_LEN (BLE_MTU_SIZE - MAA_REB_HEADER_LEN) /* 236 bytes = 118 samples */
|
||||
#define BLE_PACKET_DELAY_MS 100 /* Inter-packet delay - allow BLE TX buffer to drain */
|
||||
/* maa_async: inter-packet busy-wait (0 = rely on BLE_NUS_EVT_TX_RDY only). */
|
||||
#define MAA_ASYNC_POST_REB_MS 0U
|
||||
@@ -1162,6 +1164,7 @@ dr_adc_err_t dr_adc_transmit_channel_delta(const dr_maa_channel_t *ch_data, uint
|
||||
|
||||
/* Global async context */
|
||||
static maa_async_ctx_t g_maa_ctx = { .state = MAA_ASYNC_IDLE };
|
||||
static uint8_t g_maa_ch_session = 0;
|
||||
|
||||
/* (reb+red merged protocol) */
|
||||
|
||||
@@ -1202,7 +1205,8 @@ static dr_adc_err_t maa_async_capture_channel(uint8_t ch)
|
||||
/**
|
||||
* @brief Send reb: header + data merged for current channel
|
||||
*
|
||||
* reb: tag(4) + num_samples(2) + data(up to 238 bytes)
|
||||
* reb: tag(4) + ch_info(2) + num_samples(2) + data(up to 236 bytes)
|
||||
* ch_info: session(1) + channel(1)
|
||||
* If <= 100 samples (200B), the channel completes in this single packet
|
||||
*/
|
||||
static bool maa_async_send_header(void)
|
||||
@@ -1212,16 +1216,23 @@ static bool maa_async_send_header(void)
|
||||
uint32_t tx_err;
|
||||
|
||||
uint16_t total_data_bytes = ch->num_samples * 2;
|
||||
uint16_t ch_info = ((uint16_t)g_maa_ctx.ch_session << 8) | g_maa_ctx.current_ch;
|
||||
|
||||
/* reb: header + data merged (Big-Endian) */
|
||||
/* reb: header + channel info + data merged (Big-Endian) */
|
||||
buf[0] = 'r'; buf[1] = 'e'; buf[2] = 'b'; buf[3] = ':';
|
||||
buf[4] = (uint8_t)(ch->num_samples >> 8);
|
||||
buf[5] = (uint8_t)(ch->num_samples & 0xFF);
|
||||
buf[4] = (uint8_t)(ch_info >> 8);
|
||||
buf[5] = (uint8_t)(ch_info & 0xFF);
|
||||
buf[6] = (uint8_t)(ch->num_samples >> 8);
|
||||
buf[7] = (uint8_t)(ch->num_samples & 0xFF);
|
||||
ADC_LOG("reb ch_info: session=%u, ch=%u, value=0x%04X",
|
||||
g_maa_ctx.ch_session,
|
||||
g_maa_ctx.current_ch,
|
||||
ch_info);
|
||||
|
||||
uint16_t dst_idx = BLE_REB_HEADER_LEN;
|
||||
uint16_t first_chunk = (total_data_bytes > BLE_REB_DATA_LEN) ? BLE_REB_DATA_LEN : total_data_bytes;
|
||||
uint16_t dst_idx = MAA_REB_HEADER_LEN;
|
||||
uint16_t first_chunk = (total_data_bytes > MAA_REB_DATA_LEN) ? MAA_REB_DATA_LEN : total_data_bytes;
|
||||
uint16_t src_idx = 0;
|
||||
while (src_idx < ch->num_samples && (dst_idx - BLE_REB_HEADER_LEN) < first_chunk)
|
||||
while (src_idx < ch->num_samples && (dst_idx - MAA_REB_HEADER_LEN) < first_chunk)
|
||||
{
|
||||
uint16_t sample = ch->samples[src_idx++];
|
||||
buf[dst_idx++] = (uint8_t)(sample >> 8);
|
||||
@@ -1376,6 +1387,7 @@ dr_adc_err_t maa_async_start(uint8_t freq_option, uint16_t delay_us, uint16_t nu
|
||||
g_maa_ctx.ble_buffer = ble_buffer;
|
||||
g_maa_ctx.current_ch = 0;
|
||||
g_maa_ctx.current_pkt = 0;
|
||||
g_maa_ctx.ch_session = g_maa_ch_session++;
|
||||
g_maa_ctx.data_offset = 0;
|
||||
g_maa_ctx.on_complete_cb = NULL; /* default: no callback (maa?) */
|
||||
|
||||
|
||||
@@ -434,6 +434,7 @@ typedef struct {
|
||||
maa_async_state_t state; /**< Current state */
|
||||
uint8_t current_ch; /**< Current channel (0~7) */
|
||||
uint8_t current_pkt; /**< Current packet index */
|
||||
uint8_t ch_session; /**< reb: channel session id for one measurement set */
|
||||
uint16_t data_offset; /**< Bytes sent so far for current channel */
|
||||
uint8_t freq_option; /**< Frequency option */
|
||||
uint16_t delay_us; /**< Capture delay */
|
||||
|
||||
@@ -4,7 +4,7 @@
|
||||
* Measures battery voltage via nRF52840 SAADC on AIN2:
|
||||
* - 12-bit resolution, 4x oversampling
|
||||
* - Periodic safety check via battery_loop timer (60 s interval)
|
||||
* - Sequential: battery -> temperature measurement
|
||||
* - Sequential: battery -> IMU temperature measurement
|
||||
* - Auto power-off after 5 consecutive readings below 3500 mV or above 40 C
|
||||
* - In info4 mode (bulk sensor collection): stores to info_batt
|
||||
*
|
||||
@@ -28,6 +28,7 @@
|
||||
#include "battery_saadc.h"
|
||||
#include "main_timer.h"
|
||||
#include "tmp235_q1.h"
|
||||
#include "app_raw.h"
|
||||
#include "dr_piezo.h"
|
||||
#include "debug_print.h"
|
||||
|
||||
@@ -59,7 +60,7 @@ APP_TIMER_DEF(m_battery_loop_timer_id);
|
||||
/* Low-battery check flag — set by battery_loop, consumed by handler */
|
||||
bool low_battery_check = false;
|
||||
|
||||
/* Safety check mode flag — set by battery handler, consumed by tmp235 handler */
|
||||
/* Safety check mode flag — legacy TMP235 path */
|
||||
bool safety_check_mode = false;
|
||||
|
||||
/* SAADC callback completion flag — used by all_sensors() to wait */
|
||||
@@ -179,19 +180,28 @@ void battery_event_handler(nrf_drv_saadc_evt_t const * p_event)
|
||||
/* Resistor divider correction factor 1.42 */
|
||||
batt_lvl_in_milli_volt_1 = batt_lvl_in_milli_volt_0 * 1.42f;
|
||||
|
||||
/* --- Safety check mode: store voltage, chain temperature measurement --- */
|
||||
/* --- Safety check mode: store voltage, then check IMU die temperature --- */
|
||||
if (low_battery_check == true)
|
||||
{
|
||||
float imu_temp_c = -273.15f;
|
||||
|
||||
low_battery_check = false;
|
||||
safety_batt_mv = batt_lvl_in_milli_volt_1;
|
||||
safety_check_mode = true;
|
||||
|
||||
/* TMP235 shares piezo TX/RX power rail */
|
||||
if (!dr_piezo_is_power_on())
|
||||
if (imu_read_temperature_x100(NULL, &imu_temp_c) != 0)
|
||||
{
|
||||
dr_piezo_power_on();
|
||||
DBG_PRINTF("[SAFETY] IMU temp read failed\r\n");
|
||||
}
|
||||
tmp235_voltage_level_meas();
|
||||
else
|
||||
{
|
||||
int temp_x100 = (int)(imu_temp_c * 100.0f);
|
||||
DBG_PRINTF("[SAFETY] 60s IMU temp_x100=%d (%d.%02d C)\r\n",
|
||||
temp_x100,
|
||||
temp_x100 / 100,
|
||||
temp_x100 % 100);
|
||||
}
|
||||
|
||||
safety_check_complete(imu_temp_c);
|
||||
}
|
||||
|
||||
/* --- info4 mode: store value for mbb? bulk response --- */
|
||||
|
||||
@@ -8,7 +8,7 @@
|
||||
* battery_timer_init/start/stop() : 60-second periodic monitoring timer
|
||||
*
|
||||
* Periodic safety check (every 60 s):
|
||||
* Battery -> Temperature sequential measurement via SAADC.
|
||||
* Battery SAADC -> IMU die temperature sequential measurement.
|
||||
* Auto power-off after 5 consecutive readings below LOW_BATTERY_VOLTAGE (3500 mV)
|
||||
* or above OVER_TEMPERATURE_THRESHOLD (40 C).
|
||||
*============================================================================*/
|
||||
@@ -25,10 +25,10 @@
|
||||
/* SAADC callback completion flag (used by all_sensors() to wait) */
|
||||
extern volatile bool battery_saadc_done;
|
||||
|
||||
/* Safety check mode flag — set by battery_loop, consumed by tmp235 handler */
|
||||
/* Safety check mode flag — legacy TMP235 path */
|
||||
extern bool safety_check_mode;
|
||||
|
||||
/* Called by tmp235 handler when safety check temperature measurement completes */
|
||||
/* Called when safety check temperature measurement completes */
|
||||
void safety_check_complete(float temp_c);
|
||||
|
||||
/* Start a single async battery measurement. Result handled in callback. */
|
||||
|
||||
@@ -25,7 +25,7 @@
|
||||
* - BLE mode: sends 6-axis data via BLE with "rsp:" tag
|
||||
* - UART mode: outputs text format to serial
|
||||
* 5) imu_read_direct() - Direct I2C register read bypassing driver API
|
||||
* - Configure sensor -> power ON -> wait 80ms -> read 12 bytes -> sleep
|
||||
* - Configure sensor -> power ON -> wait 80ms -> read temp + 6-axis -> sleep
|
||||
*
|
||||
* Mounting matrix:
|
||||
* 3x3 rotation matrix in Q30 fixed-point format, correcting the sensor's
|
||||
@@ -93,6 +93,7 @@ extern which_cmd_t cmd_type_t; /* Current command source (
|
||||
uint16_t ssp_data[6]={0,}; /* 6-axis data array for BLE (accel XYZ + gyro XYZ) */
|
||||
extern bool info4; /* info4 mode flag (set by cmd_parse) */
|
||||
volatile uint16_t info_imu[6]; /* Global array storing IMU data in info4 mode */
|
||||
extern volatile uint16_t info_temp; /* Global temperature cache (deg C x 100) */
|
||||
|
||||
/* --------------------------------------------------------------------------------------
|
||||
* static function declaration
|
||||
@@ -447,7 +448,7 @@ static void apply_mounting_matrix(const int32_t matrix[9], int32_t raw[3])
|
||||
* 3) Accel config: +/-4g, 100Hz ODR (ACCEL_CONFIG0 = 0x29)
|
||||
* 4) Power ON: accel+gyro low-noise mode (PWR_MGMT0 = 0x0F)
|
||||
* 5) Wait 80ms (gyro startup: min 45ms + margin)
|
||||
* 6) Read 12 consecutive bytes from ACCEL_DATA_X1 (0x0B) (accel 6 + gyro 6)
|
||||
* 6) Read 14 consecutive bytes from TEMP_DATA1 (0x09) (temp 2 + accel 6 + gyro 6)
|
||||
* 7) Big-endian -> int16_t conversion
|
||||
* 8) Apply mounting matrix
|
||||
* 9) Send via BLE with "rsp:" tag
|
||||
@@ -462,7 +463,26 @@ static void apply_mounting_matrix(const int32_t matrix[9], int32_t raw[3])
|
||||
extern const nrfx_twi_t m_twi_icm42670;
|
||||
|
||||
#define IMU_I2C_ADDR 0x68
|
||||
#define REG_ACCEL_X1 0x0B /* ACCEL_DATA_X1 — accel X-axis upper byte register */
|
||||
#define REG_TEMP_DATA1 0x09 /* TEMP_DATA1 — temperature upper byte register */
|
||||
#define IMU_TEMP_AVG_SAMPLES 4U
|
||||
|
||||
static bool s_direct_twi_ready = false;
|
||||
|
||||
static void imu_direct_twi_init_once(void)
|
||||
{
|
||||
/* TWI (I2C) init — performed only once (re-init ensures clean state) */
|
||||
if (!s_direct_twi_ready) {
|
||||
inv_i2c_master_uninitialize();
|
||||
inv_i2c_master_initialize();
|
||||
s_direct_twi_ready = true;
|
||||
}
|
||||
}
|
||||
|
||||
static uint16_t imu_temp_raw_to_x100(int16_t temp_raw)
|
||||
{
|
||||
float temp_c = 25.0f + ((float)temp_raw / 128.0f);
|
||||
return (uint16_t)(temp_c * 100.0f);
|
||||
}
|
||||
|
||||
/* --------------------------------------------------------------------------------------
|
||||
* Direct IMU register read — raw I2C, no DRDY, sends rsp: via BLE
|
||||
@@ -470,19 +490,13 @@ extern const nrfx_twi_t m_twi_icm42670;
|
||||
* -------------------------------------------------------------------------------------- */
|
||||
int imu_read_direct(void)
|
||||
{
|
||||
uint8_t raw[12]; /* accel 6 bytes + gyro 6 bytes */
|
||||
uint8_t raw[14]; /* temp 2 bytes + accel 6 bytes + gyro 6 bytes */
|
||||
int32_t accel[3], gyro[3];
|
||||
int16_t temp_raw;
|
||||
uint8_t reg;
|
||||
uint32_t ret;
|
||||
|
||||
static bool twi_ready = false;
|
||||
|
||||
/* TWI (I2C) init — performed only once (re-init ensures clean state) */
|
||||
if (!twi_ready) {
|
||||
inv_i2c_master_uninitialize();
|
||||
inv_i2c_master_initialize();
|
||||
twi_ready = true;
|
||||
}
|
||||
imu_direct_twi_init_once();
|
||||
|
||||
/* Gyro config: GYRO_CONFIG0(0x20) = 0x09 -> +/-2000dps FSR, 100Hz ODR */
|
||||
{
|
||||
@@ -504,8 +518,8 @@ int imu_read_direct(void)
|
||||
dr_sd_delay_ms(80);
|
||||
}
|
||||
|
||||
/* Read 12 consecutive bytes from ACCEL_DATA_X1 (0x0B~0x16) */
|
||||
reg = REG_ACCEL_X1;
|
||||
/* Read 14 consecutive bytes from TEMP_DATA1 (0x09~0x16): temp + accel + gyro */
|
||||
reg = REG_TEMP_DATA1;
|
||||
ret = icm42670_twi_tx(IMU_I2C_ADDR, ®, 1, true); /* Send register address (no STOP) */
|
||||
|
||||
if (ret)
|
||||
@@ -514,7 +528,7 @@ int imu_read_direct(void)
|
||||
return -1;
|
||||
}
|
||||
|
||||
ret = icm42670_twi_rx(IMU_I2C_ADDR, raw, 12); /* Receive 12 bytes of data */
|
||||
ret = icm42670_twi_rx(IMU_I2C_ADDR, raw, sizeof(raw));
|
||||
|
||||
if (ret)
|
||||
{
|
||||
@@ -524,15 +538,17 @@ int imu_read_direct(void)
|
||||
|
||||
/*
|
||||
* Convert big-endian register layout to int16_t
|
||||
* raw[0..5] = accel X,Y,Z (2 bytes each, MSB first)
|
||||
* raw[6..11] = gyro X,Y,Z (2 bytes each, MSB first)
|
||||
* raw[0..1] = temperature
|
||||
* raw[2..7] = accel X,Y,Z (2 bytes each, MSB first)
|
||||
* raw[8..13] = gyro X,Y,Z (2 bytes each, 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]);
|
||||
temp_raw = (int16_t)((raw[0] << 8) | raw[1]);
|
||||
accel[0] = (int16_t)((raw[2] << 8) | raw[3]);
|
||||
accel[1] = (int16_t)((raw[4] << 8) | raw[5]);
|
||||
accel[2] = (int16_t)((raw[6] << 8) | raw[7]);
|
||||
gyro[0] = (int16_t)((raw[8] << 8) | raw[9]);
|
||||
gyro[1] = (int16_t)((raw[10] << 8) | raw[11]);
|
||||
gyro[2] = (int16_t)((raw[12] << 8) | raw[13]);
|
||||
|
||||
/* Apply mounting matrix — board orientation correction */
|
||||
apply_mounting_matrix(icm_mounting_matrix, accel);
|
||||
@@ -548,6 +564,8 @@ int imu_read_direct(void)
|
||||
|
||||
if (info4 == true)
|
||||
{
|
||||
uint16_t temp_x100 = imu_temp_raw_to_x100(temp_raw);
|
||||
|
||||
/* info4 mode: store in global array (sent as rbb: packet by mbb?) */
|
||||
info_imu[0] = ssp_data[0];
|
||||
info_imu[1] = ssp_data[1];
|
||||
@@ -555,6 +573,7 @@ int imu_read_direct(void)
|
||||
info_imu[3] = ssp_data[3];
|
||||
info_imu[4] = ssp_data[4];
|
||||
info_imu[5] = ssp_data[5];
|
||||
info_temp = temp_x100;
|
||||
}
|
||||
else
|
||||
{
|
||||
@@ -572,6 +591,70 @@ int imu_read_direct(void)
|
||||
return 0;
|
||||
}
|
||||
|
||||
int imu_read_temperature_x100(uint16_t *temp_x100, float *temp_c)
|
||||
{
|
||||
uint8_t raw[2];
|
||||
uint8_t reg = REG_TEMP_DATA1;
|
||||
int16_t temp_raw;
|
||||
int32_t temp_sum = 0;
|
||||
uint32_t ret;
|
||||
uint8_t i;
|
||||
|
||||
if (temp_x100 == NULL && temp_c == NULL)
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
|
||||
imu_direct_twi_init_once();
|
||||
|
||||
/* Power ON briefly so the temperature register is refreshed before reading. */
|
||||
{
|
||||
uint8_t pwr_cmd[2] = { 0x1F, 0x0F };
|
||||
icm42670_twi_tx(IMU_I2C_ADDR, pwr_cmd, 2, false);
|
||||
dr_sd_delay_ms(80);
|
||||
}
|
||||
|
||||
for (i = 0; i < IMU_TEMP_AVG_SAMPLES; i++)
|
||||
{
|
||||
ret = icm42670_twi_tx(IMU_I2C_ADDR, ®, 1, true);
|
||||
if (ret)
|
||||
{
|
||||
uint8_t pwr_off[2] = { 0x1F, 0x00 };
|
||||
icm42670_twi_tx(IMU_I2C_ADDR, pwr_off, 2, false);
|
||||
DBG_PRINTF("[IMU] temp tx FAIL %u\r\n", ret);
|
||||
return -2;
|
||||
}
|
||||
|
||||
ret = icm42670_twi_rx(IMU_I2C_ADDR, raw, sizeof(raw));
|
||||
if (ret)
|
||||
{
|
||||
uint8_t pwr_off[2] = { 0x1F, 0x00 };
|
||||
icm42670_twi_tx(IMU_I2C_ADDR, pwr_off, 2, false);
|
||||
DBG_PRINTF("[IMU] temp rx FAIL %u\r\n", ret);
|
||||
return -3;
|
||||
}
|
||||
|
||||
temp_sum += (int16_t)((raw[0] << 8) | raw[1]);
|
||||
}
|
||||
|
||||
temp_raw = (int16_t)(temp_sum / (int32_t)IMU_TEMP_AVG_SAMPLES);
|
||||
if (temp_x100 != NULL)
|
||||
{
|
||||
*temp_x100 = imu_temp_raw_to_x100(temp_raw);
|
||||
}
|
||||
if (temp_c != NULL)
|
||||
{
|
||||
*temp_c = 25.0f + ((float)temp_raw / 128.0f);
|
||||
}
|
||||
|
||||
{
|
||||
uint8_t pwr_off[2] = { 0x1F, 0x00 };
|
||||
icm42670_twi_tx(IMU_I2C_ADDR, pwr_off, 2, false);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* --------------------------------------------------------------------------------------
|
||||
* mtb? FIFO capture support
|
||||
*
|
||||
|
||||
@@ -97,6 +97,14 @@ void imu_callback(inv_imu_sensor_event_t *event);
|
||||
*/
|
||||
int imu_read_direct(void);
|
||||
|
||||
/**
|
||||
* \brief Read IMU die temperature directly and return deg C x 100.
|
||||
* \param[out] temp_x100 Optional temperature output in Celsius x 100.
|
||||
* \param[out] temp_c Optional temperature output in Celsius.
|
||||
* \return 0=success, negative=error
|
||||
*/
|
||||
int imu_read_temperature_x100(uint16_t *temp_x100, float *temp_c);
|
||||
|
||||
/**
|
||||
* \brief Start IMU internal FIFO capture for mtb? test flow.
|
||||
* Configures accel/gyro 100 Hz and flushes FIFO before capture.
|
||||
|
||||
+1
-1
@@ -1156,7 +1156,7 @@
|
||||
<GroupNumber>1</GroupNumber>
|
||||
<FileNumber>1</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>1</tvExp>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>..\..\..\main.c</PathWithFileName>
|
||||
|
||||
Reference in New Issue
Block a user