Updated RPLIDAR support and updates from UxVCtrl V360

.gitignore

@@ -269,3 +269,4 @@ TODO/
 UpgradeOSUtils.bat
 UpgradeHardware.bat
 UpgradeDLL*
+genproto*

GetAllSupportedScanModesRPLIDAR.m

@@ -0,0 +1,15 @@
+function [result, scanmodeids, scanmodeuspersamples, scanmodemaxdistances, scanmodeanstypes, scanmodenames] = GetAllSupportedScanModesRPLIDAR(pRPLIDAR)
+
+scanmodeids = repmat(int32(0), [16 1]);
+scanmodeuspersamples = repmat(double(0), [16 1]);
+scanmodemaxdistances = repmat(double(0), [16 1]);
+scanmodeanstypes = repmat(int32(0), [16 1]);
+scanmodenames = repmat('0', [16 64]);
+
+pScanModeIDs = libpointer('int32Ptr', scanmodeids);
+pScanModeusPerSamples = libpointer('doublePtr', scanmodeuspersamples);
+pScanModeMaxDistances = libpointer('doublePtr', scanmodemaxdistances);
+pScanModeAnsTypes = libpointer('int32Ptr', scanmodeanstypes);
+pScanModeNames = cellstr(scanmodenames);
+
+[result, rplidar, scanmodeids, scanmodeuspersamples, scanmodemaxdistances, scanmodeanstypes, scanmodenames] = calllib('hardwarex', 'GetAllSupportedScanModesRPLIDARx', pRPLIDAR, pScanModeIDs, pScanModeusPerSamples, pScanModeMaxDistances, pScanModeAnsTypes, pScanModeNames);

GetInfoRequestRPLIDAR.m

@@ -4,18 +4,12 @@
 hardwareVersion = 0;
 firmwareMajor = 0;
 firmwareMinor = 0;
-serialNumber = repmat('0', [1 32]);
+serialNumber = [repmat(int8('0'), [1 32]) int8(0)];
 
 pModelID = libpointer('int32Ptr', modelID);
 pHardwareVersion = libpointer('int32Ptr', hardwareVersion);
 pFirmwareMajor = libpointer('int32Ptr', firmwareMajor);
 pFirmwareMinor = libpointer('int32Ptr', firmwareMinor);
-SerialNumber = libpointer('voidPtr', [int8(serialNumber) 0]);
+SerialNumber = char(serialNumber);
 
-result = calllib('hardwarex', 'GetInfoRequestRPLIDARx', pRPLIDAR, pModelID, pHardwareVersion, pFirmwareMajor, pFirmwareMinor, SerialNumber);
-
-modelID = pModelID.value;
-hardwareVersion = pHardwareVersion.value;
-firmwareMajor = pFirmwareMajor.value;
-firmwareMinor = pFirmwareMinor.value;
-serialNumber = char(SerialNumber.value);
+[result, rplidar, modelID, hardwareVersion, firmwareMajor, firmwareMinor, serialNumber] = calllib('hardwarex', 'GetInfoRequestRPLIDARx', pRPLIDAR, pModelID, pHardwareVersion, pFirmwareMajor, pFirmwareMinor, SerialNumber);

GetOtherScanDataResponseFromThreadRPLIDAR.m

@@ -0,0 +1,15 @@
+function [result, distances, angles, bNewScan] = GetOtherScanDataResponseFromThreadRPLIDAR(pRPLIDAR)
+
+distances = repmat(0, [1 32]);
+angles = repmat(0, [1 32]);
+bNewScan = 0;
+
+pDistances = libpointer('doublePtr', distances);
+pAngles = libpointer('doublePtr', angles);
+pbNewScan = libpointer('int32Ptr', bNewScan);
+
+result = calllib('hardwarex', 'GetOtherScanDataResponseFromThreadRPLIDARx', pRPLIDAR, pDistances, pAngles, pbNewScan);
+
+distances = pDistances.value;
+angles = pAngles.value;
+bNewScan = pbNewScan.value;

