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

ll40ls: refactor driver start on i2c.

Browse Source 
Signed-off-by: Claudio Micheli <claudio@auterion.com>
This commit is contained in:
Claudio Micheli 2019-08-20 14:55:11 +02:00 committed by Nuno Marques
parent 5eefd9409b
commit fcdd2fabeb
1 changed files with 32 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

46
src/drivers/distance_sensor/ll40ls/ll40ls.cpp
View File

@ -89,7 +89,7 @@ namespace ll40ls
{
LidarLite *instance = nullptr;
int start_bus(const struct ll40ls_bus_option &i2c_bus, uint8_t rotation);
void start(enum LL40LS_BUS busid, uint8_t rotation);
void stop();
void info();
@ -126,35 +126,53 @@ void start(enum LL40LS_BUS busid, uint8_t rotation)
continue;
}
instance = new LidarLiteI2C(bus_options[i].busnum, rotation);
if (!instance) {
PX4_ERR("Failed to instantiate LidarLiteI2C");
if (start_bus(bus_options[i], rotation) == PX4_OK) {
return;
}
if (instance->init() == PX4_OK) {
break;
}
PX4_ERR("failed to initialize LidarLiteI2C on busnum=%u", bus_options[i].busnum);
stop();
}
}
if (!instance) {
if (instance == nullptr) {
PX4_WARN("No LidarLite found");
return;
}
}
/**
* Start the driver on a specific bus.
*
* This function call only returns once the driver is up and running
* or failed to detect the sensor.
*/
int
start_bus(const struct ll40ls_bus_option &i2c_bus, uint8_t rotation)
{
instance = new LidarLiteI2C(i2c_bus.busnum, rotation);
if (instance->init() != OK) {
stop();
PX4_INFO("LidarLiteI2C - no device on bus %u", (unsigned)i2c_bus.busid);
return PX4_ERROR;
}
return PX4_OK;
}
/**
* @brief Stops the driver
*/
void stop()
{
delete instance;
instance = nullptr;
if (instance != nullptr) {
delete instance;
instance = nullptr;
} else {
PX4_ERR("driver not running");
}
}
/**