Merge pull request #170 from Sharganov/dev_playTone

implemented a tone generator

trikControl/src/audioSynthDevices.cpp

@@ -0,0 +1,129 @@
+/* Copyright 2016 Artem Sharganov and Iakov Kirilenko
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License. */
+
+#include "audioSynthDevices.h"
+
+#include <QtMultimedia/QAudioDeviceInfo>
+
+AudioSynthDevice::AudioSynthDevice(QObject *parent, int sampleRate, int sampleSize)
+	: QIODevice(parent)
+	, mBuffer(0)
+	, mPos(0)
+	, mSampleRate(sampleRate)
+	, mSampleSize(sampleSize)
+{
+
+}
+
+AudioSynthDevice::~AudioSynthDevice()
+{
+
+}
+
+void AudioSynthDevice::start(int hzFreq)
+{
+	open(QIODevice::ReadOnly);
+	if(mBuffered) {
+		qint64 length = (mSampleRate * (mSampleSize / 8));
+		mBuffer.resize(length);
+		generate(mBuffer.data(), length, hzFreq);
+	}
+	else mHzFreq = hzFreq;
+
+}
+
+void AudioSynthDevice::stop()
+{
+	newCall = true;
+	mPos = 0;
+	close();
+}
+
+// Modefied coupled first-order form algorithm with fixed point arithmetic
+int AudioSynthDevice::generate(char *data, int length, int hzFreq)
+{
+	const int channelBytes = mSampleSize / 8;
+
+	qint64 maxlen = length/channelBytes;
+
+	static const int M = 1 << 30;
+	const auto w = hzFreq * M_PI / mSampleRate;
+	const long long b1 = 2.0 * cos(w)*M;
+	static const int AMPLITUDE = (1 << (mSampleSize - 1)) - 1;
+
+	unsigned char *ptr = reinterpret_cast<unsigned char *>(data);
+
+
+	// Need to save values between readData(...) calls, so static
+	static long long  y0 = 0;
+	static decltype(y0) y1 = 0;
+	static decltype(y0) y2 = 0;
+
+	if(newCall)
+	{
+		y1 = M * std::sin(-w);
+		y2 = M * std::sin(-2*w);
+		newCall = false;
+	}
+
+	int i = 0;
+
+	for(i = 0; i < maxlen; ++i){
+
+		y0 = b1*y1 / M - y2;
+		y2 = b1*y0 / M - y1;
+		y1 = b1*y2 / M - y0;
+
+		if(mSampleSize == 8) {
+			const qint8 val = static_cast<qint8>(y0*AMPLITUDE/M);
+			*reinterpret_cast<quint8*>(ptr) = val;
+		}
+		if(mSampleSize == 16) {
+			const qint16 val = static_cast<qint16>(y0*AMPLITUDE/M);
+			*reinterpret_cast<quint16*>(ptr) = val;
+		}
+
+		ptr+=channelBytes;
+	}
+
+	return i*channelBytes;
+}
+
+qint64 AudioSynthDevice::readData(char *data, qint64 len)
+{
+	if(mBuffered) {
+		qint64 total = 0;
+		while (len - total > 0) {
+			const qint64 chunk = qMin((mBuffer.size() - mPos), len - total);
+			memcpy(data + total, mBuffer.constData() + mPos, chunk);
+			mPos = (mPos + chunk) % mBuffer.size();
+			total += chunk;
+		}
+		return total;
+	} else
+		return generate(data, len, mHzFreq);
+}
+
+qint64 AudioSynthDevice::writeData(const char *data, qint64 len)
+{
+	Q_UNUSED(data);
+	Q_UNUSED(len);
+
+	return 0;
+}
+
+qint64 AudioSynthDevice::bytesAvailable() const
+{
+	return mBuffer.size() + QIODevice::bytesAvailable();
+}

trikControl/src/audioSynthDevices.h

@@ -0,0 +1,70 @@
+/* Copyright 2016 Artem Sharganov
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License. */
+
+#pragma once
+
+#include <QtCore/QByteArray>
+#include <QtCore/QIODevice>
+#include <QtMultimedia/QAudioFormat>
+
+/// QIODevice that synthesize sine wave values
+class AudioSynthDevice : public QIODevice
+{
+	Q_OBJECT
+
+public:
+	/// Constructor
+	AudioSynthDevice(QObject *parent, int sampleRate, int sampleSize);
+
+	~AudioSynthDevice();
+
+	/// Provides reading from device
+	qint64 readData(char *data, qint64 maxlen);
+
+	/// Opens device, run generation in buffered mode
+	void start(int hzFreq);
+
+	/// Close device and reset pose
+	void stop();
+
+	/// Stub, because readonly device
+	qint64 writeData(const char *data, qint64 len);
+
+	/// Returns amount of available bytes
+	qint64 bytesAvailable() const;
+
+private:
+	/// Sythesize sine wave values
+	int generate(char *data, int length, int hzFreq);
+
+private:
+
+	/// Internal buffer, used in buffered mode
+	QByteArray mBuffer;
+
+	qint64 mPos;
+
+	int mHzFreq;
+
+	const int mSampleRate;
+
+	const int mSampleSize;
+
+	/// Mode of device
+	bool mBuffered = false;
+
+	/// New call of playTone(...), not readData(...) call
+	bool newCall = true;
+};
+

