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 @@
<child>
<object class="GtkSpinButton" id="spinbutton_number_of_channels">
<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="secondary_icon_activatable">False</property>
<property name="primary_icon_sensitive">True</property>
......@@ -192,6 +193,19 @@
<property name="top_attach">3</property>
<property name="bottom_attach">4</property>
</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>
<object class="GtkSpinButton" id="spinbutton_port_number">
......@@ -201,7 +215,7 @@
<property name="secondary_icon_activatable">False</property>
<property name="primary_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="numeric">True</property>
</object>
......@@ -211,21 +225,7 @@
<property name="top_attach">4</property>
<property name="bottom_attach">5</property>
</packing>
</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">
<property name="visible">True</property>
......
......@@ -72,6 +72,7 @@ boolean CConfigurationMBTSmarting::postConfigure(void)
// For example, you can save the connection ID of the selected device:
// m_ui32ConnectionID = <value-from-gtk-widget>
::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);
}
......
......@@ -11,6 +11,8 @@ using namespace OpenViBE;
using namespace OpenViBE::Kernel;
using namespace std;
#define MBT_CHANNEL_COUNT 27
//___________________________________________________________________//
// //
......@@ -19,16 +21,18 @@ CDriverMBTSmarting::CDriverMBTSmarting(IDriverContext& rDriverContext)
,m_oSettings("AcquisitionServer_Driver_MBTSmarting", m_rDriverContext.getConfigurationManager())
,m_pCallback(NULL)
,m_ui32SampleCountPerSentBlock(0)
,m_pSample(NULL)
,m_ui32ConnectionID(1)
,m_pSmartingAmp(nullptr)
{
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
m_oSettings.add("Header", &m_oHeader);
// 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();
}
CDriverMBTSmarting::~CDriverMBTSmarting(void)
......@@ -50,18 +54,9 @@ boolean CDriverMBTSmarting::initialize(
if(m_rDriverContext.isConnected()) return false;
if(!m_oHeader.isChannelCountSet()||!m_oHeader.isSamplingFrequencySet()) return false;
// Builds up a buffer to store
// acquired samples. This buffer
// 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;
}
// n.b. We force it to be 27 despite user choice
m_oHeader.setChannelCount(MBT_CHANNEL_COUNT);
// ...
// initialize hardware and get
// available header information
......@@ -69,7 +64,7 @@ boolean CDriverMBTSmarting::initialize(
// Using for example the connection ID provided by the configuration (m_ui32ConnectionID)
// ...
m_pSmartingAmp.reset(new SmartingAmp);
m_pSmartingAmp = new SmartingAmp();
stringstream port_ss;
#ifdef TARGET_OS_Windows
......@@ -95,6 +90,10 @@ boolean CDriverMBTSmarting::initialize(
m_rDriverContext.getLogManager() << LogLevel_Info << "Setting the sampling frequency at " << 500 <<"\n";
m_pSmartingAmp->send_command(FREQUENCY_500);
break;
default:
m_rDriverContext.getLogManager() << LogLevel_Error << "Only sampling frequencies 250 and 500 are supported\n";
return false;
break;
}
// Declare channel units
......@@ -106,6 +105,10 @@ boolean CDriverMBTSmarting::initialize(
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.setChannelName(24, "Gyro 1");
m_oHeader.setChannelName(25, "Gyro 2");
m_oHeader.setChannelName(26, "Gyro 3");
// Saves parameters
m_pCallback=&rCallback;
m_ui32SampleCountPerSentBlock=ui32SampleCountPerSentBlock;
......@@ -126,11 +129,9 @@ boolean CDriverMBTSmarting::start(void)
// sending data
// ...
m_pSmartingAmp->send_command(ON);
m_byteArray.clear();
sample_number = 1;
latency = 1;
m_pSmartingAmp->start();
m_vSamples.resize(MBT_CHANNEL_COUNT * m_ui32SampleCountPerSentBlock);
return true;
}
......@@ -148,66 +149,29 @@ boolean CDriverMBTSmarting::loop(void)
// whether the buffer is full, send it to the acquisition server
//...
unsigned char* receiveBuffer = new unsigned char[MAX_PACKAGE_SIZE];
int readed = m_pSmartingAmp->read(receiveBuffer, MAX_PACKAGE_SIZE);
for (int i = 0; i < readed; i++)
std::vector<float*> samples;
while(samples.size() < m_ui32SampleCountPerSentBlock)
{
if (m_byteArray.size() > 0)
{
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] == '>')
if(!m_pSmartingAmp->get_sample(samples, m_ui32SampleCountPerSentBlock))
{
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;
m_rDriverContext.setInnerLatencySampleCount(-m_rDriverContext.getDriftSampleCount());
}
else
{
if (latency != 0)
float* pSample = samples[s];
for(size_t c=0;c<MBT_CHANNEL_COUNT;c++)
{
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
// and put them the correct way in a CStimulationSet object
......@@ -227,7 +191,8 @@ boolean CDriverMBTSmarting::stop(void)
// sending data
// ...
m_rDriverContext.setInnerLatencySampleCount(0);
m_pSmartingAmp->off();
m_pSmartingAmp->stop();
return true;
}
......@@ -243,8 +208,9 @@ boolean CDriverMBTSmarting::uninitialize(void)
m_pSmartingAmp->disconnect();
delete [] m_pSample;
m_pSample=NULL;
delete m_pSmartingAmp;
m_pSmartingAmp = nullptr;
m_pCallback=NULL;
return true;
......
......@@ -63,7 +63,6 @@ namespace OpenViBEAcquisitionServer
OpenViBEAcquisitionServer::CHeader m_oHeader;
OpenViBE::uint32 m_ui32SampleCountPerSentBlock;
OpenViBE::float32* m_pSample;
private:
......@@ -72,10 +71,10 @@ namespace OpenViBEAcquisitionServer
* Example :
*/
OpenViBE::uint32 m_ui32ConnectionID;
std::shared_ptr< SmartingAmp > m_pSmartingAmp;
std::vector< unsigned char > m_byteArray;
int sample_number;
int latency;
SmartingAmp* m_pSmartingAmp;
std::vector< OpenViBE::float32 > m_vSamples; // Buffer sent outwards, size is channels * blocksize
};
};
......
......@@ -18,7 +18,8 @@ SmartingAmp::SmartingAmp()
}
SmartingAmp::~SmartingAmp()
{}
{
}
void SmartingAmp::disconnect()
{
......@@ -41,7 +42,6 @@ int SmartingAmp::read(unsigned char *data, size_t size)
void SmartingAmp::acquire()
{
if (m_port.get() == NULL || !m_port->is_open()) return;
m_port->async_read_some(
......@@ -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 (ec) {
// Had an error ...
//acquire();
return;
}
bool gotSomething = false;
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
m_samples_lock.lock();
m_samplesBuffer.push(sample);
m_samples_lock.unlock();
gotSomething = true;
}
else
{
......@@ -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]);
}
}
if(gotSomething)
{
m_oHaveSamplesCondition.notify_one();
}
acquire();
}
......@@ -252,29 +261,40 @@ bool SmartingAmp::start()
m_failedSamples = 0;
m_receivedSamples = 0;
// start reading
// start reading.
acquire();
// the acquire callback will be run in the ioservice thread f.
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 ));
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();
m_samplesBuffer.pop();
m_samples_lock.unlock();
return sample;
samples.push_back(sample);
}
return NULL;
oLock.unlock();
return true;
}
bool SmartingAmp::stop()
......@@ -286,23 +306,31 @@ bool SmartingAmp::stop()
{
// cancel all async operations
m_io.stop();
acquire_t->join();
m_io.reset();
// TODO: Implement proper stopping of the acquiring thread
acquire_t.reset();
}
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
// flush();
// flush();"
//cout << "Failed " << m_failedSamples << endl;
//cout << "Received " << m_receivedSamples << endl;
return false;
return true;
}
float* SmartingAmp::convert_data(std::vector<unsigned char> byte_array)
......
......@@ -25,6 +25,7 @@
#include <mutex>
#include <thread>
#include <condition_variable>
#define OK_RESPONSE_SIZE 4
#define MAX_PACKAGE_SIZE 4096
......@@ -91,7 +92,7 @@ public:
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);
......@@ -117,7 +118,7 @@ private:
unsigned char m_commandReceiveBuffer[MAX_PORT_SIZE];
std::queue<float*> m_samplesBuffer;
std::queue< float* > m_samplesBuffer;
std::vector<unsigned char> m_byteArray;
......@@ -141,6 +142,8 @@ private:
// read with timeout
int m_bytes_readed;
std::condition_variable m_oHaveSamplesCondition;
};
#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