Skip to content

IsoObject using OSEM v3 #35

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

Open
sepast wants to merge 1 commit into
base: master
Choose a base branch
from
Open

IsoObject using OSEM v3 #35

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
2 changes: 1 addition & 1 deletion demoIsoObject.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -104,7 +104,7 @@ class myObject : public ISO22133::TestObject {
ISO22133::TestObject(ip),
dummyMember(0) {
ObjectSettingsType osem;
osem.testMode = TEST_MODE_UNAVAILABLE;
osem.testMode = TEST_MODE_PREPLANNED;
setMonr(1,2,3,0.4,5,6);
setObjectSettings(osem);
}
Expand Down
12 changes: 6 additions & 6 deletions inc/iso22133object.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -94,7 +94,7 @@ class TestObject {
//! Used to start the threads
void startHandleTCP() { tcpReceiveThread = std::thread(&TestObject::receiveTCP, this); }
void startHandleUDP() { udpReceiveThread = std::thread(&TestObject::receiveUDP, this); }
void startHEABCheck() { heabTimeoutThread = std::thread(&TestObject::checkHeabLoop, this); }
void startHEABCheck() { communicationTimeoutThread = std::thread(&TestObject::checkHeabLoop, this); }
void startSendMonr() { monrThread = std::thread(&TestObject::sendMonrLoop, this); }

protected:
Expand Down Expand Up @@ -161,7 +161,7 @@ class TestObject {
std::chrono::milliseconds expectedHeartbeatPeriod = std::chrono::milliseconds(1000 / HEAB_FREQUENCY_HZ);
std::chrono::milliseconds monrPeriod = std::chrono::milliseconds(1000 / MONR_EXPECTED_FREQUENCY_HZ);
std::chrono::milliseconds heartbeatTimeout = 10*expectedHeartbeatPeriod;
std::chrono::milliseconds maxSafeNetworkDelay = std::chrono::milliseconds(200);
std::chrono::milliseconds maxSafeNetworkDelay = std::chrono::milliseconds(10000);

//! Used to get estimated network delay
std::chrono::milliseconds getNetworkDelay();
Expand Down Expand Up @@ -189,9 +189,9 @@ class TestObject {
//! Sends GREM message on control channel
void sendGREM(HeaderType header, GeneralResponseStatus responseCode, bool debug = false);
//! Called if HEAB messages do not arrive on time
void onHeabTimeout();
void onCommunicationTimeout();
//! Function that checks if HEABs arrive on time
void checkHeabTimeout();
void checkCommunicationTimeout();

//! Set estimated network delay from HEAB times
void setNetworkDelay(std::chrono::milliseconds);
Expand All @@ -207,15 +207,15 @@ class TestObject {
expectedMessageCounter = (expectedMessageCounter + 1) % 256;
}

sigslot::signal<>heabTimeout;
sigslot::signal<>communicationTimeout;
std::mutex recvMutex;
std::mutex heabMutex;
std::mutex netwrkDelayMutex;
std::string localIP;
std::thread tcpReceiveThread;
std::thread udpReceiveThread;
std::thread monrThread;
std::thread heabTimeoutThread;
std::thread communicationTimeoutThread;
std::thread delayedStrtThread;
std::string name = "unnamed";
TcpServer ctrlChannel;
Expand Down
2 changes: 1 addition & 1 deletion iso22133
Open in desktop
Submodule iso22133 updated 6 files
+0 −1 include/defines.h
+6 −4 include/osem.h
+17 −36 iso22133.c
+21 −2 iso22133.h
+45 −34 src/osem.c
+43 −11 tests/osem.cpp
21 changes: 12 additions & 9 deletions src/iso22133object.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -48,14 +48,17 @@ void TestObject::initialize() {
SpeedType initSpd;
AccelerationType initAcc;
TestModeType initTm;
EmergencyBehaviorType emergencyBehavior;
ComLostType comLost;
xyzTrajPointResolutionType xyzTrajPointResolution;
localIP = "0.0.0.0";
initPos.isHeadingValid = false;
initPos.isPositionValid = false;
initSpd.isLateralValid = false;
initSpd.isLongitudinalValid = false;
initAcc.isLateralValid = false;
initAcc.isLongitudinalValid = false;
initTm = TEST_MODE_UNAVAILABLE;
initTm = TEST_MODE_PREPLANNED;
this->setPosition(initPos);
this->setSpeed(initSpd);
this->setAcceleration(initAcc);
Expand All @@ -67,7 +70,7 @@ void TestObject::initialize() {
this->ostmSig.connect(&TestObject::onOSTM, this);
this->trajSig.connect(&TestObject::onTRAJ, this);
this->strtSig.connect(&TestObject::onSTRT, this);
this->heabTimeout.connect(&TestObject::onHeabTimeout, this);
this->communicationTimeout.connect(&TestObject::onCommunicationTimeout, this);
}

TestObject::~TestObject() {
Expand All @@ -78,7 +81,7 @@ TestObject::~TestObject() {
}
monrThread.join();
tcpReceiveThread.join();
heabTimeoutThread.join();
communicationTimeoutThread.join();
if (delayedStrtThread.joinable()) {
delayedStrtThread.join();
}
Expand Down Expand Up @@ -241,7 +244,7 @@ void TestObject::receiveUDP() {
}
}

void TestObject::checkHeabTimeout() {
void TestObject::checkCommunicationTimeout() {
using namespace std::chrono;
std::scoped_lock lock(heabMutex);
// Check time difference of received HEAB and last HEAB
Expand All @@ -251,20 +254,20 @@ void TestObject::checkHeabTimeout() {
ss << "Heartbeat timeout: " << duration_cast<milliseconds>(timeSinceHeab).count()
<< " ms since last heartbeat exceeds limit of " << heartbeatTimeout.count() << " ms." << std::endl;
std::cerr << ss.str();
heabTimeout();
communicationTimeout();
}
}

void TestObject::checkHeabLoop() {
while (this->on) {
auto t = std::chrono::steady_clock::now();
checkHeabTimeout();
checkCommunicationTimeout();
// Don't lock the mutex all the time
std::this_thread::sleep_until(t + expectedHeartbeatPeriod);
}
}

void TestObject::onHeabTimeout() {
void TestObject::onCommunicationTimeout() {
disconnect();
this->state->handleEvent(*this, Events::L);
}
Expand Down Expand Up @@ -333,7 +336,7 @@ int TestObject::handleMessage(std::vector<char>& dataBuffer) {
break;
case MESSAGE_ID_OSEM:
ObjectSettingsType OSEMstruct;
bytesHandled = decodeOSEMMessage(&OSEMstruct, dataBuffer.data(), dataBuffer.size(), debug);
bytesHandled = decodeOSEMMessage(&OSEMstruct, dataBuffer.data(), dataBuffer.size(), true);
if (bytesHandled < 0) {
throw std::invalid_argument("Error decoding OSEM");
}
Expand Down Expand Up @@ -361,7 +364,7 @@ int TestObject::handleMessage(std::vector<char>& dataBuffer) {

case MESSAGE_ID_HEAB:
HeabMessageDataType HEABdata;
bytesHandled = decodeHEABMessage(dataBuffer.data(), dataBuffer.size(), currentTime, &HEABdata, debug);
bytesHandled = decodeHEABMessage(dataBuffer.data(), dataBuffer.size(), currentTime, &HEABdata, true);
if (bytesHandled < 0) {
throw std::invalid_argument("Error decoding HEAB");
}
Expand Down
1 change: 1 addition & 0 deletions src/iso22133state.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -69,6 +69,7 @@ void ISO22133::State::handleEvent(TestObject& obj, const ISO22133::Events::Event
std::cout << "Entering state: " << obj.state->getName() << std::endl;
obj.stateChangeSig();
obj.state->onEnter(obj);

}

/**
Expand Down
4 changes: 2 additions & 2 deletions tests/isoObject.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -105,8 +105,8 @@ class ControlCenterEmulator

TimeSetToCurrentSystemTime(&objSettings.currentTime);

objSettings.heabTimeout.tv_usec = 20000;
objSettings.heabTimeout.tv_sec = 0;
objSettings.communicationTimeout.tv_usec = 20000;
objSettings.communicationTimeout.tv_sec = 0;

objSettings.rate.heab = 10;
objSettings.rate.monr = 100;
Expand Down