Skip to content

Commit b17f4ff

Browse files
author
hpcdgrie
committed
Fixed matrix synchronization, removed debugging messages and make sure that only one listening thread is running
1 parent 57d33a5 commit b17f4ff

2 files changed

Lines changed: 40 additions & 76 deletions

File tree

‎src/OpenCOVER/plugins/hlrs/TacxFTMS/TacxFTMS.cpp‎

Lines changed: 36 additions & 71 deletions
Original file line numberDiff line numberDiff line change
@@ -30,9 +30,6 @@ using namespace opencover;
3030

3131
TacxFTMS::TacxFTMS()
3232
: coVRPlugin(COVER_PLUGIN_NAME)
33-
, udpNeo(NULL)
34-
, udpAlpine(NULL)
35-
, udpListen(NULL)
3633
, coVRNavigationProvider("TacxFTMS", this)
3734
{
3835
ftmsData.speed = 0.0;
@@ -55,14 +52,9 @@ TacxFTMS::TacxFTMS()
5552
TacxFTMS::~TacxFTMS() {
5653
coVRNavigationManager::instance()->unregisterNavigationProvider(this);
5754
running = false;
58-
59-
delete udpNeo;
60-
delete udpAlpine;
61-
delete udpListen;
6255
}
6356

6457
bool TacxFTMS::init() {
65-
std::cerr << "Init FTMS started";
6658
float floorHeight = VRSceneGraph::instance()->floorHeight();
6759

6860
float x =
@@ -108,19 +100,18 @@ bool TacxFTMS::init() {
108100
unsigned short listeningPort = configInt("Listening", "Port", 31322)->value();
109101

110102
std::cerr << "TacxFTMS config: UDP: start listening on port: " << listeningPort << std::endl;
111-
udpListen = new UDPComm(listeningPort);
103+
udpListen = std::make_unique<UDPComm>(listeningPort);
112104
if (!udpListen->isBad())
113105
{
114106
std::cerr << "TacxFTMS config: UDP: start listening on port: " << listeningPort << std::endl;
107+
start();
115108

116-
}
117-
109+
}
118110
}
119111
}
120112

121-
std::cerr << "Init FTMS done";
113+
std::cerr << "Init FTMS done" << std::endl;
122114
auto retval = coVRMSController::instance()->syncBool(ret);
123-
std::cerr << "Init FTMS done retval: " << retval << std::endl;
124115
return retval;
125116
}
126117

@@ -129,55 +120,43 @@ void TacxFTMS::addDevice(const opencover::deviceInfo *i)
129120

130121
std::cerr << "TacxFTMS::addDevice called" << std::endl;
131122

132-
if (coVRMSController::instance()->isMaster())
123+
//const std::string host = configString("TacxFTMS", "severHost", "192.168.178.36")->value();
124+
unsigned short serverPortNeo = configInt("TacxFTMS", "serverPort", 31319)->value();
125+
//unsigned short localPortNeo = configInt("TacxFTMS", "localPort", 31322)->value();
126+
127+
unsigned short serverPortAlpine = configInt("Alpine", "serverPort", 31319)->value();
128+
//unsigned short localPortAlpine = configInt("Alpine", "localPort", 31322)->value();
129+
std::string host = "";
130+
std::cerr << "Devicename found" << i->deviceName << std::endl;
131+
if (i->deviceName == "TacxNeo")
133132
{
134-
//const std::string host = configString("TacxFTMS", "severHost", "192.168.178.36")->value();
135-
unsigned short serverPortNeo = configInt("TacxFTMS", "serverPort", 31319)->value();
136-
//unsigned short localPortNeo = configInt("TacxFTMS", "localPort", 31322)->value();
137-
138-
unsigned short serverPortAlpine = configInt("Alpine", "serverPort", 31319)->value();
139-
//unsigned short localPortAlpine = configInt("Alpine", "localPort", 31322)->value();
140-
std::string host = "";
141-
std::cerr << "Devicename found" << i->deviceName << std::endl;
142-
if (i->deviceName == "TacxNeo")
133+
host = i->address;
134+
std::cerr << "TacxFTMS config: UDP: TacxHost: " << host << std::endl;
135+
udpNeo = std::make_unique<UDPComm>(serverPortNeo, host.c_str());
136+
if (udpNeo->isBad())
143137
{
144-
host = i->address;
145-
std::cerr << "TacxFTMS config: UDP: TacxHost: " << host << std::endl;
146-
udpNeo = new UDPComm(serverPortNeo, host.c_str());
147-
if (!udpNeo->isBad())
148-
{
149-
start();
150-
}
151-
else
152-
{
153-
//std::cerr << "TacxFTMS: failed to open local UDP port" << localPortNeo << std::endl;
154-
}
138+
udpNeo.reset();
155139
}
156-
else if (i->deviceName == "Alpine")
140+
}
141+
else if (i->deviceName == "Alpine")
142+
{
143+
host = i->address;
144+
std::cerr << "TacxFTMS config: UDP: AlpineHost: " << host << std::endl;
145+
udpAlpine = std::make_unique<UDPComm>(serverPortAlpine, host.c_str());
146+
if (udpAlpine->isBad())
157147
{
158-
host = i->address;
159-
std::cerr << "TacxFTMS config: UDP: AlpineHost: " << host << std::endl;
160-
udpAlpine = new UDPComm(serverPortAlpine, host.c_str());
161-
if (!udpAlpine->isBad())
162-
{
163-
start();
164-
}
165-
else
166-
{
167-
//std::cerr << "Alpine: failed to open local UDP port" << localPortAlpine << std::endl;
168-
}
148+
udpAlpine.reset();
169149
}
170150
}
151+
if(!running && (udpNeo || udpAlpine))
152+
{
153+
start();
154+
}
171155

