AP_Semaphore: added new library to help remove conflicts over SPI bus

libraries/AP_Common/AP_Semaphore.cpp

@@ -0,0 +1,81 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#include "AP_Semaphore.h"
+
+extern "C" {
+#include <inttypes.h>
+#include <stdint.h>
+#include <avr/interrupt.h>
+}
+#if defined(ARDUINO) && ARDUINO >= 100
+ #include "Arduino.h"
+#else
+ #include "WConstants.h"
+#endif
+
+// include some global constants
+AP_Semaphore AP_Semaphore_spi3;
+
+// Constructor
+AP_Semaphore::AP_Semaphore()
+{
+}
+
+// get - to claim ownership of the semaphore 
+bool AP_Semaphore::get(void* caller)
+{
+    bool result = false;
+    cli();
+    if( !_taken ) {
+        _taken = true;
+        _owner = caller;
+        result = true;
+    }
+    sei();
+    return result;
+}
+
+// release - to give up ownership of the semaphore
+// returns true if successfully released
+bool AP_Semaphore::release(void* caller)
+{
+    ap_semaphore_callback callback_fn;
+
+    // check legitimacy of release call
+    if( caller != _owner ) {
+        return false;
+    }
+
+    // if another process is waiting immediately call it's call back function
+    if( _waiting_callback != NULL ) {
+
+        // give ownership to waiting process
+        _owner = _waiting_owner;
+        callback_fn = _waiting_callback;
+
+        // clear waiting process
+        _waiting_callback = NULL;
+        _waiting_owner = NULL;
+
+        // callback
+        callback_fn();
+    }
+
+    // give up the semaphore
+    _taken = false;
+    return true;
+}
+
+// call_on_release - returns true if caller successfully added to the queue to be called back
+bool AP_Semaphore::call_on_release(void* caller, ap_semaphore_callback callback_fn)
+{
+    bool result = false;
+    cli();
+    if( _waiting_owner == NULL ) {
+        _waiting_owner = caller;
+        _waiting_callback = callback_fn;
+        result = true;
+    }
+    sei();
+    return result;
+}

libraries/AP_Common/AP_Semaphore.h

@@ -0,0 +1,42 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
+
+/// @file	AP_Semaphore.h
+/// @brief	class to ensure conflicts over shared resources are avoided
+
+#ifndef AP_SEMAPHORE
+#define AP_SEMAPHORE
+
+#include <FastSerial.h>
+#include <AP_Common.h>
+#include <AP_Math.h>        // ArduPilot Mega Vector/Matrix math Library
+
+// the callback type for call_on_release. They are passed in a pointer to the semaphore
+typedef void (*ap_semaphore_callback)();
+
+/// @class      AP_Semaphore
+class AP_Semaphore {
+public:
+
+    // Constructor
+    AP_Semaphore();
+
+    // get - to claim ownership of the semaphore
+    virtual bool get(void* caller);
+
+    // release - to give up ownership of the semaphore
+    virtual bool release(void* caller);
+
+    // call_on_release - returns true if caller successfully added to the queue to be called back
+    virtual bool call_on_release(void* caller, ap_semaphore_callback callback_fn);
+
+protected:
+    bool        _taken;
+    void*       _owner;
+    void*       _waiting_owner;
+    ap_semaphore_callback     _waiting_callback;       // call back procedure of process waiting for sempahore
+};
+
+// some global semaphores
+extern AP_Semaphore AP_Semaphore_spi3;
+
+#endif  // AP_SEMAPHORE

libraries/AP_Common/examples/AP_Semaphore_test/AP_Semaphore_test.pde

@@ -0,0 +1,102 @@
+
+#include <FastSerial.h>
+#include <AP_Math.h>
+#include <AP_Common.h>
+#include <AP_Semaphore.h>
+
+FastSerialPort0(Serial);
+
+AP_Semaphore my_semaphore;
+
+int16_t dummy1, dummy2, dummy3; // used to provide points to semaphore library
+
+// call back for dummy1
+void callback_1() {
+    Serial.println("dummy2 called back!");
+}
+
+// call back for dummy3
+void callback_3() {
+    Serial.println("dummy3 called back!");
+}
+
+void setup(void)
+{
+    // initialise serial port
+    Serial.begin(115200);
+
+    // print welcome message
+    Serial.println("AP_Semaphore_test ver 1.0");
+}
+
+void print_true_false(bool true_false) {
+    if( true_false ) {
+        Serial.println("success");
+    } else {
+        Serial.println("failed");
+    }
+}
+
+void loop(void)
+{
+    bool ret;
+
+    // quick test of spi semaphore
+    ret = AP_Semaphore_spi.get(&dummy3);
+    Serial.print("dummy3 gets SPI semaphore: ");
+    print_true_false(ret);
+
+    // dummy1 gets semaphore    
+    Serial.print("dummy1 gets semaphore: ");
+    ret = my_semaphore.get(&dummy1);
+    print_true_false(ret);
+
+    // dummy2 tries to get semaphore (fails)
+    Serial.print("dummy2 gets semaphore: ");
+    ret = my_semaphore.get(&dummy2);
+    print_true_false(ret);
+
+    // dummy2 tries to release semaphore (fails)
+    Serial.print("dummy2 releases semaphore (that it doesn't have): ");
+    ret = my_semaphore.release(&dummy2);
+    print_true_false(ret);
+
+    // dummy1 releases semaphore
+    Serial.print("dummy1 releases semaphore: ");
+    ret = my_semaphore.release(&dummy1);
+    print_true_false(ret);
+
+    // dummy2 tries to get semaphore (succeeds)
+    Serial.print("dummy2 gets semaphore: ");
+    ret = my_semaphore.get(&dummy2);
+    print_true_false(ret);
+
+    // dummy1 tries to get semphore (fails)
+    Serial.print("dummy1 gets semaphore: ");
+    ret = my_semaphore.get(&dummy1);
+    print_true_false(ret);
+    
+    // dummy1 asks for call back (succeeds)
+    Serial.print("dummy1 asks for call back on release: ");
+    ret = my_semaphore.call_on_release(&dummy1, callback_1);
+    print_true_false(ret);
+    
+    // dummy3 asks for call back (fails)
+    Serial.print("dummy3 asks for call back on release: ");
+    ret = my_semaphore.call_on_release(&dummy3, callback_3);
+    print_true_false(ret);
+
+    // dummy2 releases semaphore
+    // dummy1's call back should be called
+    Serial.print("dummy2 releases semaphore: ");
+    ret = my_semaphore.release(&dummy2);
+    print_true_false(ret);
+
+    Serial.println("--------------------");
+
+    // nobody has semaphore
+
+    // delay
+    delay(10000);
+}
+