HAL_Linux: switched to pthread based scheduling

libraries/AP_HAL_Linux/Scheduler.cpp

@@ -1,7 +1,11 @@
 
 #include "Scheduler.h"
+#include "Storage.h"
 #include <unistd.h>
 #include <sys/time.h>
+#include <poll.h>
+#include <unistd.h>
+#include <stdlib.h>
 
 using namespace Linux;
 
@@ -10,9 +14,36 @@ extern const AP_HAL::HAL& hal;
 LinuxScheduler::LinuxScheduler()
 {}
 
+typedef void *(*pthread_startroutine_t)(void *);
+
 void LinuxScheduler::init(void* machtnichts)
 {
     gettimeofday(&_sketch_start_time, NULL);
+
+    pthread_attr_t thread_attr;
+    struct sched_param param;
+
+    param.sched_priority = APM_LINUX_TIMER_PRIORITY;
+    (void)pthread_attr_setschedparam(&thread_attr, &param);
+    pthread_attr_setschedpolicy(&thread_attr, SCHED_FIFO);
+
+    pthread_create(&_timer_thread_ctx, &thread_attr, (pthread_startroutine_t)&Linux::LinuxScheduler::_timer_thread, this);
+
+    // the UART thread runs at a medium priority
+    pthread_attr_init(&thread_attr);
+    param.sched_priority = APM_LINUX_UART_PRIORITY;
+    (void)pthread_attr_setschedparam(&thread_attr, &param);
+    pthread_attr_setschedpolicy(&thread_attr, SCHED_FIFO);
+
+    pthread_create(&_uart_thread_ctx, &thread_attr, (pthread_startroutine_t)&Linux::LinuxScheduler::_uart_thread, this);
+    
+    // the IO thread runs at lower priority
+    pthread_attr_init(&thread_attr);
+    param.sched_priority = APM_LINUX_IO_PRIORITY;
+    (void)pthread_attr_setschedparam(&thread_attr, &param);
+    pthread_attr_setschedpolicy(&thread_attr, SCHED_FIFO);
+    
+    pthread_create(&_io_thread_ctx, &thread_attr, (pthread_startroutine_t)&Linux::LinuxScheduler::_io_thread, this);
 }
 
 void LinuxScheduler::delay(uint16_t ms)
@@ -43,28 +74,159 @@ void LinuxScheduler::delay_microseconds(uint16_t us)
     usleep(us);
 }
 
-void LinuxScheduler::register_delay_callback(AP_HAL::Proc k,
+void LinuxScheduler::register_delay_callback(AP_HAL::Proc proc,
-            uint16_t min_time_ms)
+                                             uint16_t min_time_ms)
-{}
+{
+    _delay_cb = proc;
+    _min_delay_cb_ms = min_time_ms;
+}
 
-void LinuxScheduler::register_timer_process(AP_HAL::TimedProc k)
+void LinuxScheduler::register_timer_process(AP_HAL::TimedProc proc) 
-{}
+{
+    for (uint8_t i = 0; i < _num_timer_procs; i++) {
+        if (_timer_proc[i] == proc) {
+            return;
+        }
+    }
 
-void LinuxScheduler::register_io_process(AP_HAL::TimedProc k)
+    if (_num_timer_procs < LINUX_SCHEDULER_MAX_TIMER_PROCS) {
-{}
+        _timer_proc[_num_timer_procs] = proc;
+        _num_timer_procs++;
+    } else {
+        hal.console->printf("Out of timer processes\n");
+    }
+}
 
-void LinuxScheduler::register_timer_failsafe(AP_HAL::TimedProc,
+void LinuxScheduler::register_io_process(AP_HAL::TimedProc proc) 
-            uint32_t period_us)
+{
-{}
+    for (uint8_t i = 0; i < _num_io_procs; i++) {
+        if (_io_proc[i] == proc) {
+            return;
+        }
+    }
+
+    if (_num_io_procs < LINUX_SCHEDULER_MAX_TIMER_PROCS) {
+        _io_proc[_num_io_procs] = proc;
+        _num_io_procs++;
+    } else {
+        hal.console->printf("Out of IO processes\n");
+    }
+}
+
+void LinuxScheduler::register_timer_failsafe(AP_HAL::TimedProc failsafe,
+                                             uint32_t period_us)
+{
+    _failsafe = failsafe;
+}
 
 void LinuxScheduler::suspend_timer_procs()
