Merge pull request #83 from pkendall64/head-tracker

Wireless Head Tracking

lib/MSP/msptypes.h

@@ -27,6 +27,8 @@
 
 // ELRS backpack protocol opcodes
 // See: https://docs.google.com/document/d/1u3c7OTiO4sFL2snI-hIo-uRSLfgBK4h16UrbA08Pd6U/edit#heading=h.1xw7en7jmvsj
+
+// outgoing, packets originating from the backpack or forwarded from the TX backpack to the VRx
 #define MSP_ELRS_BACKPACK_GET_CHANNEL_INDEX     0x0300
 #define MSP_ELRS_BACKPACK_SET_CHANNEL_INDEX     0x0301
 #define MSP_ELRS_BACKPACK_GET_FREQUENCY         0x0302
@@ -40,6 +42,10 @@
 #define MSP_ELRS_BACKPACK_GET_FIRMWARE          0x030A
 #define MSP_ELRS_BACKPACK_SET_BUZZER            0x030B
 #define MSP_ELRS_BACKPACK_SET_OSD_ELEMENT       0x030C
-#define MSP_ELRS_BACKPACK_SET_MODE              0x0380
-#define MSP_ELRS_BACKPACK_GET_VERSION           0x0381
-#define MSP_ELRS_BACKPACK_GET_STATUS            0x0382
+#define MSP_ELRS_BACKPACK_SET_HEAD_TRACKING     0x030D  // enable/disable head-tracking forwarding packets to the TX
+
+// incoming, packets originating from the VRx
+#define MSP_ELRS_BACKPACK_SET_MODE              0x0380  // enable wifi/binding mode
+#define MSP_ELRS_BACKPACK_GET_VERSION           0x0381  // get the bacpack firmware version
+#define MSP_ELRS_BACKPACK_GET_STATUS            0x0382  // get the status of the backpack
+#define MSP_ELRS_BACKPACK_SET_PTR               0x0383  // forwarded back to TX backpack

src/Tx_main.cpp

@@ -54,6 +54,7 @@ MSP msp;
 ELRS_EEPROM eeprom;
 TxBackpackConfig config;
 mspPacket_t cachedVTXPacket;
+mspPacket_t cachedHTPacket;
 
 /////////// FUNCTION DEFS ///////////
 
@@ -87,6 +88,11 @@ void ProcessMSPPacketFromPeer(mspPacket_t *packet)
       sendCached = true;
     }
   }
+  else if (packet->function == MSP_ELRS_BACKPACK_SET_PTR)
+  {
+    DBGLN("MSP_ELRS_BACKPACK_SET_PTR...");
+    msp.sendPacket(packet, &Serial);
+  }
 }
 
 // espnow on-receive callback
@@ -171,6 +177,12 @@ void ProcessMSPPacketFromTX(mspPacket_t *packet)
     DBGLN("Processing MSP_ELRS_GET_BACKPACK_VERSION...");
     SendVersionResponse();
     break;
+  case MSP_ELRS_BACKPACK_SET_HEAD_TRACKING:
+    DBGLN("Processing MSP_ELRS_BACKPACK_SET_HEAD_TRACKING...");
+    cachedHTPacket = *packet;
+    cacheFull = true;
+    sendMSPViaEspnow(packet);
+    break;
   default:
     // transparently forward MSP packets via espnow to any subscribers
     sendMSPViaEspnow(packet);
@@ -211,7 +223,14 @@ void SendCachedMSP()
     return;
   }
 
-  sendMSPViaEspnow(&cachedVTXPacket);
+  if (cachedVTXPacket.type != MSP_PACKET_UNKNOWN)
+  {
+    sendMSPViaEspnow(&cachedVTXPacket);
+  }
+  if (cachedHTPacket.type != MSP_PACKET_UNKNOWN)
+  {
+    sendMSPViaEspnow(&cachedHTPacket);
+  }
 }
 
 void SetSoftMACAddress()

src/Vrx_main.cpp

@@ -67,8 +67,10 @@ unsigned long bindingStart = 0;
 unsigned long rebootTime = 0;
 
 uint8_t cachedIndex = 0;
-bool sendChangesToVrx = false;
+bool sendChannelChangesToVrx = false;
+bool sendHeadTrackingChangesToVrx = false;
 bool gotInitialPacket = false;
