WebSVN – ChibiGauge – Blame – /trunk/src/useLidar.c

Rev	Author	Line No.	Line
3	mjames	1	#include "vl53l0x_api.h"
		2	#include "vl53l0x_platform.h"
		3	#include "required_version.h"
		4	#include <malloc.h>
		5
		6	#include "useLidar.h"
		7	#include "ch.h"
		8	#include "hal.h"
		9	#include "shell.h"
		10	#include "chprintf.h"
		11
		12	void print_pal_error(BaseSequentialStream *chp, VL53L0X_Error Status){
		13	char buf[VL53L0X_MAX_STRING_LENGTH];
		14	VL53L0X_GetPalErrorString(Status, buf);
		15	chprintf(chp,"API Status: %i : %s\r\n", Status, buf);
		16	}
		17
		18	void print_range_status(BaseSequentialStream chp,VL53L0X_RangingMeasurementData_t pRangingMeasurementData){
		19	char buf[VL53L0X_MAX_STRING_LENGTH];
		20	uint8_t RangeStatus;
		21
		22	/*
		23	* New Range Status: data is valid when pRangingMeasurementData->RangeStatus = 0
		24	*/
		25
		26	RangeStatus = pRangingMeasurementData->RangeStatus;
		27
		28	VL53L0X_GetRangeStatusString(RangeStatus, buf);
		29	chprintf(chp,"Range Status: %i : %s\r\n", RangeStatus, buf);
		30
		31	}
		32
		33
6	mjames	34	VL53L0X_Error VL53L0XdeviceSetup(VL53L0X_Dev_t *pMyDevice)
		35	{
		36	VL53L0X_Error Status = VL53L0X_ERROR_NONE;
		37	VL53L0X_RangingMeasurementData_t RangingMeasurementData;
		38	int i;
		39	uint32_t refSpadCount;
		40	uint8_t isApertureSpads;
		41	uint8_t VhvSettings;
		42	uint8_t PhaseCal;
		43
		44	// Initialize Comms
		45	pMyDevice->I2cDevAddr = 0x29; // chibi-style
		46	pMyDevice->comms_type = 1;
		47	pMyDevice->comms_speed_khz = 100;
		48
		49	// call init anyway
		50	VL53L0X_comms_initialise(0,0);
		51
		52
		53	if(Status == VL53L0X_ERROR_NONE)
		54	{
		55	Status = VL53L0X_StaticInit(pMyDevice); // Device Initialization
		56	}
		57
		58	if(Status == VL53L0X_ERROR_NONE)
		59	{
		60	Status = VL53L0X_PerformRefCalibration(pMyDevice,
		61	&VhvSettings, &PhaseCal); // Device Initialization
		62	}
		63
		64	if(Status == VL53L0X_ERROR_NONE)
		65	{
		66	Status = VL53L0X_PerformRefSpadManagement(pMyDevice,
		67	&refSpadCount, &isApertureSpads); // Device Initialization
		68	}
		69
		70	if(Status == VL53L0X_ERROR_NONE)
		71	{
		72	// no need to do this when we use VL53L0X_PerformSingleRangingMeasurement
		73	Status = VL53L0X_SetDeviceMode(pMyDevice, VL53L0X_DEVICEMODE_SINGLE_RANGING); // Setup in single ranging mode
		74	}
		75
		76	// Enable/Disable Sigma and Signal check
		77
		78	/* if (Status == VL53L0X_ERROR_NONE) {
		79	Status = VL53L0X_SetSequenceStepEnable(pMyDevice,VL53L0X_SEQUENCESTEP_DSS, 1);
		80	}*/
		81
		82	if (Status == VL53L0X_ERROR_NONE) {
		83	Status = VL53L0X_SetLimitCheckEnable(pMyDevice,
		84	VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, 1);
		85	}
		86	if (Status == VL53L0X_ERROR_NONE) {
		87	Status = VL53L0X_SetLimitCheckEnable(pMyDevice,
		88	VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, 1);
		89	}
		90
		91	if (Status == VL53L0X_ERROR_NONE) {
		92	Status = VL53L0X_SetLimitCheckValue(pMyDevice,
		93	VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,
		94	(FixPoint1616_t)(0.1*65536));
		95	}
		96	if (Status == VL53L0X_ERROR_NONE) {
		97	Status = VL53L0X_SetLimitCheckValue(pMyDevice,
		98	VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,
		99	(FixPoint1616_t)(60*65536));
		100	}
		101	if (Status == VL53L0X_ERROR_NONE) {
		102	Status = VL53L0X_SetMeasurementTimingBudgetMicroSeconds(pMyDevice,
		103	33000);
		104	}
		105
		106	if (Status == VL53L0X_ERROR_NONE) {
		107	Status = VL53L0X_SetVcselPulsePeriod(pMyDevice,
		108	VL53L0X_VCSEL_PERIOD_PRE_RANGE, 18);
		109	}
		110	if (Status == VL53L0X_ERROR_NONE) {
		111	Status = VL53L0X_SetVcselPulsePeriod(pMyDevice,
		112	VL53L0X_VCSEL_PERIOD_FINAL_RANGE, 14);
		113	}
		114	return Status;
		115
		116	}
		117
		118
		119
		120
