Commit 8e3699e7 authored by Jussi Lindgren's avatar Jussi Lindgren

Drivers: Fixes to the MBT Smarting driver

- Uses the asynchronous reading mode and the SmartingAmp
  class again as was likely originally intended
- Condition variable is used to wait on new samples being available
  so we don't busy-wait the CPU
- Fixed the thread joining
- Fixed several memory leaks
- Channel count is now fixed to 27
- Named the 3 gyro channels
- The driver now remembers the specified COM port
- KB entered COM port selection doesn't now need Enter key
- Removed latency/drift code that looked like adhoc hacks
parent e87b9dea
...@@ -164,7 +164,8 @@ ...@@ -164,7 +164,8 @@
<child> <child>
<object class="GtkSpinButton" id="spinbutton_number_of_channels"> <object class="GtkSpinButton" id="spinbutton_number_of_channels">
<property name="visible">True</property> <property name="visible">True</property>
<property name="can_focus">True</property> <property name="can_focus">False</property>
<property name="sensitive">False</property>
<property name="primary_icon_activatable">False</property> <property name="primary_icon_activatable">False</property>
<property name="secondary_icon_activatable">False</property> <property name="secondary_icon_activatable">False</property>
<property name="primary_icon_sensitive">True</property> <property name="primary_icon_sensitive">True</property>
...@@ -192,6 +193,19 @@ ...@@ -192,6 +193,19 @@
<property name="top_attach">3</property> <property name="top_attach">3</property>
<property name="bottom_attach">4</property> <property name="bottom_attach">4</property>
</packing> </packing>
</child>
<child>
<object class="GtkLabel" id="label_port_number">
<property name="visible">True</property>
<property name="can_focus">False</property>
<property name="label" translatable="yes">Port number :</property>
<property name="justify">right</property>
<property name="single_line_mode">True</property>
</object>
<packing>
<property name="top_attach">4</property>
<property name="bottom_attach">5</property>
</packing>
</child> </child>
<child> <child>
<object class="GtkSpinButton" id="spinbutton_port_number"> <object class="GtkSpinButton" id="spinbutton_port_number">
...@@ -201,7 +215,7 @@ ...@@ -201,7 +215,7 @@
<property name="secondary_icon_activatable">False</property> <property name="secondary_icon_activatable">False</property>
<property name="primary_icon_sensitive">True</property> <property name="primary_icon_sensitive">True</property>
<property name="secondary_icon_sensitive">True</property> <property name="secondary_icon_sensitive">True</property>
<property name="adjustment">adjustment4</property> <property name="adjustment">adjustment2</property>
<property name="snap_to_ticks">True</property> <property name="snap_to_ticks">True</property>
<property name="numeric">True</property> <property name="numeric">True</property>
</object> </object>
...@@ -211,21 +225,7 @@ ...@@ -211,21 +225,7 @@
<property name="top_attach">4</property> <property name="top_attach">4</property>
<property name="bottom_attach">5</property> <property name="bottom_attach">5</property>
</packing> </packing>
</child> </child>
<child>
<object class="GtkLabel" id="label_device">
<property name="visible">True</property>
<property name="can_focus">False</property>
<property name="label" translatable="yes">Port number :</property>
<property name="justify">right</property>
<property name="single_line_mode">True</property>
</object>
<packing>
<property name="top_attach">4</property>
<property name="bottom_attach">5</property>
</packing>
</child>
<child> <child>
<object class="GtkComboBox" id="combobox_gender"> <object class="GtkComboBox" id="combobox_gender">
<property name="visible">True</property> <property name="visible">True</property>
......
...@@ -72,6 +72,7 @@ boolean CConfigurationMBTSmarting::postConfigure(void) ...@@ -72,6 +72,7 @@ boolean CConfigurationMBTSmarting::postConfigure(void)
// For example, you can save the connection ID of the selected device: // For example, you can save the connection ID of the selected device:
// m_ui32ConnectionID = <value-from-gtk-widget> // m_ui32ConnectionID = <value-from-gtk-widget>
::GtkSpinButton* l_pSpinButtonPortNumber = GTK_SPIN_BUTTON(gtk_builder_get_object(m_pBuilderConfigureInterface, "spinbutton_port_number")); ::GtkSpinButton* l_pSpinButtonPortNumber = GTK_SPIN_BUTTON(gtk_builder_get_object(m_pBuilderConfigureInterface, "spinbutton_port_number"));
gtk_spin_button_update(l_pSpinButtonPortNumber);
m_ui32ConnectionID = ::gtk_spin_button_get_value_as_int(l_pSpinButtonPortNumber); m_ui32ConnectionID = ::gtk_spin_button_get_value_as_int(l_pSpinButtonPortNumber);
} }
......
...@@ -11,6 +11,8 @@ using namespace OpenViBE; ...@@ -11,6 +11,8 @@ using namespace OpenViBE;
using namespace OpenViBE::Kernel; using namespace OpenViBE::Kernel;
using namespace std; using namespace std;
#define MBT_CHANNEL_COUNT 27
//___________________________________________________________________// //___________________________________________________________________//
// // // //
...@@ -19,16 +21,18 @@ CDriverMBTSmarting::CDriverMBTSmarting(IDriverContext& rDriverContext) ...@@ -19,16 +21,18 @@ CDriverMBTSmarting::CDriverMBTSmarting(IDriverContext& rDriverContext)
,m_oSettings("AcquisitionServer_Driver_MBTSmarting", m_rDriverContext.getConfigurationManager()) ,m_oSettings("AcquisitionServer_Driver_MBTSmarting", m_rDriverContext.getConfigurationManager())
,m_pCallback(NULL) ,m_pCallback(NULL)
,m_ui32SampleCountPerSentBlock(0) ,m_ui32SampleCountPerSentBlock(0)
,m_pSample(NULL) ,m_ui32ConnectionID(1)
,m_pSmartingAmp(nullptr)
{ {
m_oHeader.setSamplingFrequency(500); m_oHeader.setSamplingFrequency(500);
m_oHeader.setChannelCount(27); m_oHeader.setChannelCount(MBT_CHANNEL_COUNT);
// The following class allows saving and loading driver settings from the acquisition server .conf file // The following class allows saving and loading driver settings from the acquisition server .conf file
m_oSettings.add("Header", &m_oHeader); m_oSettings.add("Header", &m_oHeader);
// To save your custom driver settings, register each variable to the SettingsHelper // To save your custom driver settings, register each variable to the SettingsHelper
//m_oSettings.add("SettingName", &variable); m_oSettings.add("ConnectionID", &m_ui32ConnectionID);
m_oSettings.load(); m_oSettings.load();
} }
CDriverMBTSmarting::~CDriverMBTSmarting(void) CDriverMBTSmarting::~CDriverMBTSmarting(void)
...@@ -50,18 +54,9 @@ boolean CDriverMBTSmarting::initialize( ...@@ -50,18 +54,9 @@ boolean CDriverMBTSmarting::initialize(
if(m_rDriverContext.isConnected()) return false; if(m_rDriverContext.isConnected()) return false;
if(!m_oHeader.isChannelCountSet()||!m_oHeader.isSamplingFrequencySet()) return false; if(!m_oHeader.isChannelCountSet()||!m_oHeader.isSamplingFrequencySet()) return false;
// Builds up a buffer to store // n.b. We force it to be 27 despite user choice
// acquired samples. This buffer m_oHeader.setChannelCount(MBT_CHANNEL_COUNT);
// will be sent to the acquisition
// server later...
m_pSample=new float32[m_oHeader.getChannelCount()];
if(!m_pSample)
{
delete [] m_pSample;
m_pSample=NULL;
return false;
}
// ... // ...
// initialize hardware and get // initialize hardware and get
// available header information // available header information
...@@ -69,7 +64,7 @@ boolean CDriverMBTSmarting::initialize( ...@@ -69,7 +64,7 @@ boolean CDriverMBTSmarting::initialize(
// Using for example the connection ID provided by the configuration (m_ui32ConnectionID) // Using for example the connection ID provided by the configuration (m_ui32ConnectionID)
// ... // ...
m_pSmartingAmp.reset(new SmartingAmp); m_pSmartingAmp = new SmartingAmp();
stringstream port_ss; stringstream port_ss;
#ifdef TARGET_OS_Windows #ifdef TARGET_OS_Windows
...@@ -95,6 +90,10 @@ boolean CDriverMBTSmarting::initialize( ...@@ -95,6 +90,10 @@ boolean CDriverMBTSmarting::initialize(
m_rDriverContext.getLogManager() << LogLevel_Info << "Setting the sampling frequency at " << 500 <<"\n"; m_rDriverContext.getLogManager() << LogLevel_Info << "Setting the sampling frequency at " << 500 <<"\n";
m_pSmartingAmp->send_command(FREQUENCY_500); m_pSmartingAmp->send_command(FREQUENCY_500);
break; break;
default:
m_rDriverContext.getLogManager() << LogLevel_Error << "Only sampling frequencies 250 and 500 are supported\n";
return false;
break;
} }
// Declare channel units // Declare channel units
...@@ -106,6 +105,10 @@ boolean CDriverMBTSmarting::initialize( ...@@ -106,6 +105,10 @@ boolean CDriverMBTSmarting::initialize(
m_oHeader.setChannelUnits(25, OVTK_UNIT_Degree_Per_Second, OVTK_FACTOR_Base); m_oHeader.setChannelUnits(25, OVTK_UNIT_Degree_Per_Second, OVTK_FACTOR_Base);
m_oHeader.setChannelUnits(26, OVTK_UNIT_Degree_Per_Second, OVTK_FACTOR_Base); m_oHeader.setChannelUnits(26, OVTK_UNIT_Degree_Per_Second, OVTK_FACTOR_Base);
m_oHeader.setChannelName(24, "Gyro 1");
m_oHeader.setChannelName(25, "Gyro 2");
m_oHeader.setChannelName(26, "Gyro 3");
// Saves parameters // Saves parameters
m_pCallback=&rCallback; m_pCallback=&rCallback;
m_ui32SampleCountPerSentBlock=ui32SampleCountPerSentBlock; m_ui32SampleCountPerSentBlock=ui32SampleCountPerSentBlock;
...@@ -126,11 +129,9 @@ boolean CDriverMBTSmarting::start(void) ...@@ -126,11 +129,9 @@ boolean CDriverMBTSmarting::start(void)
// sending data // sending data
// ... // ...
m_pSmartingAmp->send_command(ON); m_pSmartingAmp->start();
m_byteArray.clear();
m_vSamples.resize(MBT_CHANNEL_COUNT * m_ui32SampleCountPerSentBlock);
sample_number = 1;
latency = 1;
return true; return true;
} }
...@@ -148,66 +149,29 @@ boolean CDriverMBTSmarting::loop(void) ...@@ -148,66 +149,29 @@ boolean CDriverMBTSmarting::loop(void)
// whether the buffer is full, send it to the acquisition server // whether the buffer is full, send it to the acquisition server
//... //...
unsigned char* receiveBuffer = new unsigned char[MAX_PACKAGE_SIZE]; std::vector<float*> samples;
while(samples.size() < m_ui32SampleCountPerSentBlock)
int readed = m_pSmartingAmp->read(receiveBuffer, MAX_PACKAGE_SIZE);
for (int i = 0; i < readed; i++)
{ {
if (m_byteArray.size() > 0) if(!m_pSmartingAmp->get_sample(samples, m_ui32SampleCountPerSentBlock))
{
m_byteArray.push_back(receiveBuffer[i]);
if(m_byteArray.size() == 83)
{
if(m_byteArray[82] == '<')
{
if (sample_number % 5000 == 0)
{
sample_number = 1;
if (m_rDriverContext.getDriftSampleCount() < 2)
{
m_pSample = m_pSmartingAmp->convert_data(m_byteArray);
m_pCallback->setSamples(m_pSample, 1);
m_rDriverContext.correctDriftSampleCount(m_rDriverContext.getSuggestedDriftCorrectionSampleCount());
}
}
else
{
sample_number++;
m_pSample = m_pSmartingAmp->convert_data(m_byteArray);
m_pCallback->setSamples(m_pSample, 1);
m_rDriverContext.correctDriftSampleCount(m_rDriverContext.getSuggestedDriftCorrectionSampleCount());
}
}
m_byteArray.clear();
}
}
if(m_byteArray.size() == 0 && receiveBuffer[i] == '>')
{ {
m_byteArray.push_back(receiveBuffer[i]); m_rDriverContext.getLogManager() << LogLevel_Error << "The Smarting thread returned 0 samples, unexpected.\n";
return false;
} }
} }
if (latency == 300) for(uint32_t s=0;s<m_ui32SampleCountPerSentBlock;s++)
{ {
latency = 0; float* pSample = samples[s];
m_rDriverContext.setInnerLatencySampleCount(-m_rDriverContext.getDriftSampleCount()); for(size_t c=0;c<MBT_CHANNEL_COUNT;c++)
}
else
{
if (latency != 0)
{ {
latency++; m_vSamples[c*m_ui32SampleCountPerSentBlock+s] = pSample[c];
} }
delete[] pSample;
} }
m_pCallback->setSamples(&m_vSamples[0]);
m_rDriverContext.correctDriftSampleCount(m_rDriverContext.getSuggestedDriftCorrectionSampleCount());
// ... // ...
// receive events from hardware // receive events from hardware
// and put them the correct way in a CStimulationSet object // and put them the correct way in a CStimulationSet object
...@@ -227,7 +191,8 @@ boolean CDriverMBTSmarting::stop(void) ...@@ -227,7 +191,8 @@ boolean CDriverMBTSmarting::stop(void)
// sending data // sending data
// ... // ...
m_rDriverContext.setInnerLatencySampleCount(0); m_rDriverContext.setInnerLatencySampleCount(0);
m_pSmartingAmp->off();
m_pSmartingAmp->stop();
return true; return true;
} }
...@@ -243,8 +208,9 @@ boolean CDriverMBTSmarting::uninitialize(void) ...@@ -243,8 +208,9 @@ boolean CDriverMBTSmarting::uninitialize(void)
m_pSmartingAmp->disconnect(); m_pSmartingAmp->disconnect();
delete [] m_pSample; delete m_pSmartingAmp;
m_pSample=NULL; m_pSmartingAmp = nullptr;
m_pCallback=NULL; m_pCallback=NULL;
return true; return true;
......
...@@ -63,7 +63,6 @@ namespace OpenViBEAcquisitionServer ...@@ -63,7 +63,6 @@ namespace OpenViBEAcquisitionServer
OpenViBEAcquisitionServer::CHeader m_oHeader; OpenViBEAcquisitionServer::CHeader m_oHeader;
OpenViBE::uint32 m_ui32SampleCountPerSentBlock; OpenViBE::uint32 m_ui32SampleCountPerSentBlock;
OpenViBE::float32* m_pSample;
private: private:
...@@ -72,10 +71,10 @@ namespace OpenViBEAcquisitionServer ...@@ -72,10 +71,10 @@ namespace OpenViBEAcquisitionServer
* Example : * Example :
*/ */
OpenViBE::uint32 m_ui32ConnectionID; OpenViBE::uint32 m_ui32ConnectionID;
std::shared_ptr< SmartingAmp > m_pSmartingAmp; SmartingAmp* m_pSmartingAmp;
std::vector< unsigned char > m_byteArray;
int sample_number; std::vector< OpenViBE::float32 > m_vSamples; // Buffer sent outwards, size is channels * blocksize
int latency;
}; };
}; };
......
...@@ -18,7 +18,8 @@ SmartingAmp::SmartingAmp() ...@@ -18,7 +18,8 @@ SmartingAmp::SmartingAmp()
} }
SmartingAmp::~SmartingAmp() SmartingAmp::~SmartingAmp()
{} {
}
void SmartingAmp::disconnect() void SmartingAmp::disconnect()
{ {
...@@ -41,7 +42,6 @@ int SmartingAmp::read(unsigned char *data, size_t size) ...@@ -41,7 +42,6 @@ int SmartingAmp::read(unsigned char *data, size_t size)
void SmartingAmp::acquire() void SmartingAmp::acquire()
{ {
if (m_port.get() == NULL || !m_port->is_open()) return; if (m_port.get() == NULL || !m_port->is_open()) return;
m_port->async_read_some( m_port->async_read_some(
...@@ -58,9 +58,12 @@ void SmartingAmp::on_receive(const boost::system::error_code& ec, size_t bytes_t ...@@ -58,9 +58,12 @@ void SmartingAmp::on_receive(const boost::system::error_code& ec, size_t bytes_t
if (m_port.get() == NULL || !m_port->is_open()) return; if (m_port.get() == NULL || !m_port->is_open()) return;
if (ec) { if (ec) {
// Had an error ...
//acquire(); //acquire();
return; return;
} }
bool gotSomething = false;
for (unsigned int i = 0; i < bytes_transferred; i++) for (unsigned int i = 0; i < bytes_transferred; i++)
{ {
...@@ -78,6 +81,8 @@ void SmartingAmp::on_receive(const boost::system::error_code& ec, size_t bytes_t ...@@ -78,6 +81,8 @@ void SmartingAmp::on_receive(const boost::system::error_code& ec, size_t bytes_t
m_samples_lock.lock(); m_samples_lock.lock();
m_samplesBuffer.push(sample); m_samplesBuffer.push(sample);
m_samples_lock.unlock(); m_samples_lock.unlock();
gotSomething = true;
} }
else else
{ {
...@@ -92,6 +97,10 @@ void SmartingAmp::on_receive(const boost::system::error_code& ec, size_t bytes_t ...@@ -92,6 +97,10 @@ void SmartingAmp::on_receive(const boost::system::error_code& ec, size_t bytes_t
m_byteArray.push_back(m_receiveBuffer[i]); m_byteArray.push_back(m_receiveBuffer[i]);
} }
} }
if(gotSomething)
{
m_oHaveSamplesCondition.notify_one();
}
acquire(); acquire();
} }
...@@ -252,29 +261,40 @@ bool SmartingAmp::start() ...@@ -252,29 +261,40 @@ bool SmartingAmp::start()
m_failedSamples = 0; m_failedSamples = 0;
m_receivedSamples = 0; m_receivedSamples = 0;
// start reading // start reading.
acquire(); acquire();
// the acquire callback will be run in the ioservice thread f.
auto f = [this]() { this->m_io.run(); }; auto f = [this]() { this->m_io.run(); };
// auto f = boost::bind(&boost::asio::io_service::run, &m_io);
acquire_t.reset(new std::thread( f )); acquire_t.reset(new std::thread( f ));
return true; return true;
} }
float* SmartingAmp::get_sample() bool SmartingAmp::get_sample(std::vector<float*>& samples, uint32_t maxSamples)
{ {
if (m_samplesBuffer.size() > 0) std::unique_lock<std::mutex> oLock(m_samples_lock);
// Wait until something interesting happens...
m_oHaveSamplesCondition.wait(oLock,
[this]() { return (m_io.stopped() || m_samplesBuffer.size()>0); }
);
if(m_io.stopped())
{
oLock.unlock();
return false;
}
while(m_samplesBuffer.size()>0 && samples.size() < maxSamples)
{ {
m_samples_lock.lock();
float* sample = m_samplesBuffer.front(); float* sample = m_samplesBuffer.front();
m_samplesBuffer.pop(); m_samplesBuffer.pop();
m_samples_lock.unlock(); samples.push_back(sample);
return sample;
} }
return NULL;
oLock.unlock();
return true;
} }
bool SmartingAmp::stop() bool SmartingAmp::stop()
...@@ -286,23 +306,31 @@ bool SmartingAmp::stop() ...@@ -286,23 +306,31 @@ bool SmartingAmp::stop()
{ {
// cancel all async operations // cancel all async operations
m_io.stop(); m_io.stop();
acquire_t->join();
m_io.reset(); m_io.reset();
acquire_t.reset();
// TODO: Implement proper stopping of the acquiring thread
} }
catch (std::exception&) catch (std::exception&)
{ {
// to do return false;
} }
// NOTE: Currently, I am very unhappy with flushing // Clear buffers that were not sent; as thread was joined, no need to lock
while( m_samplesBuffer.size() > 0)
{
float* sample = m_samplesBuffer.front();
m_samplesBuffer.pop();
delete[] sample;
}
// @FIXME is this still relevant: "NOTE: Currently, I am very unhappy with flushing
// It slows down everything and after 6 play/stops open vibe is blocked // It slows down everything and after 6 play/stops open vibe is blocked
// flush(); // flush();"
//cout << "Failed " << m_failedSamples << endl; //cout << "Failed " << m_failedSamples << endl;
//cout << "Received " << m_receivedSamples << endl; //cout << "Received " << m_receivedSamples << endl;
return false; return true;
} }
float* SmartingAmp::convert_data(std::vector<unsigned char> byte_array) float* SmartingAmp::convert_data(std::vector<unsigned char> byte_array)
......
...@@ -25,6 +25,7 @@ ...@@ -25,6 +25,7 @@
#include <mutex> #include <mutex>
#include <thread> #include <thread>
#include <condition_variable>
#define OK_RESPONSE_SIZE 4 #define OK_RESPONSE_SIZE 4
#define MAX_PACKAGE_SIZE 4096 #define MAX_PACKAGE_SIZE 4096
...@@ -91,7 +92,7 @@ public: ...@@ -91,7 +92,7 @@ public:
void on_receive(const boost::system::error_code& ec, size_t bytes_transferred); void on_receive(const boost::system::error_code& ec, size_t bytes_transferred);
float* get_sample(); bool get_sample(std::vector<float*>& samples, uint32_t maxSamples);
float* convert_data(std::vector<unsigned char> byte_array); float* convert_data(std::vector<unsigned char> byte_array);
...@@ -117,7 +118,7 @@ private: ...@@ -117,7 +118,7 @@ private:
unsigned char m_commandReceiveBuffer[MAX_PORT_SIZE]; unsigned char m_commandReceiveBuffer[MAX_PORT_SIZE];
std::queue<float*> m_samplesBuffer; std::queue< float* > m_samplesBuffer;
std::vector<unsigned char> m_byteArray; std::vector<unsigned char> m_byteArray;
...@@ -141,6 +142,8 @@ private: ...@@ -141,6 +142,8 @@ private:
// read with timeout // read with timeout
int m_bytes_readed; int m_bytes_readed;
std::condition_variable m_oHaveSamplesCondition;
}; };
#endif #endif
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment