初始提交：龙芯2K0300智能车卖家Demo完整代码
This commit is contained in:
spdis
@@ -0,0 +1,389 @@
|
||||
|
||||
|
||||
#include <errno.h>
|
||||
#include <string.h>
|
||||
#include <stdio.h>
|
||||
#include <fcntl.h>
|
||||
#include <stdlib.h>
|
||||
#include <unistd.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <signal.h>
|
||||
#include "vl53l0x_def.h"
|
||||
|
||||
#define MODE_RANGE 0
|
||||
#define MODE_XTAKCALIB 1
|
||||
#define MODE_OFFCALIB 2
|
||||
#define MODE_HELP 3
|
||||
#define MODE_PARAMETER 6
|
||||
|
||||
|
||||
//******************************** IOCTL definitions
|
||||
#define VL53L0X_IOCTL_INIT _IO('p', 0x01)
|
||||
#define VL53L0X_IOCTL_XTALKCALB _IOW('p', 0x02, unsigned int)
|
||||
#define VL53L0X_IOCTL_OFFCALB _IOW('p', 0x03, unsigned int)
|
||||
#define VL53L0X_IOCTL_STOP _IO('p', 0x05)
|
||||
#define VL53L0X_IOCTL_SETXTALK _IOW('p', 0x06, unsigned int)
|
||||
#define VL53L0X_IOCTL_SETOFFSET _IOW('p', 0x07, int8_t)
|
||||
#define VL53L0X_IOCTL_GETDATAS _IOR('p', 0x0b, VL53L0X_RangingMeasurementData_t)
|
||||
#define VL53L0X_IOCTL_PARAMETER _IOWR('p', 0x0d, struct stmvl53l0x_parameter)
|
||||
|
||||
//modify the following macro accoring to testing set up
|
||||
#define OFFSET_TARGET 100//200
|
||||
#define XTALK_TARGET 600//400
|
||||
#define NUM_SAMPLES 20//20
|
||||
|
||||
typedef enum {
|
||||
OFFSET_PAR = 0,
|
||||
XTALKRATE_PAR = 1,
|
||||
XTALKENABLE_PAR = 2,
|
||||
GPIOFUNC_PAR = 3,
|
||||
LOWTHRESH_PAR = 4,
|
||||
HIGHTHRESH_PAR = 5,
|
||||
DEVICEMODE_PAR = 6,
|
||||
INTERMEASUREMENT_PAR = 7,
|
||||
REFERENCESPADS_PAR = 8,
|
||||
REFCALIBRATION_PAR = 9,
|
||||
} parameter_name_e;
|
||||
/*
|
||||
* IOCTL parameter structs
|
||||
*/
|
||||
struct stmvl53l0x_parameter {
|
||||
uint32_t is_read; //1: Get 0: Set
|
||||
parameter_name_e name;
|
||||
int32_t value;
|
||||
int32_t value2;
|
||||
int32_t status;
|
||||
};
|
||||
|
||||
static void help(void)
|
||||
{
|
||||
fprintf(stderr,
|
||||
"Usage: vl53l0x_test [-c] [-h]\n"
|
||||
" -h for usage\n"
|
||||
" -c for crosstalk calibration\n"
|
||||
" -o for offset calibration\n"
|
||||
" -O <low> <high> for testing interrupt threshold (out mode)\n"
|
||||
" -L <low> for testing interrupt threshold (low mode)\n"
|
||||
" -H <high> for testing interrupt threshold (high mode)\n"
|
||||
" default for ranging\n"
|
||||
);
|
||||
exit(1);
|
||||
}
|
||||
|
||||
static int loop_break=0;
|
||||
|
||||
void sig_handler(int signum)
|
||||
{
|
||||
if(signum == SIGINT)
|
||||
{
|
||||
loop_break=1;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
int main(int argc, char *argv[])
|
||||
{
|
||||
int fd;
|
||||
unsigned long data;
|
||||
VL53L0X_RangingMeasurementData_t range_datas;
|
||||
struct stmvl53l0x_parameter parameter;
|
||||
int flags = 0;
|
||||
int mode = MODE_RANGE;
|
||||
unsigned int targetDistance=0;
|
||||
int i = 0;
|
||||
int rc = 0;
|
||||
unsigned int low_threshold = 0, high_threshold = 0;
|
||||
int configure_int_thresholds = 0;
|
||||
int gpio_functionnality_threshold = 0;
|
||||
|
||||
|
||||
if (signal(SIGINT, sig_handler) == SIG_ERR)
|
||||
fprintf(stderr, "Can't catch SIGINT");
|
||||
|
||||
|
||||
/* handle (optional) flags first */
|
||||
while (1+flags < argc && argv[1+flags][0] == '-') {
|
||||
switch (argv[1+flags][1]) {
|
||||
case 'c': mode= MODE_XTAKCALIB; break;
|
||||
case 'h': mode= MODE_HELP; break;
|
||||
case 'o': mode = MODE_OFFCALIB; break;
|
||||
case 'O':
|
||||
if (argc < 4) {
|
||||
fprintf(stderr, "USAGE: %s <low_threshold_value> <high_threshold_value>\n", argv[0]);
|
||||
exit(1);
|
||||
}
|
||||
low_threshold = atoi(argv[2]);
|
||||
high_threshold = atoi(argv[3]);
|
||||
configure_int_thresholds = 1;
|
||||
|
||||
fprintf(stderr, "Configuring Low threshold = %u, High threhold = %u\n", low_threshold, high_threshold);
|
||||
break;
|
||||
case 'L':
|
||||
if (argc < 3) {
|
||||
fprintf(stderr, "USAGE: %s <low_threshold_value> \n", argv[0]);
|
||||
exit(1);
|
||||
}
|
||||
low_threshold = atoi(argv[2]);
|
||||
configure_int_thresholds = 2;
|
||||
|
||||
fprintf(stderr, "Configuring Low threshold = %u, High threhold = %u\n", low_threshold, high_threshold);
|
||||
break;
|
||||
case 'H':
|
||||
if (argc < 3) {
|
||||
fprintf(stderr, "USAGE: %s <high_threshold_value>\n", argv[0]);
|
||||
exit(1);
|
||||
}
|
||||
high_threshold = atoi(argv[2]);
|
||||
configure_int_thresholds = 3;
|
||||
|
||||
fprintf(stderr, "Configuring Low threshold = %u, High threhold = %u\n", low_threshold, high_threshold);
|
||||
break;
|
||||
default:
|
||||
fprintf(stderr, "Error: Unsupported option "
|
||||
"\"%s\"!\n", argv[1+flags]);
|
||||
help();
|
||||
exit(1);
|
||||
}
|
||||
flags++;
|
||||
}
|
||||
if (mode == MODE_HELP)
|
||||
{
|
||||
help();
|
||||
exit(0);
|
||||
}
|
||||
|
||||
fd = open("/dev/stmvl53l0x_ranging",O_RDWR | O_SYNC);
|
||||
if (fd <= 0)
|
||||
{
|
||||
fprintf(stderr,"Error open stmvl53l0x_ranging device: %s\n", strerror(errno));
|
||||
return -1;
|
||||
}
|
||||
//make sure it's not started
|
||||
if (ioctl(fd, VL53L0X_IOCTL_STOP , NULL) < 0) {
|
||||
fprintf(stderr, "Error: Could not perform VL53L0X_IOCTL_STOP : %s\n", strerror(errno));
|
||||
close(fd);
|
||||
return -1;
|
||||
}
|
||||
if (mode == MODE_XTAKCALIB)
|
||||
{
|
||||
unsigned int XtalkInt = 0;
|
||||
uint8_t XtalkEnable = 0;
|
||||
fprintf(stderr, "xtalk Calibrate place black target at %dmm from glass===\n",XTALK_TARGET);
|
||||
// to xtalk calibration
|
||||
targetDistance = XTALK_TARGET;
|
||||
if (ioctl(fd, VL53L0X_IOCTL_XTALKCALB , &targetDistance) < 0) {
|
||||
fprintf(stderr, "Error: Could not perform VL53L0X_IOCTL_XTALKCALB : %s\n", strerror(errno));
|
||||
close(fd);
|
||||
return -1;
|
||||
}
|
||||
// to get xtalk parameter
|
||||
parameter.is_read = 1;
|
||||
parameter.name = XTALKRATE_PAR;
|
||||
if (ioctl(fd, VL53L0X_IOCTL_PARAMETER , ¶meter) < 0) {
|
||||
fprintf(stderr, "Error: Could not perform VL53L0X_IOCTL_PARAMETER : %s\n", strerror(errno));
|
||||
close(fd);
|
||||
return -1;
|
||||
}
|
||||
XtalkInt = (unsigned int)parameter.value;
|
||||
parameter.name = XTALKENABLE_PAR;
|
||||
if (ioctl(fd, VL53L0X_IOCTL_PARAMETER , ¶meter) < 0) {
|
||||
fprintf(stderr, "Error: Could not perform VL53L0X_IOCTL_PARAMETER : %s\n", strerror(errno));
|
||||
close(fd);
|
||||
return -1;
|
||||
}
|
||||
XtalkEnable = (uint8_t)parameter.value;
|
||||
fprintf(stderr, "VL53L0 Xtalk Calibration get Xtalk Compensation rate in fixed 16 point as %u, enable:%u\n",XtalkInt,XtalkEnable);
|
||||
|
||||
for(i = 0; i <= NUM_SAMPLES; i++)
|
||||
{
|
||||
usleep(30 *1000); /*100ms*/
|
||||
// to get all range data
|
||||
ioctl(fd, VL53L0X_IOCTL_GETDATAS,&range_datas);
|
||||
}
|
||||
fprintf(stderr," VL53L0 DMAX calibration Range Data:%d, signalRate_mcps:%d\n",range_datas.RangeMilliMeter, range_datas.SignalRateRtnMegaCps);
|
||||
// get rangedata of last measurement to avoid incorrect datum from unempty buffer
|
||||
//to close
|
||||
close(fd);
|
||||
return -1;
|
||||
}
|
||||
else if (mode == MODE_OFFCALIB)
|
||||
{
|
||||
int offset=0;
|
||||
uint32_t SpadCount=0;
|
||||
uint8_t IsApertureSpads=0;
|
||||
uint8_t VhvSettings=0,PhaseCal=0;
|
||||
|
||||
|
||||
// to xtalk calibration
|
||||
targetDistance = OFFSET_TARGET;
|
||||
if (ioctl(fd, VL53L0X_IOCTL_OFFCALB , &targetDistance) < 0) {
|
||||
fprintf(stderr, "Error: Could not perform VL53L0X_IOCTL_OFFCALB : %s\n", strerror(errno));
|
||||
close(fd);
|
||||
return -1;
|
||||
}
|
||||
// to get current offset
|
||||
parameter.is_read = 1;
|
||||
parameter.name = OFFSET_PAR;
|
||||
if (ioctl(fd, VL53L0X_IOCTL_PARAMETER, ¶meter) < 0) {
|
||||
fprintf(stderr, "Error: Could not perform VL53L0X_IOCTL_PARAMETER : %s\n", strerror(errno));
|
||||
close(fd);
|
||||
return -1;
|
||||
}
|
||||
offset = (int)parameter.value;
|
||||
fprintf(stderr, "get offset %d micrometer===\n",offset);
|
||||
|
||||
parameter.name = REFCALIBRATION_PAR;
|
||||
if (ioctl(fd, VL53L0X_IOCTL_PARAMETER, ¶meter) < 0) {
|
||||
fprintf(stderr, "Error: Could not perform VL53L0X_IOCTL_PARAMETER : %s\n", strerror(errno));
|
||||
close(fd);
|
||||
return -1;
|
||||
}
|
||||
VhvSettings = (uint8_t) parameter.value;
|
||||
PhaseCal=(uint8_t) parameter.value2;
|
||||
fprintf(stderr, "get VhvSettings is %u ===\nget PhaseCas is %u ===\n", VhvSettings,PhaseCal);
|
||||
|
||||
parameter.name =REFERENCESPADS_PAR;
|
||||
if (ioctl(fd, VL53L0X_IOCTL_PARAMETER, ¶meter) < 0) {
|
||||
fprintf(stderr, "Error: Could not perform VL53L0X_IOCTL_PARAMETER : %s\n", strerror(errno));
|
||||
close(fd);
|
||||
return -1;
|
||||
}
|
||||
SpadCount = (uint32_t)parameter.value;
|
||||
IsApertureSpads=(uint8_t) parameter.value2;
|
||||
fprintf(stderr, "get SpadCount is %d ===\nget IsApertureSpads is %u ===\n", SpadCount,IsApertureSpads);
|
||||
|
||||
|
||||
//to close
|
||||
close(fd);
|
||||
return -1;
|
||||
}
|
||||
else
|
||||
{
|
||||
switch(configure_int_thresholds)
|
||||
{
|
||||
case 1:
|
||||
gpio_functionnality_threshold = VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_OUT;
|
||||
break;
|
||||
case 2:
|
||||
gpio_functionnality_threshold = VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_LOW;
|
||||
break;
|
||||
case 3:
|
||||
gpio_functionnality_threshold = VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_HIGH;
|
||||
break;
|
||||
default:
|
||||
gpio_functionnality_threshold = VL53L0X_GPIOFUNCTIONALITY_NEW_MEASURE_READY;
|
||||
}
|
||||
|
||||
if (configure_int_thresholds) {
|
||||
|
||||
parameter.is_read = 0;
|
||||
|
||||
|
||||
parameter.name = DEVICEMODE_PAR;
|
||||
parameter.value = VL53L0X_DEVICEMODE_CONTINUOUS_TIMED_RANGING;
|
||||
//parameter.value = VL53L0X_DEVICEMODE_CONTINUOUS_RANGING;
|
||||
if (ioctl(fd, VL53L0X_IOCTL_PARAMETER, ¶meter) < 0) {
|
||||
fprintf(stderr, "Error: Could not perform VL53L0X_IOCTL_PARAMETER(CONTINOUS_TIMED_RANGING) : %s\n",
|
||||
strerror(errno));
|
||||
close(fd);
|
||||
return -1;
|
||||
}
|
||||
|
||||
parameter.name = GPIOFUNC_PAR;
|
||||
parameter.value = gpio_functionnality_threshold;
|
||||
if (ioctl(fd, VL53L0X_IOCTL_PARAMETER, ¶meter) < 0) {
|
||||
fprintf(stderr, "Error: Could not perform VL53L0X_IOCTL_PARAMETER : %s, low_threshold = %u\n",
|
||||
strerror(errno),
|
||||
low_threshold);
|
||||
close(fd);
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (configure_int_thresholds != 3)
|
||||
{
|
||||
parameter.name = LOWTHRESH_PAR;
|
||||
parameter.value = low_threshold;
|
||||
if (ioctl(fd, VL53L0X_IOCTL_PARAMETER, ¶meter) < 0) {
|
||||
fprintf(stderr, "Error: Could not perform VL53L0X_IOCTL_PARAMETER : %s, low_threshold = %u\n",
|
||||
strerror(errno),
|
||||
low_threshold);
|
||||
close(fd);
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
if (configure_int_thresholds != 2)
|
||||
{
|
||||
parameter.name = HIGHTHRESH_PAR;
|
||||
parameter.value = high_threshold;
|
||||
if (ioctl(fd, VL53L0X_IOCTL_PARAMETER, ¶meter) < 0) {
|
||||
fprintf(stderr, "Error: Could not perform VL53L0X_IOCTL_PARAMETER : %s, high_threshold = %u\n",
|
||||
strerror(errno),
|
||||
high_threshold);
|
||||
close(fd);
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
else {
|
||||
parameter.name = DEVICEMODE_PAR;
|
||||
parameter.value = VL53L0X_DEVICEMODE_CONTINUOUS_RANGING;
|
||||
|
||||
if (ioctl(fd, VL53L0X_IOCTL_PARAMETER, ¶meter) < 0) {
|
||||
fprintf(stderr, "Error: Could not perform VL53L0X_IOCTL_PARAMETER(CONTINUOUS_RANGING) : %s\n",
|
||||
strerror(errno));
|
||||
close(fd);
|
||||
return -1;
|
||||
}
|
||||
parameter.name = GPIOFUNC_PAR;
|
||||
parameter.value = gpio_functionnality_threshold;
|
||||
if (ioctl(fd, VL53L0X_IOCTL_PARAMETER, ¶meter) < 0) {
|
||||
fprintf(stderr, "Error: Could not perform VL53L0X_IOCTL_PARAMETER : %s, low_threshold = %u\n",
|
||||
strerror(errno),
|
||||
low_threshold);
|
||||
close(fd);
|
||||
return -1;
|
||||
}
|
||||
|
||||
}
|
||||
// to init
|
||||
if (ioctl(fd, VL53L0X_IOCTL_INIT , NULL) < 0) {
|
||||
fprintf(stderr, "Error: Could not perform VL53L0X_IOCTL_INIT : %s\n", strerror(errno));
|
||||
close(fd);
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
// get data testing
|
||||
while (1)
|
||||
{
|
||||
usleep(30 *1000); /*100ms*/
|
||||
// to get all range data
|
||||
rc = ioctl(fd, VL53L0X_IOCTL_GETDATAS,&range_datas);
|
||||
if (rc) {
|
||||
fprintf(stderr, "VL53L0X_IOCTL_GETDATAS failed = %d\n", rc);
|
||||
close(fd);
|
||||
exit(1);
|
||||
}
|
||||
|
||||
fprintf(stderr,"Range:%4d, Error:%u, SigRate_mcps:%7d, AmbRate_mcps:%7d\r",
|
||||
range_datas.RangeMilliMeter, range_datas.RangeStatus, range_datas.SignalRateRtnMegaCps, range_datas.AmbientRateRtnMegaCps);
|
||||
if(loop_break)
|
||||
break;
|
||||
}
|
||||
|
||||
fprintf(stderr, "\n");
|
||||
|
||||
fprintf(stderr, "Stop driver\n");
|
||||
|
||||
if (ioctl(fd, VL53L0X_IOCTL_STOP , NULL) < 0) {
|
||||
fprintf(stderr, "Error: Could not perform VL53L0X_IOCTL_STOP : %s\n", strerror(errno));
|
||||
close(fd);
|
||||
return -1;
|
||||
}
|
||||
|
||||
close(fd);
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user