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 Antmicro.Renode.Core;
9 using Antmicro.Renode.Exceptions;
10 using Antmicro.Renode.Utilities.RESD;
11 using Antmicro.Renode.Time;
12 
13 namespace Antmicro.Renode.Peripherals.Sensors
14 {
15     public partial class ICM20948 : IUnderstandRESD
16     {
FeedAccelerationSamplesFromRESD(string path, uint channel = 0, ulong startTime = 0, RESDStreamSampleOffset sampleOffsetType = RESDStreamSampleOffset.Specified, long sampleOffsetTime = 0)17         public void FeedAccelerationSamplesFromRESD(string path, uint channel = 0, ulong startTime = 0,
18             RESDStreamSampleOffset sampleOffsetType = RESDStreamSampleOffset.Specified, long sampleOffsetTime = 0)
19         {
20             accelerometerResdStream = this.CreateRESDStream<AccelerationSample>(path, channel, sampleOffsetType, sampleOffsetTime);
21             accelerometerFeederThread?.Stop();
22             accelerometerFeederThread = accelerometerResdStream.StartSampleFeedThread(this,
23                 (uint)AccelerometerOutputDataRateHz,
24                 startTime: startTime
25             );
26         }
27 
28         public decimal DefaultAccelerationX
29         {
30             get => defaultAccelerationX;
31 
32             set
33             {
34                 if(!IsAccelerationInRange(value))
35                 {
36                     throw new RecoverableException($"Value out of currently set range. Maximum value is {AccelerometerFullScaleRangeG}[g]");
37                 }
38                 defaultAccelerationX = value;
39             }
40         }
41 
42         public decimal DefaultAccelerationY
43         {
44             get => defaultAccelerationY;
45 
46             set
47             {
48                 if(!IsAccelerationInRange(value))
49                 {
50                     throw new RecoverableException($"Value out of currently set range. Maximum value is {AccelerometerFullScaleRangeG}[g]");
51                 }
52                 defaultAccelerationY = value;
53             }
54         }
55 
56         public decimal DefaultAccelerationZ
57         {
58             get => defaultAccelerationZ;
59 
60             set
61             {
62                 if(!IsAccelerationInRange(value))
63                 {
64                     throw new RecoverableException($"Value out of currently set range. Maximum value is {AccelerometerFullScaleRangeG}[g]");
65                 }
66                 defaultAccelerationZ = value;
67             }
68         }
69 
70         public decimal AccelerationX => accelerationX ?? DefaultAccelerationX;
71         public decimal AccelerationY => accelerationY ?? DefaultAccelerationY;
72         public decimal AccelerationZ => accelerationZ ?? DefaultAccelerationZ;
73 
74         [OnRESDSample(SampleType.Acceleration)]
75         [BeforeRESDSample(SampleType.Acceleration)]
HandleAccelerationSample(AccelerationSample sample, TimeInterval timestamp)76         private void HandleAccelerationSample(AccelerationSample sample, TimeInterval timestamp)
77         {
78             if(sample != null)
79             {
80                 accelerationX = (decimal)sample.AccelerationX / 1e6m;
81                 accelerationY = (decimal)sample.AccelerationY / 1e6m;
82                 accelerationZ = (decimal)sample.AccelerationZ / 1e6m;
83             }
84             else
85             {
86                 accelerationX = null;
87                 accelerationY = null;
88                 accelerationZ = null;
89             }
90         }
91 
92         [AfterRESDSample(SampleType.Acceleration)]
HandleAccelerationSampleEnded(AccelerationSample sample, TimeInterval timestamp)93         private void HandleAccelerationSampleEnded(AccelerationSample sample, TimeInterval timestamp)
94         {
95             accelerometerFeederThread?.Stop();
96             accelerometerFeederThread = null;
97         }
98 
IsAccelerationInRange(decimal acc)99         private bool IsAccelerationInRange(decimal acc)
100         {
101             return Math.Abs(acc) <= AccelerometerFullScaleRangeG;
102         }
103 
104         private ushort RawAccelerationX => ConvertMeasurement(AccelerationX, value => value * AccelerometerSensitivityScaleFactor);
105         private ushort RawAccelerationY => ConvertMeasurement(AccelerationY, value => value * AccelerometerSensitivityScaleFactor);
106         private ushort RawAccelerationZ => ConvertMeasurement(AccelerationZ, value => value * AccelerometerSensitivityScaleFactor);
107 
108         private RESDStream<AccelerationSample> accelerometerResdStream;
109         private IManagedThread accelerometerFeederThread;
110 
111         private decimal? accelerationX = null;
112         private decimal? accelerationY = null;
113         private decimal? accelerationZ = null;
114         private decimal defaultAccelerationX;
115         private decimal defaultAccelerationY;
116         private decimal defaultAccelerationZ;
117 
118         private const decimal AccelerometerMaxOutputDataRateHz = 4500;
119         private const decimal AccelerometerWakeOnMotionThresholdStepSizeG = 0.004m;
120 
121         private decimal AccelerometerWakeOnMotionThreshold => accelerometerWakeOnMotionThreshold.Value * AccelerometerWakeOnMotionThresholdStepSizeG;
122         private decimal AccelerometerSampleRateDivider => accelerometerSampleRateDividerHigh.Value << 8 | accelerometerSampleRateDividerLow.Value;
123 
124         private decimal AccelerometerOutputDataRateHz
125         {
126             get
127             {
128                 if(accelerometerFilterChoice.Value)
129                 {
130                     return InternalSampleRateHz / (1 + AccelerometerSampleRateDivider);
131                 }
132                 return AccelerometerMaxOutputDataRateHz;
133             }
134         }
135 
136         private decimal AccelerometerFullScaleRangeG
137         {
138             get
139             {
140                 switch(accelerometerFullScaleRange.Value)
141                 {
142                     case AccelerationFullScaleRangeSelection.Mode0_2G:
143                         return 2m;
144                     case AccelerationFullScaleRangeSelection.Mode1_4G:
145                         return 4m;
146                     case AccelerationFullScaleRangeSelection.Mode2_8G:
147                         return 8m;
148                     case AccelerationFullScaleRangeSelection.Mode3_16G:
149                         return 16m;
150                     default:
151                         throw new Exception("Wrong accelerometer full scale range selection");
152                 }
153             }
154         }
155 
156         private decimal AccelerometerSensitivityScaleFactor
157         {
158             get
159             {
160                 switch(accelerometerFullScaleRange.Value)
161                 {
162                     case AccelerationFullScaleRangeSelection.Mode0_2G:
163                         return 16384m;
164                     case AccelerationFullScaleRangeSelection.Mode1_4G:
165                         return 8192m;
166                     case AccelerationFullScaleRangeSelection.Mode2_8G:
167                         return 4096m;
168                     case AccelerationFullScaleRangeSelection.Mode3_16G:
169                         return 2048m;
170                     default:
171                         throw new Exception("Wrong accelerometer full scale range selection");
172                 }
173             }
174         }
175 
176         private int AccelerometerAveragedSamples
177         {
178             get
179             {
180                 switch(accelerometerDecimatorConfig.Value)
181                 {
182                     case AccelerometerDecimator.Mode0_4Samples:
183                         if(!accelerometerFilterChoice.Value)
184                         {
185                             return 1;
186                         }
187                         return 4;
188                     case AccelerometerDecimator.Mode1_8Samples:
189                         return 8;
190                     case AccelerometerDecimator.Mode2_16Samples:
191                         return 16;
192                     case AccelerometerDecimator.Mode3_32Samples:
193                         return 32;
194                     default:
195                         throw new Exception("Wrong accelerometer decimator config selection");
196                 }
197             }
198         }
199 
200         private enum AccelerationFullScaleRangeSelection : byte
201         {
202             Mode0_2G = 0,
203             Mode1_4G = 1,
204             Mode2_8G = 2,
205             Mode3_16G = 3
206         }
207 
208         private enum AccelerometerDecimator
209         {
210             Mode0_4Samples = 0,
211             Mode1_8Samples = 1,
212             Mode2_16Samples = 2,
213             Mode3_32Samples = 3
214         }
215 
216         private enum WakeOnMotionCompareAlgorithm
217         {
218             InitialSample = 0,
219             PreviousSample = 1
220         }
221     }
222 }
223