+bool headTrackingEnabled = false;
 uint32_t lastSentRequest = 0;
 
 device_t *ui_devices[] = {
@@ -201,7 +203,7 @@ void ProcessMSPPacket(mspPacket_t *packet)
     {
       // cache changes here, to be handled outside this callback, in the main loop
       cachedIndex = packet->payload[0];;
-      sendChangesToVrx = true;
+      sendChannelChangesToVrx = true;
     }
     else
     {
@@ -225,6 +227,11 @@ void ProcessMSPPacket(mspPacket_t *packet)
   case MSP_ELRS_SET_OSD:
     vrxModule.SetOSD(packet);
     break;
+  case MSP_ELRS_BACKPACK_SET_HEAD_TRACKING:
+    DBGLN("Processing MSP_ELRS_BACKPACK_SET_HEAD_TRACKING...");
+    headTrackingEnabled = packet->readByte();
+    sendHeadTrackingChangesToVrx = true;
+    break;
   default:
     DBGLN("Unknown command from ESPNOW");
     break;
@@ -451,11 +458,16 @@ void loop()
 #endif
   }
 
-  if (sendChangesToVrx)
+  if (sendChannelChangesToVrx)
   {
-    sendChangesToVrx = false;
+    sendChannelChangesToVrx = false;
     vrxModule.SendIndexCmd(cachedIndex);
   }
+  if (sendHeadTrackingChangesToVrx)
+  {
+    sendHeadTrackingChangesToVrx = false;
+    vrxModule.SendHeadTrackingEnableCmd(headTrackingEnabled);
+  }
 
   // spam out a bunch of requests for the desired band/channel for the first 5s
   if (!gotInitialPacket && now - VRX_BOOT_DELAY < 5000 && now - lastSentRequest > 1000 && connectionState != binding)

src/hdzero.cpp

@@ -95,3 +95,16 @@ HDZero::SetRecordingState(uint8_t recordingState, uint16_t delay)
 
     msp.sendPacket(&packet, m_port);
 }
+
+void
+HDZero::SendHeadTrackingEnableCmd(bool enable)
+{
+    MSP msp;
+    mspPacket_t packet;
+    packet.reset();
+    packet.makeCommand();
+    packet.function = MSP_ELRS_BACKPACK_SET_HEAD_TRACKING;
+    packet.addByte(enable);
+
+    msp.sendPacket(&packet, m_port);
+}

src/hdzero.h

@@ -23,4 +23,5 @@ class HDZero : public MSPModuleBase
     void SetChannelIndex(uint8_t index);
     uint8_t GetRecordingState();
     void SetRecordingState(uint8_t recordingState, uint16_t delay);
+    void SendHeadTrackingEnableCmd(bool enable);
 };

src/module_base.cpp

@@ -9,6 +9,8 @@ void RebootIntoWifi();
 bool BindingExpired(uint32_t now);
 extern uint8_t backpackVersion[];
 extern uint8_t broadcastAddress[6];
+extern bool headTrackingEnabled;
+void sendMSPViaEspnow(mspPacket_t *packet);
 
 void
 ModuleBase::Init()
@@ -30,6 +32,11 @@ ModuleBase::SetOSD(mspPacket_t *packet)
 {
 }
 
+void
+ModuleBase::SendHeadTrackingEnableCmd(bool enable)
+{
+}
+
 void
 ModuleBase::Loop(uint32_t now)
 {
@@ -96,6 +103,10 @@ MSPModuleBase::Loop(uint32_t now)
                 memcpy(&response[1], broadcastAddress, 6);
                 sendResponse(MSP_ELRS_BACKPACK_GET_STATUS, response, sizeof(response));
             }
+            else if (packet->function == MSP_ELRS_BACKPACK_SET_PTR && headTrackingEnabled)
+            {
+                  sendMSPViaEspnow(packet);
+            }
             msp.markPacketReceived();
         }
     }

src/module_base.h

@@ -10,6 +10,7 @@ class ModuleBase
     void SendIndexCmd(uint8_t index);
     void SetRecordingState(uint8_t recordingState, uint16_t delay);
     void SetOSD(mspPacket_t *packet);
+    void SendHeadTrackingEnableCmd(bool enable);
     void Loop(uint32_t now);
 };
 

src/skyzone_msp.cpp

@@ -118,6 +118,19 @@ SkyzoneMSP::SetOSD(mspPacket_t *packet)
     msp.sendPacket(packet, m_port);
 }
 
+void
+SkyzoneMSP::SendHeadTrackingEnableCmd(bool enable)
+{
+    MSP msp;
+    mspPacket_t packet;
+    packet.reset();
+    packet.makeCommand();
+    packet.function = MSP_ELRS_BACKPACK_SET_HEAD_TRACKING;
+    packet.addByte(enable);
+
+    msp.sendPacket(&packet, m_port);
+}
+
 void
 SkyzoneMSP::Loop(uint32_t now)
 {

src/skyzone_msp.h

@@ -26,6 +26,7 @@ class SkyzoneMSP : public MSPModuleBase
     uint8_t GetRecordingState();
     void SetRecordingState(uint8_t recordingState, uint16_t delay);
     void SetOSD(mspPacket_t *packet);
+    void SendHeadTrackingEnableCmd(bool enable);
     void Loop(uint32_t now);
 
 private: