mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-10-06 09:02:52 +08:00
feat(drivers/imu/st/lsm6dsv): LSM6DSV 80X and 320X (#27625)
* drivers: LSM6DSV 80X and 320X * fix(drivers/lsm6dsv): flip y/z axes to match PX4 driver convention The ST sensor frame is right handed with z up. Flip y and z in the driver so it publishes x forward, y right, z down like the InvenSense drivers, and board rotations only describe physical mounting. Update FMU-v6c accordingly: -R 26 (PITCH_180_YAW_90) becomes -R 6 (YAW_270), now matching the ICM42688P sharing the same footprint. Signed-off-by: alexklimaj <alex@arkelectron.com> * fix(drivers/lsm6dsv): don't flush FIFO on high-g full-scale change Flushing discarded batched gyro and low-g samples on every range step, causing a data gap at exactly the moments the high-g channel matters. Only the high-g channel's scale changes, so instead skip publishing and clip-evaluating high-g samples for one read cycle while the words captured at the previous full-scale drain naturally. Signed-off-by: alexklimaj <alex@arkelectron.com> * fix(drivers/lsm6dsv): publish high-g samples captured before a full-scale change Instead of skipping the high-g channel for a cycle after a range step, normalize samples captured at the previous full-scale to the current one (counts scaled by the exact sensitivity ratio) and publish them. Samples are classified by reconstructed capture time since the batch drained after a change mixes old- and new-scale words. This keeps the high-g data flowing through escalations, which happen mid-impact when that data matters most. Signed-off-by: alexklimaj <alex@arkelectron.com> * fix(drivers/lsm6dsv): retry WHO_AM_I read in probe The first SPI transaction after power-up can return garbage (reads 0xFF) while the device's shared I2C/I3C/SPI interface latches onto SPI mode on the first CS falling edge. The single-shot probe read made startup fail until bus traffic preceded it; retry up to 3 times like the InvenSense drivers do. Signed-off-by: alexklimaj <alex@arkelectron.com> * fix(drivers/lsm6dsv): set full-scale before enabling the ODRs Both datasheets footnote CTRL6 that FS_G must be set while the gyro is in power-down, and the reset default FS_G=000 is reserved on the 80X, 320X and DSK320X. Configure() writes _register_cfg in array order, and that order put CTRL1/CTRL2 ahead of CTRL6/CTRL8 — so the gyro powered up at full ODR on a reserved full-scale, and FS_G was then changed while it was running. Move every full-scale, filter and FIFO register ahead of CTRL1/CTRL2, which set the ODRs and are what actually power the sensors up. * fix(drivers/lsm6dsv): drain the FIFO in a single burst read FIFORead() issued one 8-byte SPI transaction per 7-byte FIFO word. At the 7.68 kHz the high-g variants run, that is ~23k transactions/s, and each one pays a bus lock, a CS toggle and a full SPI reconfigure — roughly 0.5-1 ms of bus time per 2.5 ms drain. AN5763 / AN6119 section 9.8 document reading DIFF_FIFO words as a single (N * 7)-byte operation: with IF_INC set the address rounds from FIFO_DATA_OUT_Z_H back to FIFO_DATA_OUT_TAG at every word boundary. Read the whole drain into one buffer and dispatch by tag out of it, which is also the shape every InvenSense driver in the tree already uses. Also correct the FIFO::DEPTH comment: 512 is the ceiling of the DIFF_FIFO counter, not the buffer size (1.5 KB, ~219 uncompressed words). * fix(drivers/lsm6dsv): pin the high-g channel at its top full-scale The high-g full-scale escalated 32 -> 64 -> 80 g on high-g clipping and de-escalated after 2 s quiet, which in turn needed a rescale path to normalize samples still batched at the previous sensitivity. None of it earns its keep. The high-g channel is only ever published while the low-g channel clips, i.e. above 16 g. All the ladder buys there is resolution — 0.976 vs 3.904 mg/LSB, 0.0096 vs 0.038 m/s² — while the fallback engages with the high-g channel's ±1.5 g typ zero-g offset uncalibrated, a 15 m/s² bias step. And it climbs one step per FIFO drain (2.5 ms at the default IMU_GYRO_RATEMAX), so it needs 5-7.5 ms to reach ±80 g and lands after an impact's peak has passed — precisely the sample it exists to capture. Pin each variant at its top range instead: ±80 g on the 80X, ±320 g on the 320X. The first peak is caught unclipped, and ManageHighGFullScale(), ApplyHighGFullScale(), RescaleCount(), the stale-sample normalization and the high-g clip evaluation all go with it. Two latent bugs go away rather than needing fixes. SampleClips()'s int16 threshold (~23070 counts) could never fire at ±80 g, which saturates around 20492 (3.904 mg/LSB), so sustained >64 g would have oscillated 64<->80 g; it now only sees the low-g channel, whose ±16 g / 0.488 mg/LSB scaling does reach the rail. And escalation could drive an 80X started with -T 320 into FS codes 011/100, which are reserved on that part — nothing writes them now. * fix(drivers/lsm6dsv): drop unused ACCEL_ODR_HIGHG The high-g variants' accel ODR is never referenced by name — CTRL1, CTRL2 and the FIFO BDR all take the register code, and the sample timing is derived from GYRO_ODR_HIGHG. * fix(drivers/lsm6dsv): publish the high-g channel directly on the 80X / 320X The accelerometer channel was chosen per FIFO batch: publish low-g normally, switch to high-g for any batch where a low-g sample passed a clip threshold. A sensor_accel_fifo message carries a single scale factor, so that meant restating the scale of samples that had already been taken, and the decision had no hysteresis: sustained vibration near the threshold flips the scale batch to batch at the full publish rate, stepping the high-g channel's uncalibrated +/-1.5 g typ zero-g offset in and out of the estimator each time. On a part picked for its high-g range the low-g channel's finer resolution is not worth any of that, so publish the high-g channel and nothing else, with the scale fixed in Configure() for the lifetime of the driver — the same fixed-scale approach the ICM45686 moved to. The low-g channel stays enabled and batched; its FIFO words are simply not consumed. Dropping BDR_XL would save a third of the FIFO traffic but is only safe once XL_HG_BATCH_EN is confirmed to batch independently of it. Assisted-by: Claude:claude-opus-5 Signed-off-by: Jacob Dahl <dahl.jakejacob@gmail.com> * fix(drivers/lsm6dsv): drain whole sample periods only The transfer buffer was sized to hold a partial trailing period on top of FIFO_MAX_SAMPLES full ones, so a maximal drain yielded 33 gyro and 33 accel words into 32-slot messages. The per-channel bound checks then dropped the newest sample of each without a trace, leaving timestamp_sample one dt optimistic for that batch. Hand FIFORead whole periods instead and leave any period still being batched in the FIFO for the next cycle. The message capacity is now a static_assert rather than a runtime check, and the remaining bound is a real error path: it can only trip if the tag stream stops matching the configured batching, which is corruption, not a backlog. Assisted-by: Claude:claude-opus-5 Signed-off-by: Jacob Dahl <dahl.jakejacob@gmail.com> * refactor(drivers/lsm6dsv): drop the periodic register check Re-reading configuration registers in the run loop and resetting the sensor when one disagrees is nondeterministic behaviour guarding against something that does not happen: nothing in the FIFO read path writes to a register address, so a configured register does not spontaneously change. It only adds register traffic to the cycle and gives a transient SPI error a path to reset a healthy sensor. The 1 Hz temperature update was in the else arm of that check, so it only ran on cycles the check skipped; it now runs on its own timer. Configure() still verifies every register once after writing it, which is what makes the CONFIGURE retry work. Assisted-by: Claude:claude-opus-5 Signed-off-by: Jacob Dahl <dahl.jakejacob@gmail.com> * refactor(drivers/lsm6dsv): hold the FIFO transfer buffer as a member The transfer buffer was a stack local, zero-initialised on every FIFO read in wq:SPIx. Nothing reads past transfer_size, so the clear was never protecting against stale data; only the command byte has to be restored, because transfer() overwrites it with the byte clocked in alongside. Also drop the FIFO temperature tag handling. FIFO_CTRL4 leaves ODR_T_BATCH at 0, so no temperature word is ever batched and the branch was unreachable; UpdateTemperature() is what actually reports temperature. Assisted-by: Claude:claude-opus-5 Signed-off-by: Jacob Dahl <dahl.jakejacob@gmail.com> * perf(drivers/lsm6dsv): batch only the high-g channel on the 80X / 320X The low-g channel was still batched into the FIFO alongside the high-g one and its words simply discarded, so every sample period cost three FIFO words and three words of SPI traffic for two that were used. The high-g channel batches at its own ODR under XL_HG_BATCH_EN, independent of BDR_XL, so leave the low-g channel unbatched on those parts. One third less FIFO fill and SPI3 traffic at the same publish rate. * docs(drivers/lsm6dsv): correct the high-g clip detection comment The comment claimed PX4Accelerometer detects this channel's saturation from range/scale via UpdateClipLimit(). That limit only serves the non-FIFO update() path; updateFIFO() flags clipping off the int16 rail, which the ±80 g channel never reaches. State that the saturation goes unreported rather than the opposite. * fix(drivers/lsm6dsv): publish the low-g ±16 g channel on the 80X / 320X The high-g accelerometer is a sports-impact element: ±1.5 g typical zero-g offset, ±2 mg/°C tempco and 1000 µg/√Hz, against ±12 mg, ±0.07 mg/°C and 60 µg/√Hz for the low-g one in the same package. Pinning it at a narrower full-scale does not change the tempco. As the published accel it reads 0.4-0.6 m/s² low uncalibrated and drifts through a calibration with die temperature, which is the wrong property for a backup flight IMU. Batch the low-g channel at the same 7.68 kHz and leave the high-g accelerometer powered down: XL_HG_BATCH_EN clear, CTRL1_XL_HG at reset, no ACCEL_HG words. Scale is 0.488 mg/LSB for every variant; the gyro stays ±4000 dps. --------- Signed-off-by: alexklimaj <alex@arkelectron.com> Signed-off-by: Jacob Dahl <dahl.jakejacob@gmail.com> Co-authored-by: Jacob Dahl <dahl.jakejacob@gmail.com>
This commit is contained in:
Alex Klimaj
co-authored by
Jacob Dahl
Jacob Dahl
parent
7135e1561f
commit
29dea325db
@@ -18,7 +18,7 @@ fi
|
||||
# Internal SPI bus IMU (probe-based: LSM6DSV or ICM42688P on same CS)
|
||||
if ! icm42688p -R 6 -s -q start
|
||||
then
|
||||
lsm6dsv -R 26 -s start
|
||||
lsm6dsv -R 6 -s start
|
||||
fi
|
||||
|
||||
# Internal barometer on I2C4 (The same bus is also exposed externally, and therefore marked as external)
|
||||
|
||||
@@ -78,6 +78,8 @@
|
||||
#define DRV_FLOW_DEVTYPE_SIM 0x16
|
||||
|
||||
#define DRV_IMU_DEVTYPE_ST_LSM6DSK320X 0x17
|
||||
#define DRV_IMU_DEVTYPE_ST_LSM6DSV80X 0x18
|
||||
#define DRV_IMU_DEVTYPE_ST_LSM6DSV320X 0x19
|
||||
|
||||
#define DRV_IMU_DEVTYPE_MPU6000 0x21
|
||||
#define DRV_GYR_DEVTYPE_L3GD20 0x22
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -70,11 +70,46 @@ private:
|
||||
void exit_and_cleanup() override;
|
||||
|
||||
// Sensor Configuration
|
||||
static constexpr float FIFO_SAMPLE_DT{1e6f / GYRO_ODR};
|
||||
static constexpr float GYRO_RATE{static_cast<float>(GYRO_ODR)};
|
||||
static constexpr float ACCEL_RATE{static_cast<float>(ACCEL_ODR)};
|
||||
|
||||
static constexpr int32_t FIFO_MAX_SAMPLES{static_cast<int32_t>(FIFO::MAX_DRAIN_SAMPLES)};
|
||||
static_assert(FIFO_MAX_SAMPLES <= (int32_t)(sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])),
|
||||
"FIFO drain exceeds sensor_gyro_fifo capacity");
|
||||
static_assert(FIFO_MAX_SAMPLES <= (int32_t)(sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0])),
|
||||
"FIFO drain exceeds sensor_accel_fifo capacity");
|
||||
|
||||
// A FIFO word is a tag byte plus 6 data bytes. With IF_INC set the address rounds from
|
||||
// FIFO_DATA_OUT_Z_H back to FIFO_DATA_OUT_TAG at every word boundary, so the whole FIFO drains
|
||||
// as a single N*7 byte burst (AN5763 / AN6119 section 9.8).
|
||||
struct FIFOWord {
|
||||
uint8_t TAG;
|
||||
uint8_t DATA_X_L;
|
||||
uint8_t DATA_X_H;
|
||||
uint8_t DATA_Y_L;
|
||||
uint8_t DATA_Y_H;
|
||||
uint8_t DATA_Z_L;
|
||||
uint8_t DATA_Z_H;
|
||||
};
|
||||
static_assert(sizeof(FIFOWord) == FIFO::WORD_SIZE, "FIFO word must be 7 bytes");
|
||||
|
||||
// RunImpl() drains whole sample periods only, at most FIFO_MAX_SAMPLES of them
|
||||
static constexpr uint16_t FIFO_MAX_WORDS{static_cast<uint16_t>(FIFO_MAX_SAMPLES * FIFO::MAX_WORDS_PER_PERIOD)};
|
||||
|
||||
struct FIFOTransferBuffer {
|
||||
uint8_t cmd{static_cast<uint8_t>(Register::FIFO_DATA_OUT_TAG) | DIR_READ};
|
||||
FIFOWord words[FIFO_MAX_WORDS] {};
|
||||
};
|
||||
static_assert(sizeof(FIFOTransferBuffer) == (1 + FIFO_MAX_WORDS * FIFO::WORD_SIZE), "Invalid transfer buffer size");
|
||||
|
||||
// held here rather than on the work queue stack: a wq:SPIx frame is not the place for a buffer
|
||||
// this size, and reusing it avoids re-zeroing memory that transfer() overwrites anyway
|
||||
FIFOTransferBuffer _fifo_buffer{};
|
||||
|
||||
// Sensor ODR is variant-dependent (set in UpdateVariantRegisterConfig()):
|
||||
// default (16X/32X/DSK320X): 2000 Hz
|
||||
// LSM6DSV80X / 320X: 7680 Hz
|
||||
// Every variant batches 2 FIFO words per period: gyro + low-g accel.
|
||||
uint32_t _sensor_odr{GYRO_ODR};
|
||||
float _fifo_sample_dt{1e6f / GYRO_ODR};
|
||||
uint8_t _fifo_words_per_period{2};
|
||||
|
||||
struct register_config_t {
|
||||
Register reg;
|
||||
@@ -95,7 +130,7 @@ private:
|
||||
void RegisterWrite(Register reg, uint8_t value);
|
||||
void RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits);
|
||||
|
||||
bool FIFORead(const hrt_abstime ×tamp_sample, uint16_t samples);
|
||||
bool FIFORead(const hrt_abstime ×tamp_sample, uint16_t words);
|
||||
void FIFOReset();
|
||||
|
||||
void UpdateTemperature();
|
||||
@@ -111,7 +146,6 @@ private:
|
||||
PX4Accelerometer _px4_accel;
|
||||
PX4Gyroscope _px4_gyro;
|
||||
|
||||
perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")};
|
||||
perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad transfer")};
|
||||
perf_counter_t _fifo_empty_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO empty")};
|
||||
perf_counter_t _fifo_overflow_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO overflow")};
|
||||
@@ -119,7 +153,6 @@ private:
|
||||
perf_counter_t _drdy_missed_perf{nullptr};
|
||||
|
||||
hrt_abstime _reset_timestamp{0};
|
||||
hrt_abstime _last_config_check_timestamp{0};
|
||||
hrt_abstime _temperature_update_timestamp{0};
|
||||
int _failure_count{0};
|
||||
|
||||
@@ -138,30 +171,50 @@ private:
|
||||
LSM6DSV16X,
|
||||
LSM6DSV32X,
|
||||
LSM6DSK320X,
|
||||
LSM6DSV80X,
|
||||
LSM6DSV320X,
|
||||
};
|
||||
DeviceVariant _device_variant{DeviceVariant::LSM6DSV16X};
|
||||
|
||||
// The LSM6DSV80X and LSM6DSV320X share WHO_AM_I 0x73 and cannot be distinguished over SPI, so
|
||||
// the physically-installed part is selected explicitly at start via the -T argument (config.custom1):
|
||||
// 320 -> LSM6DSV320X, anything else (incl. 80 / unset) -> LSM6DSV80X.
|
||||
const int _highg_variant_arg;
|
||||
|
||||
// LSM6DSV80X / LSM6DSV320X: 7.68 kHz HAODR set and a ±4000 dps gyro. Their second, high-g
|
||||
// accelerometer is left powered down: the ±16 g low-g channel is the published one on every
|
||||
// variant. The high-g element is a sports-impact sensor (±1.5 g typ zero-g offset, ±2 mg/°C
|
||||
// tempco against ±12 mg / ±0.07 mg/°C for low-g) and is no use as a flight accelerometer.
|
||||
bool _dsv80x_family{false};
|
||||
|
||||
uint16_t _fifo_empty_interval_us{500}; // default 500 us / 2000 Hz
|
||||
int32_t _fifo_gyro_samples{static_cast<int32_t>(_fifo_empty_interval_us / (1000000 / GYRO_ODR))};
|
||||
|
||||
uint8_t _checked_register{0};
|
||||
static constexpr uint8_t size_register_cfg{12};
|
||||
// Variant-dependent fields (HAODR_CFG, CTRL1/2/6/8, FIFO_CTRL3) are overwritten in
|
||||
// UpdateVariantRegisterConfig(); initializers below are the default-variant (2000 Hz) values.
|
||||
//
|
||||
// Configure() writes these in array order, and the order is significant: FS_G (CTRL6) must be
|
||||
// set while the gyro is in power-down, and the reset default FS_G=000 is reserved on the 80X /
|
||||
// 320X / DSK320X. So every full-scale, filter and FIFO register comes first, and CTRL1/CTRL2 —
|
||||
// which set the ODRs and thereby power the sensors up — come last. HAODR_CFG selects the ODR
|
||||
// set that the CTRL1/CTRL2 codes index into, so it must also precede them.
|
||||
register_config_t _register_cfg[size_register_cfg] {
|
||||
// Register | Set bits | Clear bits
|
||||
{ Register::CTRL3, CTRL3_BIT::BDU | CTRL3_BIT::IF_INC, CTRL3_BIT::SW_RESET },
|
||||
{ Register::HAODR_CFG, HAODR_CFG_BIT::HAODR_MODE1, 0 },
|
||||
{ Register::CTRL1, HAODR_MODE1_ODR_2000HZ | CTRL1_BIT::CTRL1_MODE_HAODR, 0 },
|
||||
{ Register::CTRL2, HAODR_MODE1_ODR_2000HZ | CTRL2_BIT::CTRL2_MODE_HAODR, 0 },
|
||||
{ Register::CTRL6, CTRL6_BIT::FS_G_2000DPS, 0 },
|
||||
{ Register::CTRL8, CTRL8_BIT::FS_XL_16G | CTRL8_BIT::LPF2_BW_ODR_DIV_10, 0 },
|
||||
{ Register::CTRL9, CTRL9_BIT::LPF2_XL_EN, 0 },
|
||||
{ Register::CTRL4, CTRL4_BIT::DRDY_PULSED, 0 },
|
||||
{ Register::INT1_CTRL, INT1_CTRL_BIT::INT1_FIFO_TH, 0 },
|
||||
{ Register::FIFO_CTRL1, 0, 0 }, // WTM[7:0] set at runtime by ConfigureFIFOWatermark()
|
||||
{
|
||||
Register::FIFO_CTRL3, static_cast<uint8_t>(FIFO_CTRL3_BIT::BDR_GY_HAODR) |
|
||||
static_cast<uint8_t>(FIFO_CTRL3_BIT::BDR_XL_HAODR), 0
|
||||
},
|
||||
{ Register::FIFO_CTRL4, FIFO_CTRL4_BIT::FIFO_MODE_CONTINUOUS, 0 },
|
||||
{ Register::INT1_CTRL, INT1_CTRL_BIT::INT1_FIFO_TH, 0 },
|
||||
{ Register::CTRL4, CTRL4_BIT::DRDY_PULSED, 0 },
|
||||
{ Register::FIFO_CTRL1, 0, 0 }, // WTM[7:0] set at runtime by ConfigureFIFOWatermark()
|
||||
{ Register::CTRL1, HAODR_MODE1_ODR_2000HZ | CTRL1_BIT::CTRL1_MODE_HAODR, 0 },
|
||||
{ Register::CTRL2, HAODR_MODE1_ODR_2000HZ | CTRL2_BIT::CTRL2_MODE_HAODR, 0 },
|
||||
};
|
||||
};
|
||||
|
||||
@@ -61,14 +61,23 @@ static constexpr uint8_t DIR_READ = 0x80;
|
||||
|
||||
static constexpr uint8_t WHO_AM_I_ID = 0x70; // LSM6DSV16X and LSM6DSV32X (same ID)
|
||||
static constexpr uint8_t WHO_AM_I_DSK320X = 0x75; // LSM6DSK320X (unique ID)
|
||||
// The LSM6DSV80X and LSM6DSV320X share this ID and have an identical register map; they differ
|
||||
// only in their high-g channel's full-scale table and cannot be distinguished over SPI (no part-ID
|
||||
// register; only the MIPI I3C Provisioned ID differs, which is not SPI-accessible). The variant is
|
||||
// therefore selected explicitly at driver start (-T 80 | -T 320) so the device type is right.
|
||||
static constexpr uint8_t WHO_AM_I_HIGHG = 0x73; // LSM6DSV80X / LSM6DSV320X (shared ID)
|
||||
|
||||
// HAODR mode-1 ODR: 2000 Hz
|
||||
// Default-variant (16X / 32X / DSK320X) ODR: HAODR_SEL=01, code 0x0A = 2000 Hz
|
||||
static constexpr uint32_t GYRO_ODR = 2000;
|
||||
static constexpr uint32_t ACCEL_ODR = 2000;
|
||||
|
||||
// HAODR mode-1 ODR codes (written to CTRL1/CTRL2 [3:0])
|
||||
// HAODR mode-1 (HAODR_SEL=01) ODR code (written to CTRL1/CTRL2 [3:0] and FIFO BDR [3:0])
|
||||
static constexpr uint8_t HAODR_MODE1_ODR_2000HZ = 0x0A;
|
||||
|
||||
// LSM6DSV80X / LSM6DSV320X ODR: HAODR_SEL=00, code 0x0C = 7680 Hz (device max)
|
||||
static constexpr uint32_t ODR_DSV80X = 7680;
|
||||
static constexpr uint8_t HAODR_SEL0_ODR_7680HZ = 0x0C;
|
||||
|
||||
enum class Register : uint8_t {
|
||||
IF_CFG = 0x03, // Interrupt polarity and output mode
|
||||
|
||||
@@ -145,18 +154,19 @@ enum CTRL4_BIT : uint8_t {
|
||||
|
||||
// CTRL6 — Gyroscope full-scale
|
||||
enum CTRL6_BIT : uint8_t {
|
||||
// FS_G [3:0]
|
||||
FS_G_2000DPS = 0x04, // ±2000 dps (16X / 32X)
|
||||
// FS_G [2:0]
|
||||
FS_G_2000DPS = 0x04, // ±2000 dps (16X / 32X)
|
||||
FS_G_2000DPS_DSK320X = 0x0C, // ±2000 dps with bit3=1 for DSK320X (0x04 | Bit3)
|
||||
FS_G_4000DPS_HIGHG = 0x0D, // ±4000 dps for 80X/320X (FS_G=101 | bit3); CTRL6 bit3 must be 1
|
||||
};
|
||||
|
||||
// CTRL8 — Accelerometer full-scale + LPF2 bandwidth
|
||||
enum CTRL8_BIT : uint8_t {
|
||||
// bit2 is hardware-reserved and differs by variant:
|
||||
// 16X / DSK320X = 0 (value OR'd with FS_XL)
|
||||
// 16X / DSK320X / 80X = 0 (value OR'd with FS_XL)
|
||||
// 32X = 1 (must be preserved)
|
||||
// FS_XL [1:0] in bits [1:0]
|
||||
FS_XL_16G = 0x03, // ±16 g for 16X / DSK320X (bit2=0)
|
||||
FS_XL_16G = 0x03, // ±16 g for 16X / DSK320X / 80X (bit2=0)
|
||||
FS_XL_16G_DSV32X = 0x06, // ±16 g for 32X (bit2=1, FS_XL=10)
|
||||
|
||||
// HP_LPF2_XL_BW [2:0] in bits [7:5] — when LPF2 enabled via CTRL9
|
||||
@@ -178,9 +188,12 @@ enum STATUS_REG_BIT : uint8_t {
|
||||
// FIFO_CTRL3 — Batch Data Rate for accel and gyro
|
||||
enum FIFO_CTRL3_BIT : uint8_t {
|
||||
// BDR_GY [3:0] in bits [7:4], BDR_XL [3:0] in bits [3:0]
|
||||
// Set both to HAODR mode-1 code = 0x0A
|
||||
// Default variants: HAODR mode-1 code 0x0A (2000 Hz)
|
||||
BDR_XL_HAODR = HAODR_MODE1_ODR_2000HZ,
|
||||
BDR_GY_HAODR = HAODR_MODE1_ODR_2000HZ << 4,
|
||||
// LSM6DSV80X / 320X: code 0x0C (7680 Hz)
|
||||
BDR_XL_7680 = HAODR_SEL0_ODR_7680HZ,
|
||||
BDR_GY_7680 = HAODR_SEL0_ODR_7680HZ << 4,
|
||||
};
|
||||
|
||||
// FIFO_CTRL4 — FIFO mode
|
||||
@@ -199,13 +212,15 @@ enum FIFO_STATUS2_BIT : uint8_t {
|
||||
|
||||
// HAODR_CFG
|
||||
enum HAODR_CFG_BIT : uint8_t {
|
||||
HAODR_MODE1 = 0x01, // Enable HAODR mode-1
|
||||
HAODR_SEL_MASK = 0x03, // HAODR_SEL [1:0]
|
||||
HAODR_MODE1 = 0x01, // HAODR_SEL=01 (2000 Hz ODR set)
|
||||
// HAODR_SEL=00 (1920/3840/7680 Hz ODR set) is the default (0x00)
|
||||
};
|
||||
|
||||
// FIFO tag IDs (upper 5 bits of FIFO_DATA_OUT_TAG >> 3)
|
||||
enum class FifoTag : uint8_t {
|
||||
GYRO_NC = 0x01,
|
||||
ACCEL_NC = 0x02,
|
||||
ACCEL_NC = 0x02, // low-g accelerometer (the only one batched; the 80X / 320X high-g channel stays off)
|
||||
TEMPERATURE = 0x03,
|
||||
TIMESTAMP = 0x04,
|
||||
};
|
||||
@@ -214,9 +229,11 @@ namespace FIFO
|
||||
{
|
||||
// FIFO word: 1-byte tag + 6-byte data = 7 bytes
|
||||
static constexpr size_t WORD_SIZE = 7;
|
||||
// Max samples to drain per poll (avoid blocking scheduler)
|
||||
// Words batched per sample period: gyro + low-g accel
|
||||
static constexpr size_t MAX_WORDS_PER_PERIOD = 2;
|
||||
// Max sample periods to drain per poll (avoid blocking scheduler)
|
||||
static constexpr size_t MAX_DRAIN_SAMPLES = 32;
|
||||
// FIFO depth: 512 words max on LSM6DSV
|
||||
// Ceiling of the DIFF_FIFO word counter. The buffer itself is 1.5 KB, i.e. ~219 uncompressed words.
|
||||
static constexpr size_t DEPTH = 512;
|
||||
}
|
||||
|
||||
|
||||
@@ -43,6 +43,7 @@ void LSM6DSV::print_usage()
|
||||
PRINT_MODULE_USAGE_COMMAND("start");
|
||||
PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true);
|
||||
PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true);
|
||||
PRINT_MODULE_USAGE_PARAM_INT('T', 0, 0, 320, "High-g variant for WHO_AM_I 0x73 (80 = LSM6DSV80X, 320 = LSM6DSV320X)", true);
|
||||
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
|
||||
}
|
||||
|
||||
@@ -53,11 +54,17 @@ extern "C" int lsm6dsv_main(int argc, char *argv[])
|
||||
BusCLIArguments cli{false, true};
|
||||
cli.default_spi_frequency = SPI_SPEED;
|
||||
|
||||
while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) {
|
||||
while ((ch = cli.getOpt(argc, argv, "R:T:")) != EOF) {
|
||||
switch (ch) {
|
||||
case 'R':
|
||||
cli.rotation = (enum Rotation)atoi(cli.optArg());
|
||||
break;
|
||||
|
||||
case 'T':
|
||||
// High-g variant selector for the shared WHO_AM_I 0x73 (80 = LSM6DSV80X, 320 = LSM6DSV320X).
|
||||
// Set per board in rc.sensors to match the physically-installed part.
|
||||
cli.custom1 = atoi(cli.optArg());
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user