grapple_lasercan/
lib.rs

1#![no_std]
2
3extern crate alloc;
4
5pub use grapple_frc_msgs;
6
7use alloc::{string::String, borrow::{ToOwned, Cow}, boxed::Box};
8use grapple_config::{ConfigurationMarshal, ConfigurationProvider, GenericConfigurationProvider};
9use grapple_frc_msgs::{grapple::{lasercan::{LaserCanRoi, LaserCanRangingMode, LaserCanMeasurement, LaserCanRoiU4, LaserCanTimingBudget, self}, errors::{GrappleResult, GrappleError}, TaggedGrappleMessage, fragments::{FragmentReassemblerRx, FragmentReassemblerTx, FragmentReassembler}, GrappleDeviceMessage, firmware::GrappleFirmwareMessage, GrappleBroadcastMessage, device_info::{GrappleDeviceInfo, GrappleModelId}, Request, MANUFACTURER_GRAPPLE, DEVICE_TYPE_DISTANCE_SENSOR}, MessageId, ManufacturerMessage, binmarshal::{Marshal, Demarshal, BitView}, DEVICE_ID_BROADCAST, DEVICE_TYPE_FIRMWARE_UPGRADE, DEVICE_TYPE_BROADCAST};
10use grapple_frc_msgs::binmarshal;
11
12// CONFIGURATION
13
14#[derive(Debug, Clone, Marshal, Demarshal)]
15#[marshal(magic = b"LCAN//0.2.0")]
16pub struct LaserCanConfiguration {
17  pub device_id: u8,
18
19  #[marshal(align = "1")]
20  pub ranging_mode: LaserCanRangingMode,
21
22  #[marshal(align = "1")]
23  pub roi: LaserCanRoi,
24
25  #[marshal(align = "1")]
26  pub timing_budget: LaserCanTimingBudget,
27
28  #[marshal(align = "1")]
29  pub led_threshold: u16,
30
31  #[marshal(align = "1")]
32  pub name: String
33}
34
35impl Default for LaserCanConfiguration {
36  fn default() -> Self {
37    Self {
38      device_id: 0,
39      ranging_mode: LaserCanRangingMode::Short,
40      roi: LaserCanRoi { x: LaserCanRoiU4(8), y: LaserCanRoiU4(8), w: LaserCanRoiU4(16), h: LaserCanRoiU4(16) },
41      timing_budget: LaserCanTimingBudget::TB33ms,
42      led_threshold: 1000,
43      name: "LaserCAN".to_owned()
44    }
45  }
46}
47
48impl LaserCanConfiguration {
49  pub fn validate(&self) -> bool {
50    if self.device_id > 0x3F { return false }
51    if self.roi.w.0 > 16 || self.roi.h.0 > 16 { return false }
52    true
53  }
54}
55
56// TRAITS
57
58pub trait Sensor {
59  fn data_ready(&mut self) -> GrappleResult<'static, bool>;
60  fn get_result(&mut self) -> GrappleResult<'static, LaserCanMeasurement>;
61  fn stop_ranging(&mut self) -> GrappleResult<'static, ()>;
62  fn start_ranging(&mut self) -> GrappleResult<'static, ()>;
63  fn set_ranging_mode(&mut self, mode: LaserCanRangingMode) -> GrappleResult<'static, ()>;
64  fn set_roi(&mut self, roi: LaserCanRoi) -> GrappleResult<'static, ()>;
65  fn set_timing_budget_ms(&mut self, budget: LaserCanTimingBudget) -> GrappleResult<'static, ()>;
66}
67
68pub trait Watchdog {
69  fn feed(&mut self);
70}
71
72pub trait SysTick {
73  fn time(&self) -> u32;
74}
75
76pub trait DropToBootloader {
77  fn drop_to_bootloader(&mut self);
78}
79
80pub trait CanBus {
81  const MAX_MSG_SIZE: usize;
82
83  fn subscribe(&mut self, id: MessageId, mask: MessageId);
84  fn enqueue_msg(&mut self, id: MessageId, buf: &[u8]);
85}
86
87pub trait InputOutput {
88  fn set_led(&mut self, value: bool);    // false = high
89  fn toggle_led(&mut self);
90}
91
92// IMPL
93
94pub const LED_TICK_PERIOD_MS: usize = 50;
95pub const LED_TICK_IRQ_PRIORITY: usize = 1;
96
97pub const STATUS_TICK_PERIOD_MS: usize = 20;
98pub const STATUS_TICK_IRQ_PRIORITY: usize = 15;
99
100pub struct LaserCANImpl<WDG, SYST, DTB, CAN, IO> {
101  pub firmware_version: &'static str,
102  pub serial_number: u32,
103
104  pub watchdog: WDG,
105  pub systick: SYST,
106  pub to_bootloader: DTB,
107  pub can: CAN,
108  pub sensor: Box<dyn Sensor + Send + Sync>,
109  pub config: Box<dyn GenericConfigurationProvider<LaserCanConfiguration> + Send>,
110  pub io: IO,
111
112  reassemble: (FragmentReassemblerRx, FragmentReassemblerTx),
113  led_counter: u32,
114  blink_counter: u32,
115  last_result: Option<LaserCanMeasurement>,
116}
117
118impl<
119  WDG: Watchdog, SYST: SysTick, DTB: DropToBootloader,
120  CAN: CanBus, IO: InputOutput
121> LaserCANImpl<WDG, SYST, DTB, CAN, IO> {
122  pub fn new<MRSHL: ConfigurationMarshal<LaserCanConfiguration> + 'static + Send>(
123    firmware_version: &'static str,
124    serial_number: u32,
125    watchdog: WDG,
126    systick: SYST,
127    to_bootloader: DTB,
128    can: CAN,
129    sensor: Box<dyn Sensor + Send + Sync>,
130    marshal: MRSHL,
131    io: IO
132  ) -> GrappleResult<'static, Self> {
133    let provider = Box::new(ConfigurationProvider::new(marshal)
134      .map_err(|_| GrappleError::Generic(Cow::Borrowed("Configuration Provider Initialisation Failed").into()))?);
135
136    let mut s = Self {
137      firmware_version,
138      serial_number,
139
140      watchdog, systick, to_bootloader, can,
141      sensor, config: provider, io,
142
143      reassemble: FragmentReassembler::new(1000, CAN::MAX_MSG_SIZE).split(),
144      led_counter: 0, blink_counter: 40,
145      last_result: None
146    };
147
148    s.init()?;
149
150    Ok(s)
151  }
152
153  fn init(&mut self) -> GrappleResult<'static, ()> {
154    // Try to apply the configuration, but get rid of it if the configuration is invalid.
155    let cfg = self.config.current().clone();
156    match self.apply_configuration(&cfg) {
157      Ok(_) => (),
158      Err(_) => {
159        *self.config.current_mut() = LaserCanConfiguration::default();
160        self.config.commit();
161        let cfg = self.config.current().clone();
162        self.apply_configuration(&cfg).ok();
163      },
164    };
165
166    // Firmware Update Messages
167    self.can.subscribe(
168      MessageId { device_type: DEVICE_TYPE_FIRMWARE_UPGRADE, manufacturer: MANUFACTURER_GRAPPLE, api_class: 0x00, api_index: 0x00, device_id: DEVICE_ID_BROADCAST },
169      MessageId { device_type: 0xFF, manufacturer: 0xFF, api_class: 0x00, api_index: 0x00, device_id: 0xFF }
170    );
171
172    // Broadcast Messages
173    self.can.subscribe(
174      MessageId { device_type: DEVICE_TYPE_BROADCAST, manufacturer: MANUFACTURER_GRAPPLE, api_class: 0x00, api_index: 0x00, device_id: DEVICE_ID_BROADCAST },
175      MessageId { device_type: 0xFF, manufacturer: 0xFF, api_class: 0x00, api_index: 0x00, device_id: 0xFF }
176    );
177
178    // Specific to this sensor. Note we don't check for Device ID here, since it may change whilst we're still booted.
179    self.can.subscribe(
180      MessageId { device_type: DEVICE_TYPE_DISTANCE_SENSOR, manufacturer: MANUFACTURER_GRAPPLE, api_class: 0x00, api_index: 0x00, device_id: 0x00 },
181      MessageId { device_type: 0xFF, manufacturer: 0xFF, api_class: 0x00, api_index: 0x00, device_id: 0x00 }
182    );
183
184    Ok(())
185  }
186
187  // ============================
188  //        INTERRUPTS
189  // ============================
190
191  pub fn on_led_tick(&mut self) {
192    if self.blink_counter > 0 {
193      if self.led_counter % 2 == 0 {
194        self.io.toggle_led();
195      }
196      self.blink_counter -= 1;
197    } else if let Some(lr) = &self.last_result {
198      let led_thresh = self.config.current().led_threshold;
199      if lr.status == 0 && lr.distance_mm < led_thresh && led_thresh > 20 {
200        let partial = ((lr.distance_mm as u32) * 20) / (led_thresh as u32) + 1;   // Plus one so we don't modulo by zero and crash the MCU :)
201        if self.led_counter % partial == 0 {
202          self.io.toggle_led();
203        }
204      } else {
205        self.io.set_led(false);
206      }
207    } else {
208      self.io.set_led(false);
209    }
210
211    self.led_counter = self.led_counter.wrapping_add(1);
212  }
213
214  // TODO: Move this to be interrupt driven, by TOF_INT
215  pub fn on_status_tick(&mut self) {
216    self.watchdog.feed();
217
218    if Ok(true) == self.sensor.data_ready() {
219      if let Ok(r) = self.sensor.get_result() {
220        self.enqueue_can_msg(TaggedGrappleMessage::new(
221          self.config.current().device_id,
222          GrappleDeviceMessage::DistanceSensor(
223            lasercan::LaserCanMessage::Measurement(lasercan::LaserCanMeasurement {
224              status: r.status,
225              distance_mm: r.distance_mm,
226              ambient: r.ambient,
227              mode: self.config.current().ranging_mode.clone(),
228              budget: self.config.current().timing_budget.clone(),
229              roi: self.config.current().roi.clone()
230            })
231          )
232        )).ok();
233
234        self.last_result = Some(r); 
235      }
236    }
237  }
238
239  pub fn on_can_message(&mut self, id: MessageId, buf: &[u8]) -> /* was something processed? */ bool {
240    if id.device_id != DEVICE_ID_BROADCAST && id.device_id != self.config.current().device_id {
241      return false;
242    }
243
244    match ManufacturerMessage::read(&mut BitView::new(buf), id.clone()) {
245      Ok(ManufacturerMessage::Grapple(grpl_msg)) => {
246        let mut storage: smallvec::SmallVec<[u8; 64]> = smallvec::SmallVec::new();
247        
248        match self.reassemble.0.defragment(self.systick.time() as i64, &id, grpl_msg, &mut storage) {
249          Ok(Some(msg)) => match msg {
250            GrappleDeviceMessage::Broadcast(bcast) => match bcast {
251              GrappleBroadcastMessage::DeviceInfo(di) => match di {
252                GrappleDeviceInfo::EnumerateRequest => {
253                  let name = self.config.current().name.clone();
254
255                  let new_msg = TaggedGrappleMessage::new(
256                    self.config.current().device_id,
257                    GrappleDeviceMessage::Broadcast(
258                      GrappleBroadcastMessage::DeviceInfo(GrappleDeviceInfo::EnumerateResponse {
259                        model_id: GrappleModelId::LaserCan,
260                        serial: self.serial_number,
261                        is_dfu: false,
262                        is_dfu_in_progress: false,
263                        version: Cow::Borrowed(self.firmware_version).into(),
264                        name: Cow::Borrowed(name.as_str()).into()
265                      })
266                    )
267                  );
268
269                  self.enqueue_can_msg(new_msg).ok();
270                },
271                GrappleDeviceInfo::Blink { serial } if serial == self.serial_number => {
272                  self.blink_counter = 60;
273                },
274                GrappleDeviceInfo::SetName { serial, name } if serial == self.serial_number => {
275                  self.config.current_mut().name = name.into_owned();
276                  self.config.commit();
277                },
278                GrappleDeviceInfo::SetId { serial, new_id } if serial == self.serial_number => {
279                  self.config.current_mut().device_id = new_id;
280                  self.config.commit();
281                },
282                GrappleDeviceInfo::CommitConfig { serial } if serial == self.serial_number => {
283                  self.config.commit();
284                },
285                _ => ()
286              }
287            },
288            GrappleDeviceMessage::DistanceSensor(dist) => match dist {
289              lasercan::LaserCanMessage::SetRange(Request::Request(range)) => {
290                let response = self.try_configuration_change(|cfg| cfg.ranging_mode = range);
291
292                self.enqueue_can_msg(TaggedGrappleMessage::new(
293                  self.config.current().device_id,
294                  GrappleDeviceMessage::DistanceSensor(lasercan::LaserCanMessage::SetRange(
295                    Request::Ack(response)
296                  ))
297                )).ok();
298              },
299              lasercan::LaserCanMessage::SetRoi(Request::Request(roi)) => {
300                let response = self.try_configuration_change(|cfg| cfg.roi = roi);
301
302                self.enqueue_can_msg(TaggedGrappleMessage::new(
303                  self.config.current().device_id,
304                  GrappleDeviceMessage::DistanceSensor(lasercan::LaserCanMessage::SetRoi(
305                    Request::Ack(response)
306                  ))
307                )).ok();
308              },
309              lasercan::LaserCanMessage::SetTimingBudget(Request::Request(budget)) => {
310                let response = self.try_configuration_change(|cfg| cfg.timing_budget = budget);
311
312                self.enqueue_can_msg(TaggedGrappleMessage::new(
313                  self.config.current().device_id,
314                  GrappleDeviceMessage::DistanceSensor(lasercan::LaserCanMessage::SetTimingBudget(
315                    Request::Ack(response)
316                  ))
317                )).ok();
318              },
319              lasercan::LaserCanMessage::SetLedThreshold(Request::Request(thresh)) => {
320                self.config.current_mut().led_threshold = thresh;
321                self.config.commit();
322                
323                self.enqueue_can_msg(TaggedGrappleMessage::new(
324                  self.config.current().device_id,
325                  GrappleDeviceMessage::DistanceSensor(lasercan::LaserCanMessage::SetLedThreshold(
326                    Request::Ack(Ok(()))
327                  ))
328                )).ok();
329              },
330              _ => ()
331            },
332            GrappleDeviceMessage::FirmwareUpdate(GrappleFirmwareMessage::StartFieldUpgrade { serial }) if serial == self.serial_number => {
333              self.to_bootloader.drop_to_bootloader();
334            },
335            _ => ()
336          },
337          _ => ()
338        }
339      },
340      _ => ( /* Ignore any malformed messages */ ),
341    }
342
343    true
344  }
345
346  // ============================
347  //          INTERNAL
348  // ============================
349
350  fn try_configuration_change<F: FnOnce(&mut LaserCanConfiguration)>(&mut self, f: F) -> GrappleResult<'static, ()> {
351    let mut candidate = self.config.current().clone();
352    f(&mut candidate);
353
354    match self.apply_configuration(&candidate) {
355      Ok(()) => {
356        *self.config.current_mut() = candidate;
357        self.config.commit();
358        Ok(())
359      },
360      Err(e) => {
361        let cfg = self.config.current().clone();
362        self.apply_configuration(&cfg).ok();
363        Err(e)
364      }
365    }
366  }
367
368  fn apply_configuration(&mut self, cfg: &LaserCanConfiguration) -> GrappleResult<'static, ()> {
369    if !cfg.validate() {
370      Err(GrappleError::FailedAssertion(Cow::Borrowed("Invalid Configuration!").into()))?;
371    }
372
373    self.sensor.stop_ranging()?;
374    self.sensor.set_ranging_mode(cfg.ranging_mode.clone())?;
375    self.sensor.set_roi(cfg.roi.clone())?;
376    self.sensor.set_timing_budget_ms(cfg.timing_budget.clone())?;
377    self.sensor.start_ranging()?;
378    Ok(())
379  }
380
381  fn enqueue_can_msg(&mut self, msg: TaggedGrappleMessage) -> GrappleResult<'static, ()> {
382    self.reassemble.1.maybe_fragment(msg.device_id, msg.msg, &mut |id, buf| {
383      self.can.enqueue_msg(id, buf)
384    }).map_err(|_| GrappleError::Generic(Cow::Borrowed("Failed to encode message").into()))?;
385    Ok(())
386  }
387}
grapple_lasercan/lib.rs

grapple_lasercan/
lib.rs
