Skip to content

Commit 8678c07

Browse files
pre-commit-ci-lite[bot]pre-commit-ci-lite[bot]
authored
[pre-commit.ci lite] apply automatic fixes
1 parent 82aa4af commit 8678c07

5 files changed

Lines changed: 103 additions & 101 deletions

File tree

‎src/software/embedded/BUILD‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -28,8 +28,8 @@ cc_library(
2828
"//proto:tbots_cc_proto",
2929
"//software/embedded/services:motor",
3030
"//software/embedded/services:power",
31-
"//software/embedded/services/network",
3231
"//software/embedded/services/imu",
32+
"//software/embedded/services/network",
3333
"//software/embedded/toml_config",
3434
"//software/logger:network_logger",
3535
"//software/tracy:tracy_constants",

‎src/software/embedded/services/imu.cpp‎

Lines changed: 20 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -1,10 +1,12 @@
1+
#include "imu.h"
2+
13
#include <fcntl.h>
24
#include <linux/i2c-dev.h>
35
#include <linux/i2c.h>
46
#include <sys/ioctl.h>
5-
#include <climits> // For SHRT_MAX
67

7-
#include "imu.h"
8+
#include <climits> // For SHRT_MAX
9+
810
#include "shared/constants.h"
911
#include "software/logger/logger.h"
1012

@@ -139,7 +141,7 @@ std::optional<int16_t> ImuService::readAndCombineByteData(uint8_t ls_reg, uint8_
139141

140142
uint16_t combined = (static_cast<uint8_t>(most_significant) << 8) |
141143
static_cast<uint8_t>(least_significant);
142-
144+
143145
return static_cast<int16_t>(combined);
144146
}
145147

@@ -150,22 +152,22 @@ std::optional<AngularVelocity> ImuService::pollHeadingVelocity()
150152
return std::nullopt;
151153
}
152154

153-
std::optional<int16_t> opt_full_word = readAndCombineByteData(HEADING_LEAST_SIG_REG, HEADING_MOST_SIG_REG);
154-
155+
std::optional<int16_t> opt_full_word =
156+
readAndCombineByteData(HEADING_LEAST_SIG_REG, HEADING_MOST_SIG_REG);
157+
155158
if (!opt_full_word.has_value())
156159
{
157160
return std::nullopt;
158161
}
159162

160163
int16_t full_word = opt_full_word.value();
161-
164+
162165
double degrees_per_sec = static_cast<double>(full_word) /
163166
static_cast<double>(SHRT_MAX) * IMU_FULL_SCALE_DPS;
164167
return AngularVelocity::fromDegrees(degrees_per_sec - degrees_error_);
165168
}
166169

167-
168-
170+
169171

170172
std::optional<Eigen::Vector2d> ImuService::pollLinearAcceleration()
171173
{
@@ -174,8 +176,10 @@ std::optional<Eigen::Vector2d> ImuService::pollLinearAcceleration()
174176
return std::nullopt;
175177
}
176178

177-
std::optional<int16_t> opt_x = readAndCombineByteData(ACCEL_X_LEAST_SIG_REG, ACCEL_X_MOST_SIG_REG);
178-
std::optional<int16_t> opt_y = readAndCombineByteData(ACCEL_Y_LEAST_SIG_REG, ACCEL_Y_MOST_SIG_REG);
179+
std::optional<int16_t> opt_x =
180+
readAndCombineByteData(ACCEL_X_LEAST_SIG_REG, ACCEL_X_MOST_SIG_REG);
181+
std::optional<int16_t> opt_y =
182+
readAndCombineByteData(ACCEL_Y_LEAST_SIG_REG, ACCEL_Y_MOST_SIG_REG);
179183

