77#include " sfeQwiicOtos.h"
88
99sfeQwiicOtos::sfeQwiicOtos ()
10- : _commBus{nullptr }, _linearUnit{kOtosLinearUnitInches }, _angularUnit{kOtosAngularUnitDegrees },
10+ : _commBus{nullptr }, _linearUnit{kSfeOtosLinearUnitInches }, _angularUnit{kSfeOtosAngularUnitDegrees },
1111 _meterToUnit{kMeterToInch }, _radToUnit{kRadianToDegree }
1212{
1313 // Nothing to do here!
@@ -20,7 +20,7 @@ sfeTkError_t sfeQwiicOtos::begin(sfeTkII2C *commBus)
2020 return kSTkErrFail ;
2121
2222 // Check the device address
23- if (commBus->address () != kOtosDefaultAddress )
23+ if (commBus->address () != kDefaultAddress )
2424 return kSTkErrFail ;
2525
2626 // Set bus pointer
@@ -39,24 +39,24 @@ sfeTkError_t sfeQwiicOtos::isConnected()
3939
4040 // Read the product ID
4141 uint8_t prodId;
42- err = _commBus->readRegisterByte (kOtosRegProductId , prodId);
42+ err = _commBus->readRegisterByte (kRegProductId , prodId);
4343 if (err != kSTkErrOk )
4444 return err;
4545
4646 // Check if the product ID is correct
47- if (prodId != kOtosProductId )
47+ if (prodId != kProductId )
4848 return kSTkErrFail ;
4949
5050 // Everything checks out, we must be connected!
5151 return kSTkErrOk ;
5252}
5353
54- sfeTkError_t sfeQwiicOtos::getVersionInfo (otos_version_t &hwVersion, otos_version_t &fwVersion)
54+ sfeTkError_t sfeQwiicOtos::getVersionInfo (sfe_otos_version_t &hwVersion, sfe_otos_version_t &fwVersion)
5555{
5656 // Read hardware and firmware version registers
5757 uint8_t rawData[2 ];
5858 size_t readBytes;
59- sfeTkError_t err = _commBus->readRegisterRegion (kOtosRegHwVersion , rawData, 2 , readBytes);
59+ sfeTkError_t err = _commBus->readRegisterRegion (kRegHwVersion , rawData, 2 , readBytes);
6060 if (err != kSTkErrOk )
6161 return err;
6262
@@ -75,9 +75,9 @@ sfeTkError_t sfeQwiicOtos::getVersionInfo(otos_version_t &hwVersion, otos_versio
7575sfeTkError_t sfeQwiicOtos::selfTest ()
7676{
7777 // Write the self-test register to start the test
78- sfe_otos_config_self_test_t selfTest;
78+ sfe_otos_self_test_config_t selfTest;
7979 selfTest.start = 1 ;
80- sfeTkError_t err = _commBus->writeRegisterByte (kOtosRegSelfTest , selfTest.value );
80+ sfeTkError_t err = _commBus->writeRegisterByte (kRegSelfTest , selfTest.value );
8181 if (err != kSTkErrOk )
8282 return err;
8383
@@ -88,7 +88,7 @@ sfeTkError_t sfeQwiicOtos::selfTest()
8888 delayMs (5 );
8989
9090 // Read the self-test register
91- err = _commBus->readRegisterByte (kOtosRegSelfTest , selfTest.value );
91+ err = _commBus->readRegisterByte (kRegSelfTest , selfTest.value );
9292 if (err != kSTkErrOk )
9393 return err;
9494
@@ -106,7 +106,7 @@ sfeTkError_t sfeQwiicOtos::selfTest()
106106sfeTkError_t sfeQwiicOtos::calibrateImu (uint8_t numSamples, bool waitUntilDone)
107107{
108108 // Write the number of samples to the device
109- sfeTkError_t err = _commBus->writeRegisterByte (kOtosRegImuCalib , numSamples);
109+ sfeTkError_t err = _commBus->writeRegisterByte (kRegImuCalib , numSamples);
110110 if (err != kSTkErrOk )
111111 return err;
112112
@@ -126,7 +126,7 @@ sfeTkError_t sfeQwiicOtos::calibrateImu(uint8_t numSamples, bool waitUntilDone)
126126
127127 // Read the gryo calibration register value
128128 uint8_t calibrationValue;
129- err = _commBus->readRegisterByte (kOtosRegImuCalib , calibrationValue);
129+ err = _commBus->readRegisterByte (kRegImuCalib , calibrationValue);
130130 if (err != kSTkErrOk )
131131 return err;
132132
@@ -139,12 +139,12 @@ sfeTkError_t sfeQwiicOtos::calibrateImu(uint8_t numSamples, bool waitUntilDone)
139139 return kSTkErrFail ;
140140}
141141
142- otos_linear_unit_t sfeQwiicOtos::getLinearUnit ()
142+ sfe_otos_linear_unit_t sfeQwiicOtos::getLinearUnit ()
143143{
144144 return _linearUnit;
145145}
146146
147- void sfeQwiicOtos::setLinearUnit (otos_linear_unit_t unit)
147+ void sfeQwiicOtos::setLinearUnit (sfe_otos_linear_unit_t unit)
148148{
149149 // Check if this unit is already set
150150 if (unit == _linearUnit)
@@ -154,15 +154,15 @@ void sfeQwiicOtos::setLinearUnit(otos_linear_unit_t unit)
154154 _linearUnit = unit;
155155
156156 // Compute conversion factor to new units
157- _meterToUnit = (unit == kOtosLinearUnitMeters ) ? 1 .0f : kMeterToInch ;
157+ _meterToUnit = (unit == kSfeOtosLinearUnitMeters ) ? 1 .0f : kMeterToInch ;
158158}
159159
160- otos_angular_unit_t sfeQwiicOtos::getAngularUnit ()
160+ sfe_otos_angular_unit_t sfeQwiicOtos::getAngularUnit ()
161161{
162162 return _angularUnit;
163163}
164164
165- void sfeQwiicOtos::setAngularUnit (otos_angular_unit_t unit)
165+ void sfeQwiicOtos::setAngularUnit (sfe_otos_angular_unit_t unit)
166166{
167167 // Check if this unit is already set
168168 if (unit == _angularUnit)
@@ -172,14 +172,14 @@ void sfeQwiicOtos::setAngularUnit(otos_angular_unit_t unit)
172172 _angularUnit = unit;
173173
174174 // Compute conversion factor to new units
175- _radToUnit = (unit == kOtosAngularUnitRadians ) ? 1 .0f : kRadianToDegree ;
175+ _radToUnit = (unit == kSfeOtosAngularUnitRadians ) ? 1 .0f : kRadianToDegree ;
176176}
177177
178178sfeTkError_t sfeQwiicOtos::getLinearScalar (float &scalar)
179179{
180180 // Read the linear scalar from the device
181181 uint8_t rawScalar;
182- sfeTkError_t err = _commBus->readRegisterByte (kOtosRegScalarLinear , rawScalar);
182+ sfeTkError_t err = _commBus->readRegisterByte (kRegScalarLinear , rawScalar);
183183 if (err != kSTkErrOk )
184184 return kSTkErrFail ;
185185
@@ -193,21 +193,21 @@ sfeTkError_t sfeQwiicOtos::getLinearScalar(float &scalar)
193193sfeTkError_t sfeQwiicOtos::setLinearScalar (float scalar)
194194{
195195 // Check if the scalar is out of bounds
196- if (scalar < kSfeOtosMinScalar || scalar > kSfeOtosMaxScalar )
196+ if (scalar < kMinScalar || scalar > kMaxScalar )
197197 return kSTkErrFail ;
198198
199199 // Convert to integer, multiples of 0.1% (+0.5 to round instead of truncate)
200200 uint8_t rawScalar = (int8_t )((scalar - 1 .0f ) * 1000 + 0 .5f );
201201
202202 // Write the scalar to the device
203- return _commBus->writeRegisterByte (kOtosRegScalarLinear , rawScalar);
203+ return _commBus->writeRegisterByte (kRegScalarLinear , rawScalar);
204204}
205205
206206sfeTkError_t sfeQwiicOtos::getAngularScalar (float &scalar)
207207{
208208 // Read the angular scalar from the device
209209 uint8_t rawScalar;
210- sfeTkError_t err = _commBus->readRegisterByte (kOtosRegScalarAngular , rawScalar);
210+ sfeTkError_t err = _commBus->readRegisterByte (kRegScalarAngular , rawScalar);
211211 if (err != kSTkErrOk )
212212 return kSTkErrFail ;
213213
@@ -221,90 +221,90 @@ sfeTkError_t sfeQwiicOtos::getAngularScalar(float &scalar)
221221sfeTkError_t sfeQwiicOtos::setAngularScalar (float scalar)
222222{
223223 // Check if the scalar is out of bounds
224- if (scalar < kSfeOtosMinScalar || scalar > kSfeOtosMaxScalar )
224+ if (scalar < kMinScalar || scalar > kMaxScalar )
225225 return kSTkErrFail ;
226226
227227 // Convert to integer, multiples of 0.1% (+0.5 to round instead of truncate)
228228 uint8_t rawScalar = (int8_t )((scalar - 1 .0f ) * 1000 + 0 .5f );
229229
230230 // Write the scalar to the device
231- return _commBus->writeRegisterByte (kOtosRegScalarAngular , rawScalar);
231+ return _commBus->writeRegisterByte (kRegScalarAngular , rawScalar);
232232}
233233
234234sfeTkError_t sfeQwiicOtos::resetTracking ()
235235{
236236 // Set tracking reset bit
237- return _commBus->writeRegisterByte (kOtosRegReset , 0x01 );
237+ return _commBus->writeRegisterByte (kRegReset , 0x01 );
238238}
239239
240- sfeTkError_t sfeQwiicOtos::getSignalProcess (sfe_otos_config_signal_process_t &config)
240+ sfeTkError_t sfeQwiicOtos::getSignalProcess (sfe_otos_signal_process_config_t &config)
241241{
242242 // Read the signal process register
243- return _commBus->readRegisterByte (kOtosRegSignalProcess , config.value );
243+ return _commBus->readRegisterByte (kRegSignalProcess , config.value );
244244}
245245
246- sfeTkError_t sfeQwiicOtos::setSignalProcess (sfe_otos_config_signal_process_t &config)
246+ sfeTkError_t sfeQwiicOtos::setSignalProcess (sfe_otos_signal_process_config_t &config)
247247{
248248 // Write the signal process register
249- return _commBus->writeRegisterByte (kOtosRegSignalProcess , config.value );
249+ return _commBus->writeRegisterByte (kRegSignalProcess , config.value );
250250}
251251
252- sfeTkError_t sfeQwiicOtos::getOffset (otos_pose2d_t &pose)
252+ sfeTkError_t sfeQwiicOtos::getOffset (sfe_otos_pose2d_t &pose)
253253{
254- return readPoseRegs (kOtosRegOffXL , pose, kInt16ToMeter , kInt16ToRad );
254+ return readPoseRegs (kRegOffXL , pose, kInt16ToMeter , kInt16ToRad );
255255}
256256
257257sfeTkError_t sfeQwiicOtos::getStatus (sfe_otos_status_t &status)
258258{
259- return _commBus->readRegisterByte (kOtosRegStatus , status.value );
259+ return _commBus->readRegisterByte (kRegStatus , status.value );
260260}
261261
262- sfeTkError_t sfeQwiicOtos::setOffset (otos_pose2d_t &pose)
262+ sfeTkError_t sfeQwiicOtos::setOffset (sfe_otos_pose2d_t &pose)
263263{
264- return writePoseRegs (kOtosRegOffXL , pose, kMeterToInt16 , kRadToInt16 );
264+ return writePoseRegs (kRegOffXL , pose, kMeterToInt16 , kRadToInt16 );
265265}
266266
267- sfeTkError_t sfeQwiicOtos::getPosition (otos_pose2d_t &pose)
267+ sfeTkError_t sfeQwiicOtos::getPosition (sfe_otos_pose2d_t &pose)
268268{
269- return readPoseRegs (kOtosRegPosXL , pose, kInt16ToMeter , kInt16ToRad );
269+ return readPoseRegs (kRegPosXL , pose, kInt16ToMeter , kInt16ToRad );
270270}
271271
272- sfeTkError_t sfeQwiicOtos::setPosition (otos_pose2d_t &pose)
272+ sfeTkError_t sfeQwiicOtos::setPosition (sfe_otos_pose2d_t &pose)
273273{
274- return writePoseRegs (kOtosRegPosXL , pose, kMeterToInt16 , kRadToInt16 );
274+ return writePoseRegs (kRegPosXL , pose, kMeterToInt16 , kRadToInt16 );
275275}
276276
277- sfeTkError_t sfeQwiicOtos::getVelocity (otos_pose2d_t &pose)
277+ sfeTkError_t sfeQwiicOtos::getVelocity (sfe_otos_pose2d_t &pose)
278278{
279- return readPoseRegs (kOtosRegVelXL , pose, kInt16ToMps , kInt16ToRps );
279+ return readPoseRegs (kRegVelXL , pose, kInt16ToMps , kInt16ToRps );
280280}
281281
282- sfeTkError_t sfeQwiicOtos::getAcceleration (otos_pose2d_t &pose)
282+ sfeTkError_t sfeQwiicOtos::getAcceleration (sfe_otos_pose2d_t &pose)
283283{
284- return readPoseRegs (kOtosRegAccXL , pose, kInt16ToMpss , kInt16ToRpss );
284+ return readPoseRegs (kRegAccXL , pose, kInt16ToMpss , kInt16ToRpss );
285285}
286286
287- sfeTkError_t sfeQwiicOtos::getPositionStdDev (otos_pose2d_t &pose)
287+ sfeTkError_t sfeQwiicOtos::getPositionStdDev (sfe_otos_pose2d_t &pose)
288288{
289- return readPoseRegs (kOtosRegPosStdXL , pose, kInt16ToMeter , kInt16ToRad );
289+ return readPoseRegs (kRegPosStdXL , pose, kInt16ToMeter , kInt16ToRad );
290290}
291291
292- sfeTkError_t sfeQwiicOtos::getVelocityStdDev (otos_pose2d_t &pose)
292+ sfeTkError_t sfeQwiicOtos::getVelocityStdDev (sfe_otos_pose2d_t &pose)
293293{
294- return readPoseRegs (kOtosRegVelStdXL , pose, kInt16ToMps , kInt16ToRps );
294+ return readPoseRegs (kRegVelStdXL , pose, kInt16ToMps , kInt16ToRps );
295295}
296296
297- sfeTkError_t sfeQwiicOtos::getAccelerationStdDev (otos_pose2d_t &pose)
297+ sfeTkError_t sfeQwiicOtos::getAccelerationStdDev (sfe_otos_pose2d_t &pose)
298298{
299- return readPoseRegs (kOtosRegAccStdXL , pose, kInt16ToMpss , kInt16ToRpss );
299+ return readPoseRegs (kRegAccStdXL , pose, kInt16ToMpss , kInt16ToRpss );
300300}
301301
302- sfeTkError_t sfeQwiicOtos::getPosVelAcc (otos_pose2d_t &pos, otos_pose2d_t &vel, otos_pose2d_t &acc)
302+ sfeTkError_t sfeQwiicOtos::getPosVelAcc (sfe_otos_pose2d_t &pos, sfe_otos_pose2d_t &vel, sfe_otos_pose2d_t &acc)
303303{
304304 // Read all pose registers
305305 uint8_t rawData[18 ];
306306 size_t bytesRead;
307- sfeTkError_t err = _commBus->readRegisterRegion (kOtosRegPosXL , rawData, 18 , bytesRead);
307+ sfeTkError_t err = _commBus->readRegisterRegion (kRegPosXL , rawData, 18 , bytesRead);
308308 if (err != kSTkErrOk )
309309 return err;
310310
@@ -321,12 +321,12 @@ sfeTkError_t sfeQwiicOtos::getPosVelAcc(otos_pose2d_t &pos, otos_pose2d_t &vel,
321321 return kSTkErrOk ;
322322}
323323
324- sfeTkError_t sfeQwiicOtos::getPosVelAccStdDev (otos_pose2d_t &pos, otos_pose2d_t &vel, otos_pose2d_t &acc)
324+ sfeTkError_t sfeQwiicOtos::getPosVelAccStdDev (sfe_otos_pose2d_t &pos, sfe_otos_pose2d_t &vel, sfe_otos_pose2d_t &acc)
325325{
326326 // Read all pose registers
327327 uint8_t rawData[18 ];
328328 size_t bytesRead;
329- sfeTkError_t err = _commBus->readRegisterRegion (kOtosRegPosStdXL , rawData, 18 , bytesRead);
329+ sfeTkError_t err = _commBus->readRegisterRegion (kRegPosStdXL , rawData, 18 , bytesRead);
330330 if (err != kSTkErrOk )
331331 return err;
332332
@@ -343,12 +343,13 @@ sfeTkError_t sfeQwiicOtos::getPosVelAccStdDev(otos_pose2d_t &pos, otos_pose2d_t
343343 return kSTkErrOk ;
344344}
345345
346- sfeTkError_t sfeQwiicOtos::getPosVelAccAndStdDev (otos_pose2d_t &pos, otos_pose2d_t &vel, otos_pose2d_t &acc, otos_pose2d_t &posStdDev, otos_pose2d_t &velStdDev, otos_pose2d_t &accStdDev)
346+ sfeTkError_t sfeQwiicOtos::getPosVelAccAndStdDev (sfe_otos_pose2d_t &pos, sfe_otos_pose2d_t &vel, sfe_otos_pose2d_t &acc,
347+ sfe_otos_pose2d_t &posStdDev, sfe_otos_pose2d_t &velStdDev, sfe_otos_pose2d_t &accStdDev)
347348{
348349 // Read all pose registers
349350 uint8_t rawData[36 ];
350351 size_t bytesRead;
351- sfeTkError_t err = _commBus->readRegisterRegion (kOtosRegPosXL , rawData, 36 , bytesRead);
352+ sfeTkError_t err = _commBus->readRegisterRegion (kRegPosXL , rawData, 36 , bytesRead);
352353 if (err != kSTkErrOk )
353354 return err;
354355
@@ -368,7 +369,7 @@ sfeTkError_t sfeQwiicOtos::getPosVelAccAndStdDev(otos_pose2d_t &pos, otos_pose2d
368369 return kSTkErrOk ;
369370}
370371
371- sfeTkError_t sfeQwiicOtos::readPoseRegs (uint8_t reg, otos_pose2d_t &pose, float rawToXY, float rawToH)
372+ sfeTkError_t sfeQwiicOtos::readPoseRegs (uint8_t reg, sfe_otos_pose2d_t &pose, float rawToXY, float rawToH)
372373{
373374 size_t bytesRead;
374375 uint8_t rawData[6 ];
@@ -388,7 +389,7 @@ sfeTkError_t sfeQwiicOtos::readPoseRegs(uint8_t reg, otos_pose2d_t &pose, float
388389 return kSTkErrOk ;
389390}
390391
391- sfeTkError_t sfeQwiicOtos::writePoseRegs (uint8_t reg, otos_pose2d_t &pose, float xyToRaw, float hToRaw)
392+ sfeTkError_t sfeQwiicOtos::writePoseRegs (uint8_t reg, sfe_otos_pose2d_t &pose, float xyToRaw, float hToRaw)
392393{
393394 // Store raw data in a temporary buffer
394395 uint8_t rawData[6 ];
@@ -398,7 +399,7 @@ sfeTkError_t sfeQwiicOtos::writePoseRegs(uint8_t reg, otos_pose2d_t &pose, float
398399 return _commBus->writeRegisterRegion (reg, rawData, 6 );
399400}
400401
401- void sfeQwiicOtos::regsToPose (uint8_t *rawData, otos_pose2d_t &pose, float rawToXY, float rawToH)
402+ void sfeQwiicOtos::regsToPose (uint8_t *rawData, sfe_otos_pose2d_t &pose, float rawToXY, float rawToH)
402403{
403404 // Store raw data
404405 int16_t rawX = (rawData[1 ] << 8 ) | rawData[0 ];
@@ -411,7 +412,7 @@ void sfeQwiicOtos::regsToPose(uint8_t *rawData, otos_pose2d_t &pose, float rawTo
411412 pose.h = rawH * rawToH * _radToUnit;
412413}
413414
414- void sfeQwiicOtos::poseToRegs (uint8_t *rawData, otos_pose2d_t &pose, float xyToRaw, float hToRaw)
415+ void sfeQwiicOtos::poseToRegs (uint8_t *rawData, sfe_otos_pose2d_t &pose, float xyToRaw, float hToRaw)
415416{
416417 // Convert pose units to raw data
417418 int16_t rawX = pose.x * xyToRaw / _meterToUnit;
0 commit comments