From 60ba91aead78a4184fee216ec51d9c1689d8a9eb Mon Sep 17 00:00:00 2001 From: Mathieu OTHACEHE Date: Sat, 29 Oct 2016 10:23:31 +0200 Subject: [PATCH] Disco: add buzzer support Use ToneAlarm class to handle Disco buzzer. --- libraries/AP_HAL_Linux/RCOutput_Bebop.cpp | 34 +++++++++ libraries/AP_HAL_Linux/RCOutput_Bebop.h | 1 + libraries/AP_HAL_Linux/ToneAlarm_Disco.cpp | 82 ++++++++++++++++++++++ libraries/AP_HAL_Linux/ToneAlarm_Disco.h | 24 +++++++ libraries/AP_HAL_Linux/Util.cpp | 3 + libraries/AP_HAL_Linux/Util.h | 4 ++ libraries/AP_Notify/AP_Notify.cpp | 3 +- 7 files changed, 150 insertions(+), 1 deletion(-) create mode 100644 libraries/AP_HAL_Linux/ToneAlarm_Disco.cpp create mode 100644 libraries/AP_HAL_Linux/ToneAlarm_Disco.h diff --git a/libraries/AP_HAL_Linux/RCOutput_Bebop.cpp b/libraries/AP_HAL_Linux/RCOutput_Bebop.cpp index 472a7bebc5..2ae9a7b731 100644 --- a/libraries/AP_HAL_Linux/RCOutput_Bebop.cpp +++ b/libraries/AP_HAL_Linux/RCOutput_Bebop.cpp @@ -283,6 +283,40 @@ void RCOutput_Bebop::_play_sound(uint8_t sound) _dev->get_semaphore()->give(); } +/* + * pwm is the pwm power used for the note. + * It has to be >= 3, otherwise it refers to a predefined song + * (see _play_sound function) + * period is in us and duration in ms. + */ +void RCOutput_Bebop::play_note(uint8_t pwm, + uint16_t period_us, + uint16_t duration_ms) +{ + struct PACKED { + uint8_t header; + uint8_t pwm; + be16_t period; + be16_t duration; + } msg; + + if (pwm < 3) { + return; + } + + msg.header = BEBOP_BLDC_PLAY_SOUND; + msg.pwm = pwm; + msg.period = htobe16(period_us); + msg.duration = htobe16(duration_ms); + + if (!_dev->get_semaphore()->take(0)) { + return; + } + + _dev->transfer((uint8_t *)&msg, sizeof(msg), nullptr, 0); + _dev->get_semaphore()->give(); +} + uint16_t RCOutput_Bebop::_period_us_to_rpm(uint16_t period_us) { period_us = constrain_int16(period_us, _min_pwm, _max_pwm); diff --git a/libraries/AP_HAL_Linux/RCOutput_Bebop.h b/libraries/AP_HAL_Linux/RCOutput_Bebop.h index b213ced292..d8890ffa97 100644 --- a/libraries/AP_HAL_Linux/RCOutput_Bebop.h +++ b/libraries/AP_HAL_Linux/RCOutput_Bebop.h @@ -73,6 +73,7 @@ public: void read(uint16_t* period_us, uint8_t len) override; void set_esc_scaling(uint16_t min_pwm, uint16_t max_pwm) override; int read_obs_data(BebopBLDC_ObsData &data); + void play_note(uint8_t pwm, uint16_t period_us, uint16_t duration_ms); private: AP_HAL::OwnPtr _dev; diff --git a/libraries/AP_HAL_Linux/ToneAlarm_Disco.cpp b/libraries/AP_HAL_Linux/ToneAlarm_Disco.cpp new file mode 100644 index 0000000000..fa23a78ee8 --- /dev/null +++ b/libraries/AP_HAL_Linux/ToneAlarm_Disco.cpp @@ -0,0 +1,82 @@ +/* + Copyright (C) 2016 Mathieu Othacehe. All rights reserved. + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +#include +#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO +#include +#include +#include "ToneAlarm_Disco.h" + +extern const AP_HAL::HAL &hal; + +using namespace Linux; + +ToneAlarm_Disco::ToneAlarm_Disco() +{ + // initialy no tune to play + tune_num = -1; + tune_pos = 0; +} + +bool ToneAlarm_Disco::init() +{ + // play startup tune + tune_num = 0; + + bebop_out = RCOutput_Disco::from(hal.rcout); + + return true; +} + +void ToneAlarm_Disco::stop() +{ + bebop_out->play_note(0, 0, 0); +} + +bool ToneAlarm_Disco::play() +{ + uint32_t cur_time = AP_HAL::millis(); + + if (tune_num != prev_tune_num){ + tune_changed = true; + return true; + } + + if (cur_note != 0){ + bebop_out->play_note(TONEALARM_PWM_POWER, cur_note, duration); + cur_note = 0; + prev_time = cur_time; + } + + if ((cur_time - prev_time) > duration){ + stop(); + if (tune[tune_num][tune_pos] == '\0'){ + if (!tune_repeat[tune_num]){ + tune_num = -1; + } + + tune_pos = 0; + tune_comp = true; + return false; + } + return true; + } + + return false; +} + +#endif diff --git a/libraries/AP_HAL_Linux/ToneAlarm_Disco.h b/libraries/AP_HAL_Linux/ToneAlarm_Disco.h new file mode 100644 index 0000000000..e237c00c93 --- /dev/null +++ b/libraries/AP_HAL_Linux/ToneAlarm_Disco.h @@ -0,0 +1,24 @@ +#pragma once + +#include "AP_HAL_Linux.h" +#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO +#include +#include "ToneAlarm.h" + +namespace Linux { + +#define TONEALARM_PWM_POWER 20 + +class ToneAlarm_Disco : public ToneAlarm { +public: + ToneAlarm_Disco(); + bool init() override; + void stop() override; + bool play() override; + +private: + RCOutput_Bebop *bebop_out; +}; + +} +#endif diff --git a/libraries/AP_HAL_Linux/Util.cpp b/libraries/AP_HAL_Linux/Util.cpp index 9234c8dda3..afafdd8bbe 100644 --- a/libraries/AP_HAL_Linux/Util.cpp +++ b/libraries/AP_HAL_Linux/Util.cpp @@ -11,6 +11,7 @@ #include "Heat_Pwm.h" #include "ToneAlarm_Raspilot.h" +#include "ToneAlarm_Disco.h" #include "Util.h" using namespace Linux; @@ -20,6 +21,8 @@ extern const AP_HAL::HAL& hal; static int state; #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT ToneAlarm_Raspilot Util::_toneAlarm; +#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO +ToneAlarm_Disco Util::_toneAlarm; #else ToneAlarm Util::_toneAlarm; #endif diff --git a/libraries/AP_HAL_Linux/Util.h b/libraries/AP_HAL_Linux/Util.h index e9c7b867d9..de8c481599 100644 --- a/libraries/AP_HAL_Linux/Util.h +++ b/libraries/AP_HAL_Linux/Util.h @@ -7,6 +7,8 @@ #include "Perf.h" #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT #include "ToneAlarm_Raspilot.h" +#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO +#include "ToneAlarm_Disco.h" #endif #include "ToneAlarm.h" #include "Semaphores.h" @@ -99,6 +101,8 @@ public: private: #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT static ToneAlarm_Raspilot _toneAlarm; +#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO + static ToneAlarm_Disco _toneAlarm; #else static ToneAlarm _toneAlarm; #endif diff --git a/libraries/AP_Notify/AP_Notify.cpp b/libraries/AP_Notify/AP_Notify.cpp index 42519c3315..a1f03781be 100644 --- a/libraries/AP_Notify/AP_Notify.cpp +++ b/libraries/AP_Notify/AP_Notify.cpp @@ -127,7 +127,8 @@ struct AP_Notify::notify_events_type AP_Notify::events; NotifyDevice *AP_Notify::_devices[] = {&boardled, &bhled}; #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO DiscoLED discoled; - NotifyDevice *AP_Notify::_devices[] = {&discoled}; + ToneAlarm_Linux tonealarm; + NotifyDevice *AP_Notify::_devices[] = {&discoled, &tonealarm}; #else AP_BoardLED boardled; ToshibaLED_I2C toshibaled;