IMU | LSM6DSV320X SPI DMA Driver - #28
Conversation
|
Ah sorry I just saw this! I've pinged myself as a reviewer and will take a look when I get a minute. Changing your target branch to reflect our move to an unstable trunk system. |
|
Also, I've added @266-750Balloons as this introduces DMA, which I'm less familiar with than CPU-bound interrupts. |
ETSells
left a comment
ETSells
left a comment
There was a problem hiding this comment.
dude this is actually crazy...
lgtm honestly, ignore my CR comments below, I'd take this as-is...
| * @param len: Number of data bytes (address byte excluded). | ||
| * @retval 0 on success, 1 on HAL error or oversized request. | ||
| */ | ||
| static int32_t platform_spi_write |
There was a problem hiding this comment.
cr: Please use a status type in your final version (you can reuse the old IMU status enum if you want, else if this stays separate you'll need a new one.) <-- ignore; all external interfaces follow the status return convention which is good enough for me
| memcpy( &tx[1], buf, len ); | ||
|
|
||
| HAL_GPIO_WritePin( IMU_CS_GPIO_PORT, IMU_CS_PIN, GPIO_PIN_RESET ); | ||
| __NOP(); __NOP(); |
There was a problem hiding this comment.
Interesting that these are needed here -- we've never had to do NOPs since the procedure call overhead is usually sufficient delay between writing CS low.
|
|
||
| (void)handle; | ||
|
|
||
| if ( len >= SPI_WRITE_BUF_MAX ) { |
There was a problem hiding this comment.
nit: max implies that the upper bound is inclusive.
| __NOP(); __NOP(); | ||
| HAL_GPIO_WritePin( IMU_CS_GPIO_PORT, IMU_CS_PIN, GPIO_PIN_SET ); | ||
|
|
||
| return ( hal_status == HAL_OK ) ? 0 : 1; |
There was a problem hiding this comment.
nit: we don't usually like inline logic like this since its harder to parse through
| uint16_t len | ||
| ) | ||
| { | ||
| #define SPI_WRITE_BUF_MAX ( 64U ) |
There was a problem hiding this comment.
Can we define this scoped to the header file for visibility? Either that or I guess leave like an implNote in your doxygen header.
| Initializations | ||
| --------------------------------------------------------------------------*/ | ||
| imu_sflp_enabled = false; | ||
| imu_dma_ready = false; |
There was a problem hiding this comment.
doesn't look like we have DMA r/w callbacks available but i'm glad we've started laying the groundwork for them!
| xl_bdr = LSM6DSV320X_XL_BATCHED_AT_7680Hz; | ||
| g_bdr = LSM6DSV320X_GY_BATCHED_AT_7680Hz; | ||
| break; | ||
| case IMU_ODR_1920HZ: /* fall-through to default */ |
There was a problem hiding this comment.
for defensive programming, consider using a debug assert here that ODR == 1920, that way in debug mode we catch that an improper param was passed.
the construct was just added in mod/error_sdr/error_sdr.h:debug_assert
| pid_status |= lsm6dsv320x_gy_data_rate_set( &imu_ctx, g_odr ); | ||
|
|
||
| /* | ||
| * Full-scale and ODR configuration - two separate accelerometer chains. |
There was a problem hiding this comment.
just a heads up that in the future we'll want this to be runtime configurable! i'll probably ask you to write the config api for that when we're ready, but we'd want high-G mode during boost and switch to low-G in coast and beyond.
| * high-G to low-G: clears XL_HG_REGOUT_EN, restores low-G batching. | ||
| * DS14623 Rev3 §6.1.2, Tables 70, 149, 150; COUNTER_BDR_REG1 (0Bh). | ||
| */ | ||
| IMU_STATUS imu_set_accel_fs |
There was a problem hiding this comment.
oh dang, ignore me earlier, you already did it!!
| /* Assert CS and launch DMA - CS de-asserted in imu_process_async_cb() */ | ||
| HAL_GPIO_WritePin( IMU_CS_GPIO_PORT, IMU_CS_PIN, GPIO_PIN_RESET ); | ||
| __NOP(); __NOP(); | ||
| hal_status = HAL_SPI_TransmitReceive_DMA( &IMU_SPI, |
There was a problem hiding this comment.
ohhhh dang! you've got the async interface too?!
|
Also -- because of the divergence between this version and the old IMU, we should consider keeping these separate with maybe a guard at the top of imu.h that points to imu_t.h if we're not using the legacy IMU. Don't worry about merging to the old platform unless you feel its the smarter design choice. Because this is kept separate from the old IMU, we can merge as-is once the PID driver is imported somewhere (either driver or lib) |
547fdbc to
7682028
Compare
![github-advanced-security[bot]](https://avatars.githubusercontent.com/in/57789?s=60&v=4){kind=small}
MasterUser43
changed the title
|
This is in my review queue, but it might take me a second to get there, just as a heads up. |
ETSells
left a comment
ETSells
left a comment
There was a problem hiding this comment.
Significant build errors present. Please resolve these before requesting re-review.
I have added this driver to the build system via your FCF child PR. Please pull that up and test by invoking
cd app/rev3
make -jX # X = number of parallel processes you want for compilation
Build error log attached: build_err_log.txt
| * https://opensource.org/license/bsd-3-clause | ||
| * | ||
| *******************************************************************************/ | ||
| #if !defined( A0010 ) |
There was a problem hiding this comment.
Consider:
#ifndef( A0002_REV2 )
#error "Unsupported hardware platform
#endif
rather than wrapping the whole thing in a preprocessor directive.
| } IMU_RAW; | ||
|
|
||
| /* Processed IMU aata */ | ||
| typedef struct _STATE_ESTIMATION { |
There was a problem hiding this comment.
note for myself: beware of conflicts
There was a problem hiding this comment.
Assumption: I'm assuming this has been copied over with no functional changes from the previous iteration. If I'm wrong, then please fix.
There was a problem hiding this comment.
Assumption: I'm assuming this has been copied over with no functional changes from the previous iteration. If I'm wrong, then please fix.
…nt/rename changes
f64060f to
41ed939
Compare
|
Still have debug_assert to fix + maybe do more tinkering to avoid deprecated silly warnings. Will verify if effort is worth it or not (of warnings) |
|
One simple note: (1) When eventually config for rev3 is worked on: void HAL_SPI_TxRxCpltCallback(SPI_HandleTypeDef *hspi) {
if (hspi == &(IMU_SPI)) {
imu_process_async_cb();
}
}
void HAL_SPI_ErrorCallback(SPI_HandleTypeDef *hspi) {
if (hspi == &(IMU_SPI)) {
imu_process_async_error_cb();
}
} |
NArmistead
left a comment
NArmistead
left a comment
There was a problem hiding this comment.
Sorry I don't have time to fully review everything (mostly didn't get to imu_lsm.c) but I want to get this out before I'm gone for the rest of the week.
| * @brief Cached power mode most recently requested via imu_init() / | ||
| * imu_set_power_mode(). DS14623 Rev3 §6.1.2 forces the low-G chain | ||
| * into high-performance mode whenever the high-G chain is active; | ||
| * imu_set_accel_fs() uses this cache to restore the user's actual | ||
| * requested mode after a High-G -> Low-G transition removes that | ||
| * constraint, instead of leaving the sensor stuck in HP mode. |
There was a problem hiding this comment.
nit: arguably not very brief. consider moving everything but the first sentence to a new paragraph
There was a problem hiding this comment.
Not a yap enjoy-er I see
| typedef struct { | ||
| float accel[3]; /* g */ | ||
| float gyro[3]; /* dps */ | ||
| float quaternion[4]; /* [w, x, y, z], unit quat */ |
There was a problem hiding this comment.
This can be replaced by the QUAT struct once the new state estimation changes are merged. It'll be the exact same in memory.
The other issue is that on legacy this quaternion is part of the state estimation struct and is calculated from IMU values in the sensor system. It's meant to be less coupled to the IMU driver directly because the IMU is only giving accel and gyro.
There was a problem hiding this comment.
This guy I assume?
There was a problem hiding this comment.
Yep, exactly. Nick and I need to cross-review each other's changes, and I also need our software lead to approve my dashboard PR before these all get merged.
| * @brief FIFO watermark threshold reference (programmed in FIFO slots/words). | ||
| * 1 LSB = 1 FIFO slot (7 bytes: 1-byte TAG + 6-byte data). DS14623 §9.8. | ||
| * | ||
| * NOTE: This macro is a reference value; imu_init() configures the |
There was a problem hiding this comment.
nit: you can use @note here
| * The FIFO accumulates exactly one synchronized dataset (5 slots) per interval. | ||
| * | ||
| * - Mixed Rates (e.g., 1.92 kHz ODR / 480 Hz SFLP): | ||
| * The FIFO acts as an asynchronous reservoir. Raw data slots populate faster |
There was a problem hiding this comment.
Do we get noticeably better accuracy at a super high ODR? 480 Hz is already pretty fast and its probably higher than our frame rate (I don't really know on rev3) so we won't have any issues like what we have with baro on rev2. I think this setting would be continuous-to-FIFO mode but I don't see where we would want to change the FIFO mode
| gyro_y_raw = ( (uint16_t) regGyro[0] ) << 8 | regGyro[1]; | ||
| gyro_z_raw = ( (uint16_t) regGyro[0] ) << 8 | regGyro[1]; | ||
| gyro_y_raw = ( (uint16_t) regGyro[2] ) << 8 | regGyro[3]; | ||
| gyro_z_raw = ( (uint16_t) regGyro[4] ) << 8 | regGyro[5]; |
There was a problem hiding this comment.
You might be able to just remove this because it's rev1 code (@ETSells ?)
There was a problem hiding this comment.
Should I nuke all rev1 code from legacy then?
There was a problem hiding this comment.
yeah feel free to yeet. we'll be testing the rev 2 path for this prior to integration/merging, so go ahead. if it breaks smth ill lyk
| IMU_GYRO_FS_500DPS = LSM6DSV320X_500dps, /* ±500 dps, 17.50 mdps/LSB */ | ||
| IMU_GYRO_FS_1000DPS = LSM6DSV320X_1000dps, /* ±1000 dps, 35.0 mdps/LSB */ | ||
| IMU_GYRO_FS_2000DPS = LSM6DSV320X_2000dps, /* ±2000 dps, 70.0 mdps/LSB */ | ||
| IMU_GYRO_FS_4000DPS = LSM6DSV320X_4000dps, /* ±4000 dps, 140.0 mdps/LSB */ |
There was a problem hiding this comment.
Vertical acceleration during coast may be above 16G especially for higher power rockets. It's only for a couple seconds but that's long enough to be a problem
| * If numerical noise causes |xyz|² > 1, XYZ is normalised first so | ||
| * the output remains a valid unit quaternion. | ||
| * Uses lsm6dsv320x_from_quaternion_lsb_to_float() | ||
| * @param quat: Output [w, x, y, z] indexed [0..3]. |
There was a problem hiding this comment.
super nit: usually there isnt a : after the name of the param
There was a problem hiding this comment.
that meme Robert made about me never approving pull requests will be true for you too, just you wait
There was a problem hiding this comment.
I like the attention, dw
I think my plan is to couple system frame rate to the odr of the slowest main sensor (baro, accel, gyro, and state estim fusion). I dont think I would care about changing the FIFO because we'll pretty much always just want to take the most recent reading from the device. |
during coast? Under power it can go up to ~120 ish Gs in high power mode, but coast should be pure drag + gravity, which shouldn't be that high. We can add a decel phase to the fsm if we expect a really high instantaneous slowdown? |
Okay, but if we do a kalman filter we can update the state estimation as data from each sensor arrives instead of coupling it to the slowest (e.g. 10 updates with baro then one with GPS). |
lol nvm thats definitely easier from an architectural perspective, id prefer that |
Drag deceleration for a sim I have with a K400 motor gets above 50G |
|
damn, okay. so we probably want a mechanism to track deceleration to subsonic in between boost and coast? bc that decel probably also requires a decently high frame rate to capture the acceleration, whereas coast can go slower and descent under parachutes can go even slower (if all that is needed) |
ETSells
left a comment
ETSells
left a comment
There was a problem hiding this comment.
one CR remaining.
remind me to enumerate sdr coding standards somewhere in like a markdown file, i have no way of telling people what they are except showing them.
| * @brief Index of the imu_dma_rx_buf row holding the most recently completed, | ||
| * not-yet consumed transfer. | ||
| */ | ||
| static atomic_uchar imu_dma_ready_idx = 0U; |
There was a problem hiding this comment.
nit: we prefer not using types like 'char' and 'int' in favor of types that are explicit about the size they contain like 'uint8_t'. given that atomics only have so many aliases this might be fine here tho?
| */ | ||
| static void sflp2q | ||
| ( | ||
| float quat[4], |
There was a problem hiding this comment.
i'm with nick -- we should consider using the math_sdr.h QUAT type once it exists.
| * @retval IMU_LSM_STATUS | ||
| */ | ||
| IMU_STATUS imu_init | ||
| IMU_LSM_STATUS imu_init |
There was a problem hiding this comment.
note: this is gonna force us to use IMU_SYS_STATUS for anything IMU related that isn't platform dependent. we should prepare to make this switch during initial rev 3 integration.
There was a problem hiding this comment.
Honestly was keeping distinction because I never was going to touch imu_legacy.c, but tbh for measurement SI consistency, turns out I'm going to end up touching those files anyway, so probably will go back to IMU_STATUS and just unify 'em.
There was a problem hiding this comment.
Actually, don't know if I would want to unify them because of IMU_MAG_INIT_FAIL wouldn't make sense to be with the new rev3 IMU. Plus, legacy is I2C & new is SPI, so coupling a unified with both I2C and SPI errors is polluting
If you think the name is the annoying part, I can just rename IMU_STATUS on legacy side into IMU_LEGACY_STATUS and rename the unifer IMU_SYS_STATUS to IMU_STATUS.
So honestly, I'm going to wait for the QUAT PR and revist this later. Point of the imu_dflt.c is to be the adapter anyway.
There was a problem hiding this comment.
Yep that works! I wasn't critiquing imu_sys_status, just noting for myself that it changes app integration considerations since imu_status becomes platform specific. as-is is fine
| @@ -131,24 +133,41 @@ | |||
| * - Section (.dma_buffer): Forces placement in RAM_D2 (0x30000000). | |||
| * DTCM (.bss) is not reachable by the DMA1/DMA2 controllers. | |||
There was a problem hiding this comment.
man, that's unfortunate. DTCM doesn't cache, so its what we would want to use otherwise
| SCB_CleanDCache_by_Addr( (uint32_t*)imu_dma_tx_buf, | ||
| (int32_t)IMU_DMA_BUF_BYTES_ALIGNED ); | ||
| SCB_InvalidateDCache_by_Addr( (uint32_t*)imu_dma_rx_buf, | ||
| SCB_InvalidateDCache_by_Addr( (uint32_t*)imu_dma_rx_buf[ imu_dma_fill_idx ], |
There was a problem hiding this comment.
caching on our mcu is so weird... i originally was concerned at the idea of needing to invalidate each region separately since I thought they'd be in the same page, but there are no pages on the M7. how does caching even work lmao
There was a problem hiding this comment.
Ye, b/c of that M7 lack of cache snooping for DMA, basically had to manually sync the L1 Cache in code since DTCM is private :(. D2-SRAM (.dma_buffer) is behind the L1-Cache, so yoink that bad boi there.
__ALIGNED(32) __attribute__((section(".dma_buffer")))
static uint8_t imu_dma_rx_buf[ 2 ][ IMU_DMA_BUF_BYTES_ALIGNED ];Data is managed in 32-byte cache lines instead of the 4kB pages. Better for precision and efficiency, but no hardware snooping by design. Usually I just round to ensure no neighborhood destruction [false sharing according to google] & ptr reference correctly.
This section you highlighted basically flags CPU's internal cache. The clean (TX) forces 'write-back' cache to flush SPI cmd into RAM (DMA-readable). The invalidate (RX), flags current RX buffer as stale requiring re-fetching from RAM post DMA-write.
My understanding for this is just treat DMA as external master relative to the L1-Cache. DMA don't know CPU has modified data (in Cache) nor does the CPU know DMA has modified data (in RAM). The code just gotta be the mailman to let them know when to update.
| * while copying it. | ||
| */ | ||
| primask = __get_PRIMASK(); | ||
| __disable_irq(); |
There was a problem hiding this comment.
ah the other concern i forgot to mention with this is that it'll already be sitting in an IRQ disabled critical section when it's called, so double disablement of IRQs would fuck us over.
| uint8_t gy_idx; | ||
|
|
||
| /** @brief Standard gravity, used to convert g -> m/s^2 (SI). */ | ||
| #define IMU_STANDARD_GRAVITY ( 9.80665f ) |
There was a problem hiding this comment.
these should already be defined as constants somewhere, right @NArmistead?
There was a problem hiding this comment.
should be in math_sdr.h
There was a problem hiding this comment.
although the precision on that is super low. not sure why i did that
There was a problem hiding this comment.
http://jpl.nasa.gov/edu/news/how-many-decimals-of-pi-do-we-really-need/
its probably fine but we could def stand to get a bit more precise lol
| /* mg/LSB -> g/LSB -> (SI) m/s^2/LSB */ | ||
| acc_sens = ( acc_sens_mg_lsb[acc_idx] / 1000.0f ) * IMU_STANDARD_GRAVITY; | ||
| /* mdps/LSB -> dps/LSB -> (SI) rad/s/LSB */ | ||
| gyro_sens = ( gyro_sens_mdps_lsb[gy_idx] / 1000.0f ) * IMU_DEG_TO_RAD; |
There was a problem hiding this comment.
cr: expected system units for rotation are degrees per second.
There was a problem hiding this comment.
tbh we should do radians, but oh well. I would rather not change it since it feeds into state estimation algorithms that would have to change
There was a problem hiding this comment.
Ngl converting to rads would be fine because all of the state estimation stuff needs to convert from degrees to rads anyway except for the final roll rate
There was a problem hiding this comment.
Okay, in that case feel free to leave it and we'll backport rev 2. Should be no loss of precision bc floating points are sick
There was a problem hiding this comment.
Ofc, tell me after I put things in dps lmao
There was a problem hiding this comment.
PFFT- okay well dont go through the trouble to revert now that you've done it
This comment was marked as resolved.
This comment was marked as resolved.
|
Definitely don't define something that isnt true. If youre fixing it, fix the root cause. To do otherwise is to accept technical debt, and that just makes our lives harder down the road. Especially early in a system's lifetime, we should be really smart about the tech debt we introduce. |
|
I mean, it feels odd just yoinking that random include. Seems like it doesn't do anything for GPS testing? Lmk |
|
Actually might be a poptart (bad) fix... yea, Ill see to this another day. Am actually curious why GPS is asking for sensor.h anyway. Just gonna fix my guard nonetheless b/c yucky. |
| Project Includes | ||
| ------------------------------------------------------------------------------*/ | ||
| #include "sdrtf_pub.h" | ||
| #include "main.h" |
|
I think GPS shouldn't have a dependency on sensor.h. That guard might have originally existed from me trying to get the STM32F7 based ground station to compile with the telemetry.h header. I did a lot of sketchy shit for it. |
what do you have against pop tarts :( |
|
Gonna leave the GPS unmodified b/c out of scope anyway. Make a note for future updates to the test b/c random footnote imu touching test is quite silly organizationally. Wonder how long this PR is going get lmao. |
| /** | ||
| * @brief Blocking read of accelerometer X/Y/Z. | ||
| * @param pIMU: Destination struct for the accel readout (size: 12 bytes). | ||
| * @retval IMU_STATUS |
There was a problem hiding this comment.
nit: doxygen "retval" vs "return" semantics
| * BSD-3-Clause license, the following notice is retained from the | ||
| * source project and applies to the procedure below. | ||
| * | ||
| * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. |
There was a problem hiding this comment.
nit: "@copyright" tag instead of "note". this is fine in accordance with our licensing policies but for documentation generation itd be nice to have this show up in the right section.
| * @retval IMU_STATUS: IMU_BUSY until both imu_data_ready and mag_data_ready | ||
| * are set by imu_it_handler(). | ||
| */ | ||
| IMU_STATUS get_imu_it |
There was a problem hiding this comment.
observation (i will do this myself at some point): I've begun to push for an unofficial policy that all non-static functions defined in a module should be preceded by that module's name. For example, instead of print_debug we'd say debug_print.
This convention also applies to private functions within a module of multiple files. See the test framework for an example, where we preface private functions that need to be accessed by the other framework file with _test.
The idea for this is to help with intellisense autocompletion -- if you need to call a function from a module but you're not sure what it is, you should be able to type {module}_{keyword} and have your autocompletion tool come up with the right function, e.g. imu_accel should present imu_get_accel_and_gyro as an autocomplete option. Haven't tested this a ton with vscode yet, but that kind of convention works great for jetbrains IDEs & visual studio.
There was a problem hiding this comment.
Would have to scrap through all the stuff that relies on imu_legacy lmao
There was a problem hiding this comment.
yeah this is not me telling you to do it, it's me saying I'm gonna do it at some point
There was a problem hiding this comment.
Well, ye, a glhf to you :P
| default: | ||
| IMU_ASSERT( odr == IMU_ODR_1920HZ ); | ||
| return LSM6DSV320X_ODR_AT_1920Hz; | ||
| } |
There was a problem hiding this comment.
still true. i know our coding style is a little weird, but this uniformity makes it a lot easier to read
|
|
||
| /* Disable INT1 EXTI to prevent accidental ISR triggers during init. | ||
| INT1 is routed to EXTI4 (PC4 = IMU_INT1_PIN). */ | ||
| HAL_NVIC_DisableIRQ( EXTI4_IRQn ); |
There was a problem hiding this comment.
This is platform specific, so we may want to define it in the pindefs file
There was a problem hiding this comment.
Should be updated in lib.
|
|
||
| /* Disable INT1 EXTI to prevent accidental ISR triggers during init. | ||
| INT1 is routed to EXTI4 (PC4 = IMU_INT1_PIN). */ | ||
| HAL_NVIC_DisableIRQ( EXTI4_IRQn ); |
There was a problem hiding this comment.
Also, we might want to disable interrupts on the SPI handle as well, although it should be fine as long as we use tx/rx cplt callbacks.
There was a problem hiding this comment.
It depends on whether we need to support mid-flight re-initialization for error recovery, but I’ve added HAL_SPI_Abort() and a manual CS release to the top of the function anyway. Better than disabling IRQ so SPI isn't locked out by a low CS pin.
| SCB_InvalidateDCache_by_Addr( (uint32_t*)imu_dma_rx_buf[ imu_dma_fill_idx ], | ||
| (int32_t)IMU_DMA_BUF_BYTES_ALIGNED ); | ||
|
|
||
| /* Publish the ready index before setting the ready flag to prevent race conditions */ |
There was a problem hiding this comment.
shouldnt happen unless a higher priority ISR tries to consume this data, which we would not allow architecturally. still a nice protective touch.
ETSells
left a comment
ETSells
left a comment
There was a problem hiding this comment.
I only took a small sample of this PR, but everything looks like it follows style guidelines now too! fuck it we ball, ship it
|
4 participants
Description
This PR implements a unified IMU abstraction layer and the specific driver for the LSM6DSV320X (High-G) sensor, targeting the STM32H733 (A0010 Rev 3 PCB).
The implementation introduces a modular architecture that separates hardware-specific translation layers (
imu_lsm.c,imu_legacy.c) from the core flight software contract defined inimu.h. This allows individual projects to swap IMU backends without changing flight logic.Key Changes
imu_system_update) that abstracts the differences between legacy 9-axis I2C architectures and the newer 6-axis SPI-DMA architecture.HAL_SPI_TransmitReceive_DMAfor high-frequency FIFO burst reads, minimizing CPU overhead.Legacy functions were also striped
rev1support.Documentation & Readability
The code is heavily annotated with citations from the DS14623 (Rev 3) datasheet to support long-term maintenance and hardware audits.
Issue Link
Child PR Dependencies
Testing [NOT APPLICABLE]
Attach any test artifacts here, if relevant.
Other
Leave any additional notes here
Reviewer Checklist
Standards
Error Handling
Memory
Performance
Additional Elements