fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-16 05:03:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #2231 from jgoppert/lidar_lite_pwm_fix

Browse Source 
Lidar lite driver fixes.
This commit is contained in:
Lorenz Meier
2015-05-27 10:54:59 -07:00
parent 92f67a923a 57f5db544c
commit 9a67303416
4 changed files with 64 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/ll40ls/ll40ls.cpp
+44 -29
View File
@@ -36,6 +36,7 @@
* @author Allyson Kreft
* @author Johan Jansen <jnsn.johan@gmail.com>
* @author Ban Siesta <bansiesta@gmail.com>
* @author James Goppert <james.goppert@gmail.com>
*
* Interface for the PulsedLight Lidar-Lite range finders.
*/
@@ -79,6 +80,7 @@ LidarLiteI2C *g_dev_int;
LidarLiteI2C *g_dev_ext;
LidarLitePWM *g_dev_pwm;
LidarLite * get_dev(const bool use_i2c, const 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);
@@ -87,6 +89,25 @@ void info(const bool use_i2c, const int bus);
void regdump(const bool use_i2c, const int bus);
void usage();
/**
* Get the correct device pointer
*/
LidarLite * get_dev(const bool use_i2c, const int bus) {
LidarLite * g_dev = nullptr;
if (use_i2c) {
g_dev = static_cast<LidarLite*>(bus == PX4_I2C_BUS_ONBOARD ? g_dev_int : g_dev_ext);
if (g_dev == nullptr) {
errx(1, "i2c driver not running");
}
} else {
g_dev = static_cast<LidarLite*>(g_dev_pwm);
if (g_dev == nullptr) {
errx(1, "pwm driver not running");
}
}
return g_dev;
};
/**
* Start the driver.
*/
@@ -210,6 +231,11 @@ fail:
g_dev_ext = nullptr;
}
if (g_dev_pwm != nullptr) {
delete g_dev_pwm;
g_dev_pwm = nullptr;
}
errx(1, "driver start failed");
}
@@ -218,16 +244,24 @@ fail:
*/
void stop(const bool use_i2c, const int bus)
{
LidarLiteI2C **g_dev = (bus == PX4_I2C_BUS_ONBOARD ? &g_dev_int : &g_dev_ext);
if (*g_dev != nullptr) {
delete *g_dev;
*g_dev = nullptr;
if (use_i2c) {
if (bus == PX4_I2C_BUS_ONBOARD) {
if (g_dev_int != nullptr) {
delete g_dev_int;
g_dev_int = nullptr;
}
} else {
if (g_dev_ext != nullptr) {
delete g_dev_ext;
g_dev_ext = nullptr;
}
}
} else {
errx(1, "driver not running");
if (g_dev_pwm != nullptr) {
delete g_dev_pwm;
g_dev_pwm = nullptr;
}
}
exit(0);
}
@@ -346,22 +380,9 @@ reset(const bool use_i2c, const int bus)
void
info(const bool use_i2c, const int bus)
{
LidarLite *g_dev = nullptr;
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;
}
LidarLite * g_dev = get_dev(use_i2c, bus);
printf("state @ %p\n", g_dev);
g_dev->print_info();
exit(0);
}
@@ -371,15 +392,9 @@ info(const bool use_i2c, const int bus)
void
regdump(const bool use_i2c, const int bus)
{
LidarLiteI2C *g_dev = (bus == PX4_I2C_BUS_ONBOARD ? g_dev_int : g_dev_ext);
if (g_dev == nullptr) {
errx(1, "driver not running");
}
LidarLite * g_dev = get_dev(use_i2c, bus);
printf("regdump @ %p\n", g_dev);
g_dev->print_registers();
exit(0);
}