fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

LidarLite: update for new orientation convention

Browse Source 
add possibility to specify orientation and adapt to new defaults and use px4_getopt
This commit is contained in:
ChristophTobler 2017-09-19 13:26:20 +02:00 committed by ChristophTobler
parent 286d8f2bff
commit 21f97cfce6
5 changed files with 31 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
src/drivers/ll40ls/LidarLiteI2C.cpp
View File

@ -49,8 +49,9 @@
#include <stdio.h>
#include <drivers/drv_hrt.h>
LidarLiteI2C::LidarLiteI2C(int bus, const char *path, int address) :
LidarLiteI2C::LidarLiteI2C(int bus, const char *path, int address, uint8_t rotation) :
I2C("LL40LS", path, bus, address, 100000),
_rotation(rotation),
_work{},
_reports(nullptr),
_sensor_ok(false),
@ -454,7 +455,7 @@ int LidarLiteI2C::collect()
report.covariance = 0.0f;
/* the sensor is in fact a laser + sonar but there is no enum for this */
report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
report.orientation = 8;
report.orientation = _rotation;
/* TODO: set proper ID */
report.id = 0;

4
src/drivers/ll40ls/LidarLiteI2C.h
View File

@ -71,7 +71,8 @@
class LidarLiteI2C : public LidarLite, public device::I2C
{
public:
LidarLiteI2C(int bus, const char *path, int address = LL40LS_BASEADDR);
LidarLiteI2C(int bus, const char *path, int address = LL40LS_BASEADDR,
uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
virtual ~LidarLiteI2C();
int init() override;
@ -100,6 +101,7 @@ protected:
int reset_sensor();
private:
uint8_t _rotation;
work_s _work;
ringbuffer::RingBuffer *_reports;
bool _sensor_ok;

5
src/drivers/ll40ls/LidarLitePWM.cpp
View File

@ -49,8 +49,9 @@
#include <drivers/drv_hrt.h>
#include <drivers/drv_pwm_input.h>
LidarLitePWM::LidarLitePWM(const char *path) :
LidarLitePWM::LidarLitePWM(const char *path, uint8_t rotation) :
CDev("LidarLitePWM", path),
_rotation(rotation),
_work{},
_reports(nullptr),
_class_instance(-1),
@ -178,7 +179,7 @@ int LidarLitePWM::measure()
_range.min_distance = get_minimum_distance();
_range.current_distance = float(_pwm.pulse_width) * 1e-3f; /* 10 usec = 1 cm distance for LIDAR-Lite */
_range.covariance = 0.0f;
_range.orientation = 8;
_range.orientation = _rotation;
/* TODO: set proper ID */
_range.id = 0;

3
src/drivers/ll40ls/LidarLitePWM.h
View File

@ -60,7 +60,7 @@
class LidarLitePWM : public LidarLite, public device::CDev
{
public:
LidarLitePWM(const char *path);
LidarLitePWM(const char *path, uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
virtual ~LidarLitePWM();
int init() override;
@ -107,6 +107,7 @@ protected:
void task_main_trampoline(int argc, char *argv[]);
private:
uint8_t _rotation;
work_s _work;
ringbuffer::RingBuffer *_reports;
int _class_instance;

30
src/drivers/ll40ls/ll40ls.cpp
View File

@ -49,6 +49,7 @@
#include <cstdlib>
#include <string.h>
#include <stdio.h>
#include <platforms/px4_getopt.h>
#ifndef CONFIG_SCHED_WORKQUEUE
# error This requires CONFIG_SCHED_WORKQUEUE.
@ -93,7 +94,7 @@ namespace ll40ls
LidarLite *instance = nullptr;
void start(enum LL40LS_BUS busid);
void start(enum LL40LS_BUS busid, uint8_t rotation);
void stop();
void test();
void reset();
@ -104,7 +105,7 @@ void usage();
/**
* Start the driver.
*/
void start(enum LL40LS_BUS busid)
void start(enum LL40LS_BUS busid, uint8_t rotation)
{
int fd, ret;
@ -113,7 +114,7 @@ void start(enum LL40LS_BUS busid)
}
if (busid == LL40LS_BUS_PWM) {
instance = new LidarLitePWM(LL40LS_DEVICE_PATH_PWM);
instance = new LidarLitePWM(LL40LS_DEVICE_PATH_PWM, rotation);
if (!instance) {
warnx("Failed to instantiate LidarLitePWM");
@ -131,7 +132,7 @@ void start(enum LL40LS_BUS busid)
continue;
}
instance = new LidarLiteI2C(bus_options[i].busnum, bus_options[i].devname);
instance = new LidarLiteI2C(bus_options[i].busnum, bus_options[i].devname, rotation);
if (!instance) {
warnx("Failed to instantiate LidarLiteI2C");
@ -340,6 +341,7 @@ usage()
#ifdef PX4_I2C_BUS_ONBOARD
warnx(" -I only internal bus");
#endif
warnx("E.g. ll40ls start i2c -R 0");
}
} // namespace
@ -348,9 +350,12 @@ int
ll40ls_main(int argc, char *argv[])
{
int ch;
int myoptind = 1;
const char *myoptarg = NULL;
enum LL40LS_BUS busid = LL40LS_BUS_I2C_ALL;
uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
while ((ch = getopt(argc, argv, "XI")) != EOF) {
while ((ch = px4_getopt(argc, argv, "IXR:", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
#ifdef PX4_I2C_BUS_ONBOARD
@ -363,6 +368,11 @@ ll40ls_main(int argc, char *argv[])
busid = LL40LS_BUS_I2C_EXTERNAL;
break;
case 'R':
rotation = (uint8_t)atoi(myoptarg);
PX4_INFO("Setting Lidar orientation to %d", (int)rotation);
break;
default:
ll40ls::usage();
return 0;
@ -370,8 +380,8 @@ ll40ls_main(int argc, char *argv[])
}
/* determine protocol first because it's needed next */
if (argc > optind + 1) {
const char *protocol = argv[optind + 1];
if (argc > myoptind + 1) {
const char *protocol = argv[myoptind + 1];
if (!strcmp(protocol, "pwm")) {
busid = LL40LS_BUS_PWM;;
@ -387,11 +397,11 @@ ll40ls_main(int argc, char *argv[])
}
/* now determine action */
if (argc > optind) {
const char *verb = argv[optind];
if (argc > myoptind) {
const char *verb = argv[myoptind];
if (!strcmp(verb, "start")) {
ll40ls::start(busid);
ll40ls::start(busid, rotation);
} else if (!strcmp(verb, "stop")) {
ll40ls::stop();