Skip to content

fix timestamp driver #226

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 1 commit into from
Apr 14, 2025
Merged

fix timestamp driver #226

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions core/gps/nmea.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -55,6 +55,7 @@ std::optional<GPS_DATA_T> Nmea::Parse(const std::string& gps_data) {
data.height = std::stof(res[8]);
return data;
}
// TODO(simba) add support for other MSG types than GNGGA
return std::nullopt;
} catch (const std::exception& e) {
return std::nullopt;
Expand Down
12 changes: 11 additions & 1 deletion core/gps/ut/nmea_benchmark.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -21,7 +21,17 @@ static void ParseBenchmark(benchmark::State& state) { // NOLINT
"$GNGGA,182658.40,5421.08022,N,01835.23691,E,1,08,2.00,120.5,M,32.8,M,,*49",
"$GNGGA,182659.00,5421.08122,N,01835.23711,E,1,07,1.50,119.5,M,31.8,M,,*5A",
"$GNGGA,182700.20,5421.08222,N,01835.23811,E,1,06,1.20,118.5,M,30.8,M,,*4C",
"$GNGGA,182701.80,5421.08322,N,01835.23911,E,1,05,1.10,117.5,M,29.8,M,,*3F"
"$GNGGA,182701.80,5421.08322,N,01835.23911,E,1,05,1.10,117.5,M,29.8,M,,*3F",
"$GNGGA,182702.40,5421.08422,N,01835.24011,E,1,05,1.00,116.5,M,28.8,M,,*3D",
"$GNGGA,182703.00,5421.08522,N,01835.24111,E,1,06,0.90,115.4,M,27.7,M,,*3F",
"$GNGGA,182703.60,5421.08622,N,01835.24211,E,1,06,0.80,114.3,M,26.6,M,,*39",
"$GNGGA,182704.20,5421.08722,N,01835.24311,E,1,07,0.70,113.2,M,25.5,M,,*3E",
"$GNGGA,182704.80,5421.08822,N,01835.24411,E,1,08,0.60,112.1,M,24.4,M,,*3A",
"$GNGGA,182705.40,5421.08922,N,01835.24511,E,1,08,0.50,111.0,M,23.3,M,,*3F",
"$GNGGA,182706.00,5421.09022,N,01835.24611,E,1,09,0.50,110.0,M,22.2,M,,*30",
"$GNGGA,182706.60,5421.09122,N,01835.24711,E,1,09,0.40,109.0,M,21.1,M,,*32",
"$GNGGA,182707.20,5421.09222,N,01835.24811,E,1,10,0.40,108.0,M,20.0,M,,*33",
"$GNGGA,182707.80,5421.09322,N,01835.24911,E,1,10,0.30,107.0,M,19.0,M,,*3D"
};

for (auto _ : state) {
Expand Down
102 changes: 78 additions & 24 deletions core/gps/ut/nmea_tests.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -31,35 +31,89 @@ INSTANTIATE_TEST_SUITE_P(
std::make_tuple(
"182658.40,5421.08022,N,01835.23691,E,1,08,2.00,120.5,M,32.8,M,,*49",
std::vector<std::string>{"182658.40", "5421.08022", "N", "01835.23691", "E",
"1", "08", "2.00", "120.5", "M", "32.8", "M", "", "*49"}
)
"1", "08", "2.00", "120.5", "M", "32.8", "M", "", "*49"})
)
);