GetTypicalScanModeRPLIDAR.m

@@ -0,0 +1,9 @@
+function [result, scanmodeid] = GetTypicalScanModeRPLIDAR(pRPLIDAR)
+
+scanmodeid = 0;
+
+pScanModeID = libpointer('int32Ptr', scanmodeid);
+
+result = calllib('hardwarex', 'GetTypicalScanModeRPLIDARx', pRPLIDAR, pScanModeID);
+
+scanmodeid = pScanModeID.value;

NMEAProtocol.h

@@ -285,8 +285,13 @@ inline int AnalyzeSentenceNMEA(char* buf, int buflen, char* talkerid, char* mnem
 		ComputeChecksumNMEA(buf, *psentencelen, checksum);
 		if ((toupper(buf[*psentencelen-2-nb_bytes_end]) != checksum[1])||(toupper(buf[*psentencelen-1-nb_bytes_end]) != checksum[2]))
 		{ 
-			PRINT_DEBUG_WARNING(("Warning : NMEA checksum error (computed \"%.3s\", found \"*%c%c\"). \n", checksum, buf[*psentencelen-2-nb_bytes_end], buf[*psentencelen-1-nb_bytes_end]));
-			*pnbBytesToDiscard = *psentencelen;
+			PRINT_DEBUG_MESSAGE_OSUTILS(("Warning : NMEA checksum error (computed \"%.3s\", found \"*%c%c\"). \n", checksum, buf[*psentencelen-2-nb_bytes_end], buf[*psentencelen-1-nb_bytes_end]));
+			//for (i = 0; i < buflen; i++) printf("%c", buf[i]);
+			//printf("\n");
+			//for (i = 0; i < buflen; i++) printf("0x%02x ", (unsigned)buf[i]);			
+			//printf("\n");
+			//*pnbBytesToDiscard = *psentencelen;
+			*pnbBytesToDiscard = 1; // Not sure more than the start character can be discarded...
 			return EXIT_FAILURE;	
 		}
 	}
@@ -488,8 +493,13 @@ inline int AnalyzeSentenceWithAddressNMEA(char* buf, int buflen, char* talkerid,
 		ComputeChecksumNMEA(buf, *psentencelen, checksum);
 		if ((buf[*psentencelen-2-nb_bytes_end] != checksum[1])||(buf[*psentencelen-1-nb_bytes_end] != checksum[2]))
 		{ 
-			PRINT_DEBUG_WARNING(("Warning : NMEA checksum error (computed \"%.3s\", found \"*%c%c\"). \n", checksum, buf[*psentencelen-2-nb_bytes_end], buf[*psentencelen-1-nb_bytes_end]));
-			*pnbBytesToDiscard = *psentencelen;
+			PRINT_DEBUG_MESSAGE_OSUTILS(("Warning : NMEA checksum error (computed \"%.3s\", found \"*%c%c\"). \n", checksum, buf[*psentencelen-2-nb_bytes_end], buf[*psentencelen-1-nb_bytes_end]));
+			//for (i = 0; i < buflen; i++) printf("%c", buf[i]);
+			//printf("\n");
+			//for (i = 0; i < buflen; i++) printf("0x%02x ", (unsigned)buf[i]);			
+			//printf("\n");
+			//*pnbBytesToDiscard = *psentencelen;
+			*pnbBytesToDiscard = 1; // Not sure more than the start character can be discarded...
 			return EXIT_FAILURE;	
 		}
 	}

P33x.h

@@ -121,10 +121,10 @@ inline int InitP33x(P33X* pP33x)
 	buf = readbuf[6];
 	stat = readbuf[7];
 
