AP_Semaphore: ported to AP_HAL

* improved readability of test output

libraries/AP_Semaphore/AP_Semaphore.cpp

@@ -1,17 +1,9 @@
 /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
+#include <AP_HAL.h>
 #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
+extern const AP_HAL::HAL& hal;
 
 // Constructor
 AP_Semaphore::AP_Semaphore()
@@ -22,14 +14,13 @@ AP_Semaphore::AP_Semaphore()
 bool AP_Semaphore::get(void* caller)
 {
     bool result = false;
-    uint8_t oldSREG = SREG;
-    cli();
+    hal.scheduler->begin_atomic();
     if( !_taken ) {
         _taken = true;
         _owner = caller;
         result = true;
     }
-    SREG = oldSREG;
+    hal.scheduler->end_atomic();
     return result;
 }
 
@@ -37,26 +28,25 @@ bool AP_Semaphore::get(void* caller)
 // 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 ) {
+    // if another process is waiting immediately call the provided kontinuation
+    if( _waiting_k != NULL ) {
 
         // give ownership to waiting process
         _owner = _waiting_owner;
-        callback_fn = _waiting_callback;
+        AP_HAL::Proc k = _waiting_k;
 
         // clear waiting process
-        _waiting_callback = NULL;
+        _waiting_k = NULL;
         _waiting_owner = NULL;
 
         // callback
-        callback_fn();
+        k();
     }
 
     // give up the semaphore
@@ -64,17 +54,17 @@ bool AP_Semaphore::release(void* caller)
     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)
+// 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_HAL::Proc k)
 {
     bool result = false;
-    uint8_t oldSREG = SREG;
-    cli();
+    hal.scheduler->begin_atomic();
     if( _waiting_owner == NULL ) {
         _waiting_owner = caller;
-        _waiting_callback = callback_fn;
+        _waiting_k = k;
         result = true;
     }
-    SREG = oldSREG;
+    hal.scheduler->end_atomic();
     return result;
 }

libraries/AP_Semaphore/AP_Semaphore.h

@@ -6,12 +6,7 @@
 #ifndef __AP_SEMAPHORE_H__
 #define __AP_SEMAPHORE_H__
 
-#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)();
+#include <AP_HAL.h>
 
 /// @class      AP_Semaphore
 class AP_Semaphore {
@@ -26,14 +21,17 @@ public:
     // 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);
+    // call_on_release - returns true if caller successfully added to the
+    // queue to be called back
+    virtual bool call_on_release(void* caller, AP_HAL::Proc k);
 
 protected:
     bool        _taken;
     void*       _owner;
     void*       _waiting_owner;
-    ap_semaphore_callback     _waiting_callback;       // call back procedure of process waiting for sempahore
+
+    // procedure of process waiting for sempahore
+    AP_HAL::Proc _waiting_k;
 };
 
 #endif  // __AP_SEMAPHORE_H__

libraries/AP_Semaphore/examples/AP_Semaphore_test/AP_Semaphore_test.pde

@@ -1,40 +1,44 @@
 
-#include <FastSerial.h>
-#include <AP_Math.h>
 #include <AP_Common.h>
+#include <AP_Progmem.h>
+#include <AP_HAL.h>
 #include <AP_Semaphore.h>
 
-FastSerialPort0(Serial);
+#include <AP_HAL_AVR.h>
+const AP_HAL::HAL& hal = AP_HAL_AVR_APM2;
 
-AP_Semaphore my_semaphore;
-AP_Semaphore AP_Semaphore_spi3;
+static AP_Semaphore my_semaphore;
+static AP_Semaphore AP_Semaphore_spi3;
 
 int16_t dummy1, dummy2, dummy3; // used to provide points to semaphore library
 
 // call back for dummy1
 void callback_1() {
-    Serial.println("dummy2 called back!");
+    hal.console->println("dummy2 called back!");
 }
 
 // call back for dummy3
 void callback_3() {
-    Serial.println("dummy3 called back!");
+    hal.console->println("dummy3 called back!");
 }
 
 void setup(void)
 {
-    // initialise serial port
-    Serial.begin(115200);
-
     // print welcome message
-    Serial.println("AP_Semaphore_test ver 1.0");
+    hal.console->println("AP_Semaphore_test ver 1.0");
 }
 
-void print_true_false(bool true_false) {
-    if( true_false ) {
-        Serial.println("success");
+void print_test_result(bool expected, bool got) {
+    if( expected == got ) {
+        hal.console->print_P(PSTR("success: got "));
+        if (got) {
+            hal.console->println_P(PSTR("true"));
+        } else {
+            hal.console->println_P(PSTR("false"));
+        }
     } else {
-        Serial.println("failed");
+        hal.console->printf_P(PSTR("***failed************************: expected %d got %d\r\n"),
+                (int)expected, (int) got);
     }
 }
 
@@ -44,60 +48,68 @@ void loop(void)
 
     // quick test of spi semaphore
     ret = AP_Semaphore_spi3.get(&dummy3);
-    Serial.print("dummy3 gets SPI semaphore: ");
-    print_true_false(ret);
+    hal.console->print("dummy3 gets SPI semaphore: ");
+    print_test_result(true, ret);
 
     // dummy1 gets semaphore    
-    Serial.print("dummy1 gets semaphore: ");
+    hal.console->print("dummy1 gets semaphore: ");
     ret = my_semaphore.get(&dummy1);
-    print_true_false(ret);
+    print_test_result(true, ret);
 
     // dummy2 tries to get semaphore (fails)
-    Serial.print("dummy2 gets semaphore: ");
+    hal.console->print("dummy2 gets semaphore: ");
     ret = my_semaphore.get(&dummy2);
-    print_true_false(ret);
+    print_test_result(false, ret);
 
     // dummy2 tries to release semaphore (fails)
-    Serial.print("dummy2 releases semaphore (that it doesn't have): ");
+    hal.console->print("dummy2 releases semaphore (that it doesn't have): ");
     ret = my_semaphore.release(&dummy2);
-    print_true_false(ret);
+    print_test_result(false, ret);
 
     // dummy1 releases semaphore
-    Serial.print("dummy1 releases semaphore: ");
+    hal.console->print("dummy1 releases semaphore: ");
     ret = my_semaphore.release(&dummy1);
-    print_true_false(ret);
+    print_test_result(true, ret);
 
     // dummy2 tries to get semaphore (succeeds)
-    Serial.print("dummy2 gets semaphore: ");
+    hal.console->print("dummy2 gets semaphore: ");
     ret = my_semaphore.get(&dummy2);
-    print_true_false(ret);
+    print_test_result(true, ret);
 
     // dummy1 tries to get semphore (fails)
-    Serial.print("dummy1 gets semaphore: ");
+    hal.console->print("dummy1 gets semaphore: ");
     ret = my_semaphore.get(&dummy1);
-    print_true_false(ret);
+    print_test_result(false, ret);
     
     // dummy1 asks for call back (succeeds)
-    Serial.print("dummy1 asks for call back on release: ");
+    hal.console->print("dummy1 asks for call back on release: ");
     ret = my_semaphore.call_on_release(&dummy1, callback_1);
-    print_true_false(ret);
+    print_test_result(true, ret);
     
     // dummy3 asks for call back (fails)
-    Serial.print("dummy3 asks for call back on release: ");
+    hal.console->print("dummy3 asks for call back on release: ");
     ret = my_semaphore.call_on_release(&dummy3, callback_3);
-    print_true_false(ret);
+    print_test_result(false, ret);
 
     // dummy2 releases semaphore
     // dummy1's call back should be called
-    Serial.print("dummy2 releases semaphore: ");
+    hal.console->print("dummy2 releases semaphore: ");
     ret = my_semaphore.release(&dummy2);
-    print_true_false(ret);
+    print_test_result(true, ret);
 
-    Serial.println("--------------------");
+    hal.console->println("--------------------");
 
     // nobody has semaphore
 
     // delay
-    delay(10000);
+    hal.scheduler->delay(10000);
 }
 
+extern "C" {
+int main (void) {
+    hal.init(NULL);
+    setup();
+    for(;;) loop();
+    return 0;
+}
+}