TEST(NMEAParser, BasicTest) {
std::string val = "$GNGGA,182658.40,5421.08022,N,01835.23691,E,1,08,2.00,120.5,M,32.8,M,,*49";
auto res_opt = srp::core::Nmea::Parse(val);
struct NMEAParseTestCase {
std::string input;
double expected_timestamp;
double expected_latitude;
char expected_lat_dir;
double expected_longitude;
char expected_long_dir;
int expected_sat_nr;
double expected_HDOP;
double expected_height;
};

class NMEAParserTest : public ::testing::TestWithParam<NMEAParseTestCase> {};

TEST_P(NMEAParserTest, ParsesCorrectly) {
const auto& param = GetParam();
auto res_opt = srp::core::Nmea::Parse(param.input);
ASSERT_TRUE(res_opt.has_value());
auto res = res_opt.value();
EXPECT_EQ(res.timestamp, 182658.40);
EXPECT_EQ(res.latitude, 5421.08022);
EXPECT_EQ(res.latitude_dir, 'N');
EXPECT_EQ(res.longitude, 1835.23691);
EXPECT_EQ(res.longitude_dir, 'E');
EXPECT_EQ(res.satellite_nr, 8);
EXPECT_EQ(res.HDOP, 2.0);
EXPECT_EQ(res.height, 120.5);

EXPECT_DOUBLE_EQ(res.timestamp, param.expected_timestamp);
EXPECT_DOUBLE_EQ(res.latitude, param.expected_latitude);
EXPECT_EQ(res.latitude_dir, param.expected_lat_dir);
EXPECT_DOUBLE_EQ(res.longitude, param.expected_longitude);
EXPECT_EQ(res.longitude_dir, param.expected_long_dir);
EXPECT_EQ(res.satellite_nr, param.expected_sat_nr);
EXPECT_NEAR(res.HDOP, param.expected_HDOP, 0.01);
EXPECT_NEAR(res.height, param.expected_height, 0.01);
}

INSTANTIATE_TEST_SUITE_P(
NMEAParserTests,
NMEAParserTest,
::testing::Values(
NMEAParseTestCase{
"$GNGGA,182658.40,5421.08022,N,01835.23691,E,1,08,2.00,120.5,M,32.8,M,,*49",
182658.40, 5421.08022, 'N', 1835.23691, 'E', 8, 2.0, 120.5
},
NMEAParseTestCase{
"$GNGGA,120000.00,1234.56789,S,09876.54321,W,1,05,1.50,50.0,M,30.0,M,,*55",
120000.00, 1234.56789, 'S', 9876.54321, 'W', 5, 1.5, 50.0
},
NMEAParseTestCase{
"$GNGGA,235959.99,0000.00000,N,00000.00000,E,1,10,0.90,10.0,M,20.0,M,,*6A",
235959.99, 0.0, 'N', 0.0, 'E', 10, 0.9, 10.0
},
NMEAParseTestCase{
"$GNGGA,101010.10,3745.12345,N,12227.12345,W,1,07,1.10,15.3,M,10.0,M,,*5F",
101010.10, 3745.12345, 'N', 12227.12345, 'W', 7, 1.1, 15.3
},
NMEAParseTestCase{
"$GNGGA,060305.00,5150.12345,S,00007.65432,E,1,06,0.80,5.0,M,12.0,M,,*4A",
60305.00, 5150.12345, 'S', 7.65432, 'E', 6, 0.8, 5.0
},
NMEAParseTestCase{
"$GNGGA,130707.77,9000.00000,N,18000.00000,E,1,12,0.50,0.0,M,0.0,M,,*4B",
130707.77, 9000.0, 'N', 18000.0, 'E', 12, 0.5, 0.0
},
NMEAParseTestCase{
"$GNGGA,001122.33,4823.45678,N,01622.98765,E,1,09,0.95,200.0,M,45.0,M,,*61",
1122.33, 4823.45678, 'N', 1622.98765, 'E', 9, 0.95, 200.0
},
NMEAParseTestCase{
"$GNGGA,151515.15,3456.78901,S,12345.67890,W,1,04,1.75,75.5,M,25.0,M,,*58",
151515.15, 3456.78901, 'S', 12345.67890, 'W', 4, 1.75, 75.5
}
)
);

TEST(NMEAParser, ToStringTest) {
srp::core::GPS_DATA_T data;
data.timestamp = 12.2;
data.latitude = 12.2;
data.latitude_dir = 'R';
data.longitude = 11.1;
data.longitude_dir = 'E';
data.satellite_nr = 12;
data.HDOP = 11.2;
data.height = 13.2;
EXPECT_EQ(data.to_string(),
"Timestamp: 12.2, Latitude: 12.2 R, Longitude: 11.1 E, Satellites: 12, HDOP: 11.2, Height: 13.2");
srp::core::GPS_DATA_T data;
data.timestamp = 12.2;
data.latitude = 12.2;
data.latitude_dir = 'R';
data.longitude = 11.1;
data.longitude_dir = 'E';
data.satellite_nr = 12;
data.HDOP = 11.2;
data.height = 13.2;
EXPECT_EQ(data.to_string(),
"Timestamp: 12.2, Latitude: 12.2 R, Longitude: 11.1 E, Satellites: 12, HDOP: 11.2, Height: 13.2");
}
4 changes: 2 additions & 2 deletions core/i2c/i2c_driver.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,7 @@ namespace core {
namespace i2c {

namespace {
const constexpr char *path = "/dev/i2c-2";
const constexpr auto kPath = "/dev/i2c-2";
}

I2CDriver::I2CDriver(): i2c_logger_ {
Expand All @@ -29,7 +29,7 @@ I2CDriver::~I2CDriver() {
close(this->i2cFile);
}
core::ErrorCode I2CDriver::Init() {
if ((this->i2cFile = open(path, O_RDWR)) < 0) {
if ((this->i2cFile = open(kPath, O_RDWR)) < 0) {
return core::ErrorCode::kInitializeError;
}
return core::ErrorCode::kOk;
Expand Down
1 change: 1 addition & 0 deletions core/time/BUILD
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,4 +6,5 @@ cc_library(
deps = [
"//core/common:common_types",
],
deprecation = "you realy need to use this?",
)
15 changes: 11 additions & 4 deletions core/timestamp/timestamp_driver.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -26,13 +26,17 @@ int64_t GetNow() {
.count();
}

void TimestampController::Init() {
bool TimestampController::Init() {
uint8_t i = 0;
do {
std::this_thread::sleep_for(std::chrono::seconds(1));
i += 1;
} while (!proxy_.FindService() && i <= kMax_Wait);
} while (!proxy_.FindService().HasValue() && i <= kMax_Wait);
// error
if (i == kMax_Wait) {
return false;
}
return true;
}
TimestampController::TimestampController(): instance_(kTimestamp_instance), proxy_(this->instance_) {
}
Expand All @@ -51,10 +55,13 @@ std::optional<int64_t> TimestampController::GetNewTimeStamp() {
TimestampMaster::TimestampMaster(): instance_(kTimestamp_instance), skeleton_(instance_) {
}

void TimestampMaster::Init() {
skeleton_.OfferService();
bool TimestampMaster::Init() {
if (!skeleton_.OfferService().HasValue()) {
return false;
}
start = GetNow();
skeleton_.Send(start);
return true;
}

void TimestampMaster::CorrectStartPoint(const int64_t offset) {
Expand Down
4 changes: 2 additions & 2 deletions core/timestamp/timestamp_driver.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -27,7 +27,7 @@ class TimestampController {
public:
std::optional<int64_t> GetNewTimeStamp();
int64_t GetDeltaTime(const int64_t now, const int64_t previous);
void Init();
bool Init();
TimestampController();
};

Expand All @@ -41,7 +41,7 @@ class TimestampMaster {
TimestampMaster();
int64_t GetNewTimeStamp();
void CorrectStartPoint(const int64_t offset);
void Init();
bool Init();
};

} // namespace timestamp
Expand Down
45 changes: 25 additions & 20 deletions core/timestamp/ut/timestamp_driver_test.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,7 +18,7 @@
namespace srp {
namespace core {
namespace timestamp {

constexpr auto KBasicCorrection = 50;
class TimestampMasterTest : public ::testing::Test {
protected:
TimestampMaster master;
Expand All @@ -28,24 +28,28 @@ class TimestampMasterTest : public ::testing::Test {
};

TEST_F(TimestampMasterTest, GetNewTimeStampReturnsElapsedTime) {
std::this_thread::sleep_for(std::chrono::milliseconds(100));
constexpr auto K1 = 100;
constexpr auto K2 = 100;
constexpr auto K3 = 1000;
std::this_thread::sleep_for(std::chrono::milliseconds(K1));
int64_t timestamp = master.GetNewTimeStamp();
std::this_thread::sleep_for(std::chrono::milliseconds(100));
std::this_thread::sleep_for(std::chrono::milliseconds(K2));
int64_t timestamp2 = master.GetNewTimeStamp();
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
std::this_thread::sleep_for(std::chrono::milliseconds(K3));
int64_t timestamp3 = master.GetNewTimeStamp();

EXPECT_NEAR(timestamp, 100, 50);
EXPECT_NEAR(timestamp2, 200, 50);
EXPECT_NEAR(timestamp3, 1200, 150);
EXPECT_NEAR(timestamp, K1, KBasicCorrection);
EXPECT_NEAR(timestamp2, K1 + K2, KBasicCorrection);
EXPECT_NEAR(timestamp3, K1 + K2 + K3, 3 * KBasicCorrection);
}

TEST_F(TimestampMasterTest, CorrectStartPointAdjustsStartTime) {
constexpr auto K1 = 50;
int64_t initial_timestamp = master.GetNewTimeStamp();
master.CorrectStartPoint(50); // Korekcja o 50 ms
master.CorrectStartPoint(K1); // Korekcja o 50 ms
int64_t adjusted_timestamp = master.GetNewTimeStamp();

EXPECT_LT(adjusted_timestamp, initial_timestamp + 51);
EXPECT_NEAR(adjusted_timestamp, initial_timestamp + K1, KBasicCorrection);
}
TEST_F(TimestampMasterTest, InitOffersServiceAndSetsStartTime) {
EXPECT_NO_THROW(master.Init()); // Sprawdzenie, czy Init nie generuje wyjątków
Expand All @@ -58,31 +62,32 @@ TEST(TimestampIntegratedTest, FirstTest) {
TimestampMaster master;
TimestampController slave1;
TimestampController slave2;
master.Init();
std::this_thread::sleep_for(std::chrono::milliseconds(100));
slave1.Init();
slave2.Init();
std::this_thread::sleep_for(std::chrono::milliseconds(100));
EXPECT_TRUE(master.Init());
std::this_thread::sleep_for(std::chrono::milliseconds(10));
EXPECT_TRUE(slave1.Init());
EXPECT_TRUE(slave2.Init());
std::this_thread::sleep_for(std::chrono::milliseconds(10));
auto t1 = slave1.GetNewTimeStamp();
auto t2 = slave2.GetNewTimeStamp();
auto correct_val = master.GetNewTimeStamp();
ASSERT_TRUE(t1.has_value());
ASSERT_TRUE(t2.has_value());
EXPECT_NEAR(master.GetNewTimeStamp(), t1.value(), 70);
EXPECT_NEAR(master.GetNewTimeStamp(), t2.value(), 70);
EXPECT_NEAR(correct_val, t1.value(), 70);
EXPECT_NEAR(correct_val, t2.value(), 70);
}
TEST(TimestampIntegratedTest, SecTest) {
TimestampMaster master;
TimestampController slave1;
master.Init();
slave1.Init();
std::this_thread::sleep_for(std::chrono::milliseconds(100));
EXPECT_TRUE(master.Init());
EXPECT_TRUE(slave1.Init());
std::this_thread::sleep_for(std::chrono::milliseconds(10));
auto t1 = slave1.GetNewTimeStamp();
ASSERT_TRUE(t1.has_value());
EXPECT_NEAR(master.GetNewTimeStamp(), t1.value(), 70);
}
TEST(TimestampTest, Error) {
TimestampController slave;
slave.Init();
EXPECT_TRUE(slave.Init());
auto t1 = slave.GetNewTimeStamp();
ASSERT_FALSE(t1.has_value());
}
Expand Down