capsules_extra/
l3gd20.rs

1// Licensed under the Apache License, Version 2.0 or the MIT License.
2// SPDX-License-Identifier: Apache-2.0 OR MIT
3// Copyright Tock Contributors 2022.
4
5//! SyscallDriver for the MEMS L3gd20Spi motion sensor, 3 axys digital output gyroscope
6//! and temperature sensor.
7//!
8//! May be used with NineDof and Temperature
9//!
10//! SPI Interface
11//!
12//! <https://www.pololu.com/file/0J563/L3gd20Spi.pdf>
13//!
14//!
15//! Syscall Interface
16//! -----------------
17//!
18//! ### Command
19//!
20//! All commands are asynchronous, they return a one shot callback when done
21//! Only one command can be issued at a time.
22//!
23//! #### command num
24//! - `0`: Returns Ok(())
25//!   - `data`: Unused.
26//!   - Return: Success
27//! - `1`: Is Present
28//!   - `data`: unused
29//!   - Return: `Ok(())` if no other command is in progress, `BUSY` otherwise.
30//! - `2`: Power On
31//!   - `data`: unused
32//!   - Return: `Ok(())` if no other command is in progress, `BUSY` otherwise.
33//! - `3`: Set Scale
34//!   - `data1`: 0, 1 or 2
35//!   - Return: `Ok(())` if no other command is in progress, `BUSY` otherwise.
36//! - `4`: Enable high pass filter
37//!   - `data`: 1 for enable, 0 for disable
38//!   - Return: `Ok(())` if no other command is in progress, `BUSY` otherwise.
39//! - `5`: Set High Pass Filter Mode and Divider (manual page 33)
40//!   - `data1`: mode
41//!   - `data2`: divider
42//!   - Return: `Ok(())` if no other command is in progress, `BUSY` otherwise.
43//! - `6`: Read XYZ
44//!   - `data`: unused
45//!   - Return: `Ok(())` if no other command is in progress, `BUSY` otherwise.
46//! - `7`: Read Temperature
47//!   - `data`: unused
48//!   - Return: `Ok(())` if no other command is in progress, `BUSY` otherwise.
49//!
50//! ### Subscribe
51//!
52//! All commands call this callback when done, usually subscribes
53//! should be one time functions
54//!
55//! #### subscribe num
56//! - `0`: Done callback
57//!   - 'data1`: depends on command
58//!     - `1` - 1 for is present, 0 for not present
59//!     - `6` - X rotation
60//!     - `7` - temperature in deg C
61//!   - 'data2`: depends on command
62//!     - `6` - Y rotation
63//!   - 'data3`: depends on command
64//!     - `6` - Z rotation
65//!
66//! Usage
67//! -----
68//!
69//! ```rust,ignore
70//! let mux_spi = components::spi::SpiMuxComponent::new(&stm32f3xx::spi::SPI1)
71//!     .finalize(components::spi_mux_component_helper!(stm32f3xx::spi::Spi));
72//!
73//! let l3gd20 = components::l3gd20::L3gd20SpiComponent::new()
74//!     .finalize(components::l3gd20_spi_component_helper!(stm32f3xx::spi::Spi, stm32f3xx::gpio::PinId::PE03, mux_spi));
75//!
76//! ```
77//!
78//! NineDof Example
79//!
80//! ```rust,ignore
81//! let grant_cap = create_capability!(capabilities::MemoryAllocationCapability);
82//! let grant_ninedof = board_kernel.create_grant(&grant_cap);
83//!
84//! l3gd20.power_on();
85//! let ninedof = static_init!(
86//!     capsules::ninedof::NineDof<'static>,
87//!     capsules::ninedof::NineDof::new(l3gd20, grant_ninedof));
88//! hil::sensors::NineDof::set_client(l3gd20, ninedof);
89//!
90//! ```
91//!
92//! Temperature Example
93//!
94//! ```rust,ignore
95//! let grant_cap = create_capability!(capabilities::MemoryAllocationCapability);
96//! let grant_temp = board_kernel.create_grant(&grant_cap);
97//!
98//! l3gd20.power_on();
99//! let temp = static_init!(
100//! capsules::temperature::TemperatureSensor<'static>,
101//!     capsules::temperature::TemperatureSensor::new(l3gd20, grant_temperature));
102//! kernel::hil::sensors::TemperatureDriver::set_client(l3gd20, temp);
103//!
104//! ```
105//!
106//! Author: Alexandru Radovici <msg4alex@gmail.com>
107//!
108
109use core::cell::Cell;
110
111use kernel::grant::{AllowRoCount, AllowRwCount, Grant, UpcallCount};
112use kernel::hil::sensors;
113use kernel::hil::spi;
114use kernel::syscall::{CommandReturn, SyscallDriver};
115use kernel::utilities::cells::{MapCell, OptionalCell};
116use kernel::utilities::leasable_buffer::SubSliceMut;
117use kernel::{ErrorCode, ProcessId};
118
119use capsules_core::driver;
120pub const DRIVER_NUM: usize = driver::NUM::L3gd20 as usize;
121
122/* Identification number */
123const L3GD20_WHO_AM_I: u8 = 0xD4;
124
125/* Registers addresses */
126const L3GD20_REG_WHO_AM_I: u8 = 0x0F;
127const L3GD20_REG_CTRL_REG1: u8 = 0x20;
128const L3GD20_REG_CTRL_REG2: u8 = 0x21;
129// const L3GD20_REG_CTRL_REG3: u8 = 0x22;
130const L3GD20_REG_CTRL_REG4: u8 = 0x23;
131const L3GD20_REG_CTRL_REG5: u8 = 0x24;
132// const L3GD20_REG_REFERENCE: u8 = 0x25;
133const L3GD20_REG_OUT_TEMP: u8 = 0x26;
134// const L3GD20_REG_STATUS_REG: u8 = 0x27;
135const L3GD20_REG_OUT_X_L: u8 = 0x28;
136/*
137const L3GD20_REG_OUT_X_H: u8 = 0x29;
138const L3GD20_REG_OUT_Y_L: u8 = 0x2A;
139const L3GD20_REG_OUT_Y_H: u8 = 0x2B;
140const L3GD20_REG_OUT_Z_L: u8 = 0x2C;
141const L3GD20_REG_OUT_Z_H: u8 = 0x2D;
142*/
143/*
144const L3GD20_REG_FIFO_CTRL_REG: u8 = 0x2E;
145const L3GD20_REG_FIFO_SRC_REG: u8 = 0x2F;
146const L3GD20_REG_INT1_CFG: u8 = 0x30;
147const L3GD20_REG_INT1_SRC: u8 = 0x31;
148const L3GD20_REG_INT1_TSH_XH: u8 = 0x32;
149const L3GD20_REG_INT1_TSH_XL: u8 = 0x33;
150const L3GD20_REG_INT1_TSH_YH: u8 = 0x34;
151const L3GD20_REG_INT1_TSH_YL: u8 = 0x35;
152const L3GD20_REG_INT1_TSH_ZH: u8 = 0x36;
153const L3GD20_REG_INT1_TSH_ZL: u8 = 0x37;
154const L3GD20_REG_INT1_DURATION: u8 = 0x38;
155*/
156
157pub const L3GD20_TX_SIZE: usize = 10;
158pub const L3GD20_RX_SIZE: usize = 10;
159
160pub const TX_BUF_LEN: usize = L3GD20_TX_SIZE;
161pub const RX_BUF_LEN: usize = L3GD20_RX_SIZE;
162
163/* Sensitivity factors, datasheet pg. 9 */
164const L3GD20_SCALE_250: isize = 875; /* 8.75 mdps/digit */
165const L3GD20_SCALE_500: isize = 1750; /* 17.5 mdps/digit */
166const L3GD20_SCALE_2000: isize = 7000; /* 70 mdps/digit */
167
168#[derive(Copy, Clone, PartialEq)]
169enum L3gd20Status {
170    Idle,
171    IsPresent,
172    PowerOn,
173    EnableHpf,
174    SetHpfParameters,
175    SetScale,
176    ReadXYZ,
177    ReadTemperature,
178}
179
180// #[derive(Clone, Copy, PartialEq)]
181// enum L3GD20State {
182//     Idle,
183// }
184
185#[derive(Default)]
186pub struct App {}
187
188pub struct L3gd20Spi<'a, S: spi::SpiMasterDevice<'a>> {
189    spi: &'a S,
190    txbuffer: MapCell<SubSliceMut<'static, u8>>,
191    rxbuffer: MapCell<SubSliceMut<'static, u8>>,
192    status: Cell<L3gd20Status>,
193    hpf_enabled: Cell<bool>,
194    hpf_mode: Cell<u8>,
195    hpf_divider: Cell<u8>,
196    scale: Cell<u8>,
197    current_process: OptionalCell<ProcessId>,
198    grants: Grant<App, UpcallCount<1>, AllowRoCount<0>, AllowRwCount<0>>,
199    nine_dof_client: OptionalCell<&'a dyn sensors::NineDofClient>,
200    temperature_client: OptionalCell<&'a dyn sensors::TemperatureClient>,
201}
202
203impl<'a, S: spi::SpiMasterDevice<'a>> L3gd20Spi<'a, S> {
204    pub fn new(
205        spi: &'a S,
206        txbuffer: &'static mut [u8; L3GD20_TX_SIZE],
207        rxbuffer: &'static mut [u8; L3GD20_RX_SIZE],
208        grants: Grant<App, UpcallCount<1>, AllowRoCount<0>, AllowRwCount<0>>,
209    ) -> L3gd20Spi<'a, S> {
210        // setup and return struct
211        L3gd20Spi {
212            spi,
213            txbuffer: MapCell::new((&mut txbuffer[..]).into()),
214            rxbuffer: MapCell::new((&mut rxbuffer[..]).into()),
215            status: Cell::new(L3gd20Status::Idle),
216            hpf_enabled: Cell::new(false),
217            hpf_mode: Cell::new(0),
218            hpf_divider: Cell::new(0),
219            scale: Cell::new(0),
220            current_process: OptionalCell::empty(),
221            grants,
222            nine_dof_client: OptionalCell::empty(),
223            temperature_client: OptionalCell::empty(),
224        }
225    }
226
227    pub fn is_present(&self) -> bool {
228        self.status.set(L3gd20Status::IsPresent);
229        self.txbuffer.take().map(|mut buf| {
230            buf.reset();
231            buf[0] = L3GD20_REG_WHO_AM_I | 0x80;
232            buf[1] = 0x00;
233            buf.slice(..2);
234            // TODO verify SPI return value
235            let _ = self.spi.read_write_bytes(buf, self.rxbuffer.take());
236        });
237        false
238    }
239
240    pub fn power_on(&self) {
241        self.status.set(L3gd20Status::PowerOn);
242        self.txbuffer.take().map(|mut buf| {
243            buf.reset();
244            buf[0] = L3GD20_REG_CTRL_REG1;
245            buf[1] = 0x0F;
246            buf.slice(..2);
247            // TODO verify SPI return value
248            let _ = self.spi.read_write_bytes(buf, None);
249        });
250    }
251
252    fn enable_hpf(&self, enabled: bool) {
253        self.status.set(L3gd20Status::EnableHpf);
254        self.hpf_enabled.set(enabled);
255        self.txbuffer.take().map(|mut buf| {
256            buf.reset();
257            buf[0] = L3GD20_REG_CTRL_REG5;
258            buf[1] = u8::from(enabled) << 4;
259            buf.slice(..2);
260            // TODO verify SPI return value
261            let _ = self.spi.read_write_bytes(buf, None);
262        });
263    }
264
265    fn set_hpf_parameters(&self, mode: u8, divider: u8) {
266        self.status.set(L3gd20Status::SetHpfParameters);
267        self.hpf_mode.set(mode);
268        self.hpf_divider.set(divider);
269        self.txbuffer.take().map(|mut buf| {
270            buf.reset();
271            buf[0] = L3GD20_REG_CTRL_REG2;
272            buf[1] = (mode & 0x03) << 4 | (divider & 0x0F);
273            buf.slice(..2);
274            // TODO verify SPI return value
275            let _ = self.spi.read_write_bytes(buf, None);
276        });
277    }
278
279    fn set_scale(&self, scale: u8) {
280        self.status.set(L3gd20Status::SetScale);
281        self.scale.set(scale);
282        self.txbuffer.take().map(|mut buf| {
283            buf.reset();
284            buf[0] = L3GD20_REG_CTRL_REG4;
285            buf[1] = (scale & 0x03) << 4;
286            buf.slice(..2);
287            // TODO verify SPI return value
288            let _ = self.spi.read_write_bytes(buf, None);
289        });
290    }
291
292    fn read_xyz(&self) {
293        self.status.set(L3gd20Status::ReadXYZ);
294        self.txbuffer.take().map(|mut buf| {
295            buf.reset();
296            buf[0] = L3GD20_REG_OUT_X_L | 0x80 | 0x40;
297            buf[1] = 0x00;
298            buf[2] = 0x00;
299            buf[3] = 0x00;
300            buf[4] = 0x00;
301            buf[5] = 0x00;
302            buf[6] = 0x00;
303            buf.slice(..7);
304            // TODO verify SPI return value
305            let _ = self.spi.read_write_bytes(buf, self.rxbuffer.take());
306        });
307    }
308
309    fn read_temperature(&self) {
310        self.status.set(L3gd20Status::ReadTemperature);
311        self.txbuffer.take().map(|mut buf| {
312            buf.reset();
313            buf[0] = L3GD20_REG_OUT_TEMP | 0x80;
314            buf[1] = 0x00;
315            buf.slice(..2);
316            // TODO verify SPI return value
317            let _ = self.spi.read_write_bytes(buf, self.rxbuffer.take());
318        });
319    }
320
321    pub fn configure(&self) -> Result<(), ErrorCode> {
322        self.spi.configure(
323            spi::ClockPolarity::IdleHigh,
324            spi::ClockPhase::SampleTrailing,
325            1_000_000,
326        )
327    }
328}
329
330impl<'a, S: spi::SpiMasterDevice<'a>> SyscallDriver for L3gd20Spi<'a, S> {
331    fn command(
332        &self,
333        command_num: usize,
334        data1: usize,
335        data2: usize,
336        process_id: ProcessId,
337    ) -> CommandReturn {
338        if command_num == 0 {
339            return CommandReturn::success();
340        }
341
342        let match_or_empty_or_nonexistent = self.current_process.map_or(true, |current_process| {
343            self.grants
344                .enter(current_process, |_, _| current_process == process_id)
345                .unwrap_or(true)
346        });
347
348        if match_or_empty_or_nonexistent {
349            self.current_process.set(process_id);
350        } else {
351            return CommandReturn::failure(ErrorCode::RESERVE);
352        }
353
354        match command_num {
355            // Check is sensor is correctly connected
356            1 => {
357                if self.status.get() == L3gd20Status::Idle {
358                    self.is_present();
359                    CommandReturn::success()
360                } else {
361                    CommandReturn::failure(ErrorCode::BUSY)
362                }
363            }
364            // Power On
365            2 => {
366                if self.status.get() == L3gd20Status::Idle {
367                    self.power_on();
368                    CommandReturn::success()
369                } else {
370                    CommandReturn::failure(ErrorCode::BUSY)
371                }
372            }
373            // Set Scale
374            3 => {
375                if self.status.get() == L3gd20Status::Idle {
376                    let scale = data1 as u8;
377                    self.set_scale(scale);
378                    CommandReturn::success()
379                } else {
380                    CommandReturn::failure(ErrorCode::BUSY)
381                }
382            }
383            // Enable High Pass Filter
384            4 => {
385                if self.status.get() == L3gd20Status::Idle {
386                    let mode = data1 as u8;
387                    let divider = data2 as u8;
388                    self.set_hpf_parameters(mode, divider);
389                    CommandReturn::success()
390                } else {
391                    CommandReturn::failure(ErrorCode::BUSY)
392                }
393            }
394            // Set High Pass Filter Mode and Divider
395            5 => {
396                if self.status.get() == L3gd20Status::Idle {
397                    let enabled = data1 == 1;
398                    self.enable_hpf(enabled);
399                    CommandReturn::success()
400                } else {
401                    CommandReturn::failure(ErrorCode::BUSY)
402                }
403            }
404            // Read XYZ
405            6 => {
406                if self.status.get() == L3gd20Status::Idle {
407                    self.read_xyz();
408                    CommandReturn::success()
409                } else {
410                    CommandReturn::failure(ErrorCode::BUSY)
411                }
412            }
413            // Read Temperature
414            7 => {
415                if self.status.get() == L3gd20Status::Idle {
416                    self.read_temperature();
417                    CommandReturn::success()
418                } else {
419                    CommandReturn::failure(ErrorCode::BUSY)
420                }
421            }
422            // default
423            _ => CommandReturn::failure(ErrorCode::NOSUPPORT),
424        }
425    }
426
427    fn allocate_grant(&self, processid: ProcessId) -> Result<(), kernel::process::Error> {
428        self.grants.enter(processid, |_, _| {})
429    }
430}
431
432impl<'a, S: spi::SpiMasterDevice<'a>> spi::SpiMasterClient for L3gd20Spi<'a, S> {
433    fn read_write_done(
434        &self,
435        write_buffer: SubSliceMut<'static, u8>,
436        read_buffer: Option<SubSliceMut<'static, u8>>,
437        status: Result<usize, ErrorCode>,
438    ) {
439        self.current_process.map(|proc_id| {
440            let _result = self.grants.enter(proc_id, |_app, upcalls| {
441                self.status.set(match self.status.get() {
442                    L3gd20Status::IsPresent => {
443                        let present = if let Some(ref buf) = read_buffer {
444                            buf[1] == L3GD20_WHO_AM_I
445                        } else {
446                            false
447                        };
448                        upcalls
449                            .schedule_upcall(0, (1, usize::from(present), 0))
450                            .ok();
451                        L3gd20Status::Idle
452                    }
453
454                    L3gd20Status::ReadXYZ => {
455                        let mut x: usize = 0;
456                        let mut y: usize = 0;
457                        let mut z: usize = 0;
458                        let values = if let Some(ref buf) = read_buffer {
459                            if status.unwrap_or(0) >= 7 {
460                                self.nine_dof_client.map(|client| {
461                                    // compute using only integers
462                                    let scale = match self.scale.get() {
463                                        0 => L3GD20_SCALE_250,
464                                        1 => L3GD20_SCALE_500,
465                                        _ => L3GD20_SCALE_2000,
466                                    };
467                                    let x: usize =
468                                        ((buf[1] as i16 | ((buf[2] as i16) << 8)) as isize * scale
469                                            / 100000)
470                                            as usize;
471                                    let y: usize =
472                                        ((buf[3] as i16 | ((buf[4] as i16) << 8)) as isize * scale
473                                            / 100000)
474                                            as usize;
475                                    let z: usize =
476                                        ((buf[5] as i16 | ((buf[6] as i16) << 8)) as isize * scale
477                                            / 100000)
478                                            as usize;
479                                    client.callback(x, y, z);
480                                });
481                                // actual computation is this one
482
483                                x = (buf[1] as i16 | ((buf[2] as i16) << 8)) as usize;
484                                y = (buf[3] as i16 | ((buf[4] as i16) << 8)) as usize;
485                                z = (buf[5] as i16 | ((buf[6] as i16) << 8)) as usize;
486                                true
487                            } else {
488                                self.nine_dof_client.map(|client| {
489                                    client.callback(0, 0, 0);
490                                });
491                                false
492                            }
493                        } else {
494                            false
495                        };
496                        if values {
497                            upcalls.schedule_upcall(0, (x, y, z)).ok();
498                        } else {
499                            upcalls.schedule_upcall(0, (0, 0, 0)).ok();
500                        }
501                        L3gd20Status::Idle
502                    }
503
504                    L3gd20Status::ReadTemperature => {
505                        let mut temperature = 0;
506                        let value = if let Some(ref buf) = read_buffer {
507                            if status.unwrap_or(0) >= 2 {
508                                temperature = buf[1] as i32;
509                                self.temperature_client.map(|client| {
510                                    client.callback(Ok(temperature * 100));
511                                });
512                                true
513                            } else {
514                                self.temperature_client.map(|client| {
515                                    client.callback(Err(ErrorCode::FAIL));
516                                });
517                                false
518                            }
519                        } else {
520                            false
521                        };
522                        if value {
523                            upcalls
524                                .schedule_upcall(0, (temperature as usize, 0, 0))
525                                .ok();
526                        } else {
527                            upcalls.schedule_upcall(0, (0, 0, 0)).ok();
528                        }
529                        L3gd20Status::Idle
530                    }
531
532                    _ => {
533                        upcalls.schedule_upcall(0, (0, 0, 0)).ok();
534                        L3gd20Status::Idle
535                    }
536                });
537            });
538        });
539        self.txbuffer.replace(write_buffer);
540        if let Some(buf) = read_buffer {
541            self.rxbuffer.replace(buf);
542        }
543    }
544}
545
546impl<'a, S: spi::SpiMasterDevice<'a>> sensors::NineDof<'a> for L3gd20Spi<'a, S> {
547    fn set_client(&self, nine_dof_client: &'a dyn sensors::NineDofClient) {
548        self.nine_dof_client.replace(nine_dof_client);
549    }
550
551    fn read_gyroscope(&self) -> Result<(), ErrorCode> {
552        if self.status.get() == L3gd20Status::Idle {
553            self.read_xyz();
554            Ok(())
555        } else {
556            Err(ErrorCode::BUSY)
557        }
558    }
559}
560
561impl<'a, S: spi::SpiMasterDevice<'a>> sensors::TemperatureDriver<'a> for L3gd20Spi<'a, S> {
562    fn set_client(&self, temperature_client: &'a dyn sensors::TemperatureClient) {
563        self.temperature_client.replace(temperature_client);
564    }
565
566    fn read_temperature(&self) -> Result<(), ErrorCode> {
567        if self.status.get() == L3gd20Status::Idle {
568            self.read_temperature();
569            Ok(())
570        } else {
571            Err(ErrorCode::BUSY)
572        }
573    }
574}
capsules_extra/l3gd20.rs

capsules_extra/
l3gd20.rs