-{}
+{
+    _timer_suspended = true;
+}
 
 void LinuxScheduler::resume_timer_procs()
-{}
+{
+    _timer_suspended = false;
+    if (_timer_event_missed == true) {
+        _run_timers(false);
+        _timer_event_missed = false;
+    }
+}
 
-bool LinuxScheduler::in_timerprocess() {
+void LinuxScheduler::_run_timers(bool called_from_timer_thread)
-    return false;
+{
+    uint32_t tnow = micros();
+    if (_in_timer_proc) {
+        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);
+            }
+        }
+    } else if (called_from_timer_thread) {
+        _timer_event_missed = true;
+    }
+
+    // and the failsafe, if one is setup
+    if (_failsafe != NULL) {
+        _failsafe(tnow);
+    }
+
+    _in_timer_proc = false;
+}
+
+void *LinuxScheduler::_timer_thread(void)
+{
+    while (true) {
+        poll(NULL, 0, 1);
+
+        // run registered timers
+        _run_timers(true);
+
+    }
+    return NULL;
+}
+
+void LinuxScheduler::_run_io(void)
+{
+    uint32_t tnow = micros();
+    if (_in_io_proc) {
+        return;
+    }
+    _in_io_proc = true;
+
+    if (!_timer_suspended) {
+        // now call the IO based drivers
+        for (int i = 0; i < _num_io_procs; i++) {
+            if (_io_proc[i] != NULL) {
+                _io_proc[i](tnow);
+            }
+        }
+    }
+
+    _in_io_proc = false;
+}
+
+void *LinuxScheduler::_uart_thread(void)
+{
+    while (true) {
+        poll(NULL, 0, 1);
+
+        // process any pending serial bytes: TODO
+    }
+    return NULL;
+}
+
+void *LinuxScheduler::_io_thread(void)
+{
+    while (true) {
+        poll(NULL, 0, 1);
+
+        // process any pending storage writes
+        ((LinuxStorage *)hal.storage)->_timer_tick();
+
+        // run registered IO processes
+        _run_io();
+    }
+    return NULL;
+}
+
+void LinuxScheduler::panic(const prog_char_t *errormsg) 
+{
+    write(1, errormsg, strlen(errormsg));
+    write(1, "\n", 1);
+    hal.scheduler->delay_microseconds(10000);
+    exit(1);
+}
+
+bool LinuxScheduler::in_timerprocess() 
+{
+    return _in_timer_proc;
 }
 
 void LinuxScheduler::begin_atomic()
@@ -74,17 +236,18 @@ void LinuxScheduler::end_atomic()
 {}
 
 bool LinuxScheduler::system_initializing() {
-    return false;
+    return !_initialized;
 }
 
 void LinuxScheduler::system_initialized()
