From 21de22f10df762b090356ae9f2171ce71271dae2 Mon Sep 17 00:00:00 2001 From: Pierre Kancir Date: Fri, 4 Jun 2021 23:36:24 +0200 Subject: [PATCH] AP_Rangefinder: fix override in constructor --- libraries/AP_RangeFinder/AP_RangeFinder.cpp | 30 +++++++++---------- libraries/AP_RangeFinder/AP_RangeFinder.h | 2 +- .../AP_RangeFinder/AP_RangeFinder_Backend.h | 1 + .../AP_RangeFinder_Backend_Serial.cpp | 8 +++-- .../AP_RangeFinder_Backend_Serial.h | 4 +-- .../AP_RangeFinder/AP_RangeFinder_Wasp.cpp | 5 ++-- .../AP_RangeFinder/AP_RangeFinder_Wasp.h | 3 +- 7 files changed, 28 insertions(+), 25 deletions(-) diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.cpp b/libraries/AP_RangeFinder/AP_RangeFinder.cpp index 4cd9dae559..7ffb3779b5 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder.cpp @@ -314,7 +314,7 @@ void RangeFinder::update(void) #endif } -bool RangeFinder::_add_backend(AP_RangeFinder_Backend *backend, uint8_t instance) +bool RangeFinder::_add_backend(AP_RangeFinder_Backend *backend, uint8_t instance, uint8_t serial_instance) { if (!backend) { return false; @@ -326,7 +326,7 @@ bool RangeFinder::_add_backend(AP_RangeFinder_Backend *backend, uint8_t instance // we've allocated the same instance twice INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); } - + backend->init_serial(serial_instance); drivers[instance] = backend; num_instances = MAX(num_instances, instance+1); @@ -442,17 +442,17 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance) break; case Type::LWSER: if (AP_RangeFinder_LightWareSerial::detect(serial_instance)) { - _add_backend(new AP_RangeFinder_LightWareSerial(state[instance], params[instance], serial_instance++), instance); + _add_backend(new AP_RangeFinder_LightWareSerial(state[instance], params[instance]), instance, serial_instance++); } break; case Type::LEDDARONE: if (AP_RangeFinder_LeddarOne::detect(serial_instance)) { - _add_backend(new AP_RangeFinder_LeddarOne(state[instance], params[instance], serial_instance++), instance); + _add_backend(new AP_RangeFinder_LeddarOne(state[instance], params[instance]), instance, serial_instance++); } break; case Type::ULANDING: if (AP_RangeFinder_uLanding::detect(serial_instance)) { - _add_backend(new AP_RangeFinder_uLanding(state[instance], params[instance], serial_instance++), instance); + _add_backend(new AP_RangeFinder_uLanding(state[instance], params[instance]), instance, serial_instance++); } break; case Type::BEBOP: @@ -472,7 +472,7 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance) break; case Type::MBSER: if (AP_RangeFinder_MaxsonarSerialLV::detect(serial_instance)) { - _add_backend(new AP_RangeFinder_MaxsonarSerialLV(state[instance], params[instance], serial_instance++), instance); + _add_backend(new AP_RangeFinder_MaxsonarSerialLV(state[instance], params[instance]), instance, serial_instance++); } break; case Type::ANALOG: @@ -493,27 +493,27 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance) break; case Type::NMEA: if (AP_RangeFinder_NMEA::detect(serial_instance)) { - _add_backend(new AP_RangeFinder_NMEA(state[instance], params[instance], serial_instance++), instance); + _add_backend(new AP_RangeFinder_NMEA(state[instance], params[instance]), instance, serial_instance++); } break; case Type::WASP: if (AP_RangeFinder_Wasp::detect(serial_instance)) { - _add_backend(new AP_RangeFinder_Wasp(state[instance], params[instance], serial_instance++), instance); + _add_backend(new AP_RangeFinder_Wasp(state[instance], params[instance]), instance, serial_instance++); } break; case Type::BenewakeTF02: if (AP_RangeFinder_Benewake_TF02::detect(serial_instance)) { - _add_backend(new AP_RangeFinder_Benewake_TF02(state[instance], params[instance], serial_instance++), instance); + _add_backend(new AP_RangeFinder_Benewake_TF02(state[instance], params[instance]), instance, serial_instance++); } break; case Type::BenewakeTFmini: if (AP_RangeFinder_Benewake_TFMini::detect(serial_instance)) { - _add_backend(new AP_RangeFinder_Benewake_TFMini(state[instance], params[instance], serial_instance++), instance); + _add_backend(new AP_RangeFinder_Benewake_TFMini(state[instance], params[instance]), instance, serial_instance++); } break; case Type::BenewakeTF03: if (AP_RangeFinder_Benewake_TF03::detect(serial_instance)) { - _add_backend(new AP_RangeFinder_Benewake_TF03(state[instance], params[instance], serial_instance++), instance); + _add_backend(new AP_RangeFinder_Benewake_TF03(state[instance], params[instance]), instance, serial_instance++); } break; case Type::PWM: @@ -525,17 +525,17 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance) break; case Type::BLPing: if (AP_RangeFinder_BLPing::detect(serial_instance)) { - _add_backend(new AP_RangeFinder_BLPing(state[instance], params[instance], serial_instance++), instance); + _add_backend(new AP_RangeFinder_BLPing(state[instance], params[instance]), instance, serial_instance++); } break; case Type::Lanbao: if (AP_RangeFinder_Lanbao::detect(serial_instance)) { - _add_backend(new AP_RangeFinder_Lanbao(state[instance], params[instance], serial_instance++), instance); + _add_backend(new AP_RangeFinder_Lanbao(state[instance], params[instance]), instance, serial_instance++); } break; case Type::LeddarVu8_Serial: if (AP_RangeFinder_LeddarVu8::detect(serial_instance)) { - _add_backend(new AP_RangeFinder_LeddarVu8(state[instance], params[instance], serial_instance++), instance); + _add_backend(new AP_RangeFinder_LeddarVu8(state[instance], params[instance]), instance, serial_instance++); } break; @@ -552,7 +552,7 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance) case Type::GYUS42v2: if (AP_RangeFinder_GYUS42v2::detect(serial_instance)) { - _add_backend(new AP_RangeFinder_GYUS42v2(state[instance], params[instance], serial_instance++), instance); + _add_backend(new AP_RangeFinder_GYUS42v2(state[instance], params[instance]), instance, serial_instance++); } break; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.h b/libraries/AP_RangeFinder/AP_RangeFinder.h index 6e264e4c6b..b1cd63545e 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder.h @@ -212,7 +212,7 @@ private: void detect_instance(uint8_t instance, uint8_t& serial_instance); - bool _add_backend(AP_RangeFinder_Backend *driver, uint8_t instance); + bool _add_backend(AP_RangeFinder_Backend *driver, uint8_t instance, uint8_t serial_instance=0); uint32_t _log_rfnd_bit = -1; void Log_RFND() const; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Backend.h b/libraries/AP_RangeFinder/AP_RangeFinder_Backend.h index 873a615859..97a265fdec 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Backend.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Backend.h @@ -30,6 +30,7 @@ public: // update the state structure virtual void update() = 0; + virtual void init_serial(uint8_t serial_instance) {}; virtual void handle_msg(const mavlink_message_t &msg) { return; } #if HAL_MSP_RANGEFINDER_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Backend_Serial.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Backend_Serial.cpp index 4f9eaee0b2..50cc913dd0 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Backend_Serial.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Backend_Serial.cpp @@ -28,9 +28,13 @@ extern const AP_HAL::HAL& hal; */ AP_RangeFinder_Backend_Serial::AP_RangeFinder_Backend_Serial( RangeFinder::RangeFinder_State &_state, - AP_RangeFinder_Params &_params, - uint8_t serial_instance) : + AP_RangeFinder_Params &_params) : AP_RangeFinder_Backend(_state, _params) +{ + +} + +void AP_RangeFinder_Backend_Serial::init_serial(uint8_t serial_instance) { uart = AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_Rangefinder, serial_instance); if (uart != nullptr) { diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Backend_Serial.h b/libraries/AP_RangeFinder/AP_RangeFinder_Backend_Serial.h index b8b6123c34..8fb67c2ff8 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Backend_Serial.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Backend_Serial.h @@ -7,9 +7,9 @@ class AP_RangeFinder_Backend_Serial : public AP_RangeFinder_Backend public: // constructor AP_RangeFinder_Backend_Serial(RangeFinder::RangeFinder_State &_state, - AP_RangeFinder_Params &_params, - uint8_t serial_instance); + AP_RangeFinder_Params &_params); + void init_serial(uint8_t serial_instance) override; // static detection function static bool detect(uint8_t serial_instance); diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Wasp.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Wasp.cpp index 56d11fcd30..6da29e7d3c 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Wasp.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Wasp.cpp @@ -67,9 +67,8 @@ const AP_Param::GroupInfo AP_RangeFinder_Wasp::var_info[] = { }; AP_RangeFinder_Wasp::AP_RangeFinder_Wasp(RangeFinder::RangeFinder_State &_state, - AP_RangeFinder_Params &_params, - uint8_t serial_instance) : - AP_RangeFinder_Backend_Serial(_state, _params, serial_instance) + AP_RangeFinder_Params &_params) : + AP_RangeFinder_Backend_Serial(_state, _params) { AP_Param::setup_object_defaults(this, var_info); diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Wasp.h b/libraries/AP_RangeFinder/AP_RangeFinder_Wasp.h index 2c8e5d9674..2ccc2bd33f 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Wasp.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Wasp.h @@ -10,8 +10,7 @@ class AP_RangeFinder_Wasp : public AP_RangeFinder_Backend_Serial { public: AP_RangeFinder_Wasp(RangeFinder::RangeFinder_State &_state, - AP_RangeFinder_Params &_params, - uint8_t serial_instance); + AP_RangeFinder_Params &_params); void update(void) override;