1 // 2 // Copyright (c) 2010-2024 Antmicro 3 // 4 // This file is licensed under the MIT License. 5 // Full license text is available in 'licenses/MIT.txt'. 6 // 7 using System; 8 using System.Linq; 9 using System.IO; 10 using System.Collections; 11 using System.Collections.Generic; 12 using Antmicro.Renode.Core; 13 using Antmicro.Renode.Time; 14 using Antmicro.Renode.Core.Structure.Registers; 15 using Antmicro.Renode.Exceptions; 16 using Antmicro.Renode.Logging; 17 using Antmicro.Renode.Peripherals.I2C; 18 using Antmicro.Renode.Peripherals.Sensor; 19 using Antmicro.Renode.Utilities; 20 using Antmicro.Renode.Utilities.RESD; 21 22 namespace Antmicro.Renode.Peripherals.Sensors 23 { 24 // TODO: FIFO for other data than the accelerometer 25 public class LIS2DW12 : II2CPeripheral, IProvidesRegisterCollection<ByteRegisterCollection>, ITemperatureSensor, IUnderstandRESD 26 { LIS2DW12(IMachine machine)27 public LIS2DW12(IMachine machine) 28 { 29 RegistersCollection = new ByteRegisterCollection(this); 30 Interrupt1 = new GPIO(); 31 Interrupt2 = new GPIO(); 32 readyEnabledAcceleration = new IFlagRegisterField[2]; 33 fifoThresholdEnabled = new IFlagRegisterField[2]; 34 fifoFullEnabled = new IFlagRegisterField[2]; 35 DefineRegisters(); 36 37 accelerationFifo = new LIS2DW12_FIFO(this, "fifo", MaxFifoSize); 38 accelerationFifo.OnOverrun += UpdateInterrupts; 39 // Set the default acceleration here, it needs to be preserved across resets 40 DefaultAccelerationZ = 1m; 41 42 this.machine = machine; 43 } 44 FeedAccelerationSample(decimal x, decimal y, decimal z, uint repeat = 1)45 public void FeedAccelerationSample(decimal x, decimal y, decimal z, uint repeat = 1) 46 { 47 FeedAccelerationSampleInner(x, y, z, keepOnReset: true, repeat: repeat); 48 } 49 50 [OnRESDSample(SampleType.Acceleration)] 51 [BeforeRESDSample(SampleType.Acceleration)] HandleAccelerationSample(AccelerationSample sample, TimeInterval timestamp)52 private void HandleAccelerationSample(AccelerationSample sample, TimeInterval timestamp) 53 { 54 if(sample != null) 55 { 56 // Divide by 10^6 as RESD specification says AccelerationSamples are in μg, 57 // while the peripheral's measurement unit is g. 58 FeedAccelerationSampleInner( 59 sample.AccelerationX / 1e6m, 60 sample.AccelerationY / 1e6m, 61 sample.AccelerationZ / 1e6m, 62 keepOnReset: false 63 ); 64 } 65 else 66 { 67 FeedAccelerationSampleInner( 68 DefaultAccelerationX, 69 DefaultAccelerationY, 70 DefaultAccelerationZ, 71 keepOnReset: false 72 ); 73 } 74 } 75 76 [AfterRESDSample(SampleType.Acceleration)] HandleAccelerationSampleEnded(AccelerationSample sample, TimeInterval timestamp)77 private void HandleAccelerationSampleEnded(AccelerationSample sample, TimeInterval timestamp) 78 { 79 feederThread?.Stop(); 80 feederThread = null; 81 accelerationFifo.KeepFifoOnReset = true; 82 } 83 FeedAccelerationSamplesFromRESD(string path, uint channel = 0, ulong startTime = 0, RESDStreamSampleOffset sampleOffsetType = RESDStreamSampleOffset.Specified, long sampleOffsetTime = 0, RESDType type = RESDType.Normal)84 public void FeedAccelerationSamplesFromRESD(string path, uint channel = 0, ulong startTime = 0, 85 RESDStreamSampleOffset sampleOffsetType = RESDStreamSampleOffset.Specified, long sampleOffsetTime = 0, 86 RESDType type = RESDType.Normal) 87 { 88 if(type == RESDType.MultiFrequency) 89 { 90 FeedMultiFrequencyAccelerationSamplesFromRESD(path, startTime); 91 return; 92 } 93 else if(type != RESDType.Normal) 94 { 95 throw new RecoverableException($"Unhandled RESD type {type}"); 96 } 97 98 resdStream = this.CreateRESDStream<AccelerationSample>(path, channel, sampleOffsetType, sampleOffsetTime); 99 accelerationFifo.KeepFifoOnReset = false; 100 feederThread?.Stop(); 101 feederThread = resdStream.StartSampleFeedThread(this, 102 SampleRate, 103 startTime: startTime 104 ); 105 } 106 FeedAccelerationSample(string path)107 public void FeedAccelerationSample(string path) 108 { 109 accelerationFifo.KeepFifoOnReset = true; 110 accelerationFifo.FeedSamplesFromFile(path); 111 UpdateInterrupts(); 112 } 113 FinishTransmission()114 public void FinishTransmission() 115 { 116 regAddress = 0; 117 state = State.WaitingForRegister; 118 this.Log(LogLevel.Noisy, "Transmission finished"); 119 } 120 Reset()121 public void Reset() 122 { 123 RegistersCollection.Reset(); 124 SetScaleDivider(); 125 state = State.WaitingForRegister; 126 regAddress = 0; 127 Interrupt1.Set(false); 128 Interrupt2.Set(false); 129 } 130 Write(byte[] data)131 public void Write(byte[] data) 132 { 133 if(data.Length == 0) 134 { 135 this.Log(LogLevel.Warning, "Unexpected write with no data"); 136 return; 137 } 138 this.Log(LogLevel.Noisy, "Write with {0} bytes of data: {1}", data.Length, Misc.PrettyPrintCollectionHex(data)); 139 if(state == State.WaitingForRegister) 140 { 141 regAddress = (Registers)data[0]; 142 this.Log(LogLevel.Noisy, "Preparing to access register {0} (0x{0:X})", regAddress); 143 state = State.WaitingForData; 144 data = data.Skip(1).ToArray(); 145 } 146 if(state == State.WaitingForData) 147 { 148 InternalWrite(data); 149 } 150 } 151 Read(int count)152 public byte[] Read(int count) 153 { 154 if(state == State.WaitingForRegister) 155 { 156 this.Log(LogLevel.Warning, "Unexpected read in state {0}", state); 157 return new byte[count]; 158 } 159 this.Log(LogLevel.Noisy, "Reading {0} bytes from register {1} (0x{1:X})", count, regAddress); 160 var result = new byte[count]; 161 for(var i = 0; i < result.Length; i++) 162 { 163 result[i] = RegistersCollection.Read((byte)regAddress); 164 this.Log(LogLevel.Noisy, "Read value 0x{0:X}", result[i]); 165 if(autoIncrement.Value) 166 { 167 RegistersAutoIncrement(); 168 } 169 } 170 return result; 171 } 172 173 public decimal AccelerationX 174 { 175 get => CurrentSample.X; 176 set 177 { 178 if(IsAccelerationOutOfRange(value)) 179 { 180 return; 181 } 182 CurrentSample.X = value; 183 this.Log(LogLevel.Noisy, "AccelerationX set to {0}", value); 184 UpdateInterrupts(); 185 } 186 } 187 188 public decimal AccelerationY 189 { 190 get => CurrentSample.Y; 191 set 192 { 193 if(IsAccelerationOutOfRange(value)) 194 { 195 return; 196 } 197 CurrentSample.Y = value; 198 this.Log(LogLevel.Noisy, "AccelerationY set to {0}", value); 199 UpdateInterrupts(); 200 } 201 } 202 203 public decimal AccelerationZ 204 { 205 get => CurrentSample.Z; 206 set 207 { 208 if(IsAccelerationOutOfRange(value)) 209 { 210 return; 211 } 212 CurrentSample.Z = value; 213 this.Log(LogLevel.Noisy, "AccelerationZ set to {0}", value); 214 UpdateInterrupts(); 215 } 216 } 217 218 public decimal DefaultAccelerationX 219 { 220 get => DefaultSample.X; 221 set 222 { 223 if(IsAccelerationOutOfRange(value)) 224 { 225 return; 226 } 227 DefaultSample.X = value; 228 this.Log(LogLevel.Noisy, "DefaultAccelerationX set to {0}", value); 229 UpdateInterrupts(); 230 } 231 } 232 233 public decimal DefaultAccelerationY 234 { 235 get => DefaultSample.Y; 236 set 237 { 238 if(IsAccelerationOutOfRange(value)) 239 { 240 return; 241 } 242 DefaultSample.Y = value; 243 this.Log(LogLevel.Noisy, "DefaultAccelerationY set to {0}", value); 244 UpdateInterrupts(); 245 } 246 } 247 248 public decimal DefaultAccelerationZ 249 { 250 get => DefaultSample.Z; 251 set 252 { 253 if(IsAccelerationOutOfRange(value)) 254 { 255 return; 256 } 257 DefaultSample.Z = value; 258 this.Log(LogLevel.Noisy, "DefaultAccelerationZ set to {0}", value); 259 UpdateInterrupts(); 260 } 261 } 262 263 public decimal Temperature 264 { 265 get => temperature; 266 set 267 { 268 if(IsTemperatureOutOfRange(value)) 269 { 270 return; 271 } 272 temperature = value; 273 this.Log(LogLevel.Noisy, "Temperature set to {0}", temperature); 274 UpdateInterrupts(); 275 } 276 } 277 278 public GPIO Interrupt1 { get; } 279 public GPIO Interrupt2 { get; } 280 public ByteRegisterCollection RegistersCollection { get; } 281 public uint SampleRate 282 { 283 get => sampleRate; 284 private set 285 { 286 sampleRate = value; 287 if(feederThread != null) 288 { 289 feederThread.Frequency = sampleRate; 290 } 291 this.Log(LogLevel.Debug, "Sampling rate set to {0}", SampleRate); 292 SampleRateChanged?.Invoke(value); 293 } 294 } 295 FeedMultiFrequencyAccelerationSamplesFromRESD(string path, ulong startTime)296 private void FeedMultiFrequencyAccelerationSamplesFromRESD(string path, ulong startTime) 297 { 298 var parser = new LowLevelRESDParser(path); 299 var mapping = parser.GetDataBlockEnumerator<AccelerationSample>() 300 .OfType<ConstantFrequencySamplesDataBlock<AccelerationSample>>() 301 .Select(block => new { Channel = block.ChannelId, block.Frequency, block.StartTime, block.SamplesCount }) 302 .ToList(); 303 304 // Count samples in blocks with numbers between blockFirst and blockLast (both exclusive) 305 Func<int, int, ulong> countSkippedSamples = (blockFirst, blockLast) => 306 { 307 var skippedSamples = mapping.Skip(blockFirst).Take(blockLast - blockFirst - 1) 308 .Aggregate(0UL, (samplesCount, block) => samplesCount + block.SamplesCount); 309 return skippedSamples; 310 }; 311 312 // Keep track of additional statistics for improved logging 313 var previousBlockNumber = 0; 314 var numberOfSampleInBlock = 0ul; 315 var samplesInBlock = 0ul; 316 var repeatCounter = 0ul; 317 var prevSampleRate = 0u; 318 var afterSampleRateChange = false; 319 var previousRESDStreamStatus = RESDStreamStatus.BeforeStream; 320 321 Action<ulong> resetCurrentBlockStats = (samplesCnt) => 322 { 323 samplesInBlock = samplesCnt; 324 numberOfSampleInBlock = 0; 325 repeatCounter = 0; 326 }; 327 328 // We keep the number of the last block used so far so that we can skip already used blocks on each event 329 var blockNumber = 0; 330 var init = true; 331 Action<uint> findAndActivate = null; 332 findAndActivate = (sampleRate) => 333 { 334 if(sampleRate != prevSampleRate) 335 { 336 afterSampleRateChange = true; 337 } 338 else if(afterSampleRateChange) 339 { 340 if(fifoModeSelection.Value == FIFOModeSelection.FIFOMode) 341 { 342 // FIFO mode entered after changing sample rate. Repeat the previous block to synchronize with FIFO operation. 343 blockNumber = previousBlockNumber; 344 } 345 afterSampleRateChange = false; 346 } 347 previousBlockNumber = blockNumber; 348 var currentEntry = mapping 349 .Select((item, i) => new { Item = item, Index = i }) 350 .Skip(blockNumber) 351 .FirstOrDefault(o => o.Item.Frequency == sampleRate); 352 353 if(currentEntry == null) 354 { 355 // No more blocks at this sample rate 356 this.Log(LogLevel.Debug, "No more blocks for the sample rate {0}Hz in the RESD file", sampleRate); 357 feederThread?.Stop(); 358 feederThread = null; 359 resdStream?.Dispose(); 360 // If there are no more blocks, even without taking the sample rate 361 // into account, then we can unregister this sample rate change handler 362 if(blockNumber == mapping.Count) 363 { 364 this.Log(LogLevel.Debug, "No more blocks in the RESD file"); 365 SampleRateChanged -= findAndActivate; 366 FIFOModeEntered -= findAndActivate; 367 } 368 return; 369 } 370 371 blockNumber = currentEntry.Index + 1; 372 var samplesSkippedInLastBlock = previousRESDStreamStatus == RESDStreamStatus.OK ? samplesInBlock - numberOfSampleInBlock : 0; 373 this.Log(LogLevel.Noisy, "Skipped {0} blocks while moving from the RESD block with the number {1} to {2}. Skipped {3} samples from the last active block and {4} samples in total", 374 blockNumber - previousBlockNumber - 1, previousBlockNumber, blockNumber, samplesSkippedInLastBlock, samplesSkippedInLastBlock + countSkippedSamples(previousBlockNumber, blockNumber)); 375 var block = currentEntry.Item; 376 377 feederThread?.Stop(); 378 resdStream?.Dispose(); 379 380 if(machine.SystemBus.TryGetCurrentCPU(out var cpu)) 381 { 382 cpu.SyncTime(); 383 } 384 385 // Offset the start time so that we start reading the block from the beginning even if 386 // the sample rate was switched at a time different from the exact start time of the block 387 resdStream = this.CreateRESDStream<AccelerationSample>( 388 path, 389 block.Channel, 390 RESDStreamSampleOffset.Specified, 391 (long)machine.ClockSource.CurrentValue.TotalMicroseconds * -1000L + (long)block.StartTime - (init ? (long)startTime : 0), 392 b => (b as ConstantFrequencySamplesDataBlock<AccelerationSample>)?.Frequency == sampleRate 393 ); 394 resdStream.Owner = null; // turn off verbose internal RESD logging and use custom logs 395 accelerationFifo.KeepFifoOnReset = false; 396 resetCurrentBlockStats(block.SamplesCount); 397 previousRESDStreamStatus = RESDStreamStatus.BeforeStream; 398 399 // We start the thread with shouldStop: false so that it will keep feeding the last 400 // sample into the FIFO at the specified frequency until it's stopped after a sample 401 // rate switch 402 AccelerationSample previousSample = null; 403 // The same indexing as in 'ResdCommand' is used to log block's number 404 this.Log(LogLevel.Noisy, "New RESD stream for the sample rate {0}Hz with the first block with the number {1} delayed by {2}ns", sampleRate, blockNumber, init ? (long)startTime : 0); 405 feederThread = resdStream.StartSampleFeedThread(this, sampleRate, (sample, ts, status) => 406 { 407 switch(status) 408 { 409 case RESDStreamStatus.OK: 410 { 411 if(blockNumber < resdStream.CurrentBlockNumber) 412 { 413 // We received the first sample from the next block in the RESD stream 414 // as a result of internal RESD operation, not due to a triggered event 415 if(fifoModeSelection.Value == FIFOModeSelection.FIFOMode) 416 { 417 // In FIFO mode we don't allow automatic passtrough to the next block, 418 // because it would interfere with event driven FIFOModeEntered logic. 419 goto case RESDStreamStatus.BeforeStream; 420 } 421 resetCurrentBlockStats(resdStream.CurrentBlock.SamplesCount); 422 blockNumber = (int)resdStream.CurrentBlockNumber; 423 this.Log(LogLevel.Noisy, "Beginning of the new RESD block ({0}Hz) with the number {1}", sampleRate, blockNumber); 424 } 425 426 numberOfSampleInBlock++; 427 previousSample = sample; 428 this.Log(LogLevel.Noisy, "Fed current sample from the RESD block ({0}Hz) with the number {1}: {2} at {3} ({4}/{5})", sampleRate, blockNumber, previousSample, ts, numberOfSampleInBlock, samplesInBlock); 429 break; 430 } 431 case RESDStreamStatus.BeforeStream: 432 { 433 repeatCounter++; 434 this.Log(LogLevel.Noisy, "Repeated the last sample from the previous RESD block ({0}Hz) with the number {1}: {2} at {3} - {4} times", sampleRate, blockNumber, previousSample, ts, repeatCounter); 435 break; 436 } 437 case RESDStreamStatus.AfterStream: 438 { 439 repeatCounter++; 440 this.Log(LogLevel.Noisy, "No more samples in the RESD stream for the sample rate {0}Hz. Repeated the last sample from the previous RESD block with the number {1}: {2} at {3} - {4} times", sampleRate, blockNumber, previousSample, ts, repeatCounter); 441 break; 442 } 443 } 444 previousRESDStreamStatus = status; 445 446 if(previousSample != null) 447 { 448 HandleAccelerationSample(previousSample, ts); 449 } 450 }, startTime: init ? startTime : 0, shouldStop: false); 451 452 init = false; 453 prevSampleRate = sampleRate; 454 }; 455 456 findAndActivate(SampleRate); 457 SampleRateChanged += findAndActivate; 458 FIFOModeEntered += findAndActivate; 459 } 460 FeedAccelerationSampleInner(decimal x, decimal y, decimal z, bool keepOnReset, uint repeat = 1)461 private void FeedAccelerationSampleInner(decimal x, decimal y, decimal z, bool keepOnReset, uint repeat = 1) 462 { 463 if(keepOnReset) 464 { 465 // this is a simplified implementation 466 // that assumes that the `keepOnReset` 467 // status applies to all samples; 468 // might not work when mixing feeding 469 // samples from RESD and manually 470 accelerationFifo.KeepFifoOnReset = true; 471 } 472 473 var sample = new Vector3DSample(x, y, z); 474 475 if(fifoModeSelection.Value == FIFOModeSelection.Bypass && !keepOnReset) 476 { 477 CurrentSample = sample; 478 } 479 else 480 { 481 for(var i = 0; i < repeat; i++) 482 { 483 accelerationFifo.FeedSample(sample); 484 } 485 } 486 487 UpdateInterrupts(); 488 } 489 LoadNextSample()490 private void LoadNextSample() 491 { 492 if(fifoModeSelection.Value != FIFOModeSelection.Bypass || accelerationFifo.KeepFifoOnReset) 493 { 494 accelerationFifo.TryDequeueNewSample(); 495 CurrentSample = accelerationFifo.Sample; 496 } 497 this.Log(LogLevel.Noisy, "Acquired sample {0} during {1} operation", CurrentSample, fifoModeSelection.Value); 498 UpdateInterrupts(); 499 } 500 DefineRegisters()501 private void DefineRegisters() 502 { 503 Registers.TemperatureOutLow.Define(this) 504 .WithValueField(0, 8, FieldMode.Read, name: "Temperature output register in 12-bit resolution (OUT_T_L)", 505 valueProviderCallback: _ => (byte)(TwoComplementSignConvert(ReportedTemperature * TemperatureLsbsPerDegree) << 4)); 506 507 Registers.TemperatureOutHigh.Define(this) 508 .WithValueField(0, 8, FieldMode.Read, name: "Temperature output register in 12-bit resolution (OUT_T_H)", 509 valueProviderCallback: _ => (byte)(TwoComplementSignConvert(ReportedTemperature * TemperatureLsbsPerDegree) >> 4)); 510 511 Registers.WhoAmI.Define(this, 0x44); 512 513 Registers.Control1.Define(this) 514 .WithEnumField(0, 2, out lowPowerModeSelection, name: "Low-power mode selection (LP_MODE)") 515 .WithEnumField(2, 2, out modeSelection, name: "Mode selection (MODE)") 516 .WithEnumField(4, 4, out outDataRate, writeCallback: (_, __) => 517 { 518 switch(outDataRate.Value) 519 { 520 case DataRateConfig.HighPerformanceLowPower1_6Hz: 521 SampleRate = 2; 522 break; 523 case DataRateConfig.HighPerformanceLowPower12_5Hz: 524 SampleRate = 13; 525 break; 526 case DataRateConfig.HighPerformanceLowPower25Hz: 527 SampleRate = 25; 528 break; 529 case DataRateConfig.HighPerformanceLowPower50Hz: 530 SampleRate = 50; 531 break; 532 case DataRateConfig.HighPerformanceLowPower100Hz: 533 SampleRate = 100; 534 break; 535 case DataRateConfig.HighPerformanceLowPower200Hz: 536 SampleRate = 200; 537 break; 538 case DataRateConfig.HighPerformanceLowPower400Hz: 539 SampleRate = 400; 540 break; 541 case DataRateConfig.HighPerformanceLowPower800Hz: 542 SampleRate = 800; 543 break; 544 case DataRateConfig.HighPerformanceLowPower1600Hz: 545 SampleRate = 1600; 546 break; 547 default: 548 SampleRate = 0; 549 break; 550 } 551 }, name: "Output data rate and mode selection (ODR)"); 552 553 Registers.Control2.Define(this, 0x4) 554 .WithTaggedFlag("SPI serial interface mode selection (SIM)", 0) 555 .WithTaggedFlag("Disable I2C communication protocol (I2C_DISABLE)", 1) 556 .WithFlag(2, out autoIncrement, name: "Register address automatically incremented during multiple byte access with a serial interface (FF_ADD_INC)") 557 .WithTaggedFlag("Block data update (BDU)", 3) 558 .WithTaggedFlag("Disconnect CS pull-up (CS_PU_DISC)", 4) 559 .WithReservedBits(5, 1) 560 .WithFlag(6, writeCallback: (_, value) => 561 { 562 if(value) 563 { 564 Reset(); 565 } 566 }, name: "Acts as reset for all control register (SOFT_RESET)") 567 .WithTaggedFlag("Enables retrieving the correct trimming parameters from nonvolatile memory into registers (BOOT)", 7); 568 569 Registers.Control3.Define(this) 570 .WithTaggedFlag("Single data conversion on demand mode enable (SLP_MODE_1)", 0) 571 .WithTaggedFlag("Single data conversion on demand mode selection (SLP_MODE_SEL)", 1) 572 .WithReservedBits(2, 1) 573 .WithTaggedFlag("Interrupt active high, low (HL_ACTIVE)", 3) 574 .WithTaggedFlag("Latched Interrupt (LIR)", 4) 575 .WithTaggedFlag("Push-pull/open-drain selection on interrupt pad (PP_OD)", 5) 576 .WithEnumField(6, 2, out selfTestMode, name: "Self-test enable (ST)"); 577 578 Registers.Control4.Define(this) 579 .WithFlag(0, out readyEnabledAcceleration[0], name: "Data-Ready is routed to INT1 pad (INT1_DRDY)") 580 .WithFlag(1, out fifoThresholdEnabled[0], name: "FIFO threshold interrupt is routed to INT1 pad (INT1_FTH)") 581 .WithFlag(2, out fifoFullEnabled[0], name: "FIFO full recognition is routed to INT1 pad (INT1_DIFF5)") 582 .WithTaggedFlag("Double-tap recognition is routed to INT1 pad (INT1_TAP)", 3) 583 .WithTaggedFlag("Free-fall recognition is routed to INT1 pad (INT1_FF)", 4) 584 .WithTaggedFlag("Wakeup recognition is routed to INT1 pad (INT1_WU)", 5) 585 .WithTaggedFlag("Single-tap recognition is routed to INT1 pad (INT1_SINGLE_TAP)", 6) 586 .WithTaggedFlag("6D recognition is routed to INT1 pad (INT1_6D)", 7) 587 .WithWriteCallback((_, __) => UpdateInterrupts()); 588 589 Registers.Control5.Define(this) 590 .WithFlag(0, out readyEnabledAcceleration[1], name: "Data-ready is routed to INT2 pad (INT2_DRDY)") 591 .WithFlag(1, out fifoThresholdEnabled[1], name: "FIFO threshold interrupt is routed to INT2 pad (INT2_FTH)") 592 .WithFlag(2, out fifoFullEnabled[1], name: "FIFO full recognition is routed to INT2 pad (INT2_DIFF5)") 593 .WithFlag(3, out fifoOverrunEnabled, name: "FIFO overrun interrupt is routed to INT2 pad (INT2_OVR)") 594 .WithFlag(4, out readyEnabledTemperature, name: "Temperature data-ready is routed to INT2 (INT2_DRDY_T)") 595 .WithTaggedFlag("Boot state routed to INT2 pad (INT2_BOOT)", 5) 596 .WithTaggedFlag("Sleep change status routed to INT2 pad (INT2_SLEEP_CHG)", 6) 597 .WithTaggedFlag("Enable routing of SLEEP_STATE on INT2 pad (INT2_SLEEP_STATE)", 7); 598 599 Registers.Control6.Define(this) 600 .WithReservedBits(0, 2) 601 .WithTaggedFlag("Low-noise configuration (LOW_NOISE)", 2) 602 .WithTaggedFlag("Filtered data type selection (FDS)", 3) 603 .WithEnumField(4, 2, out fullScale, writeCallback: (_, __) => SetScaleDivider(), name: "Full-scale selection (FS)") 604 .WithTag("Bandwidth selection (BW_FILT1)", 6, 2); 605 606 Registers.TemperatureOut.Define(this) 607 .WithValueField(0, 8, FieldMode.Read, name: "Temperature output register in 8-bit resolution (OUT_T)", 608 valueProviderCallback: _ => (byte)TwoComplementSignConvert(ReportedTemperature)); 609 610 Registers.Status.Define(this) 611 .WithFlag(0, valueProviderCallback: _ => outDataRate.Value != DataRateConfig.PowerDown, name: "Data-ready status (DRDY)") 612 .WithTaggedFlag("Free-fall event detection status (FF_IA)", 1) 613 .WithTaggedFlag("Source of change in position portrait/landscape/face-up/face-down (6D_IA)", 2) 614 .WithTaggedFlag("Single-tap event status (SINGLE_TAP)", 3) 615 .WithTaggedFlag("Double-tap event status (DOUBLE_TAP)", 4) 616 .WithTaggedFlag("Sleep event status (SLEEP_STATE)", 5) 617 .WithTaggedFlag("Wakeup event detection status (WU_IA)", 6) 618 .WithFlag(7, FieldMode.Read, name: "FIFO threshold status flag (FIFO_THS)", 619 valueProviderCallback: _ => FifoThresholdReached); 620 621 Registers.DataOutXLow.Define(this) 622 .WithValueField(0, 8, FieldMode.Read, name: "X-axis LSB output register (OUT_X_L)", 623 valueProviderCallback: _ => 624 { 625 LoadNextSample(); 626 return Convert(ReportedAccelerationX, upperByte: false); 627 }); 628 629 Registers.DataOutXHigh.Define(this) 630 .WithValueField(0, 8, FieldMode.Read, name: "X-axis MSB output register (OUT_X_H)", 631 valueProviderCallback: _ => Convert(ReportedAccelerationX, upperByte: true)); 632 633 Registers.DataOutYLow.Define(this) 634 .WithValueField(0, 8, FieldMode.Read, name: "Y-axis LSB output register (OUT_Y_L)", 635 valueProviderCallback: _ => Convert(ReportedAccelerationY, upperByte: false)); 636 637 Registers.DataOutYHigh.Define(this) 638 .WithValueField(0, 8, FieldMode.Read, name: "Y-axis MSB output register (OUT_Y_H)", 639 valueProviderCallback: _ => Convert(ReportedAccelerationY, upperByte: true)); 640 641 Registers.DataOutZLow.Define(this) 642 .WithValueField(0, 8, FieldMode.Read, name: "Z-axis LSB output register (OUT_Z_L)", 643 valueProviderCallback: _ => Convert(ReportedAccelerationZ, upperByte: false)); 644 645 Registers.DataOutZHigh.Define(this) 646 .WithValueField(0, 8, FieldMode.Read, name: "Z-axis MSB output register (OUT_Z_H)", 647 valueProviderCallback: _ => Convert(ReportedAccelerationZ, upperByte: true)); 648 649 Registers.FifoControl.Define(this) 650 .WithValueField(0, 5, out fifoThreshold, name: "FIFO threshold level setting (FTH)") 651 .WithEnumField(5, 3, out fifoModeSelection, writeCallback: (_, newMode) => 652 { 653 if(newMode == FIFOModeSelection.Bypass) 654 { 655 accelerationFifo.Reset(); 656 } 657 }, changeCallback: (oldMode, newMode) => 658 { 659 if(oldMode == FIFOModeSelection.Bypass && newMode == FIFOModeSelection.FIFOMode) 660 { 661 FIFOModeEntered?.Invoke(SampleRate); 662 } 663 }, name: "FIFO mode selection bits (FMode)"); 664 665 Registers.FifoSamples.Define(this) 666 .WithValueField(0, 6, FieldMode.Read, name: "Number of unread samples stored in FIFO (DIFF)", 667 valueProviderCallback: _ => accelerationFifo.Disabled ? 0 : accelerationFifo.SamplesCount) 668 .WithFlag(6, FieldMode.Read, name: "FIFO overrun status (FIFO_OVR)", 669 valueProviderCallback: _ => FifoOverrunOccurred) 670 .WithFlag(7, FieldMode.Read, valueProviderCallback: _ => FifoThresholdReached, 671 name: "FIFO threshold status flag (FIFO_FTH)"); 672 673 Registers.TapThreshholdX.Define(this) 674 .WithTag("Threshold for tap recognition on X direction (TAP_THSX)", 0, 5) 675 .WithTag("Thresholds for 4D/6D function (6D_THS)", 5, 2) 676 .WithTag("4D detection portrait/landscape position enable (4D_EN)", 7, 1); 677 678 Registers.TapThreshholdY.Define(this) 679 .WithTag("Threshold for tap recognition on Y direction (TAP_THSY)", 0, 5) 680 .WithTag("Selection of priority axis for tap detection (TAP_PRIOR)", 5, 3); 681 682 Registers.TapThreshholdZ.Define(this) 683 .WithTag("Threshold for tap recognition on Z direction (TAP_THSZ)", 0, 5) 684 .WithTag("Enables Z direction in tap recognition (TAP_Z_EN)", 5, 1) 685 .WithTag("Enables Y direction in tap recognition (TAP_Y_EN)", 6, 1) 686 .WithTag("Enables X direction in tap recognition (TAP_X_EN)", 7, 1); 687 688 Registers.InterruptDuration.Define(this) 689 .WithTag("Maximum duration of over-threshold event (SHOCK)", 0, 2) 690 .WithTag("Expected quiet time after a tap detection (QUIET)", 2, 2) 691 .WithTag("Duration of maximum time gap for double-tap recognition (LATENCY)", 4, 4); 692 693 Registers.WakeupThreshhold.Define(this) 694 .WithTag("Wakeup threshold (WK_THS)", 0, 6) 695 .WithTag("Sleep (inactivity) enable (SLEEP_ON)", 6, 1) 696 .WithTag("Enable single/double-tap event (SINGLE_DOUBLE_TAP)", 7, 1); 697 698 Registers.WakeupAndSleepDuration.Define(this) 699 .WithTag("Duration to go in sleep mode (SLEEP_DUR)", 0, 4) 700 .WithTag("Enable stationary detection / motion detection (STATIONARY)", 4, 1) 701 .WithTag("Wakeup duration (WAKE_DUR)", 5, 2) 702 .WithTag("Free-fall duration (FF_DUR5)", 7, 1); 703 704 Registers.FreeFall.Define(this) 705 .WithTag("Free-fall threshold (FF_THS)", 0, 3) 706 .WithTag("Free-fall duration (FF_DUR)", 3, 5); 707 708 Registers.StatusEventDetection.Define(this) 709 .WithFlag(0, valueProviderCallback: _ => outDataRate.Value != DataRateConfig.PowerDown, name: "Data-ready status (DRDY)") 710 .WithTaggedFlag("Free-fall event detection status (FF_IA)", 1) 711 .WithTaggedFlag("Source of change in position portrait/landscape/face-up/face-down (6D_IA)", 2) 712 .WithTaggedFlag("Single-tap event status (SINGLE_TAP)", 3) 713 .WithTaggedFlag("Double-tap event status (DOUBLE_TAP)", 4) 714 .WithTaggedFlag("Sleep event status (SLEEP_STATE_IA)", 5) 715 .WithFlag(6, valueProviderCallback: _ => outDataRate.Value != DataRateConfig.PowerDown, name: "Temperature status (DRDY_T)") 716 .WithFlag(7, name: "FIFO overrun status flag (OVR)", 717 valueProviderCallback: _ => FifoOverrunOccurred); 718 719 Registers.WakeupSource.Define(this) 720 .WithTaggedFlag("Wakeup event detection status on Z-axis (Z_WU)", 0) 721 .WithTaggedFlag("Wakeup event detection status on Y-axis (Y_WU)", 1) 722 .WithTaggedFlag("Wakeup event detection status on X-axis (X_WU)", 2) 723 .WithTaggedFlag("Wakeup event detection status (WU_IA)", 3) 724 .WithTaggedFlag("Sleep event status (SLEEP_STATE_IA)", 4) 725 .WithTaggedFlag("Free-fall event detection status (FF_IA)", 5) 726 .WithReservedBits(6, 2); 727 728 Registers.TapSource.Define(this) 729 .WithTaggedFlag("Tap event detection status on Z-axis (Z_TAP)", 0) 730 .WithTaggedFlag("Tap event detection status on Y-axis (Y_TAP)", 1) 731 .WithTaggedFlag("Tap event detection status on X-axis (X_TAP)", 2) 732 .WithTaggedFlag("Sign of acceleration detected by tap event (TAP_SIGN)", 3) 733 .WithTaggedFlag("Double-tap event status (DOUBLE_TAP)", 4) 734 .WithTaggedFlag("Single-tap event status (SINGLE_TAP)", 5) 735 .WithTaggedFlag("Tap event status (TAP_IA)", 6) 736 .WithReservedBits(7, 1); 737 738 Registers.SourceOf6D.Define(this) 739 .WithTaggedFlag("XL over threshold (XL)", 0) 740 .WithTaggedFlag("XH over threshold (XH)", 1) 741 .WithTaggedFlag("YL over threshold (YL)", 2) 742 .WithTaggedFlag("YH over threshold (YH)", 3) 743 .WithTaggedFlag("ZL over threshold (ZL)", 4) 744 .WithTaggedFlag("ZH over threshold (ZL)", 5) 745 .WithTaggedFlag("Source of change in position portrait/landscape/face-up/face-down (6D_IA)", 6) 746 .WithReservedBits(7, 1); 747 748 Registers.InterruptReset.Define(this) 749 .WithTaggedFlag("Free-fall event detection status (FF_IA)", 0) 750 .WithTaggedFlag("Wakeup event detection status (WU_IA)", 1) 751 .WithTaggedFlag("Single-tap event status (SINGLE_TAP)", 2) 752 .WithTaggedFlag("Double-tap event status (DOUBLE_TAP)", 3) 753 .WithTaggedFlag("Source of change in position portrait/landscape/face-up/face-down (6D_IA)", 4) 754 .WithTaggedFlag("Sleep change status (SLEEP_CHANGE_IA)", 5) 755 .WithReservedBits(6, 2); 756 757 Registers.UserOffsetX.Define(this) 758 .WithTag("Two's complement user offset value on X-axis data, used for wakeup function (X_OFS_USR)", 0, 8); 759 760 Registers.UserOffsetY.Define(this) 761 .WithTag("Two's complement user offset value on Y-axis data, used for wakeup function (Y_OFS_USR)", 0, 8); 762 763 Registers.UserOffsetZ.Define(this) 764 .WithTag("Two's complement user offset value on Z-axis data, used for wakeup function (Z_OFS_USR)", 0, 8); 765 766 Registers.Control7.Define(this) 767 .WithTaggedFlag("Low pass filtered data sent to 6D interrupt function (LPASS_ON6D)", 0) 768 .WithTaggedFlag("High-pass filter reference mode enable (HP_REF_MODE)", 1) 769 .WithTaggedFlag("Selects the weight of the user offset words (USR_OFF_W)", 2) 770 .WithTaggedFlag("Enable application of user offset value on XL data for wakeup function only (USR_OFF_ON_WU)", 3) 771 .WithTaggedFlag("Enable application of user offset value on XL output data registers (USR_OFF_ON_OUT)", 4) 772 .WithFlag(5, out eventInterruptEnable, name: "Enable interrupts (INTERRUPTS_ENABLE)") 773 .WithTaggedFlag("Signal routing (INT2_ON_INT1)", 6) 774 .WithTaggedFlag("Switches between latched and pulsed mode for data ready interrupt (DRDY_PULSED)", 7) 775 .WithChangeCallback((_,__) => UpdateInterrupts()); 776 } 777 RegistersAutoIncrement()778 private void RegistersAutoIncrement() 779 { 780 if(regAddress >= Registers.DataOutXLow && regAddress < Registers.DataOutZHigh 781 || regAddress == Registers.TemperatureOutLow) 782 { 783 regAddress = (Registers)((int)regAddress + 1); 784 this.Log(LogLevel.Noisy, "Auto-incrementing to the next register 0x{0:X} - {0}", regAddress); 785 } 786 else if((fifoModeSelection.Value == FIFOModeSelection.FIFOMode) && (regAddress == Registers.DataOutZHigh)) 787 { 788 regAddress = Registers.DataOutXLow; 789 } 790 } 791 UpdateInterrupts()792 private void UpdateInterrupts() 793 { 794 if(outDataRate.Value == DataRateConfig.PowerDown) 795 { 796 Interrupt1.Unset(); 797 Interrupt2.Unset(); 798 return; 799 } 800 801 var int1Status = 802 readyEnabledAcceleration[0].Value || 803 fifoThresholdEnabled[0].Value && FifoThresholdReached || 804 fifoFullEnabled[0].Value && FifoFull; 805 var int2Status = 806 readyEnabledAcceleration[1].Value || 807 fifoThresholdEnabled[1].Value && FifoThresholdReached || 808 fifoOverrunEnabled.Value && FifoOverrunOccurred || 809 fifoFullEnabled[1].Value && FifoFull; 810 811 this.Log(LogLevel.Noisy, "Setting interrupts: INT1 = {0}, INT2 = {1}", int1Status, int2Status); 812 Interrupt1.Set(int1Status); 813 Interrupt2.Set(int2Status); 814 } 815 IsTemperatureOutOfRange(decimal temperature)816 private bool IsTemperatureOutOfRange(decimal temperature) 817 { 818 // This range protects from the overflow of the short variables in the 'Convert' function. 819 if (temperature < MinTemperature || temperature > MaxTemperature) 820 { 821 this.Log(LogLevel.Warning, "Temperature {0} is out of range, use value from the range <{1:F2};{2:F2}>", temperature, MinTemperature, MaxTemperature); 822 return true; 823 } 824 return false; 825 } 826 IsAccelerationOutOfRange(decimal acceleration)827 private bool IsAccelerationOutOfRange(decimal acceleration) 828 { 829 // This range protects from the overflow of the short variables in the 'Convert' function. 830 if (acceleration < MinAcceleration || acceleration > MaxAcceleration) 831 { 832 this.Log(LogLevel.Warning, "Acceleration {0} is out of range, use value from the range <{1:F2};{2:F2}>", acceleration, MinAcceleration, MaxAcceleration); 833 return true; 834 } 835 return false; 836 } 837 Convert(decimal value, bool upperByte)838 private byte Convert(decimal value, bool upperByte) 839 { 840 byte result = 0; 841 if(outDataRate.Value == DataRateConfig.PowerDown) 842 { 843 return result; 844 } 845 846 var minValue = MinValue14Bit; 847 var maxValue = MaxValue14Bit; 848 var shift = 2; 849 // Divide the divider by 4 because the divider for full scale = 2 g 850 // is 4 and the base sensitivity is given for this scale setting 851 var sensitivity = BaseSensitivity * (scaleDivider / 4); 852 853 if(modeSelection.Value == ModeSelection.HighPerformance) 854 { 855 this.Log(LogLevel.Noisy, "High performance (14-bit resolution) mode is selected."); 856 } 857 else if((modeSelection.Value == ModeSelection.LowPower) && (lowPowerModeSelection.Value != LowPowerModeSelection.LowPowerMode1_12bitResolution)) 858 { 859 this.Log(LogLevel.Noisy, "Low power (14-bit resolution) mode is selected."); 860 } 861 else if((modeSelection.Value == ModeSelection.LowPower) && (lowPowerModeSelection.Value == LowPowerModeSelection.LowPowerMode1_12bitResolution)) 862 { 863 this.Log(LogLevel.Noisy, "Low power (12-bit resolution) mode is selected."); 864 minValue = MinValue12Bit; 865 maxValue = MaxValue12Bit; 866 shift = 4; 867 sensitivity *= 4; 868 } 869 else 870 { 871 this.Log(LogLevel.Noisy, "Other conversion mode selected."); 872 } 873 874 var valueAsInt = ((int)(value * 1000 / sensitivity)).Clamp(minValue, maxValue); 875 var valueAsUshort = (ushort)(valueAsInt << shift); // left-align 876 this.Log(LogLevel.Noisy, "Conversion done with sensitivity: {0:F3}, result: 0x{1:X4}", sensitivity, valueAsUshort); 877 878 if(upperByte) 879 { 880 return (byte)(valueAsUshort >> 8); 881 } 882 result = (byte)(valueAsUshort); 883 UpdateInterrupts(); 884 885 return result; 886 } 887 InternalWrite(byte[] data)888 private void InternalWrite(byte[] data) 889 { 890 for(var i = 0; i < data.Length; i++) 891 { 892 this.Log(LogLevel.Noisy, "Writing 0x{0:X} to register {1} (0x{1:X})", data[i], regAddress); 893 RegistersCollection.Write((byte)regAddress, data[i]); 894 if(autoIncrement.Value) 895 { 896 RegistersAutoIncrement(); 897 } 898 } 899 } 900 TwoComplementSignConvert(decimal temp)901 private ushort TwoComplementSignConvert(decimal temp) 902 { 903 var tempAsUshort = Decimal.ToUInt16(Math.Abs(temp)); 904 if(temp < 0) 905 { 906 var twoComplementTemp = (ushort)(~tempAsUshort + 1); 907 return twoComplementTemp; 908 } 909 return tempAsUshort; 910 } 911 SetScaleDivider()912 private void SetScaleDivider() 913 { 914 switch(fullScale.Value) 915 { 916 case FullScaleSelect.FullScale4g: 917 scaleDivider = 8; 918 break; 919 case FullScaleSelect.FullScale8g: 920 scaleDivider = 16; 921 break; 922 case FullScaleSelect.FullScale16g: 923 scaleDivider = 32; 924 break; 925 default: 926 scaleDivider = 4; 927 break; 928 } 929 } 930 931 public Vector3DSample DefaultSample { get; } = new Vector3DSample(); 932 private Vector3DSample CurrentSample { get; set; } = new Vector3DSample(); 933 private decimal SelfTestAccelerationOffset => 934 SelfTestAcceleration * (selfTestMode.Value == SelfTestMode.PositiveSign ? 1 : selfTestMode.Value == SelfTestMode.NegativeSign ? -1 : 0); 935 private decimal ReportedAccelerationX => AccelerationX + SelfTestAccelerationOffset; 936 private decimal ReportedAccelerationY => AccelerationY + SelfTestAccelerationOffset; 937 private decimal ReportedAccelerationZ => AccelerationZ + SelfTestAccelerationOffset; 938 939 private decimal ReportedTemperature => Temperature - TemperatureBias; 940 941 private bool FifoThresholdReached => fifoThreshold.Value != 0 && accelerationFifo.SamplesCount >= fifoThreshold.Value; 942 private bool FifoFull => accelerationFifo.Full; 943 private bool FifoOverrunOccurred => accelerationFifo.OverrunOccurred; 944 945 private IFlagRegisterField autoIncrement; 946 private IFlagRegisterField[] readyEnabledAcceleration; 947 private IFlagRegisterField[] fifoThresholdEnabled; 948 private IFlagRegisterField[] fifoFullEnabled; 949 // Interrupt on overrun is only avaliable on INT2 950 private IFlagRegisterField fifoOverrunEnabled; 951 private IFlagRegisterField readyEnabledTemperature; 952 // This flag controls whether 6D, tap, fall, etc. interrupts are enabled and is not a global 953 // flag for all interrupts (FIFO and others) 954 private IFlagRegisterField eventInterruptEnable; 955 private IValueRegisterField fifoThreshold; 956 private IEnumRegisterField<LowPowerModeSelection> lowPowerModeSelection; 957 private IEnumRegisterField<DataRateConfig> outDataRate; 958 private IEnumRegisterField<ModeSelection> modeSelection; 959 private IEnumRegisterField<FIFOModeSelection> fifoModeSelection; 960 private IEnumRegisterField<FullScaleSelect> fullScale; 961 private IEnumRegisterField<SelfTestMode> selfTestMode; 962 private uint sampleRate; 963 964 private Registers regAddress; 965 private State state; 966 private int scaleDivider; 967 968 private IManagedThread feederThread; 969 private RESDStream<AccelerationSample> resdStream; 970 971 private event Action<uint> SampleRateChanged; 972 // This event is used in MultiFrequency RESD to precisely match RESD behavior with FIFO operation 973 private event Action<uint> FIFOModeEntered; 974 975 private decimal temperature; 976 977 private readonly LIS2DW12_FIFO accelerationFifo; 978 private readonly IMachine machine; 979 980 private const decimal MinTemperature = -40.0m; 981 // The temperature register has the value 0 at this temperature 982 private const decimal TemperatureBias = 25.0m; 983 private const decimal MaxTemperature = 85.0m; 984 private const decimal MinAcceleration = -16m; 985 private const decimal MaxAcceleration = 16m; 986 private const decimal SelfTestAcceleration = 1m; 987 // Calculated as floor(1000 / 0xfff, 3), 1000 mg / 12-bit max value, used as a base 988 // to calculate the sensitivity for other ranges/resolutions, multiplying it by 2^n 989 // to match the tables in the datasheet, appnote, and drivers 990 private const decimal BaseSensitivity = 0.244m; // mg / digit 991 private const int MaxFifoSize = 32; 992 private const int MinValue14Bit = -0x2000; 993 private const int MaxValue14Bit = 0x1FFF; 994 private const int MinValue12Bit = -0x0800; 995 private const int MaxValue12Bit = 0x07FF; 996 private const int TemperatureLsbsPerDegree = 16; 997 998 public enum RESDType 999 { 1000 Normal, 1001 MultiFrequency, 1002 } 1003 1004 private class LIS2DW12_FIFO 1005 { LIS2DW12_FIFO(LIS2DW12 owner, string name, uint capacity)1006 public LIS2DW12_FIFO(LIS2DW12 owner, string name, uint capacity) 1007 { 1008 Capacity = capacity; 1009 this.name = name; 1010 this.owner = owner; 1011 this.locker = new object(); 1012 this.queue = new Queue<Vector3DSample>(); 1013 } 1014 FeedAccelerationSample(decimal x, decimal y, decimal z)1015 public void FeedAccelerationSample(decimal x, decimal y, decimal z) 1016 { 1017 var sample = new Vector3DSample(x, y, z); 1018 FeedSample(sample); 1019 } 1020 FeedSample(Vector3DSample sample)1021 public void FeedSample(Vector3DSample sample) 1022 { 1023 lock(locker) 1024 { 1025 latestSample = sample; 1026 1027 if(KeepFifoOnReset) 1028 { 1029 queue.Enqueue(sample); 1030 return; 1031 } 1032 1033 if(Mode == FIFOModeSelection.FIFOMode && Full) 1034 { 1035 return; 1036 } 1037 else if(Mode == FIFOModeSelection.Continuous && Full) 1038 { 1039 if(!OverrunOccurred) 1040 { 1041 owner.Log(LogLevel.Debug, "{0}: Overrun", name); 1042 OverrunOccurred = true; 1043 OnOverrun?.Invoke(); 1044 } 1045 1046 owner.Log(LogLevel.Noisy, "{0}: Fifo filled up. Dumping the oldest sample.", name); 1047 queue.TryDequeue<Vector3DSample>(out _); 1048 } 1049 owner.Log(LogLevel.Noisy, "Enqueued sample {0} at index {1}", sample, queue.Count); 1050 queue.Enqueue(sample); 1051 } 1052 } 1053 1054 // Old, deprecated API. Added because this peripheral already included a public FeedSamplesFromFile method. FeedSamplesFromFile(string path)1055 public void FeedSamplesFromFile(string path) 1056 { 1057 try 1058 { 1059 using(var reader = File.OpenText(path)) 1060 { 1061 string line; 1062 while((line = reader.ReadLine()) != null) 1063 { 1064 line = line.Trim(); 1065 1066 if(line.StartsWith("#")) 1067 { 1068 continue; 1069 } 1070 1071 var numbers = line.Split(new [] { ' ' }, StringSplitOptions.RemoveEmptyEntries).Select(x => x.Trim()).ToArray(); 1072 var sample = new Vector3DSample(); 1073 if(!sample.TryLoad(numbers)) 1074 { 1075 sample = null; 1076 } 1077 FeedSample(sample); 1078 } 1079 } 1080 } 1081 catch(Exception e) 1082 { 1083 if(e is RecoverableException) 1084 { 1085 throw; 1086 } 1087 1088 throw new RecoverableException($"There was a problem when reading samples file: {e.Message}"); 1089 } 1090 } 1091 Reset()1092 public void Reset() 1093 { 1094 owner.Log(LogLevel.Debug, "Resetting FIFO"); 1095 OverrunOccurred = false; 1096 1097 if(KeepFifoOnReset) 1098 { 1099 owner.Log(LogLevel.Debug, "Keeping existing FIFO content"); 1100 return; 1101 } 1102 1103 queue.Clear(); 1104 latestSample = null; 1105 } 1106 TryDequeueNewSample()1107 public bool TryDequeueNewSample() 1108 { 1109 lock(locker) 1110 { 1111 if(CheckEnabled() && queue.TryDequeue(out var sample)) 1112 { 1113 owner.Log(LogLevel.Noisy, "New sample dequeued: {0}", sample); 1114 latestSample = sample; 1115 return true; 1116 } 1117 } 1118 // If we're feeding from a file, the sensor might have been polled between 1119 // two samples - in this case, the earlier of these should be returned. Otherwise, 1120 // clearing the FIFO means we ran out of data and so we should go to the default 1121 // sample. 1122 if(!Disabled) 1123 { 1124 latestSample = null; 1125 } 1126 owner.Log(LogLevel.Noisy, "Dequeueing new sample failed."); 1127 return false; 1128 } 1129 1130 public bool KeepFifoOnReset { get; set; } 1131 1132 public uint SamplesCount => (uint)Math.Min(queue.Count, Capacity); 1133 public bool Disabled => Mode == FIFOModeSelection.Bypass && !KeepFifoOnReset; 1134 public bool Empty => SamplesCount == 0; 1135 public bool Full => SamplesCount >= Capacity; 1136 1137 public FIFOModeSelection Mode => owner.fifoModeSelection.Value; 1138 1139 public bool OverrunOccurred { get; private set; } 1140 public Vector3DSample Sample => latestSample ?? owner.DefaultSample; 1141 1142 public event Action OnOverrun; 1143 CheckEnabled()1144 private bool CheckEnabled() 1145 { 1146 if(Disabled) 1147 { 1148 owner.Log(LogLevel.Debug, "Sample unavailable -- FIFO disabled."); 1149 return false; 1150 } 1151 return true; 1152 } 1153 1154 private Vector3DSample latestSample; 1155 1156 private readonly object locker; 1157 private readonly Queue<Vector3DSample> queue; 1158 private readonly string name; 1159 private readonly LIS2DW12 owner; 1160 private readonly uint Capacity; 1161 } 1162 1163 private enum State 1164 { 1165 WaitingForRegister, 1166 WaitingForData 1167 } 1168 1169 private enum FullScaleSelect : byte 1170 { 1171 FullScale2g = 0x00, 1172 FullScale4g = 0x01, 1173 FullScale8g = 0x02, 1174 FullScale16g = 0x03, 1175 } 1176 1177 private enum DataRateConfig : byte 1178 { 1179 PowerDown = 0x00, 1180 HighPerformanceLowPower1_6Hz = 0x01, 1181 HighPerformanceLowPower12_5Hz = 0x02, 1182 HighPerformanceLowPower25Hz = 0x03, 1183 HighPerformanceLowPower50Hz = 0x04, 1184 HighPerformanceLowPower100Hz = 0x05, 1185 HighPerformanceLowPower200Hz = 0x06, 1186 HighPerformanceLowPower400Hz = 0x07, 1187 HighPerformanceLowPower800Hz = 0x08, 1188 HighPerformanceLowPower1600Hz = 0x09, 1189 } 1190 1191 private enum ModeSelection : byte 1192 { 1193 LowPower = 0x00, 1194 HighPerformance = 0x01, 1195 SingleDataConversionOnDemand = 0x02, 1196 Reserved = 0x03 1197 } 1198 1199 private enum FIFOModeSelection : byte 1200 { 1201 Bypass = 0x00, 1202 FIFOMode = 0x01, 1203 // Reserved: 0x02 1204 ContinuousToFIFO = 0x03, 1205 BypassToContinuous = 0x04, 1206 // Reserved: 0x05 1207 Continuous = 0x06, 1208 // Reserved: 0x07 1209 } 1210 1211 private enum LowPowerModeSelection : byte 1212 { 1213 LowPowerMode1_12bitResolution = 0x00, 1214 LowPowerMode2_14bitResolution = 0x01, 1215 LowPowerMode3_14bitResolution = 0x02, 1216 LowPowerMode4_14bitResolution = 0x03 1217 } 1218 1219 private enum SelfTestMode : byte 1220 { 1221 Disabled = 0x00, 1222 PositiveSign = 0x01, 1223 NegativeSign = 0x02, 1224 // Reserved: 0x03 1225 } 1226 1227 private enum Registers : byte 1228 { 1229 // Reserved: 0x0 - 0xC 1230 TemperatureOutLow = 0xD, 1231 TemperatureOutHigh = 0xE, 1232 WhoAmI = 0x0F, 1233 // Reserved: 0x10 - 0x1F 1234 Control1 = 0x20, 1235 Control2 = 0x21, 1236 Control3 = 0x22, 1237 Control4 = 0x23, 1238 Control5 = 0x24, 1239 Control6 = 0x25, 1240 TemperatureOut = 0x26, 1241 Status = 0x27, 1242 DataOutXLow = 0x28, 1243 DataOutXHigh = 0x29, 1244 DataOutYLow = 0x2A, 1245 DataOutYHigh = 0x2B, 1246 DataOutZLow = 0x2C, 1247 DataOutZHigh = 0x2D, 1248 FifoControl = 0x2E, 1249 FifoSamples = 0x2F, 1250 TapThreshholdX = 0x30, 1251 TapThreshholdY = 0x31, 1252 TapThreshholdZ = 0x32, 1253 InterruptDuration = 0x33, 1254 WakeupThreshhold = 0x34, 1255 WakeupAndSleepDuration = 0x35, 1256 FreeFall = 0x36, 1257 StatusEventDetection = 0x37, 1258 WakeupSource = 0x38, 1259 TapSource = 0x39, 1260 SourceOf6D = 0x3A, 1261 InterruptReset = 0x3B, 1262 UserOffsetX = 0x3C, 1263 UserOffsetY = 0x3D, 1264 UserOffsetZ = 0x3E, 1265 Control7 = 0x3F 1266 } 1267 } 1268 } 1269
