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

ll40ls: adapt the cli interface, so that the commands work with the PWM and I2C driver

Browse Source
This commit is contained in:
Ban Siesta
2015-05-24 12:42:14 +01:00
parent bb3ad64aac
commit 0ab0de5805
1 changed files with 184 additions and 109 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/ll40ls/ll40ls.cpp
+184 -109
View File
@@ -35,11 +35,13 @@
* @file ll40ls.cpp
* @author Allyson Kreft
* @author Johan Jansen <jnsn.johan@gmail.com>
* @author Ban Siesta <bansiesta@gmail.com>
*
* Interface for the PulsedLight Lidar-Lite range finders.
*/
#include "LidarLiteI2C.h"
#include "LidarLitePWM.h"
#include <board_config.h>
#include <systemlib/err.h>
#include <fcntl.h>
@@ -53,6 +55,7 @@
#define LL40LS_DEVICE_PATH_INT "/dev/ll40ls_int"
#define LL40LS_DEVICE_PATH_EXT "/dev/ll40ls_ext"
#define LL40LS_DEVICE_PATH_PWM "/dev/ll40ls_pwm"
/*
* Driver 'main' command.
@@ -74,93 +77,122 @@ const int ERROR = -1;
LidarLiteI2C *g_dev_int;
LidarLiteI2C *g_dev_ext;
LidarLitePWM *g_dev_pwm;
void start(int bus);
void stop(int bus);
void test(int bus);
void reset(int bus);
void info(int bus);
void regdump(int bus);
void start(const bool use_i2c, const int bus);
void stop(const bool use_i2c, const int bus);
void test(const bool use_i2c, const int bus);
void reset(const bool use_i2c, const int bus);
void info(const bool use_i2c, const int bus);
void regdump(const bool use_i2c, const int bus);
void usage();
/**
* Start the driver.
*/
void start(int bus)
void start(const bool use_i2c, const int bus)
{
/* create the driver, attempt expansion bus first */
if (bus == -1 || bus == PX4_I2C_BUS_EXPANSION) {
if (g_dev_ext != nullptr) {
errx(0, "already started external");
}
if (use_i2c) {
/* create the driver, attempt expansion bus first */
if (bus == -1 || bus == PX4_I2C_BUS_EXPANSION) {
if (g_dev_ext != nullptr) {
errx(0, "already started external");
}
g_dev_ext = new LidarLiteI2C(PX4_I2C_BUS_EXPANSION, LL40LS_DEVICE_PATH_EXT);
g_dev_ext = new LidarLiteI2C(PX4_I2C_BUS_EXPANSION, LL40LS_DEVICE_PATH_EXT);
if (g_dev_ext != nullptr && OK != g_dev_ext->init()) {
delete g_dev_ext;
g_dev_ext = nullptr;
if (g_dev_ext != nullptr && OK != g_dev_ext->init()) {
delete g_dev_ext;
g_dev_ext = nullptr;
if (bus == PX4_I2C_BUS_EXPANSION) {
goto fail;
if (bus == PX4_I2C_BUS_EXPANSION) {
goto fail;
}
}
}
}
#ifdef PX4_I2C_BUS_ONBOARD
/* if this failed, attempt onboard sensor */
if (bus == -1 || bus == PX4_I2C_BUS_ONBOARD) {
if (g_dev_int != nullptr) {
errx(0, "already started internal");
}
/* if this failed, attempt onboard sensor */
if (bus == -1 || bus == PX4_I2C_BUS_ONBOARD) {
if (g_dev_int != nullptr) {
errx(0, "already started internal");
}
g_dev_int = new LidarLiteI2C(PX4_I2C_BUS_ONBOARD, LL40LS_DEVICE_PATH_INT);
g_dev_int = new LidarLiteI2C(PX4_I2C_BUS_ONBOARD, LL40LS_DEVICE_PATH_INT);
if (g_dev_int != nullptr && OK != g_dev_int->init()) {
/* tear down the failing onboard instance */
delete g_dev_int;
g_dev_int = nullptr;
if (g_dev_int != nullptr && OK != g_dev_int->init()) {
/* tear down the failing onboard instance */
delete g_dev_int;
g_dev_int = nullptr;
if (bus == PX4_I2C_BUS_ONBOARD) {
if (bus == PX4_I2C_BUS_ONBOARD) {
goto fail;
}
}
if (g_dev_int == nullptr && bus == PX4_I2C_BUS_ONBOARD) {
goto fail;
}
}
if (g_dev_int == nullptr && bus == PX4_I2C_BUS_ONBOARD) {
goto fail;
}
}
#endif
/* set the poll rate to default, starts automatic data collection */
if (g_dev_int != nullptr) {
int fd = open(LL40LS_DEVICE_PATH_INT, O_RDONLY);
/* set the poll rate to default, starts automatic data collection */
if (g_dev_int != nullptr) {
int fd = open(LL40LS_DEVICE_PATH_INT, O_RDONLY);
if (fd == -1) {
goto fail;
if (fd == -1) {
goto fail;
}
int ret = ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT);
close(fd);
if (ret < 0) {
goto fail;
}
}
int ret = ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT);
close(fd);
if (g_dev_ext != nullptr) {
int fd = open(LL40LS_DEVICE_PATH_EXT, O_RDONLY);
if (ret < 0) {
goto fail;
}
}
if (fd == -1) {
goto fail;
}
if (g_dev_ext != nullptr) {
int fd = open(LL40LS_DEVICE_PATH_EXT, O_RDONLY);
int ret = ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT);
close(fd);
if (fd == -1) {
goto fail;
if (ret < 0) {
goto fail;
}
}
int ret = ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT);
close(fd);
} else {
g_dev_pwm = new LidarLitePWM(LL40LS_DEVICE_PATH_PWM);
if (ret < 0) {
goto fail;
if (g_dev_pwm != nullptr && OK != g_dev_pwm->init()) {
delete g_dev_pwm;
g_dev_pwm = nullptr;
warnx("failed to init PWM");
}
if (g_dev_pwm != nullptr) {
int fd = open(LL40LS_DEVICE_PATH_PWM, O_RDONLY);
if (fd == -1) {
warnx("fd nothing");
goto fail;
}
int ret = ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT);
close(fd);
if (ret < 0) {
warnx("pollrate fail");
goto fail;
}
}
}
@@ -184,7 +216,7 @@ fail:
/**
* Stop the driver
*/
void stop(int bus)
void stop(const bool use_i2c, const int bus)
{
LidarLiteI2C **g_dev = (bus == PX4_I2C_BUS_ONBOARD ? &g_dev_int : &g_dev_ext);
@@ -205,17 +237,25 @@ void stop(int bus)
* and automatic modes.
*/
void
test(int bus)
test(const bool use_i2c, const int bus)
{
struct range_finder_report report;
ssize_t sz;
int ret;
const char *path = (bus == PX4_I2C_BUS_ONBOARD ? LL40LS_DEVICE_PATH_INT : LL40LS_DEVICE_PATH_EXT);
const char *path;
if (use_i2c) {
path = ((bus == PX4_I2C_BUS_ONBOARD) ? LL40LS_DEVICE_PATH_INT : LL40LS_DEVICE_PATH_EXT);
} else {
path = LL40LS_DEVICE_PATH_PWM;
}
int fd = open(path, O_RDONLY);
if (fd < 0) {
err(1, "%s open failed (try 'll40ls start' if the driver is not running", path);
err(1, "%s open failed, is the driver running?", path);
}
/* do a simple demand read */
@@ -234,7 +274,7 @@ test(int bus)
errx(1, "failed to set 2Hz poll rate");
}
/* read the sensor 5x and report each value */
/* read the sensor 5 times and report each value */
for (unsigned i = 0; i < 5; i++) {
struct pollfd fds;
@@ -255,7 +295,7 @@ test(int bus)
}
warnx("periodic read %u", i);
warnx("measurement: %0.3f", (double)report.distance);
warnx("measurement: %0.3f m", (double)report.distance);
warnx("time: %lld", report.timestamp);
}
@@ -271,9 +311,18 @@ test(int bus)
* Reset the driver.
*/
void
reset(int bus)
reset(const bool use_i2c, const int bus)
{
const char *path = (bus == PX4_I2C_BUS_ONBOARD ? LL40LS_DEVICE_PATH_INT : LL40LS_DEVICE_PATH_EXT);
const char *path;
if (use_i2c) {
path = ((bus == PX4_I2C_BUS_ONBOARD) ? LL40LS_DEVICE_PATH_INT : LL40LS_DEVICE_PATH_EXT);
} else {
path = LL40LS_DEVICE_PATH_PWM;
}
int fd = open(path, O_RDONLY);
if (fd < 0) {
@@ -295,12 +344,19 @@ reset(int bus)
* Print a little info about the driver.
*/
void
info(int bus)
info(const bool use_i2c, const int bus)
{
LidarLiteI2C *g_dev = (bus == PX4_I2C_BUS_ONBOARD ? g_dev_int : g_dev_ext);
LidarLite *g_dev = nullptr;
if (g_dev == nullptr) {
errx(1, "driver not running");
if (use_i2c) {
g_dev = (bus == PX4_I2C_BUS_ONBOARD ? g_dev_int : g_dev_ext);
if (g_dev == nullptr) {
errx(1, "driver not running");
}
} else {
g_dev = g_dev_pwm;
}
printf("state @ %p\n", g_dev);
@@ -313,7 +369,7 @@ info(int bus)
* Dump registers
*/
void
regdump(int bus)
regdump(const bool use_i2c, const int bus)
{
LidarLiteI2C *g_dev = (bus == PX4_I2C_BUS_ONBOARD ? g_dev_int : g_dev_ext);
@@ -330,8 +386,8 @@ regdump(int bus)
void
usage()
{
warnx("missing command: try 'start', 'stop', 'info', 'test', 'reset', 'info' or 'regdump'");
warnx("options:");
warnx("missing command: try 'start', 'stop', 'info', 'test', 'reset', 'info' or 'regdump' [i2c|pwm]");
warnx("options for I2C:");
warnx(" -X only external bus");
#ifdef PX4_I2C_BUS_ONBOARD
warnx(" -I only internal bus");
@@ -365,49 +421,68 @@ ll40ls_main(int argc, char *argv[])
}
}
const char *verb = argv[optind];
/* default to I2C if no protocol is given */
bool use_i2c = true;
/*
* Start/load the driver.
*/
if (!strcmp(verb, "start")) {
ll40ls::start(bus);
/* determine protocol first because it's needed next */
if (argc > optind + 1) {
const char *protocol = argv[optind + 1];
if (!strcmp(protocol, "pwm")) {
use_i2c = false;
} else if (!strcmp(protocol, "i2c")) {
use_i2c = true;
} else {
warnx("unknown protocol, choose pwm or i2c");
ll40ls::usage();
exit(0);
}
}
/*
* Stop the driver
*/
if (!strcmp(verb, "stop")) {
ll40ls::stop(bus);
/* now determine action */
if (argc > optind) {
const char *verb = argv[optind];
/* Start/load the driver. */
if (!strcmp(verb, "start")) {
ll40ls::start(use_i2c, bus);
}
/* Stop the driver */
if (!strcmp(verb, "stop")) {
ll40ls::stop(use_i2c, bus);
}
/* Test the driver/device. */
else if (!strcmp(verb, "test")) {
ll40ls::test(use_i2c, bus);
}
/* Reset the driver. */
else if (!strcmp(verb, "reset")) {
ll40ls::reset(use_i2c, bus);
}
/* dump registers */
else if (!strcmp(verb, "regdump")) {
ll40ls::regdump(use_i2c, bus);
}
/* Print driver information. */
else if (!strcmp(verb, "info") || !strcmp(verb, "status")) {
ll40ls::info(use_i2c, bus);
}
else {
ll40ls::usage();
exit(0);
}
}
/*
* Test the driver/device.
*/
if (!strcmp(verb, "test")) {
ll40ls::test(bus);
}
/*
* Reset the driver.
*/
if (!strcmp(verb, "reset")) {
ll40ls::reset(bus);
}
/*
* dump registers
*/
if (!strcmp(verb, "regdump")) {
ll40ls::regdump(bus);
}
/*
* Print driver information.
*/
if (!strcmp(verb, "info") || !strcmp(verb, "status")) {
ll40ls::info(bus);
}
errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info' or 'regdump'");
warnx("unrecognized command, try 'start', 'test', 'reset', 'info' or 'regdump'");
ll40ls::usage();
exit(0);
}