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     {
FeedAngularRateSamplesFromRESD(string path, uint channel = 0, ulong startTime = 0, RESDStreamSampleOffset sampleOffsetType = RESDStreamSampleOffset.Specified, long sampleOffsetTime = 0)17         public void FeedAngularRateSamplesFromRESD(string path, uint channel = 0, ulong startTime = 0,
18             RESDStreamSampleOffset sampleOffsetType = RESDStreamSampleOffset.Specified, long sampleOffsetTime = 0)
19         {
20             gyroResdStream = this.CreateRESDStream<AngularRateSample>(path, channel, sampleOffsetType, sampleOffsetTime);
21             gyroFeederThread?.Stop();
22             gyroFeederThread = gyroResdStream.StartSampleFeedThread(this,
23                 (uint)GyroOutputDataRateHz,
24                 startTime: startTime
25             );
26         }
27 
28         public decimal DefaultAngularRateX
29         {
30             get => defaultAngularRateX;
31 
32             set
33             {
34                 if(!IsAngularRateInRange(value))
35                 {
36                     throw new RecoverableException($"Value out of currently set range. Maximum value is {GyroFullScaleRangeDPS}[g]");
37                 }
38                 defaultAngularRateX = value;
39             }
40         }
41 
42         public decimal DefaultAngularRateY
43         {
44             get => defaultAngularRateY;
45 
46             set
47             {
48                 if(!IsAngularRateInRange(value))
49                 {
50                     throw new RecoverableException($"Value out of currently set range. Maximum value is {GyroFullScaleRangeDPS}[g]");
51                 }
52                 defaultAngularRateY = value;
53             }
54         }
55 
56         public decimal DefaultAngularRateZ
57         {
58             get => defaultAngularRateZ;
59 
60             set
61             {
62                 if(!IsAngularRateInRange(value))
63                 {
64                     throw new RecoverableException($"Value out of currently set range. Maximum value is {GyroFullScaleRangeDPS}[g]");
65                 }
66                 defaultAngularRateZ = value;
67             }
68         }
69 
70         public decimal AngularRateX => angularRateX ?? DefaultAngularRateX;
71         public decimal AngularRateY => angularRateY ?? DefaultAngularRateY;
72         public decimal AngularRateZ => angularRateZ ?? DefaultAngularRateZ;
73 
74         [OnRESDSample(SampleType.AngularRate)]
75         [BeforeRESDSample(SampleType.AngularRate)]
HandleAngularRateSample(AngularRateSample sample, TimeInterval timestamp)76         private void HandleAngularRateSample(AngularRateSample sample, TimeInterval timestamp)
77         {
78             if(sample != null)
79             {
80                 angularRateX = RadiansToDegrees * (decimal)sample.AngularRateX / 1e5m;
81                 angularRateY = RadiansToDegrees * (decimal)sample.AngularRateY / 1e5m;
82                 angularRateZ = RadiansToDegrees * (decimal)sample.AngularRateZ / 1e5m;
83             }
84             else
85             {
86                 angularRateX = null;
87                 angularRateY = null;
88                 angularRateZ = null;
89             }
90         }
91 
92         [AfterRESDSample(SampleType.AngularRate)]
HandleAngularRateSampleEnded(AngularRateSample sample, TimeInterval timestamp)93         private void HandleAngularRateSampleEnded(AngularRateSample sample, TimeInterval timestamp)
94         {
95             gyroFeederThread.Stop();
96             gyroFeederThread = null;
97         }
98 
IsAngularRateInRange(decimal value)99         private bool IsAngularRateInRange(decimal value)
100         {
101             return Math.Abs(value) <= GyroFullScaleRangeDPS;
102         }
103 
104         private ushort RawAngularRateX => ConvertMeasurement(AngularRateX, value => value * GyroSensitivityScaleFactor);
105         private ushort RawAngularRateY => ConvertMeasurement(AngularRateY, value => value * GyroSensitivityScaleFactor);
106         private ushort RawAngularRateZ => ConvertMeasurement(AngularRateZ, value => value * GyroSensitivityScaleFactor);
107 
108         private decimal? angularRateX;
109         private decimal? angularRateY;
110         private decimal? angularRateZ;
111         private decimal defaultAngularRateX;
112         private decimal defaultAngularRateY;
113         private decimal defaultAngularRateZ;
114 
115         private RESDStream<AngularRateSample> gyroResdStream;
116         private IManagedThread gyroFeederThread;
117 
118         private const decimal RadiansToDegrees = 180m / (decimal)Math.PI;
119         private const decimal GyroMaxOutputDataRateHz = 9000;
120         private const decimal GyroOffsetCancellationStepSizeDPS = 0.0305m;
121 
122         private decimal GyroOutputDataRateHz
123         {
124             get
125             {
126                 if(gyroFilterChoice.Value)
127                 {
128                     return InternalSampleRateHz / (1 + gyroSampleRateDivider.Value);
129                 }
130                 return GyroMaxOutputDataRateHz;
131             }
132         }
133 
134         private decimal GyroFullScaleRangeDPS
135         {
136             get
137             {
138                 switch(gyroFullScaleRange.Value)
139                 {
140                     case GyroFullScaleRangeSelection.Mode0_250DPS:
141                         return 250;
142                     case GyroFullScaleRangeSelection.Mode1_500DPS:
143                         return 500;
144                     case GyroFullScaleRangeSelection.Mode2_1000DPS:
145                         return 1000;
146                     case GyroFullScaleRangeSelection.Mode3_2000DPS:
147                         return 2000;
148                     default:
149                         throw new Exception("Wrong gyroscope full scale range selection");
150                 }
151             }
152         }
153 
154         private decimal GyroSensitivityScaleFactor
155         {
156             get
157             {
158                 switch(gyroFullScaleRange.Value)
159                 {
160                     case GyroFullScaleRangeSelection.Mode0_250DPS:
161                         return 131m;
162                     case GyroFullScaleRangeSelection.Mode1_500DPS:
163                         return 65.5m;
164                     case GyroFullScaleRangeSelection.Mode2_1000DPS:
165                         return 32.8m;
166                     case GyroFullScaleRangeSelection.Mode3_2000DPS:
167                         return 16.4m;
168                     default:
169                         throw new Exception("Wrong gyroscope full scale range selection");
170                 }
171             }
172         }
173 
174         private int GyroAveragedSamples => (int)Math.Pow(2, gyroAveragingFilterExponent.Value);
175 
176         private enum GyroFullScaleRangeSelection : byte
177         {
178             Mode0_250DPS = 0,
179             Mode1_500DPS = 1,
180             Mode2_1000DPS = 2,
181             Mode3_2000DPS = 3
182         }
183     }
184 }
185