diff --git a/libraries/AP_Common/AP_Semaphore.cpp b/libraries/AP_Common/AP_Semaphore.cpp new file mode 100644 index 0000000000..69a1f22036 --- /dev/null +++ b/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 +#include +#include +} +#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; +} \ No newline at end of file diff --git a/libraries/AP_Common/AP_Semaphore.h b/libraries/AP_Common/AP_Semaphore.h new file mode 100644 index 0000000000..e7ee8cc99d --- /dev/null +++ b/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 +#include +#include // 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 \ No newline at end of file diff --git a/libraries/AP_Common/examples/AP_Semaphore_test/AP_Semaphore_test.pde b/libraries/AP_Common/examples/AP_Semaphore_test/AP_Semaphore_test.pde new file mode 100644 index 0000000000..577667bc49 --- /dev/null +++ b/libraries/AP_Common/examples/AP_Semaphore_test/AP_Semaphore_test.pde @@ -0,0 +1,102 @@ + +#include +#include +#include +#include + +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); +} +