-	//PRINT_DEBUG_MESSAGE(("Device ID : %d.%d\n", devclass, group));
-	//PRINT_DEBUG_MESSAGE(("Firmware version : %d.%d\n", year, week));
-	//PRINT_DEBUG_MESSAGE(("Length of the internal receive buffer : %d\n", buf));
-	//PRINT_DEBUG_MESSAGE(("Status information : %d\n", stat));
+	//PRINT_DEBUG_MESSAGE_OSUTILS(("Device ID : %d.%d\n", devclass, group));
+	//PRINT_DEBUG_MESSAGE_OSUTILS(("Firmware version : %d.%d\n", year, week));
+	//PRINT_DEBUG_MESSAGE_OSUTILS(("Length of the internal receive buffer : %d\n", buf));
+	//PRINT_DEBUG_MESSAGE_OSUTILS(("Status information : %d\n", stat));
 
 	return EXIT_SUCCESS;
 }
@@ -202,8 +202,8 @@ inline int ReadChannelP33x(P33X* pP33x, uint8 Channel, float* pValue)
 
 	*pValue = value.v;
 
-	//PRINT_DEBUG_MESSAGE(("Channel value : %f\n", (double)*pValue));
-	//PRINT_DEBUG_MESSAGE(("Status information : %d\n", stat));
+	//PRINT_DEBUG_MESSAGE_OSUTILS(("Channel value : %f\n", (double)*pValue));
+	//PRINT_DEBUG_MESSAGE_OSUTILS(("Status information : %d\n", stat));
 
 	return EXIT_SUCCESS;
 }

RPLIDAR.h

@@ -100,6 +100,7 @@ using namespace rp::standalone::rplidar;
 #define MEASUREMENT_CAPSULED_RESPONSE_RPLIDAR 0x82 // Added in FW ver 1.17.
 #define SAMPLERATE_RESPONSE_RPLIDAR 0x15 // Added in FW ver 1.17.
 #define ACC_BOARD_FLAG_RESPONSE_RPLIDAR 0xFF
+#else
 #endif // !ENABLE_RPLIDAR_SDK_SUPPORT
 
 #define NB_BYTES_SERIAL_NUMBER_RPLIDAR 16
@@ -131,6 +132,18 @@ using namespace rp::standalone::rplidar;
 
 #define MAX_NB_MEASUREMENTS_PER_SCAN_RPLIDAR 8192
 
