stm_32/peripherals/
adis16500.rs

1// ******************************************************************************
2// * File     : platforms/stm_32/src/peripherals/adis16500.rs
3// * Date     : June 28, 2026
4// ******************************************************************************
5// *
6// * Copyright (c) 2023, AeroVironment, Inc.
7// * All rights reserved.
8// *
9// * Redistribution and use in source and binary forms, with or without
10// * modification, are permitted provided that the following conditions are met:
11// *
12// * 1.Redistributions of source code must retain the above copyright notice, this
13// * list of conditions and the following disclaimer.
14// *
15// * 2.Redistributions in binary form must reproduce the above copyright notice,
16// * this list of conditions and the following disclaimer in the documentation
17// * and/or other materials provided with the distribution.
18// *
19// * 3.Neither the name of the copyright holder nor the names of its
20// * contributors may be used to endorse or promote products derived from
21// * this software without specific prior written permission.
22// *
23// * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
24// * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
25// * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
26// * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
27// * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
28// * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
29// * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30// * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
31// * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
32// * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
33// *
34// ******************************************************************************
35
36use crate::peripherals::pwm::{self, TimerEnum};
37use embassy_embedded_hal::shared_bus::asynch::spi::SpiDevice;
38use embassy_stm32::exti::ExtiInput;
39use embassy_stm32::gpio::Output;
40use embassy_stm32::mode::Async;
41use embassy_stm32::spi;
42use embassy_sync::blocking_mutex::raw::CriticalSectionRawMutex;
43use embassy_sync::signal::Signal;
44use embassy_time::{Instant, Timer};
45use embedded_hal_async::spi::SpiDevice as _;
46use veloxity_core::errors;
47use veloxity_core::packets::{ImuPacket, RosflightPacketHeader};
48
49// Device dependent
50const SPI_READ: u8 = 0x00;
51const SPI_WRITE: u8 = 0x80;
52
53// Registers
54
55// Chip ID
56
57pub static IMU_SIGNAL: Signal<
58    CriticalSectionRawMutex,
59    Result<ImuPacket<f64>, errors::SensorError>,
60> = Signal::<CriticalSectionRawMutex, Result<ImuPacket<f64>, errors::SensorError>>::new();
61
62#[repr(u16)]
63#[derive(Clone, Copy)]
64pub enum DecRate {
65    Odr2000Hz = 0, //  2000/2000-1 = 0
66    Odr1000Hz = 1, //  2000/1000-1 = 1
67    Odr400Hz = 4,  //  2000/400-1 = 4
68}
69
70pub struct Adis16500Sensor {
71    pub dev: SpiDevice<
72        'static,
73        CriticalSectionRawMutex,
74        spi::Spi<'static, Async, spi::mode::Master>,
75        Output<'static>,
76    >,
77    pub dec_rate: DecRate,
78    pub drdy: ExtiInput<'static, Async>,
79    pub reset: Output<'static>,
80    pub timer: TimerEnum,
81}
82
83const ADIS_BUFFBYTES16: usize = 22;
84const ADIS_BUFFBYTES32: usize = 34;
85const BURST_READ: u8 = 0x68;
86
87impl Adis16500Sensor {
88    async fn read_register(&mut self, reg_addr: u8) -> Result<u16, errors::SensorError> {
89        let tx = [reg_addr | SPI_READ, 0x00];
90        self.dev.write(&tx).await.map_err(|e| match e {
91            _ => errors::SensorError::GenericSensorError("SPI failed: write_register"),
92        })?;
93        Timer::after_micros(100).await; // Required 16us delay till you can read again
94        let tx = [0u8; 2];
95        let mut rx = [0u8; 2];
96        self.dev.transfer(&mut rx, &tx).await.map_err(|e| match e {
97            _ => errors::SensorError::GenericSensorError("SPI failed: read_register"),
98        })?;
99        Timer::after_micros(100).await; // Required 16us delay till you can read again
100        Ok(rx[1] as u16 | ((rx[0] as u16) << 8))
101    }
102
103    async fn write_register(
104        &mut self,
105        reg_addr: u8,
106        value: u16,
107    ) -> Result<(), errors::SensorError> {
108        let lo = (value & 0x00FF) as u8;
109        let tx = [reg_addr | SPI_WRITE, lo];
110        // Soft Reset
111        self.dev.write(&tx).await.map_err(|e| match e {
112            _ => errors::SensorError::GenericSensorError("SPI failed: write_register"),
113        })?;
114        Timer::after_micros(100).await; // (100) Required 16us delay till you can read again
115
116        let hi = ((value >> 8) & 0x00FF) as u8; //
117        let tx = [(reg_addr + 1) | SPI_WRITE, hi];
118        // Soft Reset
119        self.dev.write(&tx).await.map_err(|e| match e {
120            _ => errors::SensorError::GenericSensorError("SPI failed: write_register"),
121        })?;
122        Timer::after_micros(100).await; // (100) Required 16us delay till you can read again
123        Ok(())
124    }
125
126    async fn initialize_sensor(&mut self) -> Result<u16, errors::SensorError> {
127        self.reset.set_low(); // Hold in reset
128
129        let _ = self.timer.enable(pwm::TimerChannel::Ch1);
130        let _ = self.timer.set_duty_cycle(pwm::TimerChannel::Ch1, 500); // 500 us
131
132        Timer::after_micros(1000).await;
133
134        self.reset.set_high();
135        Timer::after_millis(300).await; // Data sheet specifies 255ms for power-on startup empirically 300 is required
136
137        // Check the hardware ID
138        const ADIS16500_PROD_ID_ADDR: u8 = 0x72;
139        const ADIS16500_PROD_ID: u16 = 0x4074;
140        let prod_id = self.read_register(ADIS16500_PROD_ID_ADDR).await?;
141        if prod_id != ADIS16500_PROD_ID {
142            return Err(errors::SensorError::GenericSensorError(
143                "ADIS16500 ID mismatch",
144            ));
145        }
146
147        const ADIS16500_FILT_CTRL: u8 = 0x5C; // shift so we can or the data into the first 16 bit packet
148        // [15:3] not used
149        // [2:0] 0 no digital filter default)
150        self.write_register(ADIS16500_FILT_CTRL, 0).await?;
151
152        const ADIS16500_DEC_RATE: u8 = 0x64; // decimation
153        // [15:11] don't care
154        // [10:0] decimation rate minus 1, e.g., use 5-1 = 4
155
156        self.write_register(ADIS16500_DEC_RATE, self.dec_rate as u16)
157            .await?;
158
159        // Miscellaneous Control Register (MSC_CTRL)
160        const ADIS16500_MSC_CTRL: u8 = 0x60;
161        // [15:10] 0's unused
162        // [9] 1 32-bit burst data (default = 0)
163        // [8] 0 burst data has gyro and accel data (default = 0)
164
165        // [7] 1 enable linear acceleration compensation for gyros (default  0)
166        // [6] 0 point of percussion alignment
167        // [5] 0 always zero
168        // [4] 0 wide sensor bandwidth (default)
169
170        // [3:2] 01 Direct Input Sync Mode
171        // [1] 0 falling edge sync (default =0)
172        // [0] 1 active high when data is valid (default is 0, low)
173        // 0b0000 0010 1000 0101 = 0x0285 // external clock
174        // 0b0000 0010 1000 0001 = 0x0281 // internal clock
175
176        if (self.dec_rate as u16) == 0 {
177            // 2000Hz, sample rate, use 16-bit data mode
178            self.write_register(ADIS16500_MSC_CTRL, 0x0085).await?; // values 0b0000 0000 1000 0101 = 0x0085
179        } else {
180            self.write_register(ADIS16500_MSC_CTRL, 0x0285).await?; // values 0b0000 0010 1000 0101 = 0x0285
181        }
182
183        const ADIS16500_DIAG_STAT: u8 = 0x02;
184        let diag_stat = self.read_register(ADIS16500_DIAG_STAT).await?;
185
186        if diag_stat != 0 {
187            return Err(errors::SensorError::GenericSensorError(
188                "ADIS16500 diagnostic status error",
189            ));
190        }
191        Ok(diag_stat)
192    }
193
194    async fn read_data_16(&mut self) -> Result<[u8; ADIS_BUFFBYTES16], errors::SensorError> {
195        self.drdy.wait_for_rising_edge().await;
196        let mut rx = [0u8; ADIS_BUFFBYTES16];
197        let mut tx = [0u8; ADIS_BUFFBYTES16];
198        tx[0] = BURST_READ | SPI_READ;
199        self.dev.transfer(&mut rx, &tx).await.map_err(|e| match e {
200            _ => errors::SensorError::GenericSensorError("SPI failed: read_burst_data_16"),
201        })?;
202        Ok(rx)
203    }
204
205    async fn read_data_32(&mut self) -> Result<[u8; ADIS_BUFFBYTES32], errors::SensorError> {
206        self.drdy.wait_for_rising_edge().await;
207        let mut rx = [0u8; ADIS_BUFFBYTES32];
208        let mut tx = [0u8; ADIS_BUFFBYTES32];
209        tx[0] = BURST_READ | SPI_READ;
210        self.dev.transfer(&mut rx, &tx).await.map_err(|e| match e {
211            _ => errors::SensorError::GenericSensorError("SPI failed: read_burst_data_32"),
212        })?;
213        Ok(rx)
214    }
215
216    fn validate_data_16(
217        &self,
218        rx: &[u8; ADIS_BUFFBYTES16],
219        data: &[i16; ADIS_BUFFBYTES16 / 2],
220    ) -> Result<(), errors::SensorError> {
221        let rx_u16 = rx.map(|x| x as u16);
222        let rx_u16_subarray = &rx_u16[2..ADIS_BUFFBYTES16 - 2];
223        let checksum: u16 = rx_u16_subarray.iter().sum();
224
225        if checksum != data[10] as u16 {
226            return Err(errors::SensorError::GenericSensorError(
227                "ADIS16500 checksum mismatch",
228            ));
229        }
230
231        let status: u16 = data[1] as u16;
232        if status != 0 {
233            return Err(errors::SensorError::GenericSensorError(
234                "ADIS16500 status error",
235            ));
236        }
237
238        Ok(())
239    }
240
241    fn validate_data_32(
242        &self,
243        rx: &[u8; ADIS_BUFFBYTES32],
244        data: &[u16; ADIS_BUFFBYTES32 / 2],
245    ) -> Result<(), errors::SensorError> {
246        let rx_u16 = rx.map(|x| x as u16);
247        let rx_u16_subarray = &rx_u16[2..ADIS_BUFFBYTES32 - 2];
248        let checksum: u16 = rx_u16_subarray.iter().sum();
249
250        if checksum != data[16] as u16 {
251            return Err(errors::SensorError::GenericSensorError(
252                "ADIS16500 checksum mismatch",
253            ));
254        }
255
256        let status: u16 = data[1] as u16;
257        if status != 0 {
258            return Err(errors::SensorError::GenericSensorError(
259                "ADIS16500 status error",
260            ));
261        }
262
263        Ok(())
264    }
265
266    fn process_data_16(
267        &self,
268        data: &[i16; ADIS_BUFFBYTES16 / 2],
269        timestamp: embassy_time::Instant,
270    ) -> ImuPacket<f64> {
271        let gyro = [
272            -f64::from(data[2]) * 0.001745329251994,
273            -f64::from(data[3]) * 0.001745329251994,
274            f64::from(data[4]) * 0.001745329251994,
275        ];
276        let accel = [
277            -f64::from(data[5]) * 0.01225,
278            -f64::from(data[6]) * 0.01225,
279            f64::from(data[7]) * 0.01225,
280        ];
281        let temperature = f32::from(data[8]) * 0.1; // + 273.15
282        let seq = data[9] as u32; // sequence counter    
283        let status: u16 = data[1] as u16;
284        let header = RosflightPacketHeader {
285            timestamp: timestamp.as_micros(),
286            status: status,
287        };
288        ImuPacket {
289            header,
290            accel,
291            gyro,
292            temperature,
293            seq,
294        }
295    }
296
297    fn process_data_32(
298        &self,
299        data: &[u16; ADIS_BUFFBYTES32 / 2],
300        timestamp: embassy_time::Instant,
301    ) -> ImuPacket<f64> {
302        let gyros_sf: f64 = 0.001745329251994f64 / f64::from(1u32 << 16);
303        let gyro = [
304            -f64::from(((data[2] as u32) | ((data[3] as u32) << 16)) as i32) * gyros_sf,
305            -f64::from(((data[4] as u32) | ((data[5] as u32) << 16)) as i32) * gyros_sf,
306            f64::from(((data[6] as u32) | ((data[7] as u32) << 16)) as i32) * gyros_sf,
307        ];
308        let accel_sf: f64 = 0.012254f64 / f64::from(1u32 << 16);
309        let accel = [
310            -f64::from(((data[8] as u32) | ((data[9] as u32) << 16)) as i32) * accel_sf,
311            -f64::from(((data[10] as u32) | ((data[11] as u32) << 16)) as i32) * accel_sf,
312            f64::from(((data[12] as u32) | ((data[13] as u32) << 16)) as i32) * accel_sf,
313        ];
314        let temperature = f32::from(data[14] as i16) * 0.1; // + 273.15
315        let sample_period_us = 500u32 * ((self.dec_rate as u32) + 1);
316        let seq = (data[15] as u32) * sample_period_us; // sequence counter 
317        let status: u16 = data[1] as u16;
318        let header = RosflightPacketHeader {
319            timestamp: timestamp.as_micros(),
320            status: status,
321        };
322        ImuPacket {
323            header,
324            accel,
325            gyro,
326            temperature,
327            seq,
328        }
329    }
330
331    pub async fn run(&mut self) {
332        let _status = match self.initialize_sensor().await {
333            Ok(status) => status,
334            Err(e) => {
335                IMU_SIGNAL.signal(Err(e));
336                return;
337            }
338        };
339
340        loop {
341            if (self.dec_rate as u16) == 0 {
342                // 2000Hz, sample rate, use 16-bit data mode
343                let timestamp = Instant::now();
344
345                let rx = match self.read_data_16().await {
346                    Ok(data) => data,
347                    Err(e) => {
348                        IMU_SIGNAL.signal(Err(e));
349                        continue;
350                    }
351                };
352
353                let mut data = [0i16; ADIS_BUFFBYTES16 / 2];
354                for (i, x) in data.iter_mut().enumerate() {
355                    *x = ((rx[2 * i] as i16) << 8) | ((rx[2 * i + 1] as i16) & 0x00FF);
356                }
357
358                if let Err(e) = self.validate_data_16(&rx, &data) {
359                    IMU_SIGNAL.signal(Err(e));
360                    continue;
361                }
362
363                let imu_packet = self.process_data_16(&data, timestamp);
364                IMU_SIGNAL.signal(Ok(imu_packet));
365            } else {
366                let timestamp = Instant::now();
367
368                let rx = match self.read_data_32().await {
369                    Ok(data) => data,
370                    Err(e) => {
371                        IMU_SIGNAL.signal(Err(e));
372                        continue;
373                    }
374                };
375
376                let mut data = [0u16; ADIS_BUFFBYTES32 / 2];
377                for (i, x) in data.iter_mut().enumerate() {
378                    *x = ((rx[2 * i] as u16) << 8) | ((rx[2 * i + 1] as u16) & 0x00FF);
379                }
380
381                if let Err(e) = self.validate_data_32(&rx, &data) {
382                    IMU_SIGNAL.signal(Err(e));
383                    continue;
384                }
385
386                let imu_packet = self.process_data_32(&data, timestamp);
387                IMU_SIGNAL.signal(Ok(imu_packet));
388            }
389        }
390    }
391}
392
393#[embassy_executor::task]
394pub async fn task(mut adis: Adis16500Sensor) {
395    adis.run().await;
396}
stm_32/peripherals/adis16500.rs

stm_32/peripherals/
adis16500.rs
