diff options
Diffstat (limited to 'ftnoir_tracker_freepie-udp')
4 files changed, 290 insertions, 33 deletions
diff --git a/ftnoir_tracker_freepie-udp/freepie-udp-controls.ui b/ftnoir_tracker_freepie-udp/freepie-udp-controls.ui index 48290bf2..74e5a6f1 100644 --- a/ftnoir_tracker_freepie-udp/freepie-udp-controls.ui +++ b/ftnoir_tracker_freepie-udp/freepie-udp-controls.ui @@ -9,48 +9,264 @@ <rect> <x>0</x> <y>0</y> - <width>198</width> - <height>71</height> + <width>227</width> + <height>372</height> </rect> </property> <property name="windowTitle"> - <string>UDP tracker settings</string> + <string>Tracker settings</string> </property> <property name="windowIcon"> <iconset> <normaloff>../facetracknoir/images/facetracknoir.png</normaloff>../facetracknoir/images/facetracknoir.png</iconset> </property> - <property name="layoutDirection"> - <enum>Qt::LeftToRight</enum> - </property> - <property name="autoFillBackground"> - <bool>false</bool> - </property> - <layout class="QGridLayout" name="gridLayout"> - <item row="0" column="0"> - <widget class="QLabel" name="label_5"> + <layout class="QVBoxLayout" name="verticalLayout"> + <item> + <widget class="QFrame" name="frame"> <property name="sizePolicy"> - <sizepolicy hsizetype="Maximum" vsizetype="Preferred"> + <sizepolicy hsizetype="Preferred" vsizetype="Maximum"> <horstretch>0</horstretch> <verstretch>0</verstretch> </sizepolicy> </property> - <property name="text"> - <string>UDP port</string> + <property name="frameShape"> + <enum>QFrame::NoFrame</enum> + </property> + <property name="frameShadow"> + <enum>QFrame::Raised</enum> </property> + <layout class="QHBoxLayout" name="horizontalLayout"> + <item> + <widget class="QLabel" name="label_5"> + <property name="sizePolicy"> + <sizepolicy hsizetype="Maximum" vsizetype="Maximum"> + <horstretch>0</horstretch> + <verstretch>0</verstretch> + </sizepolicy> + </property> + <property name="text"> + <string>UDP port</string> + </property> + </widget> + </item> + <item> + <widget class="QSpinBox" name="port"> + <property name="minimum"> + <number>0</number> + </property> + <property name="maximum"> + <number>65535</number> + </property> + </widget> + </item> + </layout> </widget> </item> - <item row="0" column="1"> - <widget class="QSpinBox" name="port"> - <property name="minimum"> - <number>0</number> + <item> + <widget class="QGroupBox" name="groupBox"> + <property name="title"> + <string>Axis order</string> </property> - <property name="maximum"> - <number>65535</number> + <layout class="QGridLayout" name="gridLayout"> + <item row="0" column="0" rowspan="2"> + <widget class="QLabel" name="label"> + <property name="text"> + <string>output yaw</string> + </property> + </widget> + </item> + <item row="0" column="1"> + <widget class="QComboBox" name="input_x"> + <item> + <property name="text"> + <string>input yaw</string> + </property> + </item> + <item> + <property name="text"> + <string>input pitch</string> + </property> + </item> + <item> + <property name="text"> + <string>input roll</string> + </property> + </item> + </widget> + </item> + <item row="1" column="1" rowspan="2"> + <widget class="QComboBox" name="input_y"> + <item> + <property name="text"> + <string>input yaw</string> + </property> + </item> + <item> + <property name="text"> + <string>input pitch</string> + </property> + </item> + <item> + <property name="text"> + <string>input roll</string> + </property> + </item> + </widget> + </item> + <item row="2" column="0" rowspan="2"> + <widget class="QLabel" name="label_2"> + <property name="text"> + <string>output pitch</string> + </property> + </widget> + </item> + <item row="3" column="1" rowspan="2"> + <widget class="QComboBox" name="input_z"> + <item> + <property name="text"> + <string>input yaw</string> + </property> + </item> + <item> + <property name="text"> + <string>input pitch</string> + </property> + </item> + <item> + <property name="text"> + <string>input roll</string> + </property> + </item> + </widget> + </item> + <item row="4" column="0"> + <widget class="QLabel" name="label_3"> + <property name="text"> + <string>output roll</string> + </property> + </widget> + </item> + </layout> + </widget> + </item> + <item> + <widget class="QGroupBox" name="groupBox_2"> + <property name="title"> + <string>Add to axis</string> </property> + <layout class="QGridLayout" name="gridLayout_2"> + <item row="0" column="0"> + <widget class="QLabel" name="label_4"> + <property name="text"> + <string>yaw</string> + </property> + </widget> + </item> + <item row="0" column="1"> + <widget class="QComboBox" name="add_yaw"> + <item> + <property name="text"> + <string>0</string> + </property> + </item> + <item> + <property name="text"> + <string>+90</string> + </property> + </item> + <item> + <property name="text"> + <string>-90</string> + </property> + </item> + <item> + <property name="text"> + <string>+180</string> + </property> + </item> + <item> + <property name="text"> + <string>-180</string> + </property> + </item> + </widget> + </item> + <item row="1" column="0"> + <widget class="QLabel" name="label_6"> + <property name="text"> + <string>pitch</string> + </property> + </widget> + </item> + <item row="2" column="0"> + <widget class="QLabel" name="label_9"> + <property name="text"> + <string>roll</string> + </property> + </widget> + </item> + <item row="1" column="1"> + <widget class="QComboBox" name="add_pitch"> + <item> + <property name="text"> + <string>0</string> + </property> + </item> + <item> + <property name="text"> + <string>+90</string> + </property> + </item> + <item> + <property name="text"> + <string>-90</string> + </property> + </item> + <item> + <property name="text"> + <string>+180</string> + </property> + </item> + <item> + <property name="text"> + <string>-180</string> + </property> + </item> + </widget> + </item> + <item row="2" column="1"> + <widget class="QComboBox" name="add_roll"> + <item> + <property name="text"> + <string>0</string> + </property> + </item> + <item> + <property name="text"> + <string>+90</string> + </property> + </item> + <item> + <property name="text"> + <string>-90</string> + </property> + </item> + <item> + <property name="text"> + <string>+180</string> + </property> + </item> + <item> + <property name="text"> + <string>-180</string> + </property> + </item> + </widget> + </item> + </layout> </widget> </item> - <item row="1" column="0" colspan="2"> + <item> <widget class="QDialogButtonBox" name="buttonBox"> <property name="standardButtons"> <set>QDialogButtonBox::Cancel|QDialogButtonBox::Ok</set> diff --git a/ftnoir_tracker_freepie-udp/ftnoir_tracker_freepie-udp.cpp b/ftnoir_tracker_freepie-udp/ftnoir_tracker_freepie-udp.cpp index eff1103b..540ef616 100644 --- a/ftnoir_tracker_freepie-udp/ftnoir_tracker_freepie-udp.cpp +++ b/ftnoir_tracker_freepie-udp/ftnoir_tracker_freepie-udp.cpp @@ -14,12 +14,21 @@ TrackerImpl::~TrackerImpl() wait(); } +template<typename t> +static const t bound(t datum, t least, t max) +{ + if (datum < least) + return least; + if (datum > max) + return max; + return datum; +} + void TrackerImpl::run() { #pragma pack(push, 1) struct { uint8_t pad1; uint8_t flags; - uint8_t pad2; float fl[12]; } data; #pragma pack(pop) @@ -28,24 +37,29 @@ void TrackerImpl::run() { flag_Orient = 1 << 1, Mask = flag_Raw | flag_Orient }; + + (void) sock.bind(QHostAddress::Any, (int) s.port, QUdpSocket::ShareAddress | QUdpSocket::ReuseAddressHint); - while (1) { - if (should_quit) - break; - + while (!should_quit) { + int order[] = { + bound<int>(s.idx_x, 0, 2), + bound<int>(s.idx_y, 0, 2), + bound<int>(s.idx_z, 0, 2) + }; float orient[3]; bool filled = false; while (sock.hasPendingDatagrams()) { using t = decltype(data); - t tmp {0,0,0, {0,0,0, 0,0,0}}; + t tmp {0,0, {0,0,0, 0,0,0, 0,0,0, 0,0,0}}; (void) sock.readDatagram(reinterpret_cast<char*>(&tmp), sizeof(data)); int flags = tmp.flags & F::Mask; switch (flags) { + //default: case flag_Raw: continue; case flag_Raw | flag_Orient: @@ -64,10 +78,24 @@ void TrackerImpl::run() { if (filled) { + static const int add_cbx[] = { + 0, + 90, + -90, + 180, + -180, + }; + int indices[] = { s.add_yaw, s.add_pitch, s.add_roll }; QMutexLocker foo(&mtx); - static constexpr double d2r = 57.295781; + static constexpr double r2d = 57.295781; for (int i = 0; i < 3; i++) - pose[Yaw + i] = d2r * orient[i]; + { + int val = 0; + int idx = indices[order[i]]; + if (idx >= 0 && idx < (int)(sizeof(add_cbx) / sizeof(*add_cbx))) + val = add_cbx[idx]; + pose[Yaw + i] = r2d * orient[order[i]] + val; + } } usleep(4000); } @@ -75,7 +103,6 @@ void TrackerImpl::run() { void TrackerImpl::start_tracker(QFrame*) { - (void) sock.bind(QHostAddress::Any, (int) s.port, QUdpSocket::ShareAddress | QUdpSocket::ReuseAddressHint); start(); } diff --git a/ftnoir_tracker_freepie-udp/ftnoir_tracker_freepie-udp.h b/ftnoir_tracker_freepie-udp/ftnoir_tracker_freepie-udp.h index 0cec952f..0567beae 100644 --- a/ftnoir_tracker_freepie-udp/ftnoir_tracker_freepie-udp.h +++ b/ftnoir_tracker_freepie-udp/ftnoir_tracker_freepie-udp.h @@ -15,10 +15,17 @@ using namespace options; struct settings { pbundle b; - value<int> port; + value<int> port, idx_x, idx_y, idx_z; + value<int> add_yaw, add_pitch, add_roll; settings() : b(bundle("freepie-udp-tracker")), - port(b, "port", 4237) + port(b, "port", 5555), + idx_x(b, "axis-index-x", 0), + idx_y(b, "axis-index-y", 1), + idx_z(b, "axis-index-z", 2), + add_yaw(b, "add-yaw-degrees", 0), + add_pitch(b, "add-pitch-degrees", 0), + add_roll(b, "add-roll-degrees", 0) {} }; diff --git a/ftnoir_tracker_freepie-udp/ftnoir_tracker_freepie-udp_dialog.cpp b/ftnoir_tracker_freepie-udp/ftnoir_tracker_freepie-udp_dialog.cpp index ec95da90..1f17355f 100644 --- a/ftnoir_tracker_freepie-udp/ftnoir_tracker_freepie-udp_dialog.cpp +++ b/ftnoir_tracker_freepie-udp/ftnoir_tracker_freepie-udp_dialog.cpp @@ -9,6 +9,13 @@ TrackerDialog::TrackerDialog() connect(ui.buttonBox, SIGNAL(rejected()), this, SLOT(doCancel())); tie_setting(s.port, ui.port); + tie_setting(s.idx_x, ui.input_x); + tie_setting(s.idx_y, ui.input_y); + tie_setting(s.idx_z, ui.input_z); + + tie_setting(s.add_yaw, ui.add_yaw); + tie_setting(s.add_pitch, ui.add_pitch); + tie_setting(s.add_roll, ui.add_roll); } void TrackerDialog::doOK() { |