S3 RPLIDAR Does Not Respond to Changes in Motor PWM

[ismet55555]

Note that all of this is without the RPLIDAR SDK, ie: #define ENABLE_RPLIDAR_SDK_SUPPORT commented out in hardwarex.h
I am not sure if this is an issue with the RPLIDAR SDK (will investigate further).

For the A3 RPLIDAR either of these will cause a change in spin rate:

• Changing the motorPWM value to the config file, RPLIDAR0.txt
• Using the SetMotorPWMRequestRPLIDAR() endpoint
• Hardcoding #define DEFAULT_MOTOR_PWM_RPLIDAR <VALUE>

However, with the S3 none of these will cause a change of spin rate.

A slight difference between the A3 and the S1 is the connector to the little USB adapter.
The differences are shown in the attached images. There seems to be no wire to the PWM connector. I am not sure if that is the fundamental issue here.

[ismet55555]

Just checked with the RPLIDAR SDK on Linux (Ubuntu 16.04)

A3 pwm/rpm can be changed.
S1 pwm/rpm can not be changed.

Another thing is that I am trying to expose GetFIFOComputerRS232Port() in OSComputerRS232Port.h but keep getting error for RPLIDAR SDK.

Basically:
hardwarex.h - HARDWAREX_API int GetFIFORPLIDARx(RPLIDAR* pRPLIDAR, int* count);
RPLIDAR.h - inline int GetFIFORPLIDAR(RPLIDAR* pRPLIDAR, int* count){ ....}
OSComputerRS232Port.h - GetFIFOComputerRS232Port(){ .....}

It doesn't seem to touch anything SDK related, but errors out here:
if (ioctl((intptr_t)hDev, FIONREAD, &bytes_avail) != EXIT_SUCCESS)
And I can't figure out what and why really...

/*
Get the input queue length on the computer RS232 port.

HANDLE hDev : (IN) Identifier of the computer RS232 port.
int count : (OUT) bytes in input queue.

Return : EXIT_SUCCESS if no error or EXIT_FAILURE if there is an error.
*/
inline int GetFIFOComputerRS232Port(HANDLE hDev, int* count)
{
#ifdef _WIN32
	COMSTAT stats;

	memset(&stats, 0, sizeof(COMSTAT));
	if (ClearCommError(hDev, NULL, &stats))
	{
		*count = stats.cbInQue;
		return EXIT_SUCCESS;
	}
	else
	{
		return EXIT_FAILURE;
	}
#else
	int bytes_avail = 0;

	if (ioctl((intptr_t)hDev, FIONREAD, &bytes_avail) != EXIT_SUCCESS)
	{
		PRINT_DEBUG_ERROR_OSCOMPUTERRS232PORT(("CheckAvailableBytesComputerRS232Port error (%s) : %s(hDev=%#x)\n",
			strtime_m(),
			GetLastErrorMsg(),
			hDev));
		return EXIT_FAILURE;
	}
	else
	{
		*count = bytes_avail;
		return EXIT_SUCCESS;
	}
#endif // _WIN32
}

[lebarsfa]

