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

Lidar lite driver fixes.

Browse Source 
Conflicts:
	src/modules/uORB/Subscription.cpp
This commit is contained in:
James Goppert
2015-05-25 21:55:03 -04:00
parent c5f14ef785
commit 5955773574
2 changed files with 49 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ROMFS/px4fmu_common/init.d/rc.sensors
+5 -1
View File
@@ -115,7 +115,11 @@ if px4flow start
then
fi
if ll40ls start
if pwm_input start
then
fi
if ll40ls start pwm
then
fi
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);
}