172156
}
173157

174-
void TacxFTMS::preFrame() {
175-
std::cerr << "TacxFTMS::preFrame() on" << std::endl;
176-
}
177-
178158
bool TacxFTMS::update() {
179159

180-
std::cerr << "TacxFTMS::update() on" << std::endl;
181160
if (coVRMSController::instance()->syncBool(isEnabled())) {
182161
if (coVRMSController::instance()->isMaster()) {
183162
OpenThreads::ScopedLock<OpenThreads::Mutex> lock(mutex);
@@ -226,18 +205,15 @@ bool TacxFTMS::update() {
226205
auto mat = getMatrix();
227206

228207
TransformMat = mat * relTrans * relRot;
229-
std::cerr << "TacxFTMS::update: master sending matrix" << std::endl;
230-
coVRMSController::instance()->sendSlaves((char*)TransformMat.ptr(),
231-
sizeof(*TransformMat.ptr()));
208+
coVRMSController::instance()->sendSlaves(TransformMat.ptr(),
209+
sizeof(osg::Matrix::value_type) * 16);
232210
} else {
233-
std::cerr << "TacxFTMS::update: slave receiving matrix" << std::endl;
234-
coVRMSController::instance()->readMaster((char*)TransformMat.ptr(),
235-
sizeof(*TransformMat.ptr()));
211+
coVRMSController::instance()->readMaster(TransformMat.ptr(),
212+
sizeof(osg::Matrix::value_type) * 16);
236213
}
237214
VRSceneGraph::instance()->getTransform()->setMatrix(TransformMat);
238215
coVRCollaboration::instance()->SyncXform();
239216
}
240-
std::cerr << "TacxFTMS::update() off" << std::endl;
241217
usleep(5000);
242218
return false;
243219
}
@@ -287,17 +263,15 @@ void TacxFTMS::run() {
287263
this->updateThread();
288264
}
289265
}
266+
running = false;
290267
}
291268

292269
void TacxFTMS::updateThread() {
293-
std::cerr << "TacxFTMS::updateThread() on" << std::endl;
294270
if (udpListen) {
295271
char tmpBuf[10000];
296272
int status;
297-
std::cerr << "TacxFTMS::updateThread: trying to receive data" << std::endl;
298273

299274
status = udpListen->receive(&tmpBuf, 10000);
300-
std::cerr << "TacxFTMS::updateThread: received " << status << " bytes" << std::endl;
301275

302276
if (status == -1) {
303277
if (isEnabled()) // otherwise we are not supposed to receive
@@ -316,16 +290,8 @@ void TacxFTMS::updateThread() {
316290
}
317291
OpenThreads::ScopedLock<OpenThreads::Mutex> lock(mutex);
318292
memcpy(&ftmsData, tmpBuf, sizeof(FTMSBikeData));
319-
std::cerr << "TacxFTMS::update: received speed=" << ftmsData.speed
320-
<< ", cadence=" << ftmsData.cadence
321-
<< ", power=" << ftmsData.power << std::endl;
322293
sendIndoorBikeSimulationParameters();
323-
std::cerr << "TacxFTMS::update: sent control: grade=" << ftmsControl.grade
324-
<< ", crr=" << ftmsControl.crr
325-
<< ", cw=" << ftmsControl.cw
326-
<< ", weight=" << ftmsControl.weight
327-
<< ", wind_speed=" << ftmsControl.wind_speed
328-
<< std::endl;
294+
329295
} else if (status >= sizeof(AlpineData)) {
330296

331297
OpenThreads::ScopedLock<OpenThreads::Mutex> lock(mutex);
@@ -338,7 +304,6 @@ void TacxFTMS::updateThread() {
338304
else {
339305
usleep(5000);
340306
}
341-
std::cerr << "TacxFTMS::updateThread() off" << std::endl;
342307

343308
return;
344309
}

‎src/OpenCOVER/plugins/hlrs/TacxFTMS/TacxFTMS.h‎

Lines changed: 4 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -60,12 +60,11 @@ class PLUGINEXPORT TacxFTMS : public opencover::coVRPlugin, public opencover::co
6060
TacxFTMS();
6161
~TacxFTMS();
6262
bool update() override;
63-
void preFrame() override;
6463
float getAngle() const;
6564
float getBrakeForce() const;
6665
float getAccelleration() const;
6766
float getSpeed() const;
68-
volatile bool running;
67+
std::atomic<bool> running;
6968
void run() override;
7069
bool doStop;
7170

@@ -84,9 +83,9 @@ class PLUGINEXPORT TacxFTMS : public opencover::coVRPlugin, public opencover::co
8483
osg::Matrix TacxFTMSPos;
8584
void setEnabled(bool) override;
8685
void updateThread();
87-
UDPComm* udpNeo;
88-
UDPComm* udpAlpine;
89-
UDPComm* udpListen; // for listening to all devices
86+
std::unique_ptr<UDPComm> udpNeo;
87+
std::unique_ptr<UDPComm> udpAlpine;
88+
std::unique_ptr<UDPComm> udpListen; // for listening to all devices
9089
FTMSBikeData ftmsData;
9190
FTMSControlData ftmsControl;
9291
AlpineData alpineData;

0 commit comments

Comments
 (0)