Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-23 00:04:02 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_RangeFinder: allow re-init if no sensors found

Browse Source 
needed for AP_Periph with slow startup sensors or sensors that power
on separately to the CAN node
This commit is contained in:
Andrew Tridgell 2023-03-06 08:58:23 +11:00
parent 05c996bfcc
commit e300db5be0
2 changed files with 4 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/AP_RangeFinder/AP_RangeFinder.cpp
View File

@ -185,11 +185,10 @@ RangeFinder::RangeFinder()
*/ */
void RangeFinder::init(enum Rotation orientation_default) void RangeFinder::init(enum Rotation orientation_default)
{ {
if (init_done) { if (num_instances != 0) {
// init called a 2nd time? // don't re-init if we've found some sensors already
return; return;
} }
init_done = true;
// set orientation defaults // set orientation defaults
for (uint8_t i=0; i<RANGEFINDER_MAX_INSTANCES; i++) { for (uint8_t i=0; i<RANGEFINDER_MAX_INSTANCES; i++) {
@ -298,9 +297,11 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)
#if AP_RANGEFINDER_LWI2C_ENABLED #if AP_RANGEFINDER_LWI2C_ENABLED
if (params[instance].address) { if (params[instance].address) {
// the LW20 needs a long time to boot up, so we delay 1.5s here // the LW20 needs a long time to boot up, so we delay 1.5s here
#ifndef HAL_BUILD_AP_PERIPH
if (!hal.util->was_watchdog_armed()) { if (!hal.util->was_watchdog_armed()) {
hal.scheduler->delay(1500); hal.scheduler->delay(1500);
} }
#endif
#ifdef HAL_RANGEFINDER_LIGHTWARE_I2C_BUS #ifdef HAL_RANGEFINDER_LIGHTWARE_I2C_BUS
_add_backend(AP_RangeFinder_LightWareI2C::detect(state[instance], params[instance], _add_backend(AP_RangeFinder_LightWareI2C::detect(state[instance], params[instance],
hal.i2c_mgr->get_device(HAL_RANGEFINDER_LIGHTWARE_I2C_BUS, params[instance].address)), hal.i2c_mgr->get_device(HAL_RANGEFINDER_LIGHTWARE_I2C_BUS, params[instance].address)),

1
libraries/AP_RangeFinder/AP_RangeFinder.h
View File

@ -219,7 +219,6 @@ private:
RangeFinder_State state[RANGEFINDER_MAX_INSTANCES]; RangeFinder_State state[RANGEFINDER_MAX_INSTANCES];
AP_RangeFinder_Backend *drivers[RANGEFINDER_MAX_INSTANCES]; AP_RangeFinder_Backend *drivers[RANGEFINDER_MAX_INSTANCES];
uint8_t num_instances; uint8_t num_instances;
bool init_done;
HAL_Semaphore detect_sem; HAL_Semaphore detect_sem;
float estimated_terrain_height; float estimated_terrain_height;
Vector3f pos_offset_zero; // allows returning position offsets of zero for invalid requests Vector3f pos_offset_zero; // allows returning position offsets of zero for invalid requests