I did not have the time to check in details but about the pwm/rpm, I agree with what you say (I was more or less implying that in #3 (comment), i.e. SetLidarSpinSpeedRequestRPLIDAR() is the function to use for the S1 instead of SetMotorPWMRequestRPLIDAR() which is for the A2-A3, as implied by the source code of the SDK).

About GetFIFOComputerRS232Port(), in GetFIFORPLIDAR() you would probably need to call GetFIFOComputerRS232Port() with its HANDLE hDev parameter corresponding to the serial port opened internally by the RPLIDAR SDK (since pRPLIDAR->RS232Port cannot be initialized in that case because the SDK does its own things), which should be something like pRPLIDAR->drv->_chanDev->_rxtxSerial->_serial_handle (to check more in details), do you do something like that? Also, can you confirm that it works without RPLIDAR SDK?

[ismet55555]

As always, thank you for responding to this issue. And again, if happen to have a BuyMeCoffee account I can add to or something, let me know. Your responses have been more than helpful.

I must have missed the S1 only working with RPM and now PWM commands. This makes sense since using the frame grabber, and debugging it with VS code (attaching) I was able to see that setting pWM value will hit the speed endpoint.
I’ll try that and monitor.

So I attached the frame_grabber.exe to debug and I know the Rplidar SDK works and is able to manipulate the S1.

The using the SDK handle is a really good lead giving that the added GetFIFO Endpoint doesn’t reference the SDK and is based on some pre-SDK version Of your repo. The GetFIFO endpoint works without the SDK, which is more of an indicator of that I think.

Let me give it a shot and report back!

[ismet55555]

Well, SetLidarSpinSpeedRequestRPLIDAR() works, however I had it at a bad place in the code. It looks like for it to work I needed it after StartOtherScanRequestRPLIDAR().

As far as our custom added GetFIFORPLIDAR(), I am getting stuck on:

inline int GetFIFORPLIDAR(RPLIDAR* pRPLIDAR, int* count)
{
	RS232PORT* pRS232Port = &pRPLIDAR->drv->_chanDev->_rxtxSerial->_serial_handle;
	return GetFIFOComputerRS232Port(pRS232Port, count);
}

The issue is that the _rxtxSerial property is showing an error (class rp::standalone::rplidar::ChannelDevice has no member "_rxtxSeral"), which is odd since when tracing the reaction of RPlidarDriver it creates SerialChannelDevice which is the following. And it seems to have _rxtxSerial available. Not sure if I have to expose it on the SDK level so it can be referenced?

I think the reference to _rxtxSerial shows as rp::standalone::rplidar::SerialChannelDevice::_rxtxSerial in the SDK in rplidar_driver_serial.h, which I think it should be referencable in RPLIDAR.h?

[rplidar_driver_serial.h]

class SerialChannelDevice :public ChannelDevice
{
public:
    rp::hal::serial_rxtx  * _rxtxSerial;
    bool _closePending;

    SerialChannelDevice():_rxtxSerial(rp::hal::serial_rxtx::CreateRxTx()){}

    bool bind(const char * portname, uint32_t baudrate)
    {
        _closePending = false;
        return _rxtxSerial->bind(portname, baudrate);
    }
    bool open()
    {
        return _rxtxSerial->open();
    }
    void close()
    {
        _closePending = true;
        _rxtxSerial->cancelOperation();
        _rxtxSerial->close();
    }
    void flush()
    {
        _rxtxSerial->flush(0);
    }
    bool waitfordata(size_t data_count,_u32 timeout = -1, size_t * returned_size = NULL)
    {
        if (_closePending) return false;
        return (_rxtxSerial->waitfordata(data_count, timeout, returned_size) == rp::hal::serial_rxtx::ANS_OK);
    }
    int senddata(const _u8 * data, size_t size)
    {
        return _rxtxSerial->senddata(data, size) ;
    }
    int recvdata(unsigned char * data, size_t size)
    {
        size_t lenRec = 0;
        lenRec = _rxtxSerial->recvdata(data, size);
        return lenRec;
    }
    void setDTR()
    {
        _rxtxSerial->setDTR();
    }
    void clearDTR()
    {
        _rxtxSerial->clearDTR();
    }
    void ReleaseRxTx()
    {
        rp::hal::serial_rxtx::ReleaseRxTx(_rxtxSerial);
    }
};

So when debugging hardwarex in RPLIDAR.h I can see that _rxtxSerial is being refereced, however for SerialChannelDevice, (image)

(*((rp::arch::net::raw_serial*)(*((rp::standalone::rplidar::SerialChannelDevice*)pRPLIDAR->drv->_chanDev))._rxtxSerial))._serial_handle

This all is probably my lack of knowledge of C++ programming ...

[lebarsfa]

Yes, it seems due to C++ inheritance things that are blocking that (see e.g. https://stackoverflow.com/questions/19501838/get-derived-type-via-base-class-virtual-function). I quickly tried something like

rp::standalone::rplidar::SerialChannelDevice* pscd = dynamic_cast<rp::standalone::rplidar::SerialChannelDevice*>(drv->_chanDev);
rp::arch::net::raw_serial* prs = dynamic_cast<rp::arch::net::raw_serial*>(pscd->_rxtxSerial);

Then there is a problem with _serial_handle defined as protected in raw_serial class (see e.g. https://stackoverflow.com/questions/19862952/accessing-protected-function-from-another-class-c), it seems we can do that to access to it

namespace rp {
	namespace arch {
		namespace net {
			class raw_raw_serial : public raw_serial {
			public:
				raw_raw_serial(raw_serial rs) : raw_serial(rs) {}
				HANDLE get_serial_handle()
				{
					//return this->raw_serial::_serial_handle;
					return this->_serial_handle;
				}
			};
		}
	}
}

so that then we can do

rp::arch::net::raw_raw_serial rrs = *prs;
HANDLE hDev = rrs.get_serial_handle();

and then it should be possible to call Windows API functions using hDev...
You might need to include before headers

#include "arch/win32/arch_win32.h"
#include "arch/win32/net_serial.h"
#include "hal/abs_rxtx.h"
#include "hal/thread.h"
#include "hal/types.h"
#include "hal/assert.h"
#include "hal/locker.h"
#include "hal/socket.h"
#include "hal/event.h"
#include "rplidar_driver_impl.h"
#include "rplidar_driver_serial.h"

[ismet55555]

To be honest, I don't think I could have figured that out ...

So here is how I implemented it and it ALMOST works

• I added the extra raw_raw_serial class with the exposed _serial_handle to the bottom of net_serial.h in the SDK.
• Added the few lines you posted to RPLIDAR.h in to my custom GetFIFORPLIDAR() method

// Getting the length of the FIFO on the RS232 serial port
inline int GetFIFORPLIDAR(RPLIDAR* pRPLIDAR, int* count)
{
	rp::standalone::rplidar::SerialChannelDevice* pscd = dynamic_cast<rp::standalone::rplidar::SerialChannelDevice*>(pRPLIDAR->drv->_chanDev);
	rp::arch::net::raw_serial* prs = dynamic_cast<rp::arch::net::raw_serial*>(pscd->_rxtxSerial);

	rp::arch::net::raw_raw_serial rrs = *prs;
	HANDLE hDev = rrs.get_serial_handle();

	return GetFIFOComputerRS232Port(rrs.get_serial_handle(), count);
}

• Added the include statements you posted. into RPLIDAR.h BUT I was not able to build the solution as it was I needed to add rplidar_cmd.h and rplidar_driver.h so:

#include "arch/win32/arch_win32.h"
#include "arch/win32/net_serial.h"

#include "hal/abs_rxtx.h"
#include "hal/thread.h"
#include "hal/types.h"
#include "hal/assert.h"
#include "hal/locker.h"
#include "hal/socket.h"
#include "hal/event.h"

#include "rplidar_cmd.h"
#include "rplidar_driver.h"
#include "rplidar_driver_impl.h"
#include "rplidar_driver_serial.h"

• Build both, SDK and hardwarex
• Open MATLAB and run hardwarex_setup.m

Here is where I am getting an error that was odd:

Error using loadlibrary
Failed to preprocess the input file.
 Output from preprocessor is:hardwarex.h
rplidar_sdk\sdk\sdk\include\rplidar_driver.h(39): fatal error C1189: #error:  "The RPlidar SDK requires a C++
compiler to be built"


Error in hardwarex_setup (line 16)
        loadlibrary('hardwarex', 'hardwarex.h', 'notempdir', ...

Which is coming from rplidar_driver.h

#ifndef __cplusplus
#error "The RPlidar SDK requires a C++ compiler to be built"
#endif

I initially thought that somehow the C++ compiler wasn't set up so I ran mex -setup and things seem to be in order. I def am able to go through all the steps without the include statements and added code.

>> mex -setup
MEX configured to use 'Microsoft Visual C++ 2017 (C)' for C language compilation.
Warning: The MATLAB C and Fortran API has changed to support MATLAB
	 variables with more than 2^32-1 elements. You will be required
	 to update your code to utilize the new API.
	 You can find more information about this at:
	 https://www.mathworks.com/help/matlab/matlab_external/upgrading-mex-files-to-use-64-bit-api.html.

To choose a different C compiler, select one from the following:
MinGW64 Compiler (C)  mex -setup:'C:\Program Files\MATLAB\R2018b\bin\win64\mexopts\mingw64.xml' C
Microsoft Visual C++ 2015 (C)  mex -setup:'C:\Program Files\MATLAB\R2018b\bin\win64\mexopts\msvc2015.xml' C
Microsoft Visual C++ 2017 (C)  mex -setup:C:\Users\ihandzic\AppData\Roaming\MathWorks\MATLAB\R2018b\mex_C_win64.xml C

To choose a different language, select one from the following:
 mex -setup C++ 
 mex -setup FORTRAN

Checking the Visual Studio code project, the linkers seem to be pointing to where the SDK outputted the static library.

If i get rid of the block of code in the SDK with the __cplusplus macro check, it all builds, however when running the MATLAB hardware_setup.m script, it yields a ton of errors ...

[lebarsfa]

I am not sure what is happening, maybe it is related to the order or place of the #include...
If you consider it's OK to modify the SDK, the problem of _serial_handle defined as protected in raw_serial class can be more easily solved by replacing protected with public, and maybe also remove the volatile (I am not sure but maybe it was causing problems at runtime) before _serial_handle in net_serial.h.
For the rest, in RPLIDAR.h try to replace

#include "../include/rplidar.h"
using namespace rp::standalone::rplidar;

with

#include "../include/rplidar.h"

//#include "arch/win32/arch_win32.h"
#include "arch/win32/net_serial.h"
#include "hal/abs_rxtx.h"
#include "hal/thread.h"
//#include "hal/types.h"
//#include "hal/assert.h"
#include "hal/locker.h"
//#include "hal/socket.h"
#include "hal/event.h"
#include "rplidar_driver_impl.h"
#include "rplidar_driver_serial.h"

 // Because _serial_handle is protected...
namespace rp {
	namespace arch {
		namespace net {
			class raw_raw_serial : public raw_serial {
			public:
				raw_raw_serial(raw_serial rs) : raw_serial(rs) {}
				HANDLE get_serial_handle()
				{
					//return this->raw_serial::_serial_handle;
					return this->_serial_handle;
				}
			};
		}
	}
}

using namespace rp::standalone::rplidar;

and then as a test I tried to replace

	if (IS_FAIL(pRPLIDAR->drv->stop()))
	{
		printf("A RPLIDAR is not responding correctly : stop() failed. \n");
		return EXIT_FAILURE;
	}

with

	rp::standalone::rplidar::SerialChannelDevice* pscd = dynamic_cast<rp::standalone::rplidar::SerialChannelDevice*>(pRPLIDAR->drv->_chanDev);
	rp::arch::net::raw_serial* prs = dynamic_cast<rp::arch::net::raw_serial*>(pscd->_rxtxSerial);
	//rp::arch::net::raw_raw_serial rrs = rp::arch::net::raw_raw_serial(*prs);
	//HANDLE hDev = rrs.get_serial_handle();
	HANDLE hDev = prs->_serial_handle;

	unsigned char reqbuf[] = {0xA5,0x25};

	// Send request.
	if (WriteAllComputerRS232Port(hDev, reqbuf, sizeof(reqbuf)) != EXIT_SUCCESS)
	{
		printf("Error writing data to a RPLIDAR. \n");
		return EXIT_FAILURE;
	}

in StopRequestRPLIDAR (note that it is probably not fully equivalent to drv->stop())...

[lebarsfa]

Also, did you run also mex -setup C++ (and not only mex -setup) in MATLAB?

[ismet55555]

I did not run mex -setup C++ before, and only ran mex -setup. I just ran mex -setup C++, trying it with all different compiler options, however I'm getting the same result. I kept it at Microsoft Visual C++ 2017

I'm about to tinker with your updated suggestions and see if I can get it to work ...

[ismet55555]

Well I think it was the position of the include statments. I had the include statments in RPLIDAR.h right after #include "RS232Port.h" ... however needed it after using namespace rp::standalone::rplidar;

I also tried changing those SDK class properties that include _rxtxSerial to public.

Tried adding the extra class to net_serial.h and/or to RPLIDAR.h in the position you said.

I was able to build both, hardwarex and the SDK, (changing things every which way), and it looks like the reference to the handle seems off. I get this from MATLAB when running hardwarex_setup.m

c:\...\rplidar\RPLIDAR.h(735): error C2143: syntax error: missing ';' before ':'
c:\...\rplidar\RPLIDAR.h(736): error C2143: syntax error: missing ';' before ':'
c:\...\rplidar\RPLIDAR.h(741): error C2065: 'prs': undeclared identifier
c:\...\rplidar\RPLIDAR.h(741): error C2223: left of '->_serial_handle' must point to struct/union
c:\...\rplidar\RPLIDAR.h(736): error C2045: 'rp': label redefined

which seems to be that reference (tried the commented out code as well)

	rp::standalone::rplidar::SerialChannelDevice* pscd = dynamic_cast<rp::standalone::rplidar::SerialChannelDevice*>(pRPLIDAR->drv->_chanDev);
	rp::arch::net::raw_serial* prs = dynamic_cast<rp::arch::net::raw_serial*>(pscd->_rxtxSerial);

	//rp::arch::net::raw_raw_serial rrs = rp::arch::net::raw_raw_serial(*prs);
	//HANDLE hDev = rrs.get_serial_handle();

	HANDLE hDev = prs->_serial_handle;

	return GetFIFOComputerRS232Port(hDev, count);

So tried it on linux, and linux def doesn't like it since the reference seems to be different in its rplidar_sdk\sdk\sdk\src\arch\linux\net_serial.h:

    bool open(const char * portname, uint32_t baudrate, uint32_t flags = 0);
    void _init();

    char _portName[200];
    uint32_t _baudrate;
    uint32_t _flags;

    int serial_fd;

    size_t required_tx_cnt;
    size_t required_rx_cnt;

    int    _selfpipe[2];
    bool   _operation_aborted;

Ultimately we are using the LIDAR in Linux. I wasn't initially aware until now that the reference would only work on Windows, given that the Windows serial port handle is:
(\rplidar_sdk\sdk\sdk\src\arch\win32\net_serial.h)

    bool open(const char * portname, _u32 baudrate, _u32 flags);
    void _init();

    char _portName[20];
    uint32_t _baudrate;
    uint32_t _flags;

    OVERLAPPED _ro, _wo;
    OVERLAPPED _wait_o;
    HANDLE _serial_handle;
    DCB _dcb;
    COMMTIMEOUTS _co;

Would the serial port reference for linux be serial_fd?

[lebarsfa]

Yes, for Linux it is very likely to be serial_fd. Note that if you changed protected to public in net_serial.h you can remove

namespace rp {
	namespace arch {
		namespace net {
			class raw_raw_serial : public raw_serial {
			public:
				raw_raw_serial(raw_serial rs) : raw_serial(rs) {}
				HANDLE get_serial_handle()
				{
					//return this->raw_serial::_serial_handle;
					return this->_serial_handle;
				}
			};
		}
	}
}

in case that part is causing problems, otherwise I would say you have to play with the order/place of the includes and maybe add or remove some includes from some SDK files...

[ismet55555]

So maybe serial_fd is not what I am looking for. So the the GetFIFOComputerRS232Port(HANDLE hDev, int* count) I am trying to use, it needs some handle, however, the serial_fd is an integer. I know that it should be cast into HANDLE or void somehow, but I can't seem to figure it out.

So with everything in place, either with the public or protected way, and with and wihtout the exposing the protected properties (when it is set to protected), everything builds and runs fine if I exclude:

	rp::standalone::rplidar::SerialChannelDevice* pscd = dynamic_cast<rp::standalone::rplidar::SerialChannelDevice*>(pRPLIDAR->drv->_chanDev);
	rp::arch::net::raw_serial* prs = dynamic_cast<rp::arch::net::raw_serial*>(pscd->_rxtxSerial);

	rp::arch::net::raw_raw_serial rrs = *prs;
	HANDLE hDev = rrs.get_serial_handle();

	return GetFIFOComputerRS232Port(hDev, count);

That is, any combination of that block of code commented or not commented. So if any of that is used, it throws errors.
First, at hardwarex compile, it complains invalid conversion from 'int' to 'HANDLE {aka void*} [-fpremissive]

Than when running hardwarex_setup.m I get:

Error using loadlibrary
Building hardwarex_thunk_glnxa64 failed.  Compiler output is:
/usr/bin/gcc -I"MAVLinkSDK" -I"/usr/local/include" -I"sbgECom/src" -I"sbgECom/common" -I"/usr/local/include/sbgECom/src"
-I"/usr/local/include/sbgECom/common" -I"rplidar_sdk/sdk/sdk/include" -I"rplidar_sdk/sdk/sdk/src"
-I"/usr/local/include/rplidar_sdk/sdk/sdk/include" -I"/usr/local/include/rplidar_sdk/sdk/sdk/src" -I"/usr/local/MATLAB/R2018b/extern/include"
-fexceptions -fPIC -fno-omit-frame-pointer -pthread -I"/rplidar"
-I"/rplidar" "hardwarex_thunk_glnxa64.c" -o "hardwarex_thunk_glnxa64.so"
-Wl,-E -shared
In file included from /rplidar/hardwarex.h:87:0,
                 from hardwarex_thunk_glnxa64.c:27:
/rplidar/RPLIDAR.h: In function ‘GetFIFORPLIDAR’:
/rplidar/RPLIDAR.h:735:5: error: expected expression before ‘:’ token
  rp::standalone::rplidar::SerialChannelDevice* pscd = dynamic_cast<rp::standalone::rplidar::SerialChannelDevice*>(pRPLIDAR->drv->_chanDev);

     ^
/rplidar/RPLIDAR.h:736:2: error: duplicate label ‘rp’
  rp::arch::net::raw_serial* prs = dynamic_cast<rp::arch::net::raw_serial*>(pscd->_rxtxSerial);

  ^
/rplidar/RPLIDAR.h:735:2: note: previous definition of ‘rp’ was here
  rp::standalone::rplidar::SerialChannelDevice* pscd = dynamic_cast<rp::standalone::rplidar::SerialChannelDevice*>(pRPLIDAR->drv->_chanDev);

  ^
/rplidar/RPLIDAR.h:736:5: error: expected expression before ‘:’ token
  rp::arch::net::raw_serial* prs = dynamic_cast<rp::arch::net::raw_serial*>(pscd->_rxtxSerial);

     ^
/rplidar/RPLIDAR.h:738:2: error: duplicate label ‘rp’
  rp::arch::net::raw_raw_serial rrs = rp::arch::net::raw_raw_serial(*prs);

  ^
/rplidar/RPLIDAR.h:735:2: note: previous definition of ‘rp’ was here
  rp::standalone::rplidar::SerialChannelDevice* pscd = dynamic_cast<rp::standalone::rplidar::SerialChannelDevice*>(pRPLIDAR->drv->_chanDev);

  ^
/rplidar/RPLIDAR.h:738:5: error: expected expression before ‘:’ token
  rp::arch::net::raw_raw_serial rrs = rp::arch::net::raw_raw_serial(*prs);

     ^
/rplidar/RPLIDAR.h:739:16: error: ‘rrs’ undeclared (first use in this
function)
  HANDLE hDev = rrs.get_serial_handle();

                ^
/rplidar/RPLIDAR.h:739:16: note: each undeclared identifier is reported
only once for each function it appears in

I definitely need to study up on this more, because those references and dynamic_casting is pretty foreign to me

[lebarsfa]

About serial_fd, if think it is really what you need but you need to adapt GetFIFOComputerRS232Port() so that it calls Linux serial port functions, and you should probably not use HANDLE type any more since Linux is not supposed to use it.
For the build errors, I will try to check on Linux when I have the time, it looks as if RPLIDAR.h is built as C code and not C++ (but maybe it is something else...)... Also, on Windows I was using MATLAB 2017 and not 2018, I don't know if there could be changes in the behavior depending on the version...

[ismet55555]

Thanks for the swift response again.
Let me see if I can modify GetFIFOComputerRS232Port() to fit serial_fd and see if I can get around MATLABs compile errors

[ismet55555]

Question: So you mentioned you are running MATLAB R2017b, what compiler are you using with matlab's loadlibrary function to compile the interface files on Linux?

I'm getting a MATLAB compile error on the the :: part of rp::standalone::rplidar::SerialChannelDevice*, that first :: ... I really think it's the compiler I'm using on Ubuntu.

The MATLAB docs say that for MATLAB R2018b, the recommend GCC 6.3.x versions.

When running g++ --version on my system, I get, 5.4.0. That's running on Ubuntu 16.04 (i know i know, pretty retro, but that's what I'm working with now)

I will upgrade and try it out ...