180184
if (!opt_x.has_value() || !opt_y.has_value())
181185
{
@@ -185,13 +189,11 @@ std::optional<Eigen::Vector2d> ImuService::pollLinearAcceleration()
185189
int16_t raw_x = opt_x.value();
186190
int16_t raw_y = opt_y.value();
187191

188-
double a_x = (static_cast<double>(raw_x) / SHRT_MAX)
189-
* ACCELEROMETER_FULL_SCALE_G
190-
* ACCELERATION_DUE_TO_GRAVITY_METERS_PER_SECOND_SQUARED;
191-
192-
double a_y = (static_cast<double>(raw_y) / SHRT_MAX)
193-
* ACCELEROMETER_FULL_SCALE_G
194-
* ACCELERATION_DUE_TO_GRAVITY_METERS_PER_SECOND_SQUARED;
195-
192+
double a_x = (static_cast<double>(raw_x) / SHRT_MAX) * ACCELEROMETER_FULL_SCALE_G *
193+
ACCELERATION_DUE_TO_GRAVITY_METERS_PER_SECOND_SQUARED;
194+
195+
double a_y = (static_cast<double>(raw_y) / SHRT_MAX) * ACCELEROMETER_FULL_SCALE_G *
196+
ACCELERATION_DUE_TO_GRAVITY_METERS_PER_SECOND_SQUARED;
197+
196198
return Eigen::Vector2d(a_x, a_y);
197199
}

‎src/software/embedded/services/imu.h‎

Lines changed: 69 additions & 70 deletions
Original file line numberDiff line numberDiff line change
@@ -1,9 +1,9 @@
11
#pragma once
22

33
#include <Eigen/Dense>
4-
#include <utility>
5-
#include <numbers>
64
#include <climits>
5+
#include <numbers>
6+
#include <utility>
77

88
#include "proto/tbots_software_msgs.pb.h"
99
#include "software/geom/angular_velocity.h"
@@ -13,72 +13,71 @@
1313
*/
1414
class ImuService
1515
{
16-
public:
17-
/**
18-
* Constructs and initializes a new IMU service object.
19-
*
20-
* If successfully initialized, will try to do a simple calibration of the IMU.
21-
*/
22-
ImuService();
23-
/*
24-
* Polls the latest IMU reading of the angular velocity of the robot on the z axis
25-
* @return the current angular velocty of the robot on the z axis
26-
*/
27-
std::optional<AngularVelocity> pollHeadingVelocity();
28-
/*
29-
* Polls the latest IMU reading of the linear acceleration of the robot on the z plane
30-
* @return the current linear acceleration of the robot on the z plane
31-
*/
32-
std::optional<Eigen::Vector2d> pollLinearAcceleration();
33-
34-
35-
// Variance from datasheet (in rad^2/s^2)
36-
static constexpr double IMU_VARIANCE =
37-
(4.0 * 14.4222 / 1000.0 * std::numbers::pi / 180.0) *
38-
(4.0 * 14.4222 / 1000.0 * std::numbers::pi / 180.0);
39-
40-
private:
41-
42-
/*
43-
* Reads byte data from two registers, and combine them into a single value
44-
* @parama ls_reg register of the least significant register
45-
* @parama ms_reg register of the most significant register
46-
* @return the combined integer value of the two registers
47-
*/
48-
49-
std::optional<int16_t> readAndCombineByteData(uint8_t ls_reg, uint8_t ms_reg);
50-
51-
bool initialized_=false;
52-
53-
int file_descriptor_=0;
54-
55-
double degrees_error_;
56-
57-
// Maps the maximum raw reading from 16-bit integer to be 2 times gravity
58-
static constexpr double ACCELEROMETER_FULL_SCALE_G = 2.0;
59-
// Same for gyroscope, 1000 degrees per second
60-
static constexpr double IMU_FULL_SCALE_DPS=1000.0;
61-
62-
/// Various i2c registers.
63-
static const uint8_t WHOAMI_REG = 0xf;
64-
static const uint8_t ACCEL_CONTROL_REG = 0x10;
65-
static const uint8_t GYRO_CONTROL_REG = 0x11;
66-
static const uint8_t CTRL4_C = 0x13;
67-
static const uint8_t CTRL6_C = 0x15;
68-
static const uint8_t CTRL8_XL = 0x17;
69-
70-
// Device path for the IMU
71-
inline static const std::string IMU_DEVICE = "/dev/i2c-1";
72-
73-
// Gyroscope Z-axis (Yaw) Output Data Registers
74-
static constexpr uint8_t HEADING_LEAST_SIG_REG = 0x26; // OUTZ_L_G
75-
static constexpr uint8_t HEADING_MOST_SIG_REG = 0x27; // OUTZ_H_G
76-
77-
// Accelerometer X-axis Output Data Registers
78-
static constexpr uint8_t ACCEL_X_LEAST_SIG_REG = 0x28; // OUTX_L_XL
79-
static constexpr uint8_t ACCEL_X_MOST_SIG_REG = 0x29; // OUTX_H_XL
80-
81-
// Accelerometer Y-axis Output Data Registers
82-
static constexpr uint8_t ACCEL_Y_LEAST_SIG_REG = 0x2A; // OUTY_L_XL
83-
static constexpr uint8_t ACCEL_Y_MOST_SIG_REG = 0x2B; // OUTY_H_XL
16+
public:
17+
/**
18+
* Constructs and initializes a new IMU service object.
19+
*
20+
* If successfully initialized, will try to do a simple calibration of the IMU.
21+
*/
22+
ImuService();
23+
/*
24+
* Polls the latest IMU reading of the angular velocity of the robot on the z axis
25+
* @return the current angular velocty of the robot on the z axis
26+
*/
27+
std::optional<AngularVelocity> pollHeadingVelocity();
28+
/*
29+
* Polls the latest IMU reading of the linear acceleration of the robot on the z plane
30+
* @return the current linear acceleration of the robot on the z plane
31+
*/
32+
std::optional<Eigen::Vector2d> pollLinearAcceleration();
33+
34+
35+
// Variance from datasheet (in rad^2/s^2)
36+
static constexpr double IMU_VARIANCE =
37+
(4.0 * 14.4222 / 1000.0 * std::numbers::pi / 180.0) *
38+
(4.0 * 14.4222 / 1000.0 * std::numbers::pi / 180.0);
39+
40+
private:
41+
/*
42+
* Reads byte data from two registers, and combine them into a single value
43+
* @parama ls_reg register of the least significant register
44+
* @parama ms_reg register of the most significant register
45+
* @return the combined integer value of the two registers
46+
*/
47+
48+
std::optional<int16_t> readAndCombineByteData(uint8_t ls_reg, uint8_t ms_reg);
49+
50+
bool initialized_ = false;
51+
52+
int file_descriptor_ = 0;
53+
54+
double degrees_error_;
55+
56+
// Maps the maximum raw reading from 16-bit integer to be 2 times gravity
57+
static constexpr double ACCELEROMETER_FULL_SCALE_G = 2.0;
58+
// Same for gyroscope, 1000 degrees per second
59+
static constexpr double IMU_FULL_SCALE_DPS = 1000.0;
60+
61+
/// Various i2c registers.
62+
static const uint8_t WHOAMI_REG = 0xf;
63+
static const uint8_t ACCEL_CONTROL_REG = 0x10;
64+
static const uint8_t GYRO_CONTROL_REG = 0x11;
65+
static const uint8_t CTRL4_C = 0x13;
66+
static const uint8_t CTRL6_C = 0x15;
67+
static const uint8_t CTRL8_XL = 0x17;
68+
69+
// Device path for the IMU
70+
inline static const std::string IMU_DEVICE = "/dev/i2c-1";
71+
72+
// Gyroscope Z-axis (Yaw) Output Data Registers
73+
static constexpr uint8_t HEADING_LEAST_SIG_REG = 0x26; // OUTZ_L_G
74+
static constexpr uint8_t HEADING_MOST_SIG_REG = 0x27; // OUTZ_H_G
75+
76+
// Accelerometer X-axis Output Data Registers
77+
static constexpr uint8_t ACCEL_X_LEAST_SIG_REG = 0x28; // OUTX_L_XL
78+
static constexpr uint8_t ACCEL_X_MOST_SIG_REG = 0x29; // OUTX_H_XL
79+
80+
// Accelerometer Y-axis Output Data Registers
81+
static constexpr uint8_t ACCEL_Y_LEAST_SIG_REG = 0x2A; // OUTY_L_XL
82+
static constexpr uint8_t ACCEL_Y_MOST_SIG_REG = 0x2B; // OUTY_H_XL
8483
}

