imu and current frame streaming

HansonServo.ino

@@ -81,27 +81,27 @@ void runNodeAnimation() {
 
   uint32_t now = millis();
   if (now - lastTickTime < FRAME_INTERVAL_MS) return;
-  lastTickTime = now;
+    lastTickTime = now;
 
   // Tick the node graph
   animState.current->nodeGraph.tick(currentTick, *animState.current);
   auto outputs = animState.current->nodeGraph.getServoOutputs();
 
   // Collect motor commands
-  std::vector<uint8_t> motorIDs;
-  std::vector<uint16_t> positions;
-  std::vector<uint16_t> speeds;
+    std::vector<uint8_t> motorIDs;
+    std::vector<uint16_t> positions;
+    std::vector<uint16_t> speeds;
 
-  for (const auto& [motorID, value] : outputs) {
-    if (value != 65535) {
-      motorIDs.push_back(motorID);
-      positions.push_back(value);
-      speeds.push_back(0);
-      config.setMotorPosition(motorID, value);
+    for (const auto& [motorID, value] : outputs) {
+      if (value != 65535) {
+        motorIDs.push_back(motorID);
+        positions.push_back(value);
+        speeds.push_back(0);
+        config.setMotorPosition(motorID, value);
       config.setMotorEnabled(motorID, true);
-    } else {
+      } else {
       // Only disable torque for motors that should be limp
-      if (config.setMotorEnabled(motorID, false)) {
+        if (config.setMotorEnabled(motorID, false)) {
         servoManager[0]->disableTorque(motorID);
       }
     }
@@ -119,27 +119,51 @@ void runNodeAnimation() {
     );
   }
 
-  currentTick++;
+  // Emit per-frame event: [frameLo, frameHi, playMode, status=0]
+  {
+    uint8_t payload[4];
+    payload[0] = currentTick & 0xFF;
+    payload[1] = (currentTick >> 8) & 0xFF;
+    payload[2] = static_cast<uint8_t>(animState.playMode);
+    payload[3] = 0;  // in-progress
+    sendPacket(Tag::FRAME, payload, 4);
+  }
+
+    currentTick++;
 
   // Handle animation end (0 = run indefinitely for variable-only animations)
   if (animState.current->getFrameCount() > 0 && 
       currentTick > animState.current->getFrameCount()) {
     switch (animState.playMode) {
-      case PLAY_ONCE:
+        case PLAY_ONCE:
         animState.stop();
-        break;
-      case PLAY_LOOP:
+        {
+          uint8_t done[4];
+          done[0] = currentTick & 0xFF;
+          done[1] = (currentTick >> 8) & 0xFF;
+          done[2] = static_cast<uint8_t>(animState.playMode);
+          done[3] = 1;  // complete
+          sendPacket(Tag::FRAME, done, 4);
+        }
+          break;
+        case PLAY_LOOP:
         // Continue looping
-        break;
-      case PLAY_REPEAT:
+          break;
+        case PLAY_REPEAT:
         if (--animState.repeatsRemaining == 0) {
           animState.stop();
-        }
-        break;
+          uint8_t done[4];
+          done[0] = currentTick & 0xFF;
+          done[1] = (currentTick >> 8) & 0xFF;
+          done[2] = static_cast<uint8_t>(animState.playMode);
+          done[3] = 1;  // complete
+          sendPacket(Tag::FRAME, done, 4);
+          }
+          break;
       default:
         break;
-    }
-    currentTick = 0;
+      }
+      currentTick = 0;
   }
 }
 

protocol.h

@@ -43,6 +43,7 @@ namespace Tag {
   // Sensors
   constexpr char IMU[4]   = {'I','M','U','0'};  // IMU data (heading, roll, pitch)
   constexpr char RADAR[4] = {'R','D','A','R'};  // Radar targets
+  constexpr char FRAME[4] = {'F','R','M','E'};  // Animation frame events (frame, mode, status)
   
   // System
   constexpr char STATE[4] = {'S','T','A','T'};  // System state/heartbeat

sensors.cpp

@@ -178,8 +178,8 @@ bool IMU::update() {
 }
 
 uint16_t IMU::packPayload(uint8_t* buffer) const {
-  // Format: heading(2) + roll(2) + pitch(2) as int16 * 100
-  int16_t h = (int16_t)(heading * 100.0f);
+  // Format: heading(2, unsigned) + roll(2, signed) + pitch(2, signed), all *100
+  uint16_t h = (uint16_t)(heading * 100.0f);   // 0..36000 fits in uint16
   int16_t r = (int16_t)(roll * 100.0f);
   int16_t p = (int16_t)(pitch * 100.0f);
   

sensors.h

@@ -106,10 +106,10 @@ public:
   bool isRadarStreamEnabled() const { return radarStreamEnabled; }
   
 private:
-  bool imuStreamEnabled = false;
-  bool radarStreamEnabled = false;
-  uint16_t imuInterval = 100;
-  uint16_t radarInterval = 100;
+  bool imuStreamEnabled = true;
+  bool radarStreamEnabled = true;
+  uint16_t imuInterval = 10;
+  uint16_t radarInterval = 10;
   unsigned long lastIMUSend = 0;
   unsigned long lastRadarSend = 0;