AP_Notify: use WITH_SEMAPHORE()

and removed usage of hal.util->new_semaphore()

libraries/AP_Notify/ToneAlarm.cpp

@@ -18,6 +18,7 @@
 
 #include <AP_HAL/AP_HAL.h>
 #include <AP_Math/AP_Math.h>
+#include <AP_Common/Semaphore.h>
 
 #include "ToneAlarm.h"
 #include "AP_Notify.h"
@@ -100,8 +101,6 @@ bool AP_ToneAlarm::init()
         return false;
     }
 
-    _sem = hal.util->new_semaphore();
-
     // set initial boot states. This prevents us issuing a arming
     // warning in plane and rover on every boot
     flags.armed = AP_Notify::flags.armed;
@@ -131,21 +130,18 @@ void AP_ToneAlarm::play_tone(const uint8_t tone_index)
 
 void AP_ToneAlarm::_timer_task()
 {
-    if (_sem && _sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
-        _mml_player.update();
-        _sem->give();
-    }
+    WITH_SEMAPHORE(_sem);
+    _mml_player.update();
 }
 
 void AP_ToneAlarm::play_string(const char *str)
 {
-    if (_sem && _sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
-        _mml_player.stop();
-        strncpy(_tone_buf, str, AP_NOTIFY_TONEALARM_TONE_BUF_SIZE);
-        _tone_buf[AP_NOTIFY_TONEALARM_TONE_BUF_SIZE-1] = 0;
-        _mml_player.play(_tone_buf);
-        _sem->give();
-    }
+    WITH_SEMAPHORE(_sem);
+
+    _mml_player.stop();
+    strncpy(_tone_buf, str, AP_NOTIFY_TONEALARM_TONE_BUF_SIZE);
+    _tone_buf[AP_NOTIFY_TONEALARM_TONE_BUF_SIZE-1] = 0;
+    _mml_player.play(_tone_buf);
 }
 
 void AP_ToneAlarm::stop_cont_tone()
@@ -390,17 +386,16 @@ void AP_ToneAlarm::handle_play_tune(mavlink_message_t *msg)
 
     mavlink_msg_play_tune_decode(msg, &packet);
 
-    if (_sem && _sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
-        _mml_player.stop();
+    WITH_SEMAPHORE(_sem);
 
-        strncpy(_tone_buf, packet.tune, MIN(sizeof(packet.tune), sizeof(_tone_buf)-1));
-        _tone_buf[sizeof(_tone_buf)-1] = 0;
-        uint8_t len = strlen(_tone_buf);
-        uint8_t len2 = strnlen(packet.tune2, sizeof(packet.tune2));
-        len2 = MIN((sizeof(_tone_buf)-1)-len, len2);
-        strncpy(_tone_buf+len, packet.tune2, len2);
-        _tone_buf[sizeof(_tone_buf)-1] = 0;
-        _mml_player.play(_tone_buf);
-        _sem->give();
-    }
+    _mml_player.stop();
+
+    strncpy(_tone_buf, packet.tune, MIN(sizeof(packet.tune), sizeof(_tone_buf)-1));
+    _tone_buf[sizeof(_tone_buf)-1] = 0;
+    uint8_t len = strlen(_tone_buf);
+    uint8_t len2 = strnlen(packet.tune2, sizeof(packet.tune2));
+    len2 = MIN((sizeof(_tone_buf)-1)-len, len2);
+    strncpy(_tone_buf+len, packet.tune2, len2);
+    _tone_buf[sizeof(_tone_buf)-1] = 0;
+    _mml_player.play(_tone_buf);
 }

libraries/AP_Notify/ToneAlarm.h

@@ -77,7 +77,7 @@ private:
 
     const static Tone _tones[];
 
-    AP_HAL::Semaphore* _sem;
+    HAL_Semaphore _sem;
     MMLPlayer _mml_player;
     char _tone_buf[AP_NOTIFY_TONEALARM_TONE_BUF_SIZE];
 };

libraries/AP_Notify/ToshibaLED_I2C.cpp

@@ -23,6 +23,7 @@
 #include <utility>
 
 #include <AP_HAL/AP_HAL.h>
+#include <AP_Common/Semaphore.h>
 
 extern const AP_HAL::HAL& hal;
 
@@ -48,16 +49,16 @@ bool ToshibaLED_I2C::hw_init(void)
 {
     // first look for led on external bus
     _dev = std::move(hal.i2c_mgr->get_device(_bus, TOSHIBA_LED_I2C_ADDR));
-    if (!_dev || !_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
+    if (!_dev) {
         return false;
     }
+    WITH_SEMAPHORE(_dev->get_semaphore());
 
     _dev->set_retries(10);
 
     // enable the led
     bool ret = _dev->write_register(TOSHIBA_LED_ENABLE, 0x03);
     if (!ret) {
-        _dev->get_semaphore()->give();
         return false;
     }
 
@@ -67,9 +68,6 @@ bool ToshibaLED_I2C::hw_init(void)
 
     _dev->set_retries(1);
 
-    // give back i2c semaphore
-    _dev->get_semaphore()->give();
-
     if (ret) {
         _dev->register_periodic_callback(20000, FUNCTOR_BIND_MEMBER(&ToshibaLED_I2C::_timer, void));
     }