From 54fc00495639dd170b8a1026e85e3988c6a2c124 Mon Sep 17 00:00:00 2001
From: Michael Oborne <mich146@hotmail.com>
Date: Tue, 3 Sep 2013 07:31:14 +0800
Subject: [PATCH] SITL: fix sitl timer issues under cygwin

---
 libraries/AP_HAL_AVR_SITL/SITL_State.cpp | 26 ++++++++++++++
 libraries/AP_HAL_AVR_SITL/Scheduler.cpp  | 44 ++++++++++++++++++++++++
 libraries/AP_HAL_AVR_SITL/Scheduler.h    |  5 +++
 3 files changed, 75 insertions(+)

diff --git a/libraries/AP_HAL_AVR_SITL/SITL_State.cpp b/libraries/AP_HAL_AVR_SITL/SITL_State.cpp
index d4ca350e85..7f69822af0 100644
--- a/libraries/AP_HAL_AVR_SITL/SITL_State.cpp
+++ b/libraries/AP_HAL_AVR_SITL/SITL_State.cpp
@@ -21,6 +21,30 @@
 
 #include <AP_Param.h>
 
+#ifdef __CYGWIN__
+#include <stdio.h>
+#include <signal.h>
+#include <stdlib.h>
+#include <sys/wait.h>
+#include <unistd.h>
+
+void print_trace() {
+    char pid_buf[30];
+    sprintf(pid_buf, "%d", getpid());
+    char name_buf[512];
+    name_buf[readlink("/proc/self/exe", name_buf, 511)]=0;
+    int child_pid = fork();
+    if (!child_pid) {           
+        dup2(2,1); // redirect output to stderr
+        fprintf(stdout,"stack trace for %s pid=%s\n",name_buf,pid_buf);
+        execlp("gdb", "gdb", "--batch", "-n", "-ex", "thread", "-ex", "bt", name_buf, pid_buf, NULL);
+        abort(); /* If gdb failed to start */
+    } else {
+        waitpid(child_pid,NULL,0);
+    }
+}
+#endif
+
 extern const AP_HAL::HAL& hal;
 
 using namespace AVR_SITL;
@@ -225,6 +249,7 @@ void SITL_State::_timer_handler(int signum)
     count++;
 	if (hal.scheduler->millis() - last_report > 1000) {
 		fprintf(stdout, "TH %u cps\n", count);
+	//	print_trace();
 		count = 0;
 		last_report = hal.scheduler->millis();
 	}
@@ -244,6 +269,7 @@ void SITL_State::_timer_handler(int signum)
 
 	if (_update_count == 0 && _sitl != NULL) {
 		_update_gps(0, 0, 0, 0, 0, 0, false);
+		_update_barometer(0);
 		_scheduler->timer_event();
         _scheduler->sitl_end_atomic();
 		in_timer = false;
diff --git a/libraries/AP_HAL_AVR_SITL/Scheduler.cpp b/libraries/AP_HAL_AVR_SITL/Scheduler.cpp
index e94975f77f..020391c36e 100644
--- a/libraries/AP_HAL_AVR_SITL/Scheduler.cpp
+++ b/libraries/AP_HAL_AVR_SITL/Scheduler.cpp
@@ -8,6 +8,17 @@
 #include <sys/time.h>
 #include <unistd.h>
 
+#ifdef __CYGWIN__
+#include <stdio.h> 
+#include <stdlib.h> 
+#include <assert.h> 
+#include <windows.h> 
+#include <string.h> 
+#include <memory.h> 
+#include <process.h> 
+#include <time.h>
+#endif
+
 using namespace AVR_SITL;
 
 extern const AP_HAL::HAL& hal;
@@ -27,21 +38,49 @@ bool SITLScheduler::_in_io_proc = false;
 
 struct timeval SITLScheduler::_sketch_start_time;
 
+#ifdef __CYGWIN__
+double SITLScheduler::_cyg_freq = 0;
+long SITLScheduler::_cyg_start = 0;
+#endif
+
 SITLScheduler::SITLScheduler()
 {}
 
 void SITLScheduler::init(void *unused) 
 {
 	gettimeofday(&_sketch_start_time,NULL);
+
+#ifdef __CYGWIN__
+	LARGE_INTEGER lFreq, lCnt;
+	QueryPerformanceFrequency(&lFreq);
+	_cyg_freq = (double)lFreq.LowPart;
+	QueryPerformanceCounter(&lCnt);
+	_cyg_start = lCnt.LowPart;
+#endif
+
 }
+#ifdef __CYGWIN__
+double SITLScheduler::_cyg_sec()
+{
+	LARGE_INTEGER lCnt;
+	long tcnt;
+	QueryPerformanceCounter(&lCnt);
+ 	tcnt = lCnt.LowPart - _cyg_start;
+ 	return ((double)tcnt) / _cyg_freq;
+}
+#endif
 
 uint32_t SITLScheduler::_micros() 
 {
+#ifdef __CYGWIN__
+	return (uint32_t)(_cyg_sec() * 1.0e6);
+#else   
 	struct timeval tp;
 	gettimeofday(&tp,NULL);
 	return 1.0e6*((tp.tv_sec + (tp.tv_usec*1.0e-6)) - 
 		      (_sketch_start_time.tv_sec +
 		       (_sketch_start_time.tv_usec*1.0e-6)));
+#endif
 }
 
 uint32_t SITLScheduler::micros() 
@@ -51,11 +90,16 @@ uint32_t SITLScheduler::micros()
 
 uint32_t SITLScheduler::millis() 
 {
+#ifdef __CYGWIN__
+	// 1000 ms in a second
+	return (uint32_t)(_cyg_sec() * 1000);
+#else
 	struct timeval tp;
 	gettimeofday(&tp,NULL);
 	return 1.0e3*((tp.tv_sec + (tp.tv_usec*1.0e-6)) - 
 		      (_sketch_start_time.tv_sec +
 		       (_sketch_start_time.tv_usec*1.0e-6)));
+#endif
 }
 
 void SITLScheduler::delay_microseconds(uint16_t usec) 
diff --git a/libraries/AP_HAL_AVR_SITL/Scheduler.h b/libraries/AP_HAL_AVR_SITL/Scheduler.h
index 6ef5c43733..07b9cc807d 100644
--- a/libraries/AP_HAL_AVR_SITL/Scheduler.h
+++ b/libraries/AP_HAL_AVR_SITL/Scheduler.h
@@ -64,6 +64,11 @@ private:
     static uint8_t _num_io_procs;
     static bool    _in_timer_proc;
     static bool    _in_io_proc;
+#ifdef __CYGWIN__
+    static double _cyg_freq;
+    static long _cyg_start;
+    static double _cyg_sec();
+#endif
 
     bool _initialized;