Add DriverStationSim.notifyNewData (#1537)

WM_CREATE is being called at a time where the message pump cannot be called back into.

cscore/src/main/native/cpp/MjpegServerImpl.cpp

@@ -643,8 +643,9 @@ void MjpegServerImpl::ConnThread::SendStream(wpi::raw_socket_ostream& os) {
   Frame::Time lastFrameTime = 0;
   Frame::Time timePerFrame = 0;
   if (m_fps != 0) timePerFrame = 1000000.0 / m_fps;
-  // Allow fudge factor of 1 ms in frame rate
-  if (timePerFrame >= 1000) timePerFrame -= 1000;
+  Frame::Time averageFrameTime = 0;
+  Frame::Time averagePeriod = 1000000;  // 1 second window
+  if (averagePeriod < timePerFrame) averagePeriod = timePerFrame * 10;
 
   StartStream();
   while (m_active && !os.has_error()) {
@@ -665,10 +666,26 @@ void MjpegServerImpl::ConnThread::SendStream(wpi::raw_socket_ostream& os) {
       continue;
     }
 
-    if (frame.GetTime() < (lastFrameTime + timePerFrame)) {
-      // Limit FPS; sleep for 10 ms so we don't consume all processor time
-      std::this_thread::sleep_for(std::chrono::milliseconds(10));
-      continue;
+    auto thisFrameTime = frame.GetTime();
+    if (thisFrameTime != 0 && timePerFrame != 0 && lastFrameTime != 0) {
+      Frame::Time deltaTime = thisFrameTime - lastFrameTime;
+
+      // drop frame if it is early compared to the desired frame rate AND
+      // the current average is higher than the desired average
+      if (deltaTime < timePerFrame && averageFrameTime < timePerFrame) {
+        // sleep for 1 ms so we don't consume all processor time
+        std::this_thread::sleep_for(std::chrono::milliseconds(1));
+        continue;
+      }
+
+      // update average
+      if (averageFrameTime != 0) {
+        averageFrameTime =
+            averageFrameTime * (averagePeriod - timePerFrame) / averagePeriod +
+            deltaTime * timePerFrame / averagePeriod;
+      } else {
+        averageFrameTime = deltaTime;
+      }
     }
 
     int width = m_width != 0 ? m_width : frame.GetOriginalWidth();
@@ -705,7 +722,7 @@ void MjpegServerImpl::ConnThread::SendStream(wpi::raw_socket_ostream& os) {
     // print the individual mimetype and the length
     // sending the content-length fixes random stream disruption observed
     // with firefox
-    lastFrameTime = frame.GetTime();
+    lastFrameTime = thisFrameTime;
     double timestamp = lastFrameTime / 1000000.0;
     header.clear();
     oss << "\r\n--" BOUNDARY "\r\n"

cscore/src/main/native/windows/UsbCameraImpl.cpp

@@ -57,6 +57,7 @@
 static constexpr int NewImageMessage = 0x0400 + 4488;
 static constexpr int SetCameraMessage = 0x0400 + 254;
 static constexpr int WaitForStartupMessage = 0x0400 + 294;
+static constexpr int PumpReadyMessage = 0x0400 + 330;
 
 static constexpr char const* kPropWbValue = "WhiteBalance";
 static constexpr char const* kPropExValue = "Exposure";
@@ -196,6 +197,7 @@ void UsbCameraImpl::Start() {
       [this](HWND hwnd, UINT uiMsg, WPARAM wParam, LPARAM lParam) {
         return this->PumpMain(hwnd, uiMsg, wParam, lParam);
       });
+  m_messagePump->PostWindowMessage(PumpReadyMessage, nullptr, nullptr);
 }
 
 void UsbCameraImpl::PostRequestNewFrame() {
@@ -347,7 +349,7 @@ LRESULT UsbCameraImpl::PumpMain(HWND hwnd, UINT uiMsg, WPARAM wParam,
       }
       m_imageCallback.Reset();
       break;
-    case WM_CREATE:
+    case PumpReadyMessage:
       // Pump Created and ready to go
       DeviceConnect();
       break;

hal/src/main/java/edu/wpi/first/hal/sim/DriverStationSim.java

@@ -75,6 +75,9 @@ public class DriverStationSim {
   public void setDsAttached(boolean dsAttached) {
     DriverStationDataJNI.setDsAttached(dsAttached);
   }
+  public void notifyNewData() {
+    DriverStationDataJNI.notifyNewData();
+  }
 
   public void resetData() {
     DriverStationDataJNI.resetData();

hal/src/main/native/include/simulation/DriverStationSim.h

@@ -103,6 +103,8 @@ class DriverStationSim {
     HALSIM_SetDriverStationDsAttached(dsAttached);
   }
 
+  void NotifyNewData() { HALSIM_NotifyDriverStationNewData(); }
+
   void ResetData() { HALSIM_ResetDriverStationData(); }
 };
 }  // namespace sim

wpilibc/src/main/native/cpp/Watchdog.cpp

@@ -58,10 +58,15 @@ void Watchdog::Thread::Main() {
                         << "s\n";
           }
         }
+
+        // Set expiration flag before calling the callback so any manipulation
+        // of the flag in the callback (e.g., calling Disable()) isn't
+        // clobbered.
+        watchdog->m_isExpired = true;
+
         lock.unlock();
         watchdog->m_callback();
         lock.lock();
-        watchdog->m_isExpired = true;
       }
       // Otherwise, a Watchdog removed itself from the queue (it notifies the
       // scheduler of this) or a spurious wakeup occurred, so just rewait with

wpilibj/src/main/java/edu/wpi/first/wpilibj/Timer.java

@@ -70,7 +70,7 @@ public class Timer {
    */
   public synchronized double get() {
     if (m_running) {
-      return ((getMsClock() - m_startTime) + m_accumulatedTime) / 1000.0;
+      return m_accumulatedTime + (getMsClock() - m_startTime) / 1000.0;
     } else {
       return m_accumulatedTime;
     }

wpilibj/src/main/java/edu/wpi/first/wpilibj/Watchdog.java

@@ -247,10 +247,15 @@ public class Watchdog implements Closeable, Comparable<Watchdog> {
                 System.out.format("Watchdog not fed within %.6fs\n", watchdog.m_timeout / 1.0e6);
               }
             }
+
+            // Set expiration flag before calling the callback so any
+            // manipulation of the flag in the callback (e.g., calling
+            // Disable()) isn't clobbered.
+            watchdog.m_isExpired = true;
+
             m_queueMutex.unlock();
             watchdog.m_callback.run();
             m_queueMutex.lock();
-            watchdog.m_isExpired = true;
           }
           // Otherwise, a Watchdog removed itself from the queue (it notifies
           // the scheduler of this) or a spurious wakeup occurred, so just