SITL: add a simulated buzzer

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

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 <Filter/Filter.h>
 
 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;

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 <http://www.gnu.org/licenses/>.
+ */
+/*
+  simple buzzer simulator class
+*/
+
+
+#include <GCS_MAVLink/GCS.h>
+#include <SITL/SITL.h>
+
+#include "SIM_Buzzer.h"
+
+#ifdef WITH_SITL_TONEALARM
+
+#ifdef HAVE_SFML_AUDIO_H
+#include <SFML/Audio.h>
+#else
+#include <SFML/Audio.hpp>
+#endif
+
+// need to namespace this or we end up with multiple definitions of
+// things from Synth.hpp
+namespace BuzzerSynth {
+#include <AP_HAL_SITL/Synth.hpp>
+};
+
+#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

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 <http://www.gnu.org/licenses/>.
+ */
+/*
+  simple buzzer simulation class
+*/
+
+#pragma once
+
+#include <AP_Param/AP_Param.h>
+#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<bool>(_enable);}
+
+ private:
+
+    AP_Int8  _enable;  // enable buzzer sim
+    AP_Int8  _pin;
+    bool was_on;
+
+    uint32_t on_time;
+
+    bool prep_done;
+};
+
+}

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_GROUPEND
 
 };

libraries/SITL/SITL.h

@@ -4,6 +4,7 @@
 #include <GCS_MAVLink/GCS_MAVLink.h>
 #include <AP_Common/Location.h>
 
+#include "SIM_Buzzer.h"
 #include "SIM_Sprayer.h"
 #include "SIM_Gripper_Servo.h"
 #include "SIM_Gripper_EPM.h"
@@ -297,6 +298,7 @@ public:
     Gripper_EPM gripper_epm_sim;
 
     Parachute parachute_sim;
+    Buzzer buzzer_sim;
     SIM_Precland precland_sim;
 };