1 //
2 // Copyright (c) 2010-2020 Antmicro
3 //
4 // This file is licensed under the MIT License.
5 // Full license text is available in 'licenses/MIT.txt'.
6 //
7 
8 using System;
9 using Antmicro.Renode.Core.Structure.Registers;
10 using Antmicro.Renode.Exceptions;
11 using Antmicro.Renode.Logging;
12 using Antmicro.Renode.Peripherals.I2C;
13 using Antmicro.Renode.Peripherals.Sensor;
14 using Antmicro.Renode.Utilities;
15 
16 namespace Antmicro.Renode.Peripherals.Sensors
17 {
18     public class LSM9DS1_IMU: II2CPeripheral, IProvidesRegisterCollection<ByteRegisterCollection>, ITemperatureSensor
19     {
LSM9DS1_IMU()20         public LSM9DS1_IMU()
21         {
22             accelerationFifo = new SensorSamplesFifo<Vector3DSample>();
23             angularRateFifo = new SensorSamplesFifo<Vector3DSample>();
24             temperatureFifo = new SensorSamplesFifo<ScalarSample>();
25 
26             RegistersCollection = new ByteRegisterCollection(this);
27 
28             DefineRegisters();
29         }
30 
Reset()31         public void Reset()
32         {
33             RegistersCollection.Reset();
34 
35             angularRateSensitivity = AngularRateSensitivity.DPS245;
36             accelerationSensitivity = AccelerationSensitivity.G2;
37 
38             address = 0;
39             currentReportedFifoDepth = 0;
40             state = State.Idle;
41         }
42 
FinishTransmission()43         public void FinishTransmission()
44         {
45             this.NoisyLog("Finishing transmission, going to the Idle state");
46             state = State.Idle;
47         }
48 
Write(byte[] data)49         public void Write(byte[] data)
50         {
51             this.Log(LogLevel.Noisy, "Written {0} bytes: {1}", data.Length, Misc.PrettyPrintCollectionHex(data));
52             foreach(var b in data)
53             {
54                 WriteByte(b);
55             }
56         }
57 
WriteByte(byte b)58         public void WriteByte(byte b)
59         {
60             switch(state)
61             {
62                 case State.Idle:
63                     address = BitHelper.GetValue(b, offset: 0, size: 7);
64                     this.Log(LogLevel.Noisy, "Setting register address to {0} (0x{0:X})", (Registers)address);
65                     state = State.Processing;
66                     break;
67 
68                 case State.Processing:
69                     this.Log(LogLevel.Noisy, "Writing value 0x{0:X} to register {1} (0x{1:X})", b, (Registers)address);
70                     RegistersCollection.Write(address, b);
71                     TryIncrementAddress();
72                     break;
73 
74                 default:
75                     throw new ArgumentException($"Unexpected state: {state}");
76             }
77         }
78 
Read(int count = 1)79         public byte[] Read(int count = 1)
80         {
81             var dequeued = false;
82             switch(address)
83             {
84                 case (byte)Registers.AccelerometerOutputXLow:
85                     dequeued = accelerationFifo.TryDequeueNewSample();
86                     break;
87 
88                 case (byte)Registers.GyroscopeOutputXLow:
89                     dequeued = angularRateFifo.TryDequeueNewSample();
90                     break;
91 
92                 case (byte)Registers.TemperatureOutputLow:
93                     // temperature data is not queued
94                     temperatureFifo.TryDequeueNewSample();
95                     break;
96             }
97 
98             if(dequeued)
99             {
100                 if(currentReportedFifoDepth > 0)
101                 {
102                     currentReportedFifoDepth--;
103                 }
104             }
105 
106             var result = RegistersCollection.Read(address);
107             this.NoisyLog("Reading register {1} (0x{1:X}) from device: 0x{0:X}", result, (Registers)address);
108             TryIncrementAddress();
109 
110             return new byte [] { result };
111         }
112 
FeedAccelerationSample(decimal x, decimal y, decimal z, uint repeat = 1)113         public void FeedAccelerationSample(decimal x, decimal y, decimal z, uint repeat = 1)
114         {
115             var sample = new Vector3DSample(x, y, z);
116 
117             for(var i = 0; i < repeat; i++)
118             {
119                 accelerationFifo.FeedSample(sample);
120             }
121         }
122 
FeedAccelerationSample(string path)123         public void FeedAccelerationSample(string path)
124         {
125             accelerationFifo.FeedSamplesFromFile(path);
126         }
127 
FeedAngularRateSample(decimal x, decimal y, decimal z, uint repeat = 1)128         public void FeedAngularRateSample(decimal x, decimal y, decimal z, uint repeat = 1)
129         {
130             var sample = new Vector3DSample(x, y, z);
131 
132             for(var i = 0; i < repeat; i++)
133             {
134                 angularRateFifo.FeedSample(sample);
135             }
136         }
137 
FeedAgularRateSample(string path)138         public void FeedAgularRateSample(string path)
139         {
140             angularRateFifo.FeedSamplesFromFile(path);
141         }
142 
FeedTemperatureSample(decimal value, uint repeat = 1)143         public void FeedTemperatureSample(decimal value, uint repeat = 1)
144         {
145             var sample = new ScalarSample(value);
146 
147             for(var i = 0; i < repeat; i++)
148             {
149                 temperatureFifo.FeedSample(sample);
150             }
151         }
152 
FeedTemperatureSample(string path)153         public void FeedTemperatureSample(string path)
154         {
155             temperatureFifo.FeedSamplesFromFile(path);
156         }
157 
158         // NOTE: The meaning of this field
159         // is the default value of the channel
160         // when there are no samples in the fifo
161         public decimal AccelerationX
162         {
163             get => accelerationFifo.DefaultSample.X;
164             set
165             {
166                 accelerationFifo.DefaultSample.X = value;
167             }
168         }
169 
170         // NOTE: The meaning of this field
171         // is the default value of the channel
172         // when there are no samples in the fifo
173         public decimal AccelerationY
174         {
175             get => accelerationFifo.DefaultSample.Y;
176             set
177             {
178                 accelerationFifo.DefaultSample.Y = value;
179             }
180         }
181 
182         // NOTE: The meaning of this field
183         // is the default value of the channel
184         // when there are no samples in the fifo
185         public decimal AccelerationZ
186         {
187             get => accelerationFifo.DefaultSample.Z;
188             set
189             {
190                 accelerationFifo.DefaultSample.Z = value;
191             }
192         }
193 
194         // NOTE: The meaning of this field
195         // is the default value of the channel
196         // when there are no samples in the fifo
197         public decimal Temperature
198         {
199             get => temperatureFifo.DefaultSample.Value;
200             set
201             {
202                 temperatureFifo.DefaultSample.Value = value;
203             }
204         }
205 
206         // NOTE: The meaning of this field
207         // is the default value of the channel
208         // when there are no samples in the fifo
209         public decimal AngularRateX
210         {
211             get => angularRateFifo.DefaultSample.X;
212             set
213             {
214                 angularRateFifo.DefaultSample.X = value;
215             }
216         }
217 
218         // NOTE: The meaning of this field
219         // is the default value of the channel
220         // when there are no samples in the fifo
221         public decimal AngularRateY
222         {
223             get => angularRateFifo.DefaultSample.Y;
224             set
225             {
226                 angularRateFifo.DefaultSample.Y = value;
227             }
228         }
229 
230         // NOTE: The meaning of this field
231         // is the default value of the channel
232         // when there are no samples in the fifo
233         public decimal AngularRateZ
234         {
235             get => angularRateFifo.DefaultSample.Z;
236             set
237             {
238                 angularRateFifo.DefaultSample.Z = value;
239             }
240         }
241 
242         public ByteRegisterCollection RegistersCollection { get; }
243 
244         // When we provide a collection of samples from the file, they are not timestamped in any way
245         // - we simplify and assume that each time SW reads a sample, we will advance to the next record.
246         //
247         // This works fine with single reads, but not with fifos.
248         // Some software (e.g., TensorFlow) might periodically poll the device and ask how many samples there are waiting in the queue.
249         // For us it's a tricky question - we could say that all the samples in the buffer, but...
250         // as a result SW could read all of them at once.
251         // This in turn could result in dropping those not fitting in the local buffer and causing the rest of the algorithm not to work.
252         // So what we do instead is we report maximally MaxFifoDepth, giving SW chance to read and process samples in chunks.
253         //
254         // Since the value is SW-specific, we can't hardcode any value in the model.
255         public uint MaxFifoDepth { get; set; }
256 
TryIncrementAddress()257         private void TryIncrementAddress()
258         {
259             if(!addressAutoIncrement.Value)
260             {
261                 return;
262             }
263             address = (byte)((address + 1) % 0x80);
264         }
265 
DefineRegisters()266         private void DefineRegisters()
267         {
268             Registers.AccelerometerOutputXLow.Define(this)
269                 .WithValueField(0, 8, FieldMode.Read, name: "OUT_X_L_XL", valueProviderCallback: _ => GetScaledValue(accelerationFifo.Sample.X, (short)accelerationSensitivity, upperByte: false))
270             ;
271 
272             Registers.AccelerometerOutputXHigh.Define(this)
273                 .WithValueField(0, 8, FieldMode.Read, name: "OUT_X_H_XL", valueProviderCallback: _ => GetScaledValue(accelerationFifo.Sample.X, (short)accelerationSensitivity, upperByte: true))
274             ;
275 
276             Registers.AccelerometerOutputYLow.Define(this)
277                 .WithValueField(0, 8, FieldMode.Read, name: "OUT_Y_L_XL", valueProviderCallback: _ => GetScaledValue(accelerationFifo.Sample.Y, (short)accelerationSensitivity, upperByte: false))
278             ;
279 
280             Registers.AccelerometerOutputYHigh.Define(this)
281                 .WithValueField(0, 8, FieldMode.Read, name: "OUT_Y_H_XL", valueProviderCallback: _ => GetScaledValue(accelerationFifo.Sample.Y, (short)accelerationSensitivity, upperByte: true))
282             ;
283 
284             Registers.AccelerometerOutputZLow.Define(this)
285                 .WithValueField(0, 8, FieldMode.Read, name: "OUT_Z_L_XL", valueProviderCallback: _ => GetScaledValue(accelerationFifo.Sample.Z, (short)accelerationSensitivity, upperByte: false))
286             ;
287 
288             Registers.AccelerometerOutputZHigh.Define(this)
289                 .WithValueField(0, 8, FieldMode.Read, name: "OUT_Z_H_XL", valueProviderCallback: _ => GetScaledValue(accelerationFifo.Sample.Z, (short)accelerationSensitivity, upperByte: true))
290             ;
291 
292             Registers.TemperatureOutputLow.Define(this)
293                 .WithValueField(0, 8, FieldMode.Read, name: "OUT_TEMP_L", valueProviderCallback: _ => GetScaledTemperatureValue(upperByte: false))
294             ;
295 
296             Registers.TemperatureOutputHigh.Define(this)
297                 .WithValueField(0, 8, FieldMode.Read, name: "OUT_TEMP_H", valueProviderCallback: _ => GetScaledTemperatureValue(upperByte: true))
298             ;
299 
300             Registers.GyroscopeOutputXLow.Define(this)
301                 .WithValueField(0, 8, FieldMode.Read, name: "OUT_X_L_G", valueProviderCallback: _ => GetScaledValue(angularRateFifo.Sample.X, (short)angularRateSensitivity, upperByte: false))
302             ;
303 
304             Registers.GyroscopeOutputXHigh.Define(this)
305                 .WithValueField(0, 8, FieldMode.Read, name: "OUT_X_H_G", valueProviderCallback: _ => GetScaledValue(angularRateFifo.Sample.X, (short)angularRateSensitivity, upperByte: true))
306             ;
307 
308             Registers.GyroscopeOutputYLow.Define(this)
309                 .WithValueField(0, 8, FieldMode.Read, name: "OUT_Y_L_G", valueProviderCallback: _ => GetScaledValue(angularRateFifo.Sample.Y, (short)angularRateSensitivity, upperByte: false))
310             ;
311 
312             Registers.GyroscopeOutputYHigh.Define(this)
313                 .WithValueField(0, 8, FieldMode.Read, name: "OUT_Y_H_G", valueProviderCallback: _ => GetScaledValue(angularRateFifo.Sample.Y, (short)angularRateSensitivity, upperByte: true))
314             ;
315 
316             Registers.GyroscopeOutputZLow.Define(this)
317                 .WithValueField(0, 8, FieldMode.Read, name: "OUT_Z_L_G", valueProviderCallback: _ => GetScaledValue(angularRateFifo.Sample.Z, (short)angularRateSensitivity, upperByte: false))
318             ;
319 
320             Registers.GyroscopeOutputZHigh.Define(this)
321                 .WithValueField(0, 8, FieldMode.Read, name: "OUT_Z_H_G", valueProviderCallback: _ => GetScaledValue(angularRateFifo.Sample.Z, (short)angularRateSensitivity, upperByte: true))
322             ;
323 
324             Registers.WhoAmI.Define(this, 0x68);
325 
326             Registers.FifoSource.Define(this)
327                 .WithValueField(0, 6, FieldMode.Read, name: "FSS: Number of unread samples in FIFO", valueProviderCallback: _ =>
328                 {
329                     if(currentReportedFifoDepth == 0)
330                     {
331                         // FIFO is supported in hardware only for accelerometer and gyroscope
332                         currentReportedFifoDepth = Math.Min(MaxFifoDepth, Math.Max(accelerationFifo.SamplesCount, angularRateFifo.SamplesCount));
333                         if(currentReportedFifoDepth == 0)
334                         {
335                             currentReportedFifoDepth = 1;
336                         }
337                         return 0;
338                     }
339                     return currentReportedFifoDepth;
340                 })
341                 .WithTaggedFlag("OVRN: FIFO overrun status", 6)
342                 .WithTaggedFlag("FTH: FIFO threshold status", 7)
343             ;
344 
345             Registers.Control1.Define(this)
346                 .WithTag("BW: Gyroscope bandwith selection", 0, 2)
347                 .WithReservedBits(2, 1)
348                 .WithValueField(3, 2, name: "FS_G: Gyroscope full-scale selection",
349                     valueProviderCallback: _ =>
350                     {
351                         switch(angularRateSensitivity)
352                         {
353                             case AngularRateSensitivity.DPS245:
354                                 return 0;
355                             case AngularRateSensitivity.DPS500:
356                                 return 1;
357                             case AngularRateSensitivity.DPS2000:
358                                 return 3;
359                             default:
360                                 this.Log(LogLevel.Error, "Selected a not supported angular rate sensitivity");
361                                 return 2;
362                         }
363                     },
364                     writeCallback: (_, val) =>
365                     {
366                         switch(val)
367                         {
368                             case 0:
369                                 angularRateSensitivity = AngularRateSensitivity.DPS245;
370                                 break;
371                             case 1:
372                                 angularRateSensitivity = AngularRateSensitivity.DPS500;
373                                 break;
374                             case 3:
375                                 angularRateSensitivity = AngularRateSensitivity.DPS2000;
376                                 break;
377                             default:
378                                 this.Log(LogLevel.Warning, "Tried to set a not supported angular rate sensitivity");
379                                 break;
380                         }
381                     })
382                 .WithTag("ODR: Gyroscope output data rate", 5, 3)
383             ;
384 
385             Registers.Control6.Define(this)
386                 .WithTag("BW_XL: Accelerometer anti-aliasing filter bandwith selection", 0, 2)
387                 .WithTag("BW_SCAL_ODR: Accelerometer bandwith selection", 2, 1)
388                 .WithValueField(3, 2, name: "FS_XL: Accelerometer full-scale selection",
389                     valueProviderCallback: _ =>
390                     {
391                         switch(accelerationSensitivity)
392                         {
393                             case AccelerationSensitivity.G2:
394                                 return 0;
395                             case AccelerationSensitivity.G16:
396                                 return 1;
397                             case AccelerationSensitivity.G4:
398                                 return 2;
399                             case AccelerationSensitivity.G8:
400                                 return 3;
401                             default:
402                                 throw new ArgumentException("This should never happen");
403                         }
404                     },
405                     writeCallback: (_, val) =>
406                     {
407                         switch(val)
408                         {
409                             case 0:
410                                 accelerationSensitivity = AccelerationSensitivity.G2;
411                                 break;
412                             case 1:
413                                 accelerationSensitivity = AccelerationSensitivity.G16;
414                                 break;
415                             case 2:
416                                 accelerationSensitivity = AccelerationSensitivity.G4;
417                                 break;
418                             case 3:
419                                 accelerationSensitivity = AccelerationSensitivity.G8;
420                                 break;
421                         }
422                     })
423                 .WithTag("ODR_XL: Accelerometer output data rate", 5, 3)
424             ;
425 
426             Registers.Control8.Define(this, 0x4)
427                 .WithTaggedFlag("SW_RESET: Software reset", 0)
428                 .WithTaggedFlag("BLE: Big/Little Endian data selection", 1)
429                 .WithFlag(2, out addressAutoIncrement, name: "IF_ADD_INC: Register address automatically incremented")
430                 .WithTaggedFlag("SIM: SPI serial interface mode", 3)
431                 .WithTaggedFlag("PP_OD: Push-pull/open-drain", 4)
432                 .WithTaggedFlag("H_LACTIVE: Interrupt activation level", 5)
433                 .WithTaggedFlag("BDU: Block data update", 6)
434                 .WithTaggedFlag("BOOT: Reboot memory", 7)
435             ;
436         }
437 
GetScaledTemperatureValue(bool upperByte)438         private byte GetScaledTemperatureValue(bool upperByte)
439         {
440             // temperature read is 0 for 25C
441             // the sensivity is 16 per 1C
442             var scaled = (short)((temperatureFifo.Sample.Value - 25) * 16);
443             return upperByte
444                 ? (byte)(scaled >> 8)
445                 : (byte)scaled;
446         }
447 
GetScaledValue(decimal value, short sensitivity, bool upperByte)448         private byte GetScaledValue(decimal value, short sensitivity, bool upperByte)
449         {
450             var scaled = (short)(value * sensitivity);
451             return upperByte
452                 ? (byte)(scaled >> 8)
453                 : (byte)scaled;
454         }
455 
456         private uint currentReportedFifoDepth;
457         private byte address;
458         private State state;
459         private AccelerationSensitivity accelerationSensitivity = AccelerationSensitivity.G2;
460         private AngularRateSensitivity angularRateSensitivity = AngularRateSensitivity.DPS245;
461         private IFlagRegisterField addressAutoIncrement;
462 
463         private readonly SensorSamplesFifo<Vector3DSample> accelerationFifo;
464         private readonly SensorSamplesFifo<Vector3DSample> angularRateFifo;
465         private readonly SensorSamplesFifo<ScalarSample> temperatureFifo;
466 
467         private enum AccelerationSensitivity : ushort
468         {
469             G2 = 16384,
470             G4 = 8192,
471             G8 = 4096,
472             G16 = 2048
473         }
474 
475         private enum AngularRateSensitivity : ushort
476         {
477             DPS245 = 120,
478             DPS500 = 60,
479             DPS2000 = 15
480         }
481 
482         private enum State
483         {
484             Idle,
485             Processing
486         }
487 
488         private enum Registers: byte
489         {
490             // 0x0 - 0x03 are reserved
491             ActivityThreshold = 0x04,
492             ActivityDuration = 0x05,
493             AccelerometerInterruptGeneratorConfig = 0x06,
494             AccelerometerInterruptGeneratorThresholdX = 0x07,
495             AccelerometerInterruptGeneratorThresholdY = 0x08,
496             AccelerometerInterruptGeneratorThresholdZ = 0x09,
497             AccelerometerInterruptGeneratorDuration = 0x0A,
498             GyroscopeReference = 0x0B,
499             InterruptControl1 = 0x0C,
500             InterruptControl2 = 0x0D,
501             // 0xE is reserved
502             WhoAmI = 0x0F,
503             Control1 = 0x10,
504             Control2 = 0x11,
505             Control3 = 0x12,
506             GyroscopeOrientationConfiguration = 0x13,
507             GyroscopeInterruptGeneratorSource = 0x14,
508             TemperatureOutputLow = 0x15,
509             TemperatureOutputHigh = 0x16,
510             Status1 = 0x17,
511             GyroscopeOutputXLow = 0x18,
512             GyroscopeOutputXHigh = 0x19,
513             GyroscopeOutputYLow = 0x1A,
514             GyroscopeOutputYHigh = 0x1B,
515             GyroscopeOutputZLow = 0x1C,
516             GyroscopeOutputZHigh = 0x1D,
517             Control4 = 0x1E,
518             Control5 = 0x1F,
519             Control6 = 0x20,
520             Control7 = 0x21,
521             Control8 = 0x22,
522             Control9 = 0x23,
523             Control10 = 0x24,
524             // 0x25 is reserved
525             AccelerometerInterruptGeneratorSource = 0x26,
526             Status2 = 0x27,
527             AccelerometerOutputXLow = 0x28,
528             AccelerometerOutputXHigh = 0x29,
529             AccelerometerOutputYLow = 0x2A,
530             AccelerometerOutputYHigh = 0x2B,
531             AccelerometerOutputZLow = 0x2C,
532             AccelerometerOutputZHigh = 0x2D,
533             FifoControl = 0x2E,
534             FifoSource = 0x2F,
535             GyroscopeInterruptGeneratorConfiguration = 0x30,
536             GyroscopeInterruptGeneratorThresholdXHigh = 0x31,
537             GyroscopeInterruptGeneratorThresholdXLow = 0x32,
538             GyroscopeInterruptGeneratorThresholdYHigh = 0x33,
539             GyroscopeInterruptGeneratorThresholdYLow = 0x34,
540             GyroscopeInterruptGeneratorThresholdZHigh = 0x35,
541             GyroscopeInterruptGeneratorThresholdZLow = 0x36,
542             GyroscopeInterruptGeneratorDuration = 0x37,
543             // 0x38 to 0x7F are reserved
544         }
545     }
546 }
547