3	mjames	121	VL53L0X_Error rangingTest(BaseSequentialStream chp,VL53L0X_Dev_t pMyDevice)
		122	{
		123	VL53L0X_Error Status = VL53L0X_ERROR_NONE;
		124	VL53L0X_RangingMeasurementData_t RangingMeasurementData;
		125	int i;
		126	uint32_t refSpadCount;
		127	uint8_t isApertureSpads;
		128	uint8_t VhvSettings;
		129	uint8_t PhaseCal;
		130
		131	if(Status == VL53L0X_ERROR_NONE)
		132	{
		133	chprintf(chp,"Call of VL53L0X_StaticInit\r\n");
		134	Status = VL53L0X_StaticInit(pMyDevice); // Device Initialization
		135	print_pal_error(chp,Status);
		136	}
		137
		138	if(Status == VL53L0X_ERROR_NONE)
		139	{
		140	chprintf(chp,"Call of VL53L0X_PerformRefCalibration\r\n");
		141	Status = VL53L0X_PerformRefCalibration(pMyDevice,
		142	&VhvSettings, &PhaseCal); // Device Initialization
		143	print_pal_error(chp,Status);
		144	}
		145
		146	if(Status == VL53L0X_ERROR_NONE)
		147	{
		148	chprintf(chp,"Call of VL53L0X_PerformRefSpadManagement\r\n");
		149	Status = VL53L0X_PerformRefSpadManagement(pMyDevice,
		150	&refSpadCount, &isApertureSpads); // Device Initialization
		151	chprintf(chp,"refSpadCount = %d, isApertureSpads = %d\r\n", refSpadCount, isApertureSpads);
		152	print_pal_error(chp,Status);
		153	}
		154
		155	if(Status == VL53L0X_ERROR_NONE)
		156	{
		157
		158	// no need to do this when we use VL53L0X_PerformSingleRangingMeasurement
		159	chprintf(chp,"Call of VL53L0X_SetDeviceMode\r\n");
		160	Status = VL53L0X_SetDeviceMode(pMyDevice, VL53L0X_DEVICEMODE_SINGLE_RANGING); // Setup in single ranging mode
		161	print_pal_error(chp,Status);
		162	}
		163
		164	// Enable/Disable Sigma and Signal check
		165
		166	/* if (Status == VL53L0X_ERROR_NONE) {
		167	Status = VL53L0X_SetSequenceStepEnable(pMyDevice,VL53L0X_SEQUENCESTEP_DSS, 1);
		168	}*/
		169
		170	if (Status == VL53L0X_ERROR_NONE) {
		171	Status = VL53L0X_SetLimitCheckEnable(pMyDevice,
		172	VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, 1);
		173	}
		174	if (Status == VL53L0X_ERROR_NONE) {
		175	Status = VL53L0X_SetLimitCheckEnable(pMyDevice,
		176	VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, 1);
		177	}
		178
		179	if (Status == VL53L0X_ERROR_NONE) {
		180	Status = VL53L0X_SetLimitCheckValue(pMyDevice,
		181	VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,
		182	(FixPoint1616_t)(0.1*65536));
		183	}
		184	if (Status == VL53L0X_ERROR_NONE) {
		185	Status = VL53L0X_SetLimitCheckValue(pMyDevice,
		186	VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,
		187	(FixPoint1616_t)(60*65536));
		188	}
		189	if (Status == VL53L0X_ERROR_NONE) {
		190	Status = VL53L0X_SetMeasurementTimingBudgetMicroSeconds(pMyDevice,
		191	33000);
		192	}
		193
		194	if (Status == VL53L0X_ERROR_NONE) {
		195	Status = VL53L0X_SetVcselPulsePeriod(pMyDevice,
		196	VL53L0X_VCSEL_PERIOD_PRE_RANGE, 18);
		197	}
		198	if (Status == VL53L0X_ERROR_NONE) {
		199	Status = VL53L0X_SetVcselPulsePeriod(pMyDevice,
		200	VL53L0X_VCSEL_PERIOD_FINAL_RANGE, 14);
		201	}
		202
		203
		204	/*
		205	* Step 4 : Test ranging mode
		206	*/
		207
		208	if(Status == VL53L0X_ERROR_NONE)
		209	{
		210	for(i=0;i<50;i++){
		211	chprintf(chp,"Call of VL53L0X_PerformSingleRangingMeasurement\r\n");
		212	Status = VL53L0X_PerformSingleRangingMeasurement(pMyDevice,
		213	&RangingMeasurementData);
		214
		215	print_pal_error(chp,Status);
		216	print_range_status(chp,&RangingMeasurementData);
		217
		218
		219	if (Status != VL53L0X_ERROR_NONE) break;
		220
		221	chprintf(chp,"Measured distance: %i\r\n\r\n", RangingMeasurementData.RangeMilliMeter);
		222
		223
		224	}
		225	}
		226	return Status;
		227	}
		228	// make this into a shell command
