1+ // AzimuthConsole/CalibrationManager.cs
2+ using System . Globalization ;
3+ using AZMLib ;
4+ using UCNLDrivers . uAux ;
5+
6+ namespace AzimuthConsole ;
7+
8+ public enum CalibrationState
9+ {
10+ Idle ,
11+ Moving ,
12+ Measuring ,
13+ Completed ,
14+ Failed
15+ }
16+
17+ public class CalibrationDataPoint
18+ {
19+ public double TargetAngle_deg { get ; set ; }
20+ public double HAngle_deg { get ; set ; }
21+ public double VAngle_deg { get ; set ; }
22+ public double PTime_s { get ; set ; }
23+ public double SlantRange_m { get ; set ; }
24+ public double SlantRangeProjection_m { get ; set ; }
25+ public double StationDepth_m { get ; set ; }
26+ public double MSR_dB { get ; set ; }
27+ }
28+
29+ public class CalibrationManager
30+ {
31+ private readonly string _calDataPath ;
32+ private readonly uAuxRadantPort _rotator ;
33+ private readonly AZMManager _azmManager ;
34+ private readonly Action < string > _log ;
35+
36+ private CalibrationState _state = CalibrationState . Idle ;
37+ public CalibrationState State => _state ;
38+
39+ private double _stepAngle_deg = 15 ;
40+ public double StepAngle_deg => _stepAngle_deg ;
41+
42+ private int _measurementsPerPoint = 10 ;
43+
44+ private double _currentTargetAngle_deg ;
45+ private int _currentMeasurementCount ;
46+
47+ public double CurrentRotatorAngle => _rotator . CurrentAngle ;
48+ public int CollectedPoints => _calibrationPairs . Count ;
49+ public int TotalPoints => ( int ) ( 360.0 / _stepAngle_deg ) ;
50+
51+ private readonly List < CalibrationDataPoint > _rawData = new ( ) ;
52+ private readonly List < ( double EncoderAngle , double MeasuredAzimuth ) > _calibrationPairs = new ( ) ;
53+
54+ public IReadOnlyList < ( double EncoderAngle , double MeasuredAzimuth ) > CalibrationPairs => _calibrationPairs ;
55+
56+ public CalibrationManager ( uAuxRadantPort rotator , AZMManager azmManager ,
57+ Action < string > log , string basePath )
58+ {
59+ _rotator = rotator ?? throw new ArgumentNullException ( nameof ( rotator ) ) ;
60+ _azmManager = azmManager ?? throw new ArgumentNullException ( nameof ( azmManager ) ) ;
61+ _log = log ;
62+
63+ _azmManager . USBLRawDataHandler += OnUSBLRawData ;
64+ _azmManager . USBLRawDataEventEnabled = true ;
65+ _rotator . WaitingToFinishRotationChanged += OnWaitingChanged ;
66+
67+ _calDataPath = Path . Combine ( basePath , "caldata" ) ;
68+ Directory . CreateDirectory ( _calDataPath ) ;
69+ }
70+
71+ public void Start ( double startAngle , double stepAngle , int measurementsPerPoint )
72+ {
73+ if ( _state != CalibrationState . Idle )
74+ {
75+ _log ( "Calibration already in progress" ) ;
76+ return ;
77+ }
78+
79+ if ( _rotator . Status != AuxStatus . Detected )
80+ {
81+ _log ( "Antenna rotator is not connected" ) ;
82+ _state = CalibrationState . Failed ;
83+ return ;
84+ }
85+
86+ _stepAngle_deg = stepAngle ;
87+ _measurementsPerPoint = measurementsPerPoint ;
88+ _rawData . Clear ( ) ;
89+ _calibrationPairs . Clear ( ) ;
90+
91+ _log ( $ "Starting calibration from { startAngle : F1} ° with step { stepAngle : F1} °") ;
92+ MoveToAngle ( startAngle ) ;
93+ }
94+
95+ public void Start ( double stepAngle , int measurementsPerPoint )
96+ {
97+ Start ( 0.0 , stepAngle , measurementsPerPoint ) ;
98+ }
99+
100+ public void Stop ( )
101+ {
102+ _state = CalibrationState . Idle ;
103+ _rotator . RequestStop ( ) ;
104+ _log ( "Calibration stopped" ) ;
105+ }
106+
107+ private void MoveToAngle ( double angle )
108+ {
109+ if ( _rotator . Status != AuxStatus . Detected )
110+ {
111+ _log ( "Antenna rotator is not connected" ) ;
112+ _state = CalibrationState . Failed ;
113+ return ;
114+ }
115+
116+ _state = CalibrationState . Moving ;
117+ _currentTargetAngle_deg = angle ;
118+ _log ( $ "Moving to { angle : F1} °") ;
119+
120+ if ( ! _rotator . RequestSetAngle ( angle ) )
121+ {
122+ _log ( $ "Failed to set angle { angle : F1} °") ;
123+ _state = CalibrationState . Failed ;
124+ }
125+ }
126+
127+ private void OnWaitingChanged ( object ? sender , EventArgs e )
128+ {
129+ if ( _state != CalibrationState . Moving ) return ;
130+
131+ if ( _rotator . Status != AuxStatus . Detected )
132+ {
133+ _log ( "Antenna rotator disconnected during rotation" ) ;
134+ _state = CalibrationState . Failed ;
135+ return ;
136+ }
137+
138+ if ( ! _rotator . WaitingToFinishRotation )
139+ {
140+ if ( Math . Abs ( _rotator . CurrentAngle - _currentTargetAngle_deg ) < 1.0 )
141+ {
142+ _state = CalibrationState . Measuring ;
143+ _currentMeasurementCount = 0 ;
144+ _log ( $ "Measuring at { _currentTargetAngle_deg : F1} ° ({ _measurementsPerPoint } samples)") ;
145+ }
146+ else
147+ {
148+ _log ( $ "Failed to reach target angle. Current={ _rotator . CurrentAngle : F1} °, Target={ _currentTargetAngle_deg : F1} °") ;
149+ _state = CalibrationState . Failed ;
150+ }
151+ }
152+ }
153+
154+ private void OnUSBLRawData ( object ? sender , USBLRawDataEventArgs e )
155+ {
156+ if ( _state != CalibrationState . Measuring ) return ;
157+
158+ _rawData . Add ( new CalibrationDataPoint
159+ {
160+ TargetAngle_deg = _currentTargetAngle_deg ,
161+ HAngle_deg = e . HAngle_deg ,
162+ VAngle_deg = e . VAngle_deg ,
163+ PTime_s = e . PTime_s ,
164+ SlantRange_m = e . SlantRange_m ,
165+ SlantRangeProjection_m = e . SlantRangeProjection_m ,
166+ StationDepth_m = e . StationDepth_m ,
167+ MSR_dB = e . MSR_dB
168+ } ) ;
169+
170+ _currentMeasurementCount ++ ;
171+
172+ if ( _currentMeasurementCount >= _measurementsPerPoint )
173+ {
174+ var pointsForAngle = _rawData
175+ . Where ( p => Math . Abs ( p . TargetAngle_deg - _currentTargetAngle_deg ) < 0.1 )
176+ . ToList ( ) ;
177+
178+ if ( pointsForAngle . Count > 0 )
179+ {
180+ double avgHAngle = pointsForAngle . Average ( p => p . HAngle_deg ) ;
181+ _calibrationPairs . Add ( ( _currentTargetAngle_deg , avgHAngle ) ) ;
182+ _log ( $ "Angle { _currentTargetAngle_deg : F1} ° → avg azimuth = { avgHAngle : F2} °") ;
183+ }
184+
185+ double nextAngle = _currentTargetAngle_deg + _stepAngle_deg ;
186+ if ( nextAngle >= 360.0 - _stepAngle_deg / 2.0 )
187+ {
188+ Complete ( ) ;
189+ }
190+ else
191+ {
192+ MoveToAngle ( nextAngle ) ;
193+ }
194+ }
195+ }
196+
197+ private void Complete ( )
198+ {
199+ _state = CalibrationState . Completed ;
200+ _log ( $ "Calibration completed. { _calibrationPairs . Count } points collected.") ;
201+ SaveCalibrationData ( ) ;
202+ }
203+
204+ private void SaveCalibrationData ( )
205+ {
206+ try
207+ {
208+ string fileName = Path . Combine ( _calDataPath ,
209+ $ "cal_raw_{ _azmManager . DeviceSerialNumber } _{ DateTime . Now : yyyyMMdd_HHmmss} .csv") ;
210+ using var writer = new StreamWriter ( fileName ) ;
211+ writer . WriteLine ( "EncoderAngle_deg,MeasuredAzimuth_deg" ) ;
212+ foreach ( var ( enc , azm ) in _calibrationPairs . OrderBy ( p => p . EncoderAngle ) )
213+ {
214+ writer . WriteLine ( string . Format ( CultureInfo . InvariantCulture , "{0:F3},{1:F3}" , enc , azm ) ) ;
215+ }
216+ _log ( $ "Raw calibration data saved to { fileName } ") ;
217+ }
218+ catch ( Exception ex )
219+ {
220+ _log ( $ "Failed to save calibration data: { ex . Message } ") ;
221+ }
222+ }
223+ }
0 commit comments