AP_HAL_SITL: add support for ToneAlarm via sfml

libraries/AP_HAL_SITL/Synth.hpp

@@ -0,0 +1,192 @@
+// See https://github.com/OneLoneCoder/synth
+
+#ifndef SYNTH_HPP
+#define SYNTH_HPP
+
+////////////////////////////////////////////////////////////
+// Headers
+////////////////////////////////////////////////////////////
+#include <SFML/Audio.hpp>
+#include <cmath>
+
+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<sTone> 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<sTone> tones;
+    tones.push_back(tone);
+    return generate(buffer, env, tones, uMasterVol, uSampleRate);
+}
+}
+#endif  // SYNTH_HPP

libraries/AP_HAL_SITL/ToneAlarm_SF.cpp

@@ -0,0 +1,53 @@
+#ifdef WITH_SITL_TONEALARM
+
+#include "ToneAlarm_SF.h"
+
+#include <stdio.h>
+
+#ifdef HAVE_SFML_AUDIO_H
+#include <SFML/Audio.h>
+#else
+#include <SFML/Audio.hpp>
+#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

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();
+    };
+}

libraries/AP_HAL_SITL/Util.cpp

@@ -1,6 +1,10 @@
 #include "Util.h"
 #include <sys/time.h>
 
+#ifdef WITH_SITL_TONEALARM
+HALSITL::ToneAlarm_SF HALSITL::Util::_toneAlarm;
+#endif
+
 uint64_t HALSITL::Util::get_hw_rtc() const
 {
 #ifndef CLOCK_REALTIME

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