AP_RangeFinder: Make singleton

This commit is contained in:
Michael du Breuil 2018-05-09 00:45:26 -07:00 committed by Francisco Ferreira
parent 5393337dfa
commit 7d6c3ec683
2 changed files with 13 additions and 0 deletions

View File

@ -540,6 +540,13 @@ RangeFinder::RangeFinder(AP_SerialManager &_serial_manager, enum Rotation orient
for (uint8_t i=0; i<RANGEFINDER_MAX_INSTANCES; i++) {
state[i].orientation.set_default(orientation_default);
}
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
if (_singleton != nullptr) {
AP_HAL::panic("Rangefinder must be singleton");
}
#endif // CONFIG_HAL_BOARD == HAL_BOARD_SITL
_singleton = this;
}
/*
@ -864,3 +871,5 @@ MAV_DISTANCE_SENSOR RangeFinder::get_mav_distance_sensor_type_orient(enum Rotati
}
return backend->get_mav_distance_sensor_type();
}
RangeFinder *RangeFinder::_singleton;

View File

@ -157,8 +157,12 @@ public:
*/
bool pre_arm_check() const;
static RangeFinder *get_singleton(void) { return _singleton; }
private:
static RangeFinder *_singleton;
RangeFinder_State state[RANGEFINDER_MAX_INSTANCES];
AP_RangeFinder_Backend *drivers[RANGEFINDER_MAX_INSTANCES];
uint8_t num_instances:3;