7 #include <visiontransfer/internalinformation.h>
8 #include <visiontransfer/networking.h>
9 #include <visiontransfer/datachannelservicebase.h>
10 #include <visiontransfer/datachannel-control.h>
11 #include <visiontransfer/datachannelservice.h>
13 #include <visiontransfer/datachannel-imu-bno080.h>
14 #include <visiontransfer/protocol-sh2-imu-bno080.h>
28 using namespace visiontransfer;
29 using namespace visiontransfer::internal;
31 namespace visiontransfer {
36 sockaddr_in serverAddr;
38 std::shared_ptr<std::thread> receiverThread;
39 unsigned long pollDelay;
41 std::shared_ptr<ClientSideDataChannelIMUBNO080> channelBNO080;
44 void initiateHandshake();
46 void unsubscribeAll();
47 void receiverRoutine();
50 std::vector<DataChannelInfo> channelsAvailable;
51 std::map<DataChannel::Type, std::set<DataChannel::ID>> channelsAvailableByType;
56 void launch(
unsigned long pollDelayUSec);
60 return channelBNO080->lastRotationQuaternion;
62 std::vector<TimestampedQuaternion> getRotationQuaternionSeries(
int fromSec,
int fromUSec,
int untilSec,
int untilUSec) {
63 return channelBNO080->ringbufRotationQuaternion.popBetweenTimes(fromSec, fromUSec, untilSec, untilUSec);
67 return channelBNO080->lastXYZ[idx - 1];
69 std::vector<TimestampedVector> getSensorVectorSeries(
int idx,
int fromSec,
int fromUSec,
int untilSec,
int untilUSec) {
70 return channelBNO080->ringbufXYZ[idx - 1].popBetweenTimes(fromSec, fromUSec, untilSec, untilUSec);
74 return channelBNO080->lastScalar[idx - 0x0a];
76 std::vector<TimestampedScalar> getSensorScalarSeries(
int idx,
int fromSec,
int fromUSec,
int untilSec,
int untilUSec) {
77 return channelBNO080->ringbufScalar[idx - 0x0a].popBetweenTimes(fromSec, fromUSec, untilSec, untilUSec);
83 class DataChannelService::Pimpl {
85 std::shared_ptr<internal::DataChannelServiceImpl> impl;
87 impl = std::make_shared<internal::DataChannelServiceImpl>(deviceInfo);
89 Pimpl(
const char* ipAddress) {
90 impl = std::make_shared<internal::DataChannelServiceImpl>(ipAddress);
94 void internal::DataChannelServiceImpl::receiverRoutine() {
96 while (threadRunning) {
98 std::this_thread::sleep_for(std::chrono::microseconds(pollDelay));
102 void internal::DataChannelServiceImpl::launch(
unsigned long pollDelayUSec) {
104 channelBNO080 = std::make_shared<ClientSideDataChannelIMUBNO080>();
105 registerChannel(channelBNO080);
107 pollDelay = pollDelayUSec;
108 receiverThread = std::make_shared<std::thread>(std::bind(&internal::DataChannelServiceImpl::receiverRoutine,
this));
109 receiverThread->detach();
115 void internal::DataChannelServiceImpl::initiateHandshake() {
116 uint16_t cmd = htons((uint16_t) DataChannelControlCommands::CTLRequestAdvertisement);
117 sendDataIsolatedPacket((DataChannel::ID) 0x00, DataChannel::Types::CONTROL, (
unsigned char*) &cmd,
sizeof(cmd), &serverAddr);
120 void internal::DataChannelServiceImpl::subscribeAll() {
121 unsigned char data[1024];
122 int len = DataChannelControlUtil::packSubscriptionMessage(data, 1024, DataChannelControlCommands::CTLRequestSubscriptions, {0});
123 sendDataIsolatedPacket((DataChannel::ID) 0x00, DataChannel::Types::CONTROL, data, len, &serverAddr);
126 void internal::DataChannelServiceImpl::unsubscribeAll() {
127 unsigned char data[1024];
128 int len = DataChannelControlUtil::packSubscriptionMessage(data, 1024, DataChannelControlCommands::CTLRequestUnsubscriptions, {0});
129 sendDataIsolatedPacket((DataChannel::ID) 0x00, DataChannel::Types::CONTROL, data, len, &serverAddr);
132 int internal::DataChannelServiceImpl::handleChannel0Message(
DataChannelMessage& message, sockaddr_in* sender) {
133 auto cmd = DataChannelControlUtil::getCommand(message.payload, message.header.payloadSize);
135 case DataChannelControlCommands::CTLProvideAdvertisement: {
137 channelsAvailable = DataChannelControlUtil::unpackAdvertisementMessage(message.payload, message.header.payloadSize);
138 for (
auto& dci: channelsAvailable) {
139 channelsAvailableByType[dci.getChannelType()].insert(dci.getChannelID());
145 case DataChannelControlCommands::CTLProvideSubscriptions: {
155 internal::DataChannelServiceImpl::DataChannelServiceImpl(
DeviceInfo deviceInfo)
159 internal::DataChannelServiceImpl::DataChannelServiceImpl(
const char* ipAddress)
161 serverAddr.sin_family = AF_INET;
162 serverAddr.sin_port = htons(InternalInformation::DATACHANNELSERVICE_PORT);
163 auto result = inet_addr(ipAddress);
164 if (result == INADDR_NONE) {
165 throw std::runtime_error(
"Failed to set address for DataChannelService");
167 serverAddr.sin_addr.s_addr = result;
176 pimpl =
new DataChannelService::Pimpl(deviceInfo);
177 pimpl->impl->launch(pollDelayUSec);
181 pimpl =
new DataChannelService::Pimpl(ipAddress);
182 pimpl->impl->launch(pollDelayUSec);
186 DataChannelService::~DataChannelService() {
187 pimpl->impl->threadRunning =
false;
192 return pimpl->impl->channelsAvailableByType.count(DataChannel::Types::BNO080);
200 return pimpl->impl->getLastRotationQuaternion();
203 return pimpl->impl->getRotationQuaternionSeries(fromSec, fromUSec, untilSec, untilUSec);
207 return pimpl->impl->getLastSensorVector(SH2Constants::SENSOR_ACCELEROMETER);
210 return pimpl->impl->getSensorVectorSeries(SH2Constants::SENSOR_ACCELEROMETER, fromSec, fromUSec, untilSec, untilUSec);
214 return pimpl->impl->getLastSensorVector(SH2Constants::SENSOR_GYROSCOPE);
217 return pimpl->impl->getSensorVectorSeries(SH2Constants::SENSOR_GYROSCOPE, fromSec, fromUSec, untilSec, untilUSec);
221 return pimpl->impl->getLastSensorVector(SH2Constants::SENSOR_MAGNETOMETER);
224 return pimpl->impl->getSensorVectorSeries(SH2Constants::SENSOR_MAGNETOMETER, fromSec, fromUSec, untilSec, untilUSec);
228 return pimpl->impl->getLastSensorVector(SH2Constants::SENSOR_LINEAR_ACCELERATION);
231 return pimpl->impl->getSensorVectorSeries(SH2Constants::SENSOR_LINEAR_ACCELERATION, fromSec, fromUSec, untilSec, untilUSec);
235 return pimpl->impl->getLastSensorVector(SH2Constants::SENSOR_GRAVITY);
238 return pimpl->impl->getSensorVectorSeries(SH2Constants::SENSOR_GRAVITY, fromSec, fromUSec, untilSec, untilUSec);