/* +------------------------------------------------------------------------+ | Mobile Robot Programming Toolkit (MRPT) | | https://www.mrpt.org/ | | | | Copyright (c) 2005-2026, Individual contributors, see AUTHORS file | | See: https://www.mrpt.org/Authors - All rights reserved. | | Released under BSD License. See: https://www.mrpt.org/License | +------------------------------------------------------------------------+ */ #include "hwdrivers-precomp.h" // Precompiled headers // #include #include #include #include #include #include #include using namespace mrpt::math; using namespace mrpt::obs; using namespace mrpt::poses; using namespace mrpt::hwdrivers; using namespace mrpt::io; using namespace std; IMPLEMENTS_GENERIC_SENSOR(CEnoseModular, mrpt::hwdrivers) /*------------------------------------------------------------- CEnoseModular -------------------------------------------------------------*/ CEnoseModular::CEnoseModular() : m_usbSerialNumber("ENOSE002"), m_COM_port() { m_sensorLabel = "EnoseModular"; first_reading = true; } /*------------------------------------------------------------- loadConfig_sensorSpecific -------------------------------------------------------------*/ void CEnoseModular::loadConfig_sensorSpecific( const mrpt::config::CConfigFileBase& configSource, const std::string& iniSection) { MRPT_START m_usbSerialNumber = configSource.read_string(iniSection, "USB_serialname", "", false); #ifdef _WIN32 m_COM_port = configSource.read_string(iniSection, "COM_port_WIN", "COM1"); #else m_COM_port = configSource.read_string(iniSection, "COM_port_LIN", m_COM_port); #endif m_COM_baud = configSource.read_uint64_t(iniSection, "COM_baudRate", m_COM_baud); MRPT_END } /*------------------------------------------------------------- checkConnectionAndConnect -------------------------------------------------------------*/ CStream* CEnoseModular::checkConnectionAndConnect() { // Make sure one of the two possible pipes is open: if (!m_stream_FTDI && !m_stream_SERIAL) { if (!m_COM_port.empty()) m_stream_SERIAL = std::make_unique(); else m_stream_FTDI = std::make_unique(); } if (m_stream_FTDI) { // FTDI pipe ================== if (m_stream_FTDI->isOpen()) return m_stream_FTDI.get(); try { m_stream_FTDI->OpenBySerialNumber(m_usbSerialNumber); std::this_thread::sleep_for(10ms); m_stream_FTDI->Purge(); std::this_thread::sleep_for(10ms); m_stream_FTDI->SetLatencyTimer(1); m_stream_FTDI->SetTimeouts(10, 100); return m_stream_FTDI.get(); } catch (...) { // Error opening device: m_stream_FTDI->Close(); return nullptr; } } else { // Serial pipe ================== ASSERT_(m_stream_SERIAL); if (m_stream_SERIAL->isOpen()) return m_stream_SERIAL.get(); try { m_stream_SERIAL->open(m_COM_port); m_stream_SERIAL->setConfig(m_COM_baud); // m_stream_SERIAL->setTimeouts(25,1,100, 1,20); m_stream_SERIAL->setTimeouts(50, 1, 100, 1, 20); std::this_thread::sleep_for(10ms); m_stream_SERIAL->purgeBuffers(); std::this_thread::sleep_for(10ms); return m_stream_SERIAL.get(); } catch (...) { // Error opening device: m_stream_SERIAL->close(); return nullptr; } } } /*------------------------------------------------------------- getObservation -------------------------------------------------------------*/ bool CEnoseModular::getObservation(mrpt::obs::CObservationGasSensors& obs) { try { // Connected? CStream* comms = checkConnectionAndConnect(); if (!comms) { cout << "ERORR: Problem connecting to Device." << endl; return false; } CObservationGasSensors::TObservationENose newRead; obs.m_readings.clear(); //---------------------------- Enose Modular FRAME //-------------------------------------------------- // Wait for e-nose frame: <0x69><0x91><0x96> "Bytes" // Where = [] x // N_senosrs // Modular-nose provides a 4B+body frame length mrpt::serialization::CMessage msg; bool time_out = false; mrpt::system::TTimeStamp t1 = mrpt::Clock::now(); double time_out_val = 1; // seconds auto arch = mrpt::serialization::archiveFrom(*comms); while (!arch.receiveMessage(msg) && !time_out) { if (mrpt::system::timeDifference(t1, mrpt::Clock::now()) > time_out_val) time_out = true; } if (time_out) { cout << "[CEnoseModular - getObservation] measurement Timed-Out" << endl; return false; } if (msg.content.size() > 0) { // Each sensor reading is composed of 3 Bytes // [] ASSERT_((msg.content.size() - 1) % 3 == 0); size_t numSensors = (msg.content.size() - 1) / 3; // Prepare the Enose observation newRead.sensorTypes.clear(); newRead.readingsVoltage.clear(); newRead.hasTemperature = true; newRead.isActive = true; // Do we have the sensor position? if (enose_poses_x.size() != 0) { newRead.eNosePoseOnTheRobot = TPose3D( enose_poses_x[0], enose_poses_y[0], enose_poses_z[0], enose_poses_yaw[0], enose_poses_pitch[0], enose_poses_roll[0]); } else newRead.eNosePoseOnTheRobot = TPose3D(0, 0, 0, 0, 0, 0); // Get Temperature (degrees C) newRead.temperature = msg.content[0] * 1.65214 - 277.74648; // process all sensors for (size_t idx = 0; idx < numSensors; idx++) { // Copy ID (2 BYTES) To integer int sensorType_temp = 0; // memcpy( &sensorType, &msg.content[idx*3+1], // 2*sizeof(msg.content[0]) ); memcpy(&sensorType_temp, &msg.content[idx * 3 + 1], sizeof(msg.content[0])); int sensorType = sensorType_temp << (8); memcpy(&sensorType, &msg.content[idx * 3 + 2], sizeof(msg.content[0])); // Add sensor Type (ID) newRead.sensorTypes.push_back(sensorType); // Transform from ADC value[8bits] to [0-0.6] volt range: newRead.readingsVoltage.push_back((msg.content[idx * 3 + 3] * 0.6f) / 255.0f); } // Purge buffers purgeBuffers(); // Add data to observation: obs.m_readings.push_back(newRead); obs.sensorLabel = m_sensorLabel; obs.timestamp = mrpt::Clock::now(); return !obs.m_readings.empty(); // Done OK! } else { cout << "Message was empty" << endl; return false; } } catch (exception& e) { cerr << "[CEnoseModular::getObservation] Returning false due to " "exception: " << endl; cerr << e.what() << endl; return false; } catch (...) { return false; } } /*------------------------------------------------------------- doProcess -------------------------------------------------------------*/ /** This method should be called periodically (at least at 1Hz to capture ALL * the real-time data) * It is thread safe, i.e. you can call this from one thread, then to other * methods from other threads. */ void CEnoseModular::doProcess() { CObservationGasSensors::Ptr obs = std::make_shared(); if (getObservation(*obs)) { m_state = ssWorking; appendObservation(obs); } else { m_state = ssError; cout << "No observation received from the USB board!" << endl; // THROW_EXCEPTION("No observation received from the USB board!"); } } /*------------------------------------------------------------- purgeBuffers -------------------------------------------------------------*/ void CEnoseModular::purgeBuffers() { if (!checkConnectionAndConnect()) return; if (m_stream_FTDI) { // FTDI pipe m_stream_FTDI->Purge(); } else { // Serial pipe m_stream_SERIAL->purgeBuffers(); } }