trikControl/src/brick.cpp

@@ -44,6 +44,7 @@
 #include "rangeSensor.h"
 #include "servoMotor.h"
 #include "soundSensor.h"
+#include "tonePlayer.h"
 #include "vectorSensor.h"
 
 #include "mspBusAutoDetector.h"
@@ -71,6 +72,7 @@ Brick::Brick(const trikKernel::DifferentOwnerPointer<trikHal::HardwareAbstractio
 		, const QString &modelConfig
 		, const QString &mediaPath)
 	: mHardwareAbstraction(hardwareAbstraction)
+	, mTonePlayer(new TonePlayer())
 	, mMediaPath(mediaPath)
 	, mConfigurer(systemConfig, modelConfig)
 {
@@ -204,6 +206,22 @@ void Brick::playSound(const QString &soundFileName)
 	}
 }
 
+
+void Brick::playTone(int hzFreq, int msDuration)
+{
+	QLOG_INFO() << "Playing tone (" << hzFreq << "," << msDuration << ")";
+
+	if (msDuration < 10)
+		return;
+	if (hzFreq > 8000)
+		return;
+	if (hzFreq <  20)
+		return;
+	//mHardwareAbstraction->systemSound()->playTone(hzFreq, msDuration);
+	//mTonePlayer->play(hzFreq, msDuration);
+	QMetaObject::invokeMethod(mTonePlayer.data(), "play", Q_ARG(int, hzFreq), Q_ARG(int, msDuration));
+}
+
 void Brick::say(const QString &text)
 {
 	QStringList args{"-c", "espeak -v russian_test -s 100 \"" + text + "\""};

trikControl/src/brick.h

@@ -47,6 +47,7 @@ class PowerMotor;
 class PwmCapture;
 class RangeSensor;
 class ServoMotor;
+class TonePlayer;
 class VectorSensor;
 
 /// Class representing TRIK controller board and devices installed on it, also provides access
@@ -83,6 +84,8 @@ public slots:
 
 	void playSound(const QString &soundFileName) override;
 
+	void playTone(int hzFreq, int msDuration);
+
 	void say(const QString &text) override;
 
 	void stop() override;
@@ -154,6 +157,7 @@ public slots:
 	QScopedPointer<Keys> mKeys;
 	QScopedPointer<Display> mDisplay;
 	QScopedPointer<Led> mLed;
+	QScopedPointer<TonePlayer> mTonePlayer;
 
 	QHash<QString, ServoMotor *> mServoMotors;  // Has ownership.
 	QHash<QString, PwmCapture *> mPwmCaptures;  // Has ownership.

trikControl/src/tonePlayer.cpp

@@ -0,0 +1,69 @@
+/* Copyright 2016 Artem Sharganov
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License. */
+
+
+#include "tonePlayer.h"
+
+
+namespace trikControl{
+
+TonePlayer::TonePlayer()
+{
+	mTimer.setSingleShot(true);
+	initializeAudio();
+	mDevice = new AudioSynthDevice(this, mFormat.sampleRate(), mFormat.sampleSize());
+	mOutput = new QAudioOutput(mFormat, this);
+}
+
+void TonePlayer::initializeAudio()
+{
+
+	mFormat.setChannelCount(1);
+	mFormat.setSampleRate(16000);
+	mFormat.setSampleSize(16);
+	mFormat.setSampleType(QAudioFormat::SampleType::SignedInt);
+	mFormat.setCodec("audio/pcm");
+
+	connect(&mTimer, SIGNAL(timeout()), this, SLOT(stop()));
+
+	QAudioDeviceInfo info(QAudioDeviceInfo::defaultOutputDevice());
+	if (!info.isFormatSupported(mFormat)) {
+		mFormat = info.nearestFormat(mFormat);
+	}
+}
+
+void TonePlayer::play(int hzFreq, int msDuration)
+{
+	mOutput->reset();
+	switch (mOutput->state()) {
+		case QAudio::IdleState: break;
+		default:break;
+	}
+
+	mTimer.setInterval(msDuration);
+	mDevice->start(hzFreq);
+	mTimer.start();
+	mOutput->start(mDevice);
+}
+
+void TonePlayer::stop()
+{
+	mDevice->stop();
+	mTimer.stop();
+	mOutput->stop();
+
+}
+}
+
+

trikControl/src/tonePlayer.h

@@ -0,0 +1,56 @@
+/* Copyright 2016 Artem Sharganov
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License. */
+
+#pragma once
+
+#include "audioSynthDevices.h"
+
+#include <QtCore/QObject>
+#include <QtCore/QTimer>
+#include <QtMultimedia/QAudioOutput>
+
+
+
+namespace trikControl {
+
+/// Tone player. Play tones
+class TonePlayer : public QObject
+{
+	Q_OBJECT
+
+public:
+
+	/// Constructor
+	TonePlayer();
+
+public slots:
+
+	/// Play sound
+	void play(int freqHz, int durationMs);
+
+private:
+	QAudioFormat mFormat;
+
+	AudioSynthDevice *mDevice; // Has ownership.
+
+	QAudioOutput *mOutput; // Has ownership.
+
+	QTimer mTimer;
+
+	void initializeAudio();
+
+public slots:
+	void stop();
+};
+}