Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-25 09:13:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Notify: use WITH_SEMAPHORE()

Browse Source 
and removed usage of hal.util->new_semaphore()
This commit is contained in:
Andrew Tridgell 2018-10-12 10:35:04 +11:00
parent dedfa54d5a
commit bf829cd792
3 changed files with 24 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
libraries/AP_Notify/ToneAlarm.cpp
View File

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

2
libraries/AP_Notify/ToneAlarm.h
View File

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

8
libraries/AP_Notify/ToshibaLED_I2C.cpp
View File

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