diff --git a/libraries/AP_HAL_SITL/Synth.hpp b/libraries/AP_HAL_SITL/Synth.hpp new file mode 100644 index 0000000000..c0d82b0e0b --- /dev/null +++ b/libraries/AP_HAL_SITL/Synth.hpp @@ -0,0 +1,192 @@ +// See https://github.com/OneLoneCoder/synth + +#ifndef SYNTH_HPP +#define SYNTH_HPP + +//////////////////////////////////////////////////////////// +// Headers +//////////////////////////////////////////////////////////// +#include +#include + +namespace Synth +{ +enum eWaveType { OSC_SINE, OSC_SQUARE, OSC_TRIANGLE, OSC_SAW_ANA, OSC_SAW_DIG, OSC_NOISE }; + +struct sEnvelope +{ + double dAttackTime = 0.0; + double dDecayTime = 0.0; + double dSustainTime = 1.0; + double dReleaseTime = 0.0; + double dStartAmplitude = 1.0; + double dSustainAmplitude = 1.0; +}; +struct sTone +{ + eWaveType waveType = OSC_SINE; + double dStartFrequency = 440.0; + double dEndFrequency = 440.0; + double dAmplitude = 1.0; +}; + +//////////////////////////////////////////////////////////// +// Converts frequency (Hz) to angular velocity +//////////////////////////////////////////////////////////// +const double PI = 3.14159265359; +double w(const double dHertz) +{ + return dHertz * 2.0 * PI; +} + + +//////////////////////////////////////////////////////////// +// Multi-Function Oscillator +// +// This function was mostly "borrowed" from One Lone Coder blog at +// wwww.onelonecoder.com +// +//////////////////////////////////////////////////////////// +double osc(double dHertz, double dTime, eWaveType waveType) +{ + switch (waveType) + { + case OSC_SINE: // Sine wave bewteen -1 and +1 + return sin(w(dHertz) * dTime); + + case OSC_SQUARE: // Square wave between -1 and +1 + return sin(w(dHertz) * dTime) > 0 ? 1.0 : -1.0; + + case OSC_TRIANGLE: // Triangle wave between -1 and +1 + return asin(sin(w(dHertz) * dTime)) * (2.0 / PI); + + case OSC_SAW_ANA: // Saw wave (analogue / warm / slow) + { + double dOutput = 0.0; + // this variable defines warmth, larger the number harsher and more similar do OSC_SAW_DIG sound becomes + double dWarmth = 30.0; + + for (double n = 1.0; n < dWarmth; n++) + dOutput += (sin(n * w(dHertz) * dTime)) / n; + + return dOutput * (2.0 / PI); + } + + case OSC_SAW_DIG: // Saw Wave (optimised / harsh / fast) + return (2.0 / PI) * (dHertz * PI * fmod(dTime, 1.0 / dHertz) - (PI / 2.0)); + + case OSC_NOISE: // Pseudorandom noise + return 2.0 * ((double)rand() / (double)RAND_MAX) - 1.0; + + default: + return 0.0; + } +} + +//////////////////////////////////////////////////////////// +// Amplitude Modulator +//////////////////////////////////////////////////////////// +double amplitude(double dTime, sEnvelope env) +{ + // double dTimeOn = 0; + double dTimeOff = env.dAttackTime + env.dDecayTime + env.dSustainTime; + double dAmplitude = 0.0; + + if (dTime > dTimeOff) // Release phase + dAmplitude = ((dTime - dTimeOff) / env.dReleaseTime) * (0.0 - env.dSustainAmplitude) + env.dSustainAmplitude; + + else if (dTime > (env.dAttackTime + env.dDecayTime)) // Sustain phase + dAmplitude = env.dSustainAmplitude; + + else if (dTime > env.dAttackTime && dTime <= (env.dAttackTime + env.dDecayTime)) // Decay phase + dAmplitude = ((dTime - env.dAttackTime) / env.dDecayTime) * (env.dSustainAmplitude - env.dStartAmplitude) + env.dStartAmplitude; + + else if (dTime <= env.dAttackTime) // Attack phase + dAmplitude = (dTime / env.dAttackTime) * env.dStartAmplitude; + + // Amplitude should not be negative, check just in case + if (dAmplitude <= 0.000) + dAmplitude = 0.0; + + return dAmplitude; +} + + + +//////////////////////////////////////////////////////////// +/// \brief Generate sound and store in SoundBuffer +/// +/// This function uses case-in +/// +/// \param buffer is address to SoundBuffer where the result will be stored +/// \param envelope structure defining the ADSR Envelope +/// \param tones vector of tone structures to be stacked +/// \param master volume for the volume of sound +/// \param sample rate to set quality of sound +/// +/// \return True if the sound was generate, false if it failed +/// +//////////////////////////////////////////////////////////// +bool generate(sf::SoundBuffer* buffer, sEnvelope env, std::vector tones, unsigned uMasterVol, unsigned uSampleRate) +{ + if (!buffer) + return false; + + // Calculate and allocate buffer + double dTotalDuration = env.dAttackTime + env.dDecayTime + env.dSustainTime + env.dReleaseTime; + unsigned iBufferSize = unsigned(dTotalDuration * uSampleRate); + sf::Int16 * iRaw; + iRaw = new sf::Int16[iBufferSize]; + + // Generate sound + double dIncrement = 1.0 / double(uSampleRate); + double dTime = 0.0; + for (unsigned i = 0; i < iBufferSize; i++) + { + double signal = 0.0; + + // Generate multiple tones and stack them together + for (sTone t : tones) + { + double dFrequency = t.dStartFrequency + ((t.dEndFrequency - t.dStartFrequency) * (double(i) / double(iBufferSize))); + signal = signal + (osc(dFrequency, dTime, t.waveType) * t.dAmplitude); + } + + // Calculate Amplitude based on ADSR envelope + double dEnvelopeAmplitude = amplitude(dTime, env); + + // Apply master volume, envelope and store to buffer + *(iRaw + i) = sf::Int16(uMasterVol * signal * dEnvelopeAmplitude); + + dTime += dIncrement; + } + + // Load into SFML SoundBuffer + bool bSuccess = buffer->loadFromSamples(iRaw, iBufferSize, 1, uSampleRate); + delete[] iRaw; + + return bSuccess; +} + +//////////////////////////////////////////////////////////// +/// \brief Generate sound and store in SoundBuffer +/// +/// This function uses case-in +/// +/// \param buffer is address to SoundBuffer where the result will be stored +/// \param envelope structure defining the ADSR Envelope +/// \param tone structure for simple tone definition +/// \param master volume for the volume of sound +/// \param sample rate to set quality of sound +/// +/// \return True if the sound was generate, false if it failed +/// +//////////////////////////////////////////////////////////// +bool generate(sf::SoundBuffer* buffer, sEnvelope env, sTone tone, unsigned uMasterVol, unsigned uSampleRate) +{ + std::vector tones; + tones.push_back(tone); + return generate(buffer, env, tones, uMasterVol, uSampleRate); +} +} +#endif // SYNTH_HPP diff --git a/libraries/AP_HAL_SITL/ToneAlarm_SF.cpp b/libraries/AP_HAL_SITL/ToneAlarm_SF.cpp new file mode 100644 index 0000000000..66e43682fe --- /dev/null +++ b/libraries/AP_HAL_SITL/ToneAlarm_SF.cpp @@ -0,0 +1,53 @@ +#ifdef WITH_SITL_TONEALARM + +#include "ToneAlarm_SF.h" + +#include + +#ifdef HAVE_SFML_AUDIO_H +#include +#else +#include +#endif + +#include "Synth.hpp" + +using namespace HALSITL; + +sf::SoundBuffer soundBuffer; +sf::Sound demoSound; +Synth::sEnvelope envelope; + +void ToneAlarm_SF::set_buzzer_tone(float frequency, float volume, float duration_ms) +{ + if (frequency <= 0) { + return; + } + + Synth::sTone tone; + tone.waveType = Synth::OSC_SQUARE; + tone.dStartFrequency = frequency; + tone.dEndFrequency = frequency; + tone.dAmplitude = 1; + + envelope.dSustainTime = duration_ms/1000.0f; + + Synth::generate(&soundBuffer, envelope, tone, 20000, 44100); + demoSound.setBuffer(soundBuffer); + demoSound.play(); +} + +bool ToneAlarm_SF::init() +{ + // LASER + envelope.dAttackTime = 0.00001; + envelope.dDecayTime = 0.00001; + envelope.dSustainTime = 0.3; + envelope.dReleaseTime = 0.00001; + envelope.dStartAmplitude = 1.0; + envelope.dSustainAmplitude = 1.0; + + return true; +} + +#endif diff --git a/libraries/AP_HAL_SITL/ToneAlarm_SF.h b/libraries/AP_HAL_SITL/ToneAlarm_SF.h new file mode 100644 index 0000000000..fe39beceba --- /dev/null +++ b/libraries/AP_HAL_SITL/ToneAlarm_SF.h @@ -0,0 +1,11 @@ +#pragma once + +#include "AP_HAL_SITL.h" + +namespace HALSITL { + class ToneAlarm_SF { + public: + void set_buzzer_tone(float frequency, float volume, float duration_ms); + bool init(); + }; +} diff --git a/libraries/AP_HAL_SITL/Util.cpp b/libraries/AP_HAL_SITL/Util.cpp index 2c3c6e8d89..d7f8369cb3 100644 --- a/libraries/AP_HAL_SITL/Util.cpp +++ b/libraries/AP_HAL_SITL/Util.cpp @@ -1,6 +1,10 @@ #include "Util.h" #include +#ifdef WITH_SITL_TONEALARM +HALSITL::ToneAlarm_SF HALSITL::Util::_toneAlarm; +#endif + uint64_t HALSITL::Util::get_hw_rtc() const { #ifndef CLOCK_REALTIME diff --git a/libraries/AP_HAL_SITL/Util.h b/libraries/AP_HAL_SITL/Util.h index 5a69f8d7b0..87bfec126d 100644 --- a/libraries/AP_HAL_SITL/Util.h +++ b/libraries/AP_HAL_SITL/Util.h @@ -4,6 +4,7 @@ #include "AP_HAL_SITL_Namespace.h" #include "AP_HAL_SITL.h" #include "Semaphores.h" +#include "ToneAlarm_SF.h" class HALSITL::Util : public AP_HAL::Util { public: @@ -37,10 +38,21 @@ public: virtual void *allocate_heap_memory(size_t size); virtual void *heap_realloc(void *heap, void *ptr, size_t new_size); #endif // ENABLE_HEAP - + +#ifdef WITH_SITL_TONEALARM + bool toneAlarm_init() override { return _toneAlarm.init(); } + void toneAlarm_set_buzzer_tone(float frequency, float volume, uint32_t duration_ms) override { + _toneAlarm.set_buzzer_tone(frequency, volume, duration_ms); + } +#endif + private: SITL_State *sitlState; +#ifdef WITH_SITL_TONEALARM + static ToneAlarm_SF _toneAlarm; +#endif + #ifdef ENABLE_HEAP struct heap_allocation_header { size_t allocation_size; // size of allocated block, not including this header