mirror of https://github.com/ArduPilot/ardupilot
AP_Rangefinder: Remove parameter manipulation from initializer
This commit is contained in:
parent
3f5999a6ad
commit
21b64ad19b
|
@ -147,16 +147,11 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = {
|
|||
|
||||
const AP_Param::GroupInfo *RangeFinder::backend_var_info[RANGEFINDER_MAX_INSTANCES];
|
||||
|
||||
RangeFinder::RangeFinder(AP_SerialManager &_serial_manager, enum Rotation orientation_default) :
|
||||
RangeFinder::RangeFinder(AP_SerialManager &_serial_manager) :
|
||||
serial_manager(_serial_manager)
|
||||
{
|
||||
AP_Param::setup_object_defaults(this, var_info);
|
||||
|
||||
// set orientation defaults
|
||||
for (uint8_t i=0; i<RANGEFINDER_MAX_INSTANCES; i++) {
|
||||
params[i].orientation.set_default(orientation_default);
|
||||
}
|
||||
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
||||
if (_singleton != nullptr) {
|
||||
AP_HAL::panic("Rangefinder must be singleton");
|
||||
|
@ -290,7 +285,7 @@ void RangeFinder::convert_params(void) {
|
|||
finders here. For now we won't allow for hot-plugging of
|
||||
rangefinders.
|
||||
*/
|
||||
void RangeFinder::init(void)
|
||||
void RangeFinder::init(enum Rotation orientation_default)
|
||||
{
|
||||
if (num_instances != 0) {
|
||||
// init called a 2nd time?
|
||||
|
@ -299,6 +294,11 @@ void RangeFinder::init(void)
|
|||
|
||||
convert_params();
|
||||
|
||||
// set orientation defaults
|
||||
for (uint8_t i=0; i<RANGEFINDER_MAX_INSTANCES; i++) {
|
||||
params[i].orientation.set_default(orientation_default);
|
||||
}
|
||||
|
||||
for (uint8_t i=0, serial_instance = 0; i<RANGEFINDER_MAX_INSTANCES; i++) {
|
||||
// serial_instance will be increased inside detect_instance
|
||||
// if a serial driver is loaded for this instance
|
||||
|
|
|
@ -37,7 +37,7 @@ class RangeFinder
|
|||
friend class AP_RangeFinder_Backend;
|
||||
|
||||
public:
|
||||
RangeFinder(AP_SerialManager &_serial_manager, enum Rotation orientation_default);
|
||||
RangeFinder(AP_SerialManager &_serial_manager);
|
||||
|
||||
/* Do not allow copies */
|
||||
RangeFinder(const RangeFinder &other) = delete;
|
||||
|
@ -109,7 +109,7 @@ public:
|
|||
}
|
||||
|
||||
// detect and initialise any available rangefinders
|
||||
void init(void);
|
||||
void init(enum Rotation orientation_default);
|
||||
|
||||
// update state of all rangefinders. Should be called at around
|
||||
// 10Hz from main loop
|
||||
|
|
|
@ -11,7 +11,7 @@ void loop();
|
|||
const AP_HAL::HAL& hal = AP_HAL::get_HAL();
|
||||
|
||||
static AP_SerialManager serial_manager;
|
||||
static RangeFinder sonar{serial_manager, ROTATION_PITCH_270};
|
||||
static RangeFinder sonar{serial_manager};
|
||||
|
||||
void setup()
|
||||
{
|
||||
|
@ -25,7 +25,7 @@ void setup()
|
|||
|
||||
// initialise sensor, delaying to make debug easier
|
||||
hal.scheduler->delay(2000);
|
||||
sonar.init();
|
||||
sonar.init(ROTATION_PITCH_270);
|
||||
hal.console->printf("RangeFinder: %d devices detected\n", sonar.num_sensors());
|
||||
}
|
||||
|
||||
|
|
Loading…
Reference in New Issue