6	mjames	229	extern void setPause(bool pause);
3	mjames	230
		231	void shellLidar(BaseSequentialStream chp, int argc, char argv[])
		232	{
		233	VL53L0X_Error Status = VL53L0X_ERROR_NONE;
		234	VL53L0X_Dev_t MyDevice;
		235	VL53L0X_Dev_t *pMyDevice = &MyDevice;
		236	VL53L0X_Version_t Version;
		237	VL53L0X_Version_t *pVersion = &Version;
		238	VL53L0X_DeviceInfo_t DeviceInfo;
		239
		240	int32_t status_int;
		241	int32_t init_done = 0;
		242	// TCHAR SerialCommStr[MAX_VALUE_NAME];
		243
6	mjames	244	setPause(true);
3	mjames	245
		246	chprintf(chp,"VL53L0X API Simple Ranging example\r\n\r\n");
		247	// chprintf(chp,"Press a Key to continue!\r\n\r\n");
		248	// getchar();
		249
		250
		251	// Initialize Comms
		252	pMyDevice->I2cDevAddr = 0x29; // chibi-style
		253	pMyDevice->comms_type = 1;
4	mjames	254	pMyDevice->comms_speed_khz = 100;
3	mjames	255
6	mjames	256	// call init anyway
		257	VL53L0X_comms_initialise(0,0);
3	mjames	258	/*
		259	* Disable VL53L0X API logging if you want to run at full speed
		260	*/
		261	#ifdef VL53L0X_LOG_ENABLE
		262	VL53L0X_trace_config("test.log", TRACE_MODULE_ALL, TRACE_LEVEL_ALL, TRACE_FUNCTION_ALL);
		263	#endif
		264
		265	/*
		266	* Get the version of the VL53L0X API running in the firmware
		267	*/
		268
		269	if(Status == VL53L0X_ERROR_NONE)
		270	{
		271	status_int = VL53L0X_GetVersion(pVersion);
		272	if (status_int != 0)
		273	Status = VL53L0X_ERROR_CONTROL_INTERFACE;
		274	}
		275
		276	/*
		277	* Verify the version of the VL53L0X API running in the firmware
		278	*/
		279
		280	if(Status == VL53L0X_ERROR_NONE)
		281	{
		282	if( pVersion->major != VERSION_REQUIRED_MAJOR \|\|
		283	pVersion->minor != VERSION_REQUIRED_MINOR \|\|
		284	pVersion->build != VERSION_REQUIRED_BUILD )
		285	{
		286	chprintf(chp,"VL53L0X API Version Error: Your firmware has %d.%d.%d (revision %d). This example requires %d.%d.%d.\r\n",
		287	pVersion->major, pVersion->minor, pVersion->build, pVersion->revision,
		288	VERSION_REQUIRED_MAJOR, VERSION_REQUIRED_MINOR, VERSION_REQUIRED_BUILD);
		289	}
		290	}
		291
		292
		293	if(Status == VL53L0X_ERROR_NONE)
		294	{
		295	chprintf(chp,"Call of VL53L0X_DataInit\r\n");
		296	Status = VL53L0X_DataInit(&MyDevice); // Data initialization
		297	print_pal_error(chp,Status);
		298	}
		299
		300	if(Status == VL53L0X_ERROR_NONE)
		301	{
		302	Status = VL53L0X_GetDeviceInfo(&MyDevice, &DeviceInfo);
		303	if(Status == VL53L0X_ERROR_NONE)
		304	{
		305	chprintf(chp,"VL53L0X_GetDeviceInfo:\r\n");
		306	chprintf(chp,"Device Name : %s\r\n", DeviceInfo.Name);
		307	chprintf(chp,"Device Type : %s\r\n", DeviceInfo.Type);
		308	chprintf(chp,"Device ID : %s\r\n", DeviceInfo.ProductId);
		309	chprintf(chp,"ProductRevisionMajor : %d\r\n", DeviceInfo.ProductRevisionMajor);
		310	chprintf(chp,"ProductRevisionMinor : %d\r\n", DeviceInfo.ProductRevisionMinor);
		311
		312	if ((DeviceInfo.ProductRevisionMinor != 1) && (DeviceInfo.ProductRevisionMinor != 1)) {
		313	chprintf(chp,"Error expected cut 1.1 but found cut %d.%d\r\n",
		314	DeviceInfo.ProductRevisionMajor, DeviceInfo.ProductRevisionMinor);
		315	Status = VL53L0X_ERROR_NOT_SUPPORTED;
		316	}
		317	}
		318	print_pal_error(chp,Status);
		319	}
		320
		321	if(Status == VL53L0X_ERROR_NONE)
		322	{
		323	Status = rangingTest(chp,pMyDevice);
		324	}
		325
		326	print_pal_error(chp,Status);
		327
		328	// Implementation specific
		329
		330	/*
		331	* Disconnect comms - part of VL53L0X_platform.c
		332	*/
		333
		334	if(init_done == 0)
		335	{
		336	chprintf(chp,"Close Comms\r\n");
		337	status_int = VL53L0X_comms_close();
		338	if (status_int != 0)
		339	Status = VL53L0X_ERROR_CONTROL_INTERFACE;
		340	}
		341
		342	print_pal_error(chp,Status);
		343
		344	// chprintf(chp,"\r\nPress a Key to continue!");
		345	// getchar();
6	mjames	346	setPause(false);
3	mjames	347
		348	return (0);
		349	}
		350

Subversion Repositories ChibiGauge

(root)/trunk/src/useLidar.c – Rev 6