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;
55 void launch(
unsigned long pollDelayUSec);
59 return channelBNO080->lastRotationQuaternion;
61 std::vector<TimestampedQuaternion> getRotationQuaternionSeries(
int fromSec,
int fromUSec,
int untilSec,
int untilUSec) {
62 return channelBNO080->ringbufRotationQuaternion.popBetweenTimes(fromSec, fromUSec, untilSec, untilUSec);
66 return channelBNO080->lastXYZ[idx - 1];
68 std::vector<TimestampedVector> getSensorVectorSeries(
int idx,
int fromSec,
int fromUSec,
int untilSec,
int untilUSec) {
69 return channelBNO080->ringbufXYZ[idx - 1].popBetweenTimes(fromSec, fromUSec, untilSec, untilUSec);
73 return channelBNO080->lastScalar[idx - 0x0a];
75 std::vector<TimestampedScalar> getSensorScalarSeries(
int idx,
int fromSec,
int fromUSec,
int untilSec,
int untilUSec) {
76 return channelBNO080->ringbufScalar[idx - 0x0a].popBetweenTimes(fromSec, fromUSec, untilSec, untilUSec);
80 class DataChannelService::Pimpl {
82 std::shared_ptr<DataChannelServiceImpl> impl;
84 impl = std::make_shared<DataChannelServiceImpl>(deviceInfo);
88 void DataChannelServiceImpl::receiverRoutine() {
90 while (threadRunning) {
92 std::this_thread::sleep_for(std::chrono::microseconds(pollDelay));
96 void DataChannelServiceImpl::launch(
unsigned long pollDelayUSec) {
98 channelBNO080 = std::make_shared<ClientSideDataChannelIMUBNO080>();
99 registerChannel(channelBNO080);
101 pollDelay = pollDelayUSec;
102 receiverThread = std::make_shared<std::thread>(std::bind(&DataChannelServiceImpl::receiverRoutine,
this));
103 receiverThread->detach();
109 void DataChannelServiceImpl::initiateHandshake() {
110 uint16_t cmd = htons((uint16_t) DataChannelControlCommands::CTLRequestAdvertisement);
111 sendDataIsolatedPacket((DataChannel::ID) 0x00, DataChannel::Types::CONTROL, (
unsigned char*) &cmd,
sizeof(cmd), &serverAddr);
114 void DataChannelServiceImpl::subscribeAll() {
115 unsigned char data[1024];
116 int len = DataChannelControlUtil::packSubscriptionMessage(data, 1024, DataChannelControlCommands::CTLRequestSubscriptions, {0});
117 sendDataIsolatedPacket((DataChannel::ID) 0x00, DataChannel::Types::CONTROL, data, len, &serverAddr);
120 void DataChannelServiceImpl::unsubscribeAll() {
121 unsigned char data[1024];
122 int len = DataChannelControlUtil::packSubscriptionMessage(data, 1024, DataChannelControlCommands::CTLRequestUnsubscriptions, {0});
123 sendDataIsolatedPacket((DataChannel::ID) 0x00, DataChannel::Types::CONTROL, data, len, &serverAddr);
126 int DataChannelServiceImpl::handleChannel0Message(
DataChannelMessage& message, sockaddr_in* sender) {
127 auto cmd = DataChannelControlUtil::getCommand(message.payload, message.header.payloadSize);
129 case DataChannelControlCommands::CTLProvideAdvertisement: {
131 channelsAvailable = DataChannelControlUtil::unpackAdvertisementMessage(message.payload, message.header.payloadSize);
132 for (
auto& dci: channelsAvailable) {
133 channelsAvailableByType[dci.getChannelType()].insert(dci.getChannelID());
139 case DataChannelControlCommands::CTLProvideSubscriptions: {
149 DataChannelServiceImpl::DataChannelServiceImpl(
DeviceInfo deviceInfo)
151 serverAddr.sin_family = AF_INET;
152 serverAddr.sin_port = htons(InternalInformation::DATACHANNELSERVICE_PORT);
153 auto result = inet_addr(deviceInfo.
getIpAddress().c_str());
154 if (result == INADDR_NONE) {
155 throw std::runtime_error(
"Failed to set address for DataChannelService");
157 serverAddr.sin_addr.s_addr = result;
166 pimpl =
new DataChannelService::Pimpl(deviceInfo);
167 pimpl->impl->launch(pollDelayUSec);
170 DataChannelService::~DataChannelService() {
171 pimpl->impl->threadRunning =
false;
176 return pimpl->impl->channelsAvailableByType.count(DataChannel::Types::BNO080);
184 return pimpl->impl->getLastRotationQuaternion();
187 return pimpl->impl->getRotationQuaternionSeries(fromSec, fromUSec, untilSec, untilUSec);
191 return pimpl->impl->getLastSensorVector(SH2Constants::SENSOR_ACCELEROMETER);
194 return pimpl->impl->getSensorVectorSeries(SH2Constants::SENSOR_ACCELEROMETER, fromSec, fromUSec, untilSec, untilUSec);
198 return pimpl->impl->getLastSensorVector(SH2Constants::SENSOR_GYROSCOPE);
201 return pimpl->impl->getSensorVectorSeries(SH2Constants::SENSOR_GYROSCOPE, fromSec, fromUSec, untilSec, untilUSec);
205 return pimpl->impl->getLastSensorVector(SH2Constants::SENSOR_MAGNETOMETER);
208 return pimpl->impl->getSensorVectorSeries(SH2Constants::SENSOR_MAGNETOMETER, fromSec, fromUSec, untilSec, untilUSec);
212 return pimpl->impl->getLastSensorVector(SH2Constants::SENSOR_LINEAR_ACCELERATION);
215 return pimpl->impl->getSensorVectorSeries(SH2Constants::SENSOR_LINEAR_ACCELERATION, fromSec, fromUSec, untilSec, untilUSec);
219 return pimpl->impl->getLastSensorVector(SH2Constants::SENSOR_GRAVITY);
222 return pimpl->impl->getSensorVectorSeries(SH2Constants::SENSOR_GRAVITY, fromSec, fromUSec, untilSec, untilUSec);
Encapsulate a 4D (quaternion) sensor report, containing i, j, k, r, 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.