‎src/software/embedded/thunderloop.cpp‎

Lines changed: 12 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -9,8 +9,8 @@
99
#include "proto/tbots_software_msgs.pb.h"
1010
#include "shared/constants.h"
1111
#include "software/embedded/primitive_executor.h"
12-
#include "software/embedded/services/motor.h"
1312
#include "software/embedded/services/imu.h"
13+
#include "software/embedded/services/motor.h"
1414
#include "software/logger/logger.h"
1515
#include "software/logger/network_logger.h"
1616
#include "software/networking/tbots_network_exception.h"
@@ -120,13 +120,14 @@ Thunderloop::Thunderloop(const robot_constants::RobotConstants& robot_constants,
120120
<< "THUNDERLOOP: Power Service initialized! Next initializing Motor Service";
121121

122122
motor_service_ = std::make_unique<MotorService>(robot_constants, loop_hz);
123-
g_motor_service = motor_service_.get();
124-
motor_service_->setup();
123+
g_motor_service = motor_service_.get();
124+
motor_service_->setup();
125125

126126
mLOG(INFO) << "THUNDERLOOP: Motor Service initialized! Next initializing IMU Service";
127127

128128
imu_service_ = std::make_unique<ImuService>();
129-
LOG(INFO) << "THUNDERLOOP: IMU Service initialized!";otor_service_->setup();
129+
LOG(INFO) << "THUNDERLOOP: IMU Service initialized!";
130+
otor_service_->setup();
130131

131132
LOG(INFO) << "THUNDERLOOP: finished initialization with ROBOT ID: " << robot_id_
132133
<< ", CHANNEL ID: " << channel_id_
@@ -182,14 +183,14 @@ void Thunderloop::runLoop()
182183
robot_status_.set_thunderloop_version(thunderloop_hash);
183184
robot_status_.set_thunderloop_date_flashed(thunderloop_date_flashed);
184185

185-
186-
std::optional<Angle> imu_poll = imu_service_->pollHeadingVelocity();
187-
188-
//TODO: Replace with actual IMU data usage
189-
if (imu_poll.has_value()){
190186

191-
LOG(INFO) << "IMU Heading Velocity" << imu_poll.value() ;
192-
}
187+
std::optional<Angle> imu_poll = imu_service_->pollHeadingVelocity();
188+
189+
// TODO: Replace with actual IMU data usage
190+
if (imu_poll.has_value())
191+
{
192+
LOG(INFO) << "IMU Heading Velocity" << imu_poll.value();
193+
}
193194

194195
for (;;)
195196
{

‎src/software/embedded/thunderloop.h‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -10,10 +10,10 @@
1010
#include "shared/constants.h"
1111
#include "shared/robot_constants.h"
1212
#include "software/embedded/primitive_executor.h"
13+
#include "software/embedded/services/imu.h"
1314
#include "software/embedded/services/motor.h"
1415
#include "software/embedded/services/network/network.h"
1516
#include "software/embedded/services/power.h"
16-
#include "software/embedded/services/imu.h"
1717
#include "software/embedded/toml_config/toml_config_client.h"
1818
#include "software/logger/logger.h"
1919

0 commit comments

Comments
 (0)