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> 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)
156 : DataChannelServiceImpl::DataChannelServiceImpl(deviceInfo.
getIpAddress().c_str())
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);
Encapsulate a 4D (quaternion) sensor report, containing X, Y, Z, W, as well as timestamp and status f...
Encapsulate a scalar sensor measurement, containing the value, as well as timestamp and status fields...
std::vector< TimestampedQuaternion > imuGetRotationQuaternionSeries(int fromSec=0, int fromUSec=0, int untilSec=0x7FFFffffl, int untilUSec=0x7FFFffffl)
Return the current contents of the rotation quaternion data buffer, optionally between specified time...
Encapsulate a 3D sensor report, containing X, Y, Z, as well as timestamp and status fields...
std::string getIpAddress() const
Gets the IP address of the device.
TimestampedVector imuGetAcceleration()
Return the most recent calibrated accelerometer reading.
std::vector< TimestampedVector > imuGetAccelerationSeries(int fromSec=0, int fromUSec=0, int untilSec=0x7FFFffffl, int untilUSec=0x7FFFffffl)
Return the current contents of the calibrated accelerometer data buffer, optionally between specified...
std::vector< TimestampedVector > imuGetLinearAccelerationSeries(int fromSec=0, int fromUSec=0, int untilSec=0x7FFFffffl, int untilUSec=0x7FFFffffl)
Return the current contents of the linear acceleration (without gravity) data buffer, optionally between specified timestamps.
DataChannelService(DeviceInfo deviceInfo, unsigned long pollDelayUSec=1000)
TimestampedVector imuGetGyroscope()
Return the most recent calibrated angular accelerations from the gyroscope.
std::vector< TimestampedVector > imuGetMagnetometerSeries(int fromSec=0, int fromUSec=0, int untilSec=0x7FFFffffl, int untilUSec=0x7FFFffffl)
Return the current contents of the magnetometer data buffer, optionally between specified timestamps...
bool imuAvailable()
Return whether the device will provide data from an Inertial Measurement Unit.
TimestampedQuaternion imuGetRotationQuaternion()
Return the most recent rotation quaternion, relative to gravity and magnetic north.
Base class for the data service (background sending and receiving, dispatching to channels) ...
std::vector< TimestampedVector > imuGetGravitySeries(int fromSec=0, int fromUSec=0, int untilSec=0x7FFFffffl, int untilUSec=0x7FFFffffl)
Return the current contents of the gravity data buffer, optionally between specified timestamps...
Aggregates information about a discovered device.
std::vector< TimestampedVector > imuGetGyroscopeSeries(int fromSec=0, int fromUSec=0, int untilSec=0x7FFFffffl, int untilUSec=0x7FFFffffl)
Return the current contents of the gyroscope data buffer, optionally between specified timestamps...
TimestampedVector imuGetGravity()
Return the most recent gravity measurement.
TimestampedVector imuGetLinearAcceleration()
Return the most recent linear acceleration, i.e. with gravity factored out.
TimestampedVector imuGetMagnetometer()
Return the most recent magnetometer readings.