2020#include < math.h>
2121#include < stdint.h>
2222
23- // Struct to define a 2D pose, including x and y coordinates and heading angle
23+ // / @brief 2D pose structure, including x and y coordinates and heading angle
24+ // / @note Although pose is traditionally used for position and orientation, this
25+ // / structure is also used for velocity and accleration by the OTOS driver
2426typedef struct
2527{
2628 float x;
2729 float y;
2830 float h;
2931} sfe_otos_pose2d_t ;
3032
31- // Enumerations for the linear units
33+ // / @brief Enumerations for the linear units
3234typedef enum
3335{
3436 kSfeOtosLinearUnitMeters = 0 ,
3537 kSfeOtosLinearUnitInches = 1
3638} sfe_otos_linear_unit_t ;
3739
38- // Enumerations for the angular units
40+ // / @brief Enumerations for the angular units
3941typedef enum
4042{
4143 kSfeOtosAngularUnitRadians = 0 ,
4244 kSfeOtosAngularUnitDegrees = 1
4345} sfe_otos_angular_unit_t ;
4446
45- // Version register bit fields
47+ // / @brief Version register bit fields
4648typedef union {
4749 struct
4850 {
@@ -52,7 +54,7 @@ typedef union {
5254 uint8_t value;
5355} sfe_otos_version_t ;
5456
55- // Signal process config register bit fields
57+ // / @brief Signal process config register bit fields
5658typedef union {
5759 struct
5860 {
@@ -65,7 +67,7 @@ typedef union {
6567 uint8_t value;
6668} sfe_otos_signal_process_config_t ;
6769
68- // Self test register bit fields
70+ // / @brief Self test register bit fields
6971typedef union {
7072 struct
7173 {
@@ -78,7 +80,7 @@ typedef union {
7880 uint8_t value;
7981} sfe_otos_self_test_config_t ;
8082
81- // Status register bit fields
83+ // / @brief Status register bit fields
8284typedef union {
8385 struct
8486 {
@@ -91,13 +93,17 @@ typedef union {
9193 uint8_t value;
9294} sfe_otos_status_t ;
9395
96+ // / @brief Class for the SparkFun Qwiic Optical Tracking Odometry Sensor (OTOS).
97+ // / Includes methods to communicate with the sensor, such as getting the tracked
98+ // / location, configuring the sensor, etc. This class is a base class that must
99+ // / be derived to implement the delay function and I2C communication bus.
94100class sfeQwiicOtos
95101{
96102 public:
97- // / @brief Default constructor
103+ // / @brief Default constructor, only initializes member variables
98104 sfeQwiicOtos ();
99105
100- // / @brief Begins the Qwiic OTOS
106+ // / @brief Begins the Qwiic OTOS and verifies it is connected
101107 // / @param commBus I2C bus to use for communication
102108 // / @return 0 for succuss, negative for errors, positive for warnings
103109 sfeTkError_t begin (sfeTkII2C *commBus = nullptr );
@@ -106,69 +112,185 @@ class sfeQwiicOtos
106112 // / @return 0 for succuss, negative for errors, positive for warnings
107113 sfeTkError_t isConnected ();
108114
115+ // / @brief Gets the hardware and firmware version numbers from the OTOS
116+ // / @param hwVersion Hardware version number
117+ // / @param fwVersion Firmware version number
118+ // / @return 0 for succuss, negative for errors, positive for warnings
109119 sfeTkError_t getVersionInfo (sfe_otos_version_t &hwVersion, sfe_otos_version_t &fwVersion);
110120
121+ // / @brief Performs a self test of the OTOS
122+ // / @return 0 for succuss, negative for errors, positive for warnings
111123 sfeTkError_t selfTest ();
112124
125+ // / @brief Calibrates the IMU on the OTOS, which removes the accelerometer
126+ // / and gyroscope offsets
127+ // / @param numSamples Number of samples to take for calibration. Each sample
128+ // / takes about 2.4ms, so fewer samples can be taken for faster calibration
129+ // / @param waitUntilDone Whether to wait until the calibration is complete.
130+ // / Set false to calibrate asynchronously, see getImuCalibrationProgress()
131+ // / @return 0 for succuss, negative for errors, positive for warnings
113132 sfeTkError_t calibrateImu (uint8_t numSamples = 255 , bool waitUntilDone = true );
114133
134+ // / @brief Gets the progress of the IMU calibration. Used for asynchronous
135+ // / calibration with calibrateImu()
136+ // / @param numSamples Number of samples remaining for calibration
137+ // / @return 0 for succuss, negative for errors, positive for warnings
115138 sfeTkError_t getImuCalibrationProgress (uint8_t &numSamples);
116139
140+ // / @brief Gets the linear unit used by all methods using a pose
141+ // / @return Linear unit
117142 sfe_otos_linear_unit_t getLinearUnit ();
118143
144+ // / @brief Sets the linear unit used by all methods using a pose
145+ // / @param unit Linear unit
119146 void setLinearUnit (sfe_otos_linear_unit_t unit);
120147
148+ // / @brief Gets the angular unit used by all methods using a pose
149+ // / @return Angular unit
121150 sfe_otos_angular_unit_t getAngularUnit ();
122151
152+ // / @brief Sets the angular unit used by all methods using a pose
153+ // / @param unit Angular unit
123154 void setAngularUnit (sfe_otos_angular_unit_t unit);
124155
156+ // / @brief Gets the linear scalar used by the OTOS
157+ // / @param scalar Linear scalar
158+ // / @return 0 for succuss, negative for errors, positive for warnings
125159 sfeTkError_t getLinearScalar (float &scalar);
126160
161+ // / @brief Sets the linear scalar used by the OTOS. Can be used to
162+ // / compensate for scaling issues with the sensor measurements
163+ // / @param scalar Linear scalar, must be between 0.872 and 1.127
164+ // / @return 0 for succuss, negative for errors, positive for warnings
127165 sfeTkError_t setLinearScalar (float scalar);
128166
167+ // / @brief Gets the angular scalar used by the OTOS
168+ // / @param scalar Angular scalar
169+ // / @return 0 for succuss, negative for errors, positive for warnings
129170 sfeTkError_t getAngularScalar (float &scalar);
130171
172+ // / @brief Sets the angular scalar used by the OTOS. Can be used to
173+ // / compensate for scaling issues with the sensor measurements
174+ // / @param scalar Angular scalar, must be between 0.872 and 1.127
175+ // / @return 0 for succuss, negative for errors, positive for warnings
131176 sfeTkError_t setAngularScalar (float scalar);
132177
178+ // / @brief Resets the tracking algorithm, which resets the position to the
179+ // / origin, but can also be used to recover from some rare tracking errors
180+ // / @return 0 for succuss, negative for errors, positive for warnings
133181 sfeTkError_t resetTracking ();
134182
183+ // / @brief Gets the signal processing configuration from the OTOS
184+ // / @param config Signal processing configuration
185+ // / @return 0 for succuss, negative for errors, positive for warnings
135186 sfeTkError_t getSignalProcessConfig (sfe_otos_signal_process_config_t &config);
136187
188+ // / @brief Sets the signal processing configuration on the OTOS. This is
189+ // / primarily useful for creating and testing a new lookup table calibration
190+ // / @param config Signal processing configuration
191+ // / @return 0 for succuss, negative for errors, positive for warnings
137192 sfeTkError_t setSignalProcessConfig (sfe_otos_signal_process_config_t &config);
138193
194+ // / @brief Gets the status register from the OTOS, which includes warnings
195+ // / and errors reported by the sensor
196+ // / @param status Status register value
197+ // / @return 0 for succuss, negative for errors, positive for warnings
139198 sfeTkError_t getStatus (sfe_otos_status_t &status);
140199
200+ // / @brief Gets the offset of the OTOS
201+ // / @param pose Offset of the sensor relative to the center of the robot
202+ // / @return 0 for succuss, negative for errors, positive for warnings
141203 sfeTkError_t getOffset (sfe_otos_pose2d_t &pose);
142204
205+ // / @brief Sets the offset of the OTOS. This is useful if your sensor is
206+ // / mounted off-center from a robot. Rather than returning the position of
207+ // / the sensor, the OTOS will return the position of the robot
208+ // / @param pose Offset of the sensor relative to the center of the robot
209+ // / @return 0 for succuss, negative for errors, positive for warnings
143210 sfeTkError_t setOffset (sfe_otos_pose2d_t &pose);
144211
212+ // / @brief Gets the position measured by the OTOS
213+ // / @param pose Position measured by the OTOS
214+ // / @return 0 for succuss, negative for errors, positive for warnings
145215 sfeTkError_t getPosition (sfe_otos_pose2d_t &pose);
146216
217+ // / @brief Sets the position measured by the OTOS. This is useful if your
218+ // / robot does not start at the origin, or you have another source of
219+ // / location information (eg. vision odometry); the OTOS will continue
220+ // / tracking from this position
221+ // / @param pose New position for the OTOS to track from
222+ // / @return 0 for succuss, negative for errors, positive for warnings
147223 sfeTkError_t setPosition (sfe_otos_pose2d_t &pose);
148224
225+ // / @brief Gets the velocity measured by the OTOS
226+ // / @param pose Velocity measured by the OTOS
227+ // / @return 0 for succuss, negative for errors, positive for warnings
149228 sfeTkError_t getVelocity (sfe_otos_pose2d_t &pose);
150229
230+ // / @brief Gets the acceleration measured by the OTOS
231+ // / @param pose Acceleration measured by the OTOS
232+ // / @return 0 for succuss, negative for errors, positive for warnings
151233 sfeTkError_t getAcceleration (sfe_otos_pose2d_t &pose);
152234
235+ // / @brief Gets the standard deviation of the measured position
236+ // / @param pose Standard deviation of the position measured by the OTOS
237+ // / @return 0 for succuss, negative for errors, positive for warnings
238+ // / @note These values are just the square root of the diagonal elements of
239+ // / the covariance matrices of the Kalman filters used in the firmware, so
240+ // / they are just statistical quantities and do not represent actual error!
153241 sfeTkError_t getPositionStdDev (sfe_otos_pose2d_t &pose);
154242
243+ // / @brief Gets the standard deviation of the measured velocity
244+ // / @param pose Standard deviation of the velocity measured by the OTOS
245+ // / @return 0 for succuss, negative for errors, positive for warnings
246+ // / @note These values are just the square root of the diagonal elements of
247+ // / the covariance matrices of the Kalman filters used in the firmware, so
248+ // / they are just statistical quantities and do not represent actual error!
155249 sfeTkError_t getVelocityStdDev (sfe_otos_pose2d_t &pose);
156250
251+ // / @brief Gets the standard deviation of the measured acceleration
252+ // / @param pose Standard deviation of the acceleration measured by the OTOS
253+ // / @return 0 for succuss, negative for errors, positive for warnings
254+ // / @note These values are just the square root of the diagonal elements of
255+ // / the covariance matrices of the Kalman filters used in the firmware, so
256+ // / they are just statistical quantities and do not represent actual error!
157257 sfeTkError_t getAccelerationStdDev (sfe_otos_pose2d_t &pose);
158258
259+ // / @brief Gets the position, velocity, and acceleration measured by the
260+ // / OTOS in a single burst read
261+ // / @param pos Position measured by the OTOS
262+ // / @param vel Velocity measured by the OTOS
263+ // / @param acc Acceleration measured by the OTOS
264+ // / @return 0 for succuss, negative for errors, positive for warnings
159265 sfeTkError_t getPosVelAcc (sfe_otos_pose2d_t &pos, sfe_otos_pose2d_t &vel, sfe_otos_pose2d_t &acc);
160266
267+ // / @brief Gets the standard deviation of the measured position, velocity,
268+ // / and acceleration in a single burst read
269+ // / @param pos Standard deviation of the position measured by the OTOS
270+ // / @param vel Standard deviation of the velocity measured by the OTOS
271+ // / @param acc Standard deviation of the acceleration measured by the OTOS
272+ // / @return 0 for succuss, negative for errors, positive for warnings
161273 sfeTkError_t getPosVelAccStdDev (sfe_otos_pose2d_t &pos, sfe_otos_pose2d_t &vel, sfe_otos_pose2d_t &acc);
162274
275+ // / @brief Gets the position, velocity, acceleration, and standard deviation
276+ // / of each in a single burst read
277+ // / @param pos Position measured by the OTOS
278+ // / @param vel Velocity measured by the OTOS
279+ // / @param acc Acceleration measured by the OTOS
280+ // / @param posStdDev Standard deviation of the position measured by the OTOS
281+ // / @param velStdDev Standard deviation of the velocity measured by the OTOS
282+ // / @param accStdDev Standard deviation of the acceleration measured by the OTOS
283+ // / @return 0 for succuss, negative for errors, positive for warnings
163284 sfeTkError_t getPosVelAccAndStdDev (sfe_otos_pose2d_t &pos, sfe_otos_pose2d_t &vel, sfe_otos_pose2d_t &acc,
164285 sfe_otos_pose2d_t &posStdDev, sfe_otos_pose2d_t &velStdDev, sfe_otos_pose2d_t &accStdDev);
165286
166- // Default I2C addresses of the Qwiic OTOS
287+ // / @brief Default I2C addresses of the Qwiic OTOS
167288 static constexpr uint8_t kDefaultAddress = 0x17 ;
168289
169- // Min and max scalar values for the linear and angular scalars (8-bit
170- // signed integer representing 0.1% increments)
290+ // / @brief Minimum scalar value for the linear and angular scalars
171291 static constexpr float kMinScalar = 0 .872f ;
292+
293+ // / @brief Maximum scalar value for the linear and angular scalars
172294 static constexpr float kMaxScalar = 1 .127f ;
173295
174296 protected:
0 commit comments