+#define MAX_NB_SCAN_MODES_RPLIDAR 16
+
+struct RPLIDARSCANMODE
+{
+	int id;
+	double us_per_sample;
+	double max_distance;
+	int ans_type;
+	char scan_mode[64];
+};
+typedef struct RPLIDARSCANMODE RPLIDARSCANMODE;
+
 #pragma endregion
 struct RPLIDAR
 {
@@ -842,6 +855,57 @@ inline int GetSampleRateRequestRPLIDAR(RPLIDAR* pRPLIDAR, int* pTstandard, int*
 #endif // ENABLE_RPLIDAR_SDK_SUPPORT
 }
 
+inline int GetTypicalScanModeRPLIDAR(RPLIDAR* pRPLIDAR, int* pScanModeID)
+{
+#ifdef ENABLE_RPLIDAR_SDK_SUPPORT
+	_u16 scanmodeid = 0;
+
+	if (IS_FAIL(pRPLIDAR->drv->getTypicalScanMode(scanmodeid)))
+	{
+		printf("A RPLIDAR is not responding correctly : getTypicalScanMode() failed. \n");
+		return EXIT_FAILURE;
+	}
+	*pScanModeID = (int)scanmodeid;
+	return EXIT_SUCCESS;
+#else
+	UNREFERENCED_PARAMETER(pRPLIDAR);
+	UNREFERENCED_PARAMETER(pScanModeID);
+	printf("RPLIDAR : Not implemented. \n");
+	return EXIT_NOT_IMPLEMENTED;
+#endif // ENABLE_RPLIDAR_SDK_SUPPORT
+}
+
+// MAX_NB_SCAN_MODES_RPLIDAR scanmodes...
+inline int GetAllSupportedScanModesRPLIDAR(RPLIDAR* pRPLIDAR, RPLIDARSCANMODE* pScanModes)
+{
+#ifdef ENABLE_RPLIDAR_SDK_SUPPORT
+	std::vector<RplidarScanMode> outModes;
+	unsigned int i = 0;
+
+	if (IS_FAIL(pRPLIDAR->drv->getAllSupportedScanModes(outModes)))
+	{
+		printf("A RPLIDAR is not responding correctly : getAllSupportedScanModes() failed. \n");
+		return EXIT_FAILURE;
+	}
+	if (outModes.size() > MAX_NB_SCAN_MODES_RPLIDAR) printf("Warning : RPLIDAR unsupported scanmodes. \n");
+	memset(pScanModes, 0, sizeof(RPLIDARSCANMODE)*MAX_NB_SCAN_MODES_RPLIDAR);
+	for (i = 0; i < min(outModes.size(), MAX_NB_SCAN_MODES_RPLIDAR); i++)
+	{
+		pScanModes[i].id = (int)outModes[i].id;
+		pScanModes[i].us_per_sample = (double)outModes[i].us_per_sample;
+		pScanModes[i].max_distance = (double)outModes[i].max_distance;
+		pScanModes[i].ans_type = (int)outModes[i].ans_type;
+		memcpy(pScanModes[i].scan_mode, outModes[i].scan_mode, sizeof(pScanModes[i].scan_mode));
+	}
+	return EXIT_SUCCESS;
+#else
+	UNREFERENCED_PARAMETER(pRPLIDAR);
+	UNREFERENCED_PARAMETER(pScanModes);
+	printf("RPLIDAR : Not implemented. \n");
+	return EXIT_NOT_IMPLEMENTED;
+#endif // ENABLE_RPLIDAR_SDK_SUPPORT
+}
+
 // Undocumented...
 inline int CheckMotorControlSupportRequestRPLIDAR(RPLIDAR* pRPLIDAR)
 {
@@ -1241,20 +1305,19 @@ inline int GetExpressScanDataResponseRPLIDAR(RPLIDAR* pRPLIDAR, double* pDistanc
 	return EXIT_SUCCESS;
 }
 
-inline int StartOtherScanRequestRPLIDAR(RPLIDAR* pRPLIDAR, int scanmode)
+inline int StartOtherScanRequestRPLIDAR(RPLIDAR* pRPLIDAR, int scanmodeid)
 {
 #ifdef ENABLE_RPLIDAR_SDK_SUPPORT
-	if (IS_FAIL(pRPLIDAR->drv->startScanExpress(false, (_u16)scanmode)))
+	if (IS_FAIL(pRPLIDAR->drv->startScanExpress(false, (_u16)scanmodeid)))
 	{
 		printf("A RPLIDAR is not responding correctly : startScanExpress() failed. \n");
 		return EXIT_FAILURE;
 	}
 	return EXIT_SUCCESS;
 #else
-	UNREFERENCED_PARAMETER(pRPLIDAR);
-	UNREFERENCED_PARAMETER(scanmode);
-	printf("RPLIDAR : Not implemented. \n");
-	return EXIT_NOT_IMPLEMENTED;
+	UNREFERENCED_PARAMETER(scanmodeid);
+	printf("RPLIDAR : Not implemented, defaulting to SCAN. \n");
+	return StartScanRequestRPLIDAR(pRPLIDAR);
 #endif // ENABLE_RPLIDAR_SDK_SUPPORT
 }
 
@@ -1302,12 +1365,24 @@ inline int GetOtherScanDataResponseRPLIDAR(RPLIDAR* pRPLIDAR, double* pDistances
 
 	return EXIT_SUCCESS;
 #else
-	UNREFERENCED_PARAMETER(pRPLIDAR);
-	UNREFERENCED_PARAMETER(pDistances);
-	UNREFERENCED_PARAMETER(pAngles);
-	UNREFERENCED_PARAMETER(pbNewScan);
-	printf("RPLIDAR : Not implemented. \n");
-	return EXIT_NOT_IMPLEMENTED;
+	int j = 0;
+
+	for (j = 0; j < NB_MEASUREMENTS_OTHER_SCAN_DATA_RESPONSE_RPLIDAR; j++)
+	{
+		double distance = 0, angle = 0;
+		BOOL bNewScan = FALSE;
+		int quality = 0;
+
+		if (GetScanDataResponseRPLIDAR(pRPLIDAR, &distance, &angle, &bNewScan, &quality) != EXIT_SUCCESS)
+		{
+			return EXIT_FAILURE;
+		}
+		*pbNewScan = (*pbNewScan) | (bNewScan);
+		pAngles[j] = angle;
+		pDistances[j] = distance;
+	}
+
+	return EXIT_SUCCESS;
 #endif // ENABLE_RPLIDAR_SDK_SUPPORT
 }
 
@@ -1549,18 +1624,7 @@ inline int ConnectRPLIDAR(RPLIDAR* pRPLIDAR, char* szCfgFilePath)
 		//	return EXIT_FAILURE;
 		//}
 
-		if (SetMotorPWMRequestRPLIDAR(pRPLIDAR, DEFAULT_MOTOR_PWM_RPLIDAR) != EXIT_SUCCESS)
-		{
-			printf("Unable to connect to a RPLIDAR : SET_MOTOR_PWM failure.\n");
-#ifdef ENABLE_RPLIDAR_SDK_SUPPORT
-			pRPLIDAR->drv->stopMotor();
-			pRPLIDAR->drv->disconnect();
-			RPlidarDriver::DisposeDriver(pRPLIDAR->drv); pRPLIDAR->drv = NULL;
-#else
-			CloseRS232Port(&pRPLIDAR->RS232Port);
-#endif // ENABLE_RPLIDAR_SDK_SUPPORT
-			return EXIT_FAILURE;
-		}
+		SetMotorPWMRequestRPLIDAR(pRPLIDAR, DEFAULT_MOTOR_PWM_RPLIDAR);
 
 		memset(pRPLIDAR->esdata_prev, 0, sizeof(pRPLIDAR->esdata_prev));
 		switch (pRPLIDAR->ScanMode)
@@ -1652,18 +1716,7 @@ inline int DisconnectRPLIDAR(RPLIDAR* pRPLIDAR)
 		return EXIT_FAILURE;
 	}
 
-	if (SetMotorPWMRequestRPLIDAR(pRPLIDAR, 0) != EXIT_SUCCESS)
-	{
-		printf("Error while disconnecting a RPLIDAR.\n");
-#ifdef ENABLE_RPLIDAR_SDK_SUPPORT
-		pRPLIDAR->drv->stopMotor();
-		pRPLIDAR->drv->disconnect();
-		RPlidarDriver::DisposeDriver(pRPLIDAR->drv); pRPLIDAR->drv = NULL;
-#else
-		CloseRS232Port(&pRPLIDAR->RS232Port);
-#endif // ENABLE_RPLIDAR_SDK_SUPPORT
-		return EXIT_FAILURE;
-	}
+	SetMotorPWMRequestRPLIDAR(pRPLIDAR, 0);
 
 #ifdef ENABLE_RPLIDAR_SDK_SUPPORT
 	pRPLIDAR->drv->stopMotor();

RPLIDAR0.txt

@@ -14,7 +14,7 @@ COM9
 1
 % bStartScanModeAtStartup
 1
-% ScanMode (0 : SCAN, 1 : EXPRESS_SCAN, 2 : FORCE_SCAN)
+% ScanMode (0 : SCAN, 1 : EXPRESS_SCAN, 2 : FORCE_SCAN, or if using GetOtherScanDataResponseRPLIDAR(), the available values and modes might be different, see https://github.com/ENSTABretagneRobotics/Hardware-MATLAB/issues/3#issuecomment-661349278)
 0
 % motordelay (in ms)
 2000

SBG.h

@@ -1251,6 +1251,8 @@ inline int ConnectSBG(SBG* pSBG, char* szCfgFilePath)
 	} 	  
 #endif // DISABLE_SBG_TCP
 
+	mSleep(500); // Needed sometimes to allow non-blocking sockets to be ready...
+
 	if (sbgEComInit(&pSBG->comHandle, &pSBG->sbgInterface) != SBG_NO_ERROR)
 	{
 		printf("Unable to connect to a SBG : Unable to initialize the sbgECom library.\n");

StartOtherScanRequestRPLIDAR.m

@@ -1,2 +1,2 @@
-function [result] = StartOtherScanRequestRPLIDAR(pRPLIDAR, scanmode)
-result = calllib('hardwarex', 'StartOtherScanRequestRPLIDARx', pRPLIDAR, scanmode);
+function [result] = StartOtherScanRequestRPLIDAR(pRPLIDAR, scanmodeid)
+result = calllib('hardwarex', 'StartOtherScanRequestRPLIDARx', pRPLIDAR, scanmodeid);

StartOtherScanThreadRPLIDAR.m

@@ -0,0 +1,2 @@
+function [result] = StartOtherScanThreadRPLIDAR(pRPLIDAR)
+result = calllib('hardwarex', 'StartOtherScanThreadRPLIDARx', pRPLIDAR);

StopOtherScanThreadRPLIDAR.m

@@ -0,0 +1,2 @@
+function [result] = StopOtherScanThreadRPLIDAR(pRPLIDAR)
+result = calllib('hardwarex', 'StopOtherScanThreadRPLIDARx', pRPLIDAR);

TODO.txt

@@ -1,15 +1,23 @@
+For rebuild, always check defines (to reset after build except for 3rd release), mavlink version (to reset after build), UpgradeDLL.bat, genproto...
+
 Update Hardware-CPP, MATLAB, Python (check structures), in particular mavlink messages...
 
-critical sections should be everywhere especially for Pololu and SSC32...
+char/array of char parameters : see GetInfoRequestRPLIDAR(), GetAllSupportedScanModesRPLIDAR...
+
+
+Script that copy downloaded drafts, put them in hardwarex_with_3rd_support, change defines and mavlink, run genproto, patch it (in a vm/docker with MATLAB 32 and 64?)...
+patch hardwarex_proto.m for structures indicated in warnings...
+https://fr.mathworks.com/matlabcentral/answers/103180-why-do-i-get-warnings-when-loading-a-dll-with-loadlibrary-in-matlab-7-13-r2011b
+sed FcnPtr, s_LIST_ENTRYPtr voidPtr
+Bitfields 'error' in UBX, so that UBX could be supported at least in MATLAB...
+Fix also for latest mavlink MAVPACKED mavlink_types.h (check if it is also a problem for Linux) and auto test with sitl?
+http://fr.mathworks.com/help/matlab/matlab_external/limitations-to-shared-library-support.html
 
 RPLIDAR : Ne marche pas avec appli du fabricant parfois, on peut pas renommer le port car une autre application l'utilise, mais quelle autre...? Faut utiliser vieux SDK, et il faudrait que le code CPP r�cup�re les donn�es tout le temps dans un thread...
 Crash matlab when exiting getotherscan...?
 BSOD?
 
-For rebuild, always check defines (to reset after build), mavlink version, update proto.m...
-
-Probl�me parsing UBXProtocol.h (bitfields?) : �diter manuellement hardwarex_proto.m, faire un ifdef sp�cial cas MATLAB...?
-http://fr.mathworks.com/help/matlab/matlab_external/limitations-to-shared-library-support.html
+critical sections should be everywhere especially for Pololu and SSC32...
 
 For the thread, only 1 device of the same type can be used...
 Simplify by gathering Create, Connect...?
@@ -21,5 +29,3 @@ OLD
 libhardwarex.so vs hardwarex.so...?
 
 hardwarex_setup->error rplidar sdk C++->tweak rplidar struct (add padding when not using sdk?) so that size corresponds...?
-
-Hardware-MATLAB/Python : if ENABLE_SBG_SDK_SUPPORT disabled, SBG MATLAB and Python functions should not be disabled...