diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index ae64aba2e0..874f2d2ab0 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/libraries/SITL/SIM_Aircraft.cpp @@ -731,6 +731,11 @@ void Aircraft::update_external_payload(const struct sitl_input &input) external_payload_mass += sprayer->payload_mass(); } + // update buzzer + if (buzzer && buzzer->is_enabled()) { + buzzer->update(input); + } + // update grippers if (gripper && gripper->is_enabled()) { gripper->set_alt(hagl()); diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h index 7eb4966dd9..c65f87c702 100644 --- a/libraries/SITL/SIM_Aircraft.h +++ b/libraries/SITL/SIM_Aircraft.h @@ -28,6 +28,7 @@ #include "SIM_Gripper_EPM.h" #include "SIM_Parachute.h" #include "SIM_Precland.h" +#include "SIM_Buzzer.h" #include namespace SITL { @@ -121,6 +122,7 @@ public: const Location &get_home() const { return home; } float get_home_yaw() const { return home_yaw; } + void set_buzzer(Buzzer *_buzzer) { buzzer = _buzzer; } void set_sprayer(Sprayer *_sprayer) { sprayer = _sprayer; } void set_parachute(Parachute *_parachute) { parachute = _parachute; } void set_gripper_servo(Gripper_Servo *_gripper) { gripper = _gripper; } @@ -272,6 +274,7 @@ private: LowPassFilterFloat servo_filter[4]; + Buzzer *buzzer; Sprayer *sprayer; Gripper_Servo *gripper; Gripper_EPM *gripper_epm; diff --git a/libraries/SITL/SIM_Buzzer.cpp b/libraries/SITL/SIM_Buzzer.cpp new file mode 100644 index 0000000000..39f0cbd3e4 --- /dev/null +++ b/libraries/SITL/SIM_Buzzer.cpp @@ -0,0 +1,133 @@ +/* + 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 . + */ +/* + simple buzzer simulator class +*/ + + +#include +#include + +#include "SIM_Buzzer.h" + +#ifdef WITH_SITL_TONEALARM + +#ifdef HAVE_SFML_AUDIO_H +#include +#else +#include +#endif + +// need to namespace this or we end up with multiple definitions of +// things from Synth.hpp +namespace BuzzerSynth { +#include +}; + +#endif + +using namespace SITL; + +// table of user settable parameters +const AP_Param::GroupInfo Buzzer::var_info[] = { + + // @Param: ENABLE + // @DisplayName: Buzzer enable/disable + // @Description: Allows you to enable (1) or disable (0) the simulated buzzer + // @Values: 0:Disabled,1:Enabled + // @User: Advanced + AP_GROUPINFO("ENABLE", 0, Buzzer, _enable, 0), + + // @Param: PIN + // @DisplayName: buzzer pin + // @Description: The pin number that the Buzzer is connected to (start at 1) + // @Range: 0 15 + // @User: Advanced + AP_GROUPINFO("PIN", 1, Buzzer, _pin, -1), + + AP_GROUPEND +}; + +#ifdef WITH_SITL_TONEALARM + +static sf::SoundBuffer xsoundBuffer; +static sf::Sound xdemoSound; + +static uint32_t duration_ms = 10000; + +Buzzer::Buzzer() { + AP_Param::setup_object_defaults(this, var_info); +}; + +void Buzzer::update(const struct sitl_input &input) +{ + // prepare buzz tone +// BuzzerSynth::Synth::sEnvelope xenvelope; + if (!prep_done) { + BuzzerSynth::Synth::sEnvelope xenvelope; + xenvelope.dAttackTime = 0.000001; + xenvelope.dDecayTime = 0.000001; + xenvelope.dSustainTime = 10; + xenvelope.dReleaseTime = 0.00001; + xenvelope.dStartAmplitude = 1.0; + xenvelope.dSustainAmplitude = 1.0; + + const uint32_t frequency = 2000; + BuzzerSynth::Synth::sTone tone; + tone.waveType = BuzzerSynth::Synth::OSC_SQUARE; + tone.dStartFrequency = frequency; + tone.dEndFrequency = frequency; + tone.dAmplitude = 1; + + xenvelope.dSustainTime = duration_ms/1000.0f; + + BuzzerSynth::Synth::generate(&xsoundBuffer, xenvelope, tone, 20000, 44100); + xdemoSound.setBuffer(xsoundBuffer); + prep_done = true; + } + + const bool on = _pin >= 1 && (AP::sitl()->pin_mask.get() & (1<<_pin)); + const uint32_t now = AP_HAL::millis(); + if (on) { + if (!was_on) { + gcs().send_text(MAV_SEVERITY_WARNING, "%u: Buzzer on", now); + on_time = now; + was_on = true; + xdemoSound.play(); + } + if (now - on_time > duration_ms/2) { + gcs().send_text(MAV_SEVERITY_WARNING, "%u: Buzzer on again", now); + on_time = now; + xdemoSound.play(); + } + } else { + if (was_on) { + gcs().send_text(MAV_SEVERITY_WARNING, "%u: Buzzer off", now); + xdemoSound.stop(); + was_on = false; + } + } + +} + +#else + +using namespace SITL; + +Buzzer::Buzzer() { }; + +void Buzzer::update(const struct sitl_input &input) {} + +#endif diff --git a/libraries/SITL/SIM_Buzzer.h b/libraries/SITL/SIM_Buzzer.h new file mode 100644 index 0000000000..74c2309472 --- /dev/null +++ b/libraries/SITL/SIM_Buzzer.h @@ -0,0 +1,50 @@ +/* + 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 . + */ +/* + simple buzzer simulation class +*/ + +#pragma once + +#include +#include "SITL_Input.h" + +#include "stdint.h" + +namespace SITL { + +class Buzzer { +public: + Buzzer(); + + // update buzzer state + void update(const struct sitl_input &input); + + static const struct AP_Param::GroupInfo var_info[]; + + bool is_enabled() const {return static_cast(_enable);} + + private: + + AP_Int8 _enable; // enable buzzer sim + AP_Int8 _pin; + bool was_on; + + uint32_t on_time; + + bool prep_done; +}; + +} diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp index 1cb35a078a..7f5e204c8f 100644 --- a/libraries/SITL/SITL.cpp +++ b/libraries/SITL/SITL.cpp @@ -189,6 +189,9 @@ const AP_Param::GroupInfo SITL::var_info2[] = { // extra delay per main loop AP_GROUPINFO("LOOP_DELAY", 55, SITL, loop_delay, 0), + // @Path: ./SIM_Buzzer.cpp + AP_SUBGROUPINFO(buzzer_sim, "BZ_", 56, SITL, Buzzer), + AP_GROUPINFO("MAG_SCALING", 60, SITL, mag_scaling, 1), AP_GROUPEND diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h index 8b870a047a..47a17d57bc 100644 --- a/libraries/SITL/SITL.h +++ b/libraries/SITL/SITL.h @@ -4,6 +4,7 @@ #include #include +#include "SIM_Buzzer.h" #include "SIM_Sprayer.h" #include "SIM_Gripper_Servo.h" #include "SIM_Gripper_EPM.h" @@ -298,6 +299,7 @@ public: Gripper_EPM gripper_epm_sim; Parachute parachute_sim; + Buzzer buzzer_sim; SIM_Precland precland_sim; };