SITL: filled in more of the HAL SITL backend

libraries/AP_HAL_AVR_SITL/AP_HAL_AVR_SITL.h

@@ -7,7 +7,7 @@
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
 #define AP_HAL_MAIN() extern "C" {\
-    int main (int argc, const char *argv[]) {	\
+    int main (int argc, char * const argv[]) {	\
 	hal.init(argc, argv);		\
         setup();\
         for(;;) loop();\

libraries/AP_HAL_AVR_SITL/AP_HAL_AVR_SITL_Namespace.h

@@ -5,6 +5,11 @@
 namespace AVR_SITL {
     class SITLUARTDriver;
     class SITLScheduler;
+    class SITL_State;
+    class SITLConsoleDriver;
+    class SITLEEPROMStorage;
+    class SITLAnalogIn;
+    class ADCSource;
 }
 
 #endif // __AP_HAL_AVR_SITL_NAMESPACE_H__

libraries/AP_HAL_AVR_SITL/Console.h

@@ -8,7 +8,7 @@
 #include <AP_HAL.h>
 #include "AP_HAL_AVR_SITL_Namespace.h"
 
-class SITLConsoleDriver : public AP_HAL::ConsoleDriver {
+class AVR_SITL::SITLConsoleDriver : public AP_HAL::ConsoleDriver {
 public:
 	SITLConsoleDriver();
     void init(void* baseuartdriver);

libraries/AP_HAL_AVR_SITL/HAL_AVR_SITL_Class.cpp

@@ -10,17 +10,23 @@
 #include "AP_HAL_AVR_SITL_Namespace.h"
 #include "HAL_AVR_SITL_Class.h"
 #include "Scheduler.h"
+#include "AnalogIn.h"
 #include "UARTDriver.h"
 #include "Storage.h"
 #include "Console.h"
+#include "SITL_State.h"
+
+using namespace AVR_SITL;
 
 static SITLScheduler sitlScheduler;
 static SITLEEPROMStorage sitlEEPROMStorage;
 static SITLConsoleDriver consoleDriver;
+static SITLAnalogIn sitlAnalogIn;
+static SITL_State sitlState;
 
-static AVR_SITL::SITLUARTDriver sitlUart0Driver(0);
-static AVR_SITL::SITLUARTDriver sitlUart1Driver(1);
-static AVR_SITL::SITLUARTDriver sitlUart2Driver(2);
+static SITLUARTDriver sitlUart0Driver(0);
+static SITLUARTDriver sitlUart1Driver(1);
+static SITLUARTDriver sitlUart2Driver(2);
 
 HAL_AVR_SITL::HAL_AVR_SITL() :
     AP_HAL::HAL(
@@ -29,26 +35,28 @@ HAL_AVR_SITL::HAL_AVR_SITL() :
         &sitlUart2Driver,  /* uartC */
         NULL, /* i2c */
         NULL, /* spi */
-        NULL, /* analogin */
+        &sitlAnalogIn, /* analogin */
         &sitlEEPROMStorage, /* storage */
         &consoleDriver, /* console */
         NULL, /* gpio */
         NULL, /* rcinput */
         NULL, /* rcoutput */
-        &sitlScheduler) /* scheduler */
+        &sitlScheduler), /* scheduler */
+    _sitl_state(&sitlState)
 {}
 
-void HAL_AVR_SITL::init(int argc, const char *argv[]) const {
-
+void HAL_AVR_SITL::init(int argc, char * const argv[]) const 
+{
+    _sitl_state->init(argc, argv);
     scheduler->init(NULL);
     uartA->begin(115200);
     console->init((void*) uartA);
 
-    rcin->init(NULL);
-    rcout->init(NULL);
-    spi->init(NULL);
-    i2c->begin();
-    i2c->setTimeout(100);
+    //rcin->init(NULL);
+    //rcout->init(NULL);
+    //spi->init(NULL);
+    //i2c->begin();
+    //i2c->setTimeout(100);
     analogin->init(NULL);
 }
 

libraries/AP_HAL_AVR_SITL/HAL_AVR_SITL_Class.h

@@ -8,11 +8,15 @@
 
 #include <AP_HAL_AVR_SITL.h>
 #include "AP_HAL_AVR_SITL_Namespace.h"
+#include "SITL_State.h"
 
 class HAL_AVR_SITL : public AP_HAL::HAL {
 public:
     HAL_AVR_SITL();    
-    void init(int argc, const char *argv[]) const;
+    void init(int argc, char * const argv[]) const;
+
+private:
+    AVR_SITL::SITL_State *_sitl_state;
 };
 
 extern const HAL_AVR_SITL AP_HAL_AVR_SITL;

libraries/AP_HAL_AVR_SITL/Scheduler.cpp

@@ -9,7 +9,7 @@
 #include <sys/time.h>
 #include <unistd.h>
 
-using namespace AP_HAL_AVR;
+using namespace AVR_SITL;
 
 extern const AP_HAL::HAL& hal;
 
@@ -20,6 +20,7 @@ AP_HAL::TimedProc SITLScheduler::_timer_proc[SITL_SCHEDULER_MAX_TIMER_PROCS] = {
 AP_HAL::TimedProc SITLScheduler::_defered_timer_proc = NULL;
 uint8_t SITLScheduler::_num_timer_procs = 0;
 bool SITLScheduler::_in_timer_proc = false;
+struct timeval SITLScheduler::_sketch_start_time;
 
 SITLScheduler::SITLScheduler()
 {}
@@ -29,7 +30,7 @@ void SITLScheduler::init(void *unused)
 	gettimeofday(&_sketch_start_time,NULL);
 }
 
-uint32_t SITLScheduler::micros() 
+uint32_t SITLScheduler::_micros() 
 {
 	struct timeval tp;
 	gettimeofday(&tp,NULL);
@@ -38,6 +39,11 @@ uint32_t SITLScheduler::micros()
 		       (_sketch_start_time.tv_usec*1.0e-6)));
 }
 
+uint32_t SITLScheduler::micros() 
+{
+    return _micros();
+}
+
 uint32_t SITLScheduler::millis() 
 {
 	struct timeval tp;
@@ -138,4 +144,52 @@ void SITLScheduler::reboot()
     hal.uartA->println_P(PSTR("REBOOT NOT IMPLEMENTED\r\n"));
 }
 
+void SITLScheduler::timer_event() 
+{
+    uint32_t tnow = _micros();
+    if (_in_timer_proc) {
+        // the timer calls took longer than the period of the
+        // timer. This is bad, and may indicate a serious
+        // driver failure. We can't just call the drivers
+        // again, as we could run out of stack. So we only
+        // call the _failsafe call. It's job is to detect if
+        // the drivers or the main loop are indeed dead and to
+        // activate whatever failsafe it thinks may help if
+        // need be.  We assume the failsafe code can't
+        // block. If it does then we will recurse and die when
+        // we run out of stack
+        if (_failsafe != NULL) {
+            _failsafe(tnow);
+        }
+        return;
+    }
+    _in_timer_proc = true;
+
+    if (!_timer_suspended) {
+        // now call the timer based drivers
+        for (int i = 0; i < _num_timer_procs; i++) {
+            if (_timer_proc[i] != NULL) {
+                _timer_proc[i](tnow);
+            }
+        }
+    }
+
+    /* Run any defered procedures, if they exist.*/
+    /* Atomic read and clear: */
+    AP_HAL::TimedProc defered = _defered_timer_proc;
+    _defered_timer_proc = NULL;
+    if (defered != NULL) {
+        _timer_suspended = true;
+        defered(tnow);
+        _timer_suspended = false;
+    }
+
+    // and the failsafe, if one is setup
+    if (_failsafe != NULL) {
+        _failsafe(tnow);
+    }
+
+    _in_timer_proc = false;
+}
+
 #endif

libraries/AP_HAL_AVR_SITL/Scheduler.h

@@ -3,13 +3,14 @@
 #define __AP_HAL_SITL_SCHEDULER_H__
 
 #include <AP_HAL.h>
+#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
 #include "AP_HAL_AVR_SITL_Namespace.h"
 #include <sys/time.h>
 
 #define SITL_SCHEDULER_MAX_TIMER_PROCS 4
 
 /* Scheduler implementation: */
-class SITLScheduler : public AP_HAL::Scheduler {
+class AVR_SITL::SITLScheduler : public AP_HAL::Scheduler {
 public:
     SITLScheduler();
     /* AP_HAL::Scheduler methods */
@@ -29,11 +30,18 @@ public:
     void     end_atomic();
     void     reboot();
 
+    bool     interrupts_are_blocked(void) { return _nested_atomic_ctr != 0; }
+
+    static void timer_event(void);
+
+    // callable from interrupt handler
+    static uint32_t _micros();
+
 private:
     uint8_t _nested_atomic_ctr;
     AP_HAL::Proc _delay_cb;
     uint16_t _min_delay_cb_ms;
-    struct timeval _sketch_start_time;
+    static struct timeval _sketch_start_time;
     static AP_HAL::TimedProc _failsafe;
 
     static volatile bool _timer_suspended;
@@ -43,5 +51,7 @@ private:
     static bool    _in_timer_proc;
 
 };
+#endif
 #endif // __AP_HAL_SITL_SCHEDULER_H__
 
+

libraries/AP_HAL_AVR_SITL/Storage.h

@@ -6,7 +6,7 @@
 #include <AP_HAL.h>
 #include "AP_HAL_AVR_SITL_Namespace.h"
 
-class SITLEEPROMStorage : public AP_HAL::Storage {
+class AVR_SITL::SITLEEPROMStorage : public AP_HAL::Storage {
 public:
     SITLEEPROMStorage() {
 	    _eeprom_fd = -1;

libraries/AP_HAL_AVR_SITL/UARTDriver.cpp

@@ -31,6 +31,8 @@ using namespace AVR_SITL;
 
 #define LISTEN_BASE_PORT 5760
 
+bool SITLUARTDriver::_console;
+
 /* UARTDriver method implementations */
 
 void SITLUARTDriver::begin(uint32_t baud, uint16_t rxSpace, uint16_t txSpace) 
@@ -202,6 +204,15 @@ void SITLUARTDriver::_tcp_start_connection(bool wait_for_connection)
 	struct sockaddr_in sockaddr;
 	int ret;
 
+	if (_console) {
+            // hack for console access
+            _connected = true;
+            _listen_fd = -1;
+            _fd = 1;
+            _set_nonblocking(0);
+            return;
+	}
+
 	memset(&sockaddr,0,sizeof(sockaddr));
 
 #ifdef HAVE_SOCK_SIN_LEN

libraries/AP_HAL_AVR_SITL/UARTDriver.h

@@ -11,6 +11,8 @@
 
 class AVR_SITL::SITLUARTDriver : public AP_HAL::UARTDriver {
 public:
+    friend class AVR_SITL::SITL_State;
+
 	SITLUARTDriver(const uint8_t portNumber) {
 		_portNumber = portNumber;
         _rxSpace = _default_rx_buffer_size;
@@ -56,7 +58,7 @@ private:
 	int _listen_fd;  // socket we are listening on
 	int _fd;         // data socket
 	int _serial_port;
-	bool _console;
+	static bool _console;
 	bool _nonblocking_writes;
     uint16_t _rxSpace;
     uint16_t _txSpace;