-{}
+{
-
+    if (_initialized) {
-void LinuxScheduler::panic(const prog_char_t *errormsg) {
+        panic("PANIC: scheduler::system_initialized called more than once");
-    hal.console->println_P(errormsg);
+    }
-    for(;;);
+    _initialized = true;
 }
 
-void LinuxScheduler::reboot(bool hold_in_bootloader) {
+void LinuxScheduler::reboot(bool hold_in_bootloader) 
+{
     for(;;);
 }

libraries/AP_HAL_Linux/Scheduler.h

@@ -4,6 +4,14 @@
 
 #include <AP_HAL_Linux.h>
 #include <sys/time.h>
+#include <pthread.h>
+
+#define LINUX_SCHEDULER_MAX_TIMER_PROCS 10
+
+#define APM_LINUX_MAIN_PRIORITY    180
+#define APM_LINUX_TIMER_PRIORITY   181
+#define APM_LINUX_UART_PRIORITY     60
+#define APM_LINUX_IO_PRIORITY       59
 
 class Linux::LinuxScheduler : public AP_HAL::Scheduler {
 public:
@@ -37,6 +45,38 @@ public:
 
 private:
     struct timeval _sketch_start_time;    
+    void _timer_handler(int signum);
+
+    AP_HAL::Proc _delay_cb;
+    uint16_t _min_delay_cb_ms;
+
+    AP_HAL::TimedProc _failsafe;
+
+    bool _initialized;
+    volatile bool _timer_pending;
+
+    volatile bool _timer_suspended;
+
+    AP_HAL::TimedProc _timer_proc[LINUX_SCHEDULER_MAX_TIMER_PROCS];
+    uint8_t _num_timer_procs;
+    volatile bool _in_timer_proc;
+
+    AP_HAL::TimedProc _io_proc[LINUX_SCHEDULER_MAX_TIMER_PROCS];
+    uint8_t _num_io_procs;
+    volatile bool _in_io_proc;
+
+    volatile bool _timer_event_missed;
+
+    pthread_t _timer_thread_ctx;
+    pthread_t _io_thread_ctx;
+    pthread_t _uart_thread_ctx;
+
+    void *_timer_thread(void);
+    void *_io_thread(void);
+    void *_uart_thread(void);
+
+    void _run_timers(bool called_from_timer_thread);
+    void _run_io(void);
 };
 
 #endif // __AP_HAL_LINUX_SCHEDULER_H__

libraries/AP_HAL_Linux/Storage.cpp

@@ -1,40 +1,239 @@
+#include <AP_HAL.h>
+#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
+
+#include <assert.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <errno.h>
+#include <stdio.h>
 
-#include <string.h>
 #include "Storage.h"
-
 using namespace Linux;
 
-LinuxStorage::LinuxStorage()
+/*
-{}
+  This stores 'eeprom' data on the SD card, with a 4k size, and a
+  in-memory buffer. This keeps the latency down.
+ */
 
-void LinuxStorage::init(void*)
+// name the storage file after the sketch so you can use the same board
-{}
+// card for ArduCopter and ArduPlane
+#define STORAGE_DIR "/var/run/APM"
+#define STORAGE_FILE STORAGE_DIR "/" SKETCHNAME ".stg"
 
-uint8_t LinuxStorage::read_byte(uint16_t loc){
+extern const AP_HAL::HAL& hal;
-    return 0;
+
+void LinuxStorage::_storage_create(void)
+{
+	mkdir(STORAGE_DIR, 0777);
+	unlink(STORAGE_FILE);
+	int fd = open(STORAGE_FILE, O_RDWR|O_CREAT, 0666);
+	if (fd == -1) {
+		hal.scheduler->panic("Failed to create " STORAGE_FILE);
+	}
+	for (uint16_t loc=0; loc<sizeof(_buffer); loc += LINUX_STORAGE_MAX_WRITE) {
+		if (write(fd, &_buffer[loc], LINUX_STORAGE_MAX_WRITE) != LINUX_STORAGE_MAX_WRITE) {
+			hal.scheduler->panic("Error filling " STORAGE_FILE);			
+		}
+	}
+	// ensure the directory is updated with the new size
+	fsync(fd);
+	close(fd);
 }
 
-uint16_t LinuxStorage::read_word(uint16_t loc){
+void LinuxStorage::_storage_open(void)
-    return 0;
+{
+	if (_initialised) {
+		return;
+	}
+
+	_dirty_mask = 0;
+	int fd = open(STORAGE_FILE, O_RDONLY);
+	if (fd == -1) {
+		_storage_create();
+		fd = open(STORAGE_FILE, O_RDONLY);
+		if (fd == -1) {
+			hal.scheduler->panic("Failed to open " STORAGE_FILE);
+		}
+	}
+	if (read(fd, _buffer, sizeof(_buffer)) != sizeof(_buffer)) {
+		close(fd);
+		_storage_create();
+		fd = open(STORAGE_FILE, O_RDONLY);
+		if (fd == -1) {
+			hal.scheduler->panic("Failed to open " STORAGE_FILE);
+		}
+		if (read(fd, _buffer, sizeof(_buffer)) != sizeof(_buffer)) {
+			hal.scheduler->panic("Failed to read " STORAGE_FILE);
+		}
+	}
+	close(fd);
+	_initialised = true;
 }
 
-uint32_t LinuxStorage::read_dword(uint16_t loc){
+/*
-    return 0;
+  mark some lines as dirty. Note that there is no attempt to avoid
+  the race condition between this code and the _timer_tick() code
+  below, which both update _dirty_mask. If we lose the race then the
+  result is that a line is written more than once, but it won't result
+  in a line not being written.
+ */
+void LinuxStorage::_mark_dirty(uint16_t loc, uint16_t length)
+{
+	uint16_t end = loc + length;
+	while (loc < end) {
+		uint8_t line = (loc >> LINUX_STORAGE_LINE_SHIFT);
+		_dirty_mask |= 1 << line;
+		loc += LINUX_STORAGE_LINE_SIZE;
+	}
 }
 
-void LinuxStorage::read_block(void* dst, uint16_t src, size_t n) {
+uint8_t LinuxStorage::read_byte(uint16_t loc) 
-    memset(dst, 0, n);
+{
+	if (loc >= sizeof(_buffer)) {
+		return 0;
+	}
+	_storage_open();
+	return _buffer[loc];
 }
 
-void LinuxStorage::write_byte(uint16_t loc, uint8_t value)
+uint16_t LinuxStorage::read_word(uint16_t loc) 
-{}
+{
+	uint16_t value;
+	if (loc >= sizeof(_buffer)-(sizeof(value)-1)) {
+		return 0;
+	}
+	_storage_open();
+	memcpy(&value, &_buffer[loc], sizeof(value));
+	return value;
+}
 
-void LinuxStorage::write_word(uint16_t loc, uint16_t value)
+uint32_t LinuxStorage::read_dword(uint16_t loc) 
-{}
+{
+	uint32_t value;
+	if (loc >= sizeof(_buffer)-(sizeof(value)-1)) {
+		return 0;
+	}
+	_storage_open();
+	memcpy(&value, &_buffer[loc], sizeof(value));
+	return value;
+}
 
-void LinuxStorage::write_dword(uint16_t loc, uint32_t value)
+void LinuxStorage::read_block(void *dst, uint16_t loc, size_t n) 
-{}
+{
+	if (loc >= sizeof(_buffer)-(n-1)) {
+		return;
+	}
+	_storage_open();
+	memcpy(dst, &_buffer[loc], n);
+}
 
-void LinuxStorage::write_block(uint16_t loc, const void* src, size_t n)
+void LinuxStorage::write_byte(uint16_t loc, uint8_t value) 
-{}
+{
+	if (loc >= sizeof(_buffer)) {
+		return;
+	}
+	if (_buffer[loc] != value) {
+		_storage_open();
+		_buffer[loc] = value;
+		_mark_dirty(loc, sizeof(value));
+	}
+}
 
+void LinuxStorage::write_word(uint16_t loc, uint16_t value) 
+{
+	if (loc >= sizeof(_buffer)-(sizeof(value)-1)) {
+		return;
+	}
+	if (memcmp(&value, &_buffer[loc], sizeof(value)) != 0) {
+		_storage_open();
+		memcpy(&_buffer[loc], &value, sizeof(value));
+		_mark_dirty(loc, sizeof(value));
+	}
+}
+
+void LinuxStorage::write_dword(uint16_t loc, uint32_t value) 
+{
+	if (loc >= sizeof(_buffer)-(sizeof(value)-1)) {
+		return;
+	}
+	if (memcmp(&value, &_buffer[loc], sizeof(value)) != 0) {
+		_storage_open();
+		memcpy(&_buffer[loc], &value, sizeof(value));
+		_mark_dirty(loc, sizeof(value));
+	}
+}
+
+void LinuxStorage::write_block(uint16_t loc, const void *src, size_t n) 
+{
+	if (loc >= sizeof(_buffer)-(n-1)) {
+		return;
+	}
+	if (memcmp(src, &_buffer[loc], n) != 0) {
+		_storage_open();
+		memcpy(&_buffer[loc], src, n);
+		_mark_dirty(loc, n);
+	}
+}
+
+void LinuxStorage::_timer_tick(void)
+{
+	if (!_initialised || _dirty_mask == 0) {
+		return;
+	}
+
+	if (_fd == -1) {
+		_fd = open(STORAGE_FILE, O_WRONLY);
+		if (_fd == -1) {
+			return;	
+		}
+	}
+
+	// write out the first dirty set of lines. We don't write more
+	// than one to keep the latency of this call to a minimum
+	uint8_t i, n;
+	for (i=0; i<LINUX_STORAGE_NUM_LINES; i++) {
+		if (_dirty_mask & (1<<i)) {
+			break;
+		}
+	}
+	if (i == LINUX_STORAGE_NUM_LINES) {
+		// this shouldn't be possible
+		return;
+	}
+	uint32_t write_mask = (1U<<i);
+	// see how many lines to write
+	for (n=1; (i+n) < LINUX_STORAGE_NUM_LINES && 
+		     n < (LINUX_STORAGE_MAX_WRITE>>LINUX_STORAGE_LINE_SHIFT); n++) {
+		if (!(_dirty_mask & (1<<(n+i)))) {
+			break;
+		}		
+		// mark that line clean
+		write_mask |= (1<<(n+i));
+	}
+
+	/*
+	  write the lines. This also updates _dirty_mask. Note that
+	  because this is a SCHED_FIFO thread it will not be preempted
+	  by the main task except during blocking calls. This means we
+	  don't need a semaphore around the _dirty_mask updates.
+	 */
+	if (lseek(_fd, i<<LINUX_STORAGE_LINE_SHIFT, SEEK_SET) == (i<<LINUX_STORAGE_LINE_SHIFT)) {
+		_dirty_mask &= ~write_mask;
+		if (write(_fd, &_buffer[i<<LINUX_STORAGE_LINE_SHIFT], n<<LINUX_STORAGE_LINE_SHIFT) != n<<LINUX_STORAGE_LINE_SHIFT) {
+			// write error - likely EINTR
+			_dirty_mask |= write_mask;
+			close(_fd);
+			_fd = -1;
+		}
+		if (_dirty_mask == 0) {
+			if (fsync(_fd) != 0) {
+				close(_fd);
+				_fd = -1;
+			}
+		}
+	}
+}
+
+#endif // CONFIG_HAL_BOARD

libraries/AP_HAL_Linux/Storage.h

@@ -1,13 +1,25 @@
 
+
 #ifndef __AP_HAL_LINUX_STORAGE_H__
 #define __AP_HAL_LINUX_STORAGE_H__
 
-#include <AP_HAL_Linux.h>
+#include <AP_HAL.h>
+#include "AP_HAL_Linux_Namespace.h"
 
-class Linux::LinuxStorage : public AP_HAL::Storage {
+#define LINUX_STORAGE_SIZE 4096
+#define LINUX_STORAGE_MAX_WRITE 512
+#define LINUX_STORAGE_LINE_SHIFT 9
+#define LINUX_STORAGE_LINE_SIZE (1<<LINUX_STORAGE_LINE_SHIFT)
+#define LINUX_STORAGE_NUM_LINES (LINUX_STORAGE_SIZE/LINUX_STORAGE_LINE_SIZE)
+
+class Linux::LinuxStorage : public AP_HAL::Storage 
+{
 public:
-    LinuxStorage();
+    LinuxStorage() :
-    void init(void *);
+	_fd(-1),
+	_dirty_mask(0)
+	{}
+    void init(void* machtnichts) {}
     uint8_t  read_byte(uint16_t loc);
     uint16_t read_word(uint16_t loc);
     uint32_t read_dword(uint16_t loc);
@@ -17,6 +29,17 @@ public:
     void write_word(uint16_t loc, uint16_t value);
     void write_dword(uint16_t loc, uint32_t value);
     void write_block(uint16_t dst, const void* src, size_t n);
+
+    void _timer_tick(void);
+
+private:
+    int _fd;
+    volatile bool _initialised;
+    void _storage_create(void);
+    void _storage_open(void);
+    void _mark_dirty(uint16_t loc, uint16_t length);
+    uint8_t _buffer[LINUX_STORAGE_SIZE];
+    volatile uint32_t _dirty_mask;
 };
 
 #endif // __AP_HAL_LINUX_STORAGE_H__