2 * Copyright (c) 2017-2020 Tilman Sauerbeck (tilman at code-monkey de)
4 * Permission is hereby granted, free of charge, to any person obtaining
5 * a copy of this software and associated documentation files (the
6 * "Software"), to deal in the Software without restriction, including
7 * without limitation the rights to use, copy, modify, merge, publish,
8 * distribute, sublicense, and/or sell copies of the Software, and to
9 * permit persons to whom the Software is furnished to do so, subject to
10 * the following conditions:
12 * The above copyright notice and this permission notice shall be
13 * included in all copies or substantial portions of the Software.
15 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
16 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
17 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
18 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
19 * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
20 * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
21 * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
27 use systick::elapsed_ms;
29 pub const MEMORY_SIZE: usize = 2 << 20;
30 const SECTOR_SIZE: usize = 4 << 10;
32 const NUM_SECTORS: usize = MEMORY_SIZE / SECTOR_SIZE;
38 // Overrides InUse. The idea is to have erased flash sectors
39 // (0xff..ff) be detected as not in use.
44 #[derive(Clone, Copy)]
46 flags: u16, // Combination of SectorFlag items.
47 recording_id: u16, // Zero is considered invalid.
48 start_time: u32, // UNIX time. Only present if flag DataRecord isn't set.
52 fn new() -> SectorHeader {
60 fn is_in_use(&self) -> bool {
61 let mask = (SectorFlag::InUse as u16) | (SectorFlag::NotInUse as u16);
62 let value = SectorFlag::InUse as u16;
64 (self.flags & mask) == value
67 fn starts_recording(&self) -> bool {
68 let mask = (SectorFlag::InUse as u16)
69 | (SectorFlag::DataRecord as u16)
70 | (SectorFlag::NotInUse as u16);
71 let value = SectorFlag::InUse as u16;
73 (self.flags & mask) == value
77 #[derive(Clone, Copy)]
85 fn new() -> InFlight {
94 pub struct Logger<'a> {
95 storage: &'a mut dyn Storage,
97 recording_id: u16, // Zero is considered invalid.
99 // The index of the first sector of the currently running recording.
100 // Only written in logger_start_recording.
103 recording_started: u32,
105 // The number of slots filled in num_flight.
106 num_in_flight: usize,
108 // Deltas not yet written out to write_buffer.
110 // Limiting ourselves to 7 items here means we can use
111 // 0xff as a padding byte.
112 in_flight: [InFlight; 7],
114 sector_header: [SectorHeader; NUM_SECTORS],
116 sectors_written: u16,
118 write_buffer_offset: usize,
119 write_buffer: [u8; SECTOR_SIZE],
122 struct SectorHeaderIter<'a> {
123 sector_header: &'a [SectorHeader; NUM_SECTORS],
126 indices: [u16; NUM_SECTORS],
129 fn cmp_sector_header_indices(a: u16, b: u16,
130 sector_header: &[SectorHeader]) -> i32 {
131 let header_a = §or_header[a as usize];
132 let header_b = §or_header[b as usize];
134 // Latest entries come first.
135 if header_a.start_time > header_b.start_time {
137 } else if header_a.start_time < header_b.start_time {
144 fn downheap(heap: &mut [u16], mut index: usize, num_elts: usize,
145 sector_header: &[SectorHeader]) {
146 let orig = heap[index];
149 let mut worker = index * 2;
151 if worker < num_elts &&
152 cmp_sector_header_indices(heap[worker], heap[worker + 1], sector_header) < 0 {
156 if worker > num_elts ||
157 cmp_sector_header_indices(orig, heap[worker], sector_header) >= 0 {
161 heap[index] = heap[worker];
168 impl<'a> SectorHeaderIter<'a> {
169 fn new(logger: &'a Logger) -> SectorHeaderIter<'a> {
170 let mut iter = SectorHeaderIter {
171 sector_header: &logger.sector_header,
173 it_back: NUM_SECTORS,
174 indices: [0; NUM_SECTORS]
177 let mut num_used = 0;
179 // Put the indices of the used directory entries at the beginning
180 // of the array. Ignore the unused ones since we are not going
181 // to sort them anyway.
182 for i in 0..NUM_SECTORS {
183 let sector_header = &iter.sector_header[i];
185 if sector_header.starts_recording() {
186 iter.indices[num_used] = i as u16;
191 let num_elts_to_sort = num_used;
193 if num_elts_to_sort != 0 {
194 // Sort the used directory entries.
195 for i in (1..((num_elts_to_sort + 1) / 2) + 1).rev() {
196 downheap(&mut iter.indices, i - 1, num_elts_to_sort - 1,
200 for i in (1..num_elts_to_sort).rev() {
201 let t = iter.indices[0];
202 iter.indices[0] = iter.indices[i];
205 downheap(&mut iter.indices, 0, i - 1, iter.sector_header);
209 // Now put the indices of the unused directory entries in the array.
211 for i in 0..NUM_SECTORS {
212 iter.indices[i] = i as u16;
215 let latest_used = iter.indices[0] as usize;
216 let mut offset_unused = num_used;
218 // First put the entries that come after the latest one in use...
219 for i in (latest_used + 1)..NUM_SECTORS {
220 let sector_header = &iter.sector_header[i];
222 if !sector_header.is_in_use() {
223 iter.indices[offset_unused] = i as u16;
228 // ... then wrap around if necessary.
229 for i in 0..latest_used {
230 let sector_header = &iter.sector_header[i];
232 if !sector_header.is_in_use() {
233 iter.indices[offset_unused] = i as u16;
240 // Need to handle those sectors that don't start recordings
241 // but that are still used.
247 impl<'a> Iterator for SectorHeaderIter<'a> {
250 fn next(&mut self) -> Option<usize> {
251 if self.it_front == self.it_back {
254 let next_index = self.indices[self.it_front] as usize;
263 impl<'a> DoubleEndedIterator for SectorHeaderIter<'a> {
264 fn next_back(&mut self) -> Option<usize> {
265 if self.it_back == self.it_front {
270 let next_index = self.indices[self.it_back] as usize;
277 fn normalize_angle(mut angle: i32) -> i32 {
278 let deg90 = 90 * 60 * 10000;
279 let deg180 = deg90 << 1;
280 let deg360 = deg180 << 1;
282 while angle >= deg180 {
286 while angle <= -deg180 {
293 fn max<T>(a: T, b: T) -> T
294 where T: PartialOrd {
302 impl<'a> Logger<'a> {
303 pub fn new(storage: &'a mut dyn Storage) -> Logger {
309 recording_started: 0,
312 in_flight: [InFlight::new(); 7],
313 sector_header: [SectorHeader::new(); NUM_SECTORS],
317 write_buffer_offset: 0,
318 write_buffer: [0xff; SECTOR_SIZE],
322 pub fn init(&mut self) {
323 // Reading the directory entries one by one means
324 // we won't need an as large buffer on the stack.
325 for i in 0..NUM_SECTORS {
326 self.read_sector_header(i);
330 fn read_sector_header(&mut self, sector_index: usize) {
331 let address = sector_index * SECTOR_SIZE;
332 let mut chunk = [0u8; 4];
334 self.storage.read(address, &mut chunk);
336 let sector_header_ptr: *mut SectorHeader =
337 &mut self.sector_header[sector_index];
340 core::ptr::copy(chunk.as_ptr(),
341 sector_header_ptr as *mut u8,
346 fn prepare_write_buffer(&mut self, is_initial: bool) {
347 self.write_buffer = [0xff; SECTOR_SIZE];
349 let flags = if is_initial {
350 (SectorFlag::InUse as u16)
352 (SectorFlag::InUse as u16) | (SectorFlag::DataRecord as u16)
355 // Write sector header.
356 self.write_buffer[0..2].copy_from_slice(&flags.to_le_bytes());
357 self.write_buffer[2..4].copy_from_slice(&self.recording_id.to_le_bytes());
359 self.write_buffer_offset = 4;
362 let start = self.write_buffer_offset;
365 self.write_buffer[start..end].copy_from_slice(
366 &self.recording_started.to_le_bytes());
368 self.write_buffer_offset += 4;
372 pub fn start_recording(&mut self, tap: &TimeAndPos) -> u16 {
373 self.find_next_record_slot();
375 self.sectors_written = 0;
376 self.recording_started = tap.unix_time;
377 self.num_in_flight = 0;
379 self.prepare_write_buffer(true);
381 self.write_packet(0, tap.latitude, tap.longitude);
386 pub fn log(&mut self, prev_tap: &TimeAndPos, tap: &TimeAndPos) {
387 let d_time_ms = elapsed_ms(tap.system_time, prev_tap.system_time);
389 // We know that our hardware cannot deliver updates more often
390 // than once a second. However when there's a delay in evaluating
391 // the hardware's messages, we will end up with intervals like
392 // 1050ms and 950ms (the latter will "make up" for the slowness
393 // in the former). To avoid logging deltas of 0 seconds, we round
394 // the intervals to full seconds.
395 let d_time_s = (d_time_ms + 500) / 1000;
397 let d_lat = tap.latitude - prev_tap.latitude;
398 let d_lon = tap.longitude - prev_tap.longitude;
400 if self.write_packet(d_time_s, d_lat, d_lon) {
401 self.flush_in_flight(false);
405 pub fn stop_recording(&mut self, tap: &TimeAndPos) -> u16 {
406 // Mark the end of the points stream.
407 self.write_packet(0xffffffff, 0, 0);
408 self.flush_in_flight(true);
411 let duration = (tap.unix_time - self.recording_started) as u16;
413 let start = self.write_buffer_offset;
415 let dst = &mut self.write_buffer[start..end];
417 dst.copy_from_slice(&duration.to_le_bytes());
419 let this_sector = self.first_sector + self.sectors_written;
421 self.storage.write(this_sector as usize * SECTOR_SIZE,
424 self.sectors_written += 1;
426 for i in 0..self.sectors_written {
427 self.read_sector_header((self.first_sector + i) as usize);
433 fn sector_header_iter(&self) -> SectorHeaderIter {
434 SectorHeaderIter::new(self)
437 fn find_next_record_slot(&mut self) {
438 let mut candidate_index = 0;
439 let mut max_recording_id = 0;
441 for index in self.sector_header_iter() {
442 candidate_index = index;
444 let sector_header = &self.sector_header[index];
446 if !sector_header.is_in_use() {
447 // Due to our sorting we know that there will be no more
448 // used directory entries following. At this point
449 // we aren't interested in unused ones, so break the loop.
454 max(max_recording_id, sector_header.recording_id);
457 self.first_sector = candidate_index as u16;
458 self.recording_id = max_recording_id.wrapping_add(1);
461 fn write_packet(&mut self, d_time_s: u32, d_lat: i32, d_lon: i32) -> bool {
463 let in_flight = &mut self.in_flight[self.num_in_flight];
465 in_flight.d_time_s = d_time_s;
466 in_flight.d_lat = normalize_angle(d_lat);
467 in_flight.d_lon = normalize_angle(d_lon);
470 self.num_in_flight += 1;
472 self.num_in_flight == self.in_flight.len()
475 // Flushes the "in flight" items to the write buffer.
477 // @param is_final @c true iff this is the final flush in this recording.
479 // @note May only be called if logger.num_in_flight is greater than zero.
480 fn flush_in_flight(&mut self, is_final: bool) {
483 // Normally our items will have a time delta of one second.
484 // Mark the ones that differ from that.
485 for i in 0..self.num_in_flight {
486 if self.in_flight[i].d_time_s != 1 {
491 let mut buffer = [0u8; 128];
494 buffer[offset] = flags;
497 for i in 0..(self.num_in_flight - 1) {
498 let in_flight = &self.in_flight[i];
500 // Only write the time delta for the atypical cases.
501 if (flags & (1 << i)) != 0 {
503 varint::write_u32(&mut buffer[offset..], in_flight.d_time_s);
507 varint::write_s32(&mut buffer[offset..], in_flight.d_lat);
510 varint::write_s32(&mut buffer[offset..], in_flight.d_lon);
513 let i = self.num_in_flight - 1;
514 let in_flight = &self.in_flight[i];
516 // Only write the time delta for the atypical cases.
517 if (flags & (1 << i)) != 0 {
519 varint::write_u32(&mut buffer[offset..], in_flight.d_time_s);
522 // The final point is an end-of-stream marker and doesn't store
526 varint::write_s32(&mut buffer[offset..], in_flight.d_lat);
529 varint::write_s32(&mut buffer[offset..], in_flight.d_lon);
532 self.num_in_flight = 0;
534 let num_bytes_written = offset;
536 let remaining = self.write_buffer.len() - self.write_buffer_offset;
538 if remaining < num_bytes_written {
539 // We may use 0xff as padding bytes, since 0xff isn't a valid
540 // first byte in a points batch. prepare_write_buffer() fills
541 // our buffer with 0xff, so we don't need to do anything here.
542 let this_sector = self.first_sector + self.sectors_written;
544 self.storage.write(this_sector as usize * SECTOR_SIZE,
547 self.sectors_written += 1;
549 self.prepare_write_buffer(false);
552 let start = self.write_buffer_offset;
553 let end = start + num_bytes_written;
554 let dst = &mut self.write_buffer[start..end];
556 dst.copy_from_slice(&buffer[0..num_bytes_written]);
558 self.write_buffer_offset += num_bytes_written;