From 5eff01a86f9381c6f5a5f9a6a6c38623c409ccd4 Mon Sep 17 00:00:00 2001 From: Dmitri Ranfft Date: Wed, 4 Jul 2018 16:22:17 +0200 Subject: [PATCH] RangeFinder: allow up to 10 range finders to be used at once --- .../AP_RangeFinder/AP_RangeFinder_BBB_PRU.cpp | 4 +- .../AP_RangeFinder/AP_RangeFinder_BBB_PRU.h | 2 +- .../AP_RangeFinder_Benewake.cpp | 3 +- .../AP_RangeFinder/AP_RangeFinder_Benewake.h | 1 + .../AP_RangeFinder_LeddarOne.cpp | 3 +- .../AP_RangeFinder/AP_RangeFinder_LeddarOne.h | 1 + .../AP_RangeFinder_LightWareI2C.cpp | 10 +- .../AP_RangeFinder_LightWareI2C.h | 3 +- .../AP_RangeFinder_LightWareSerial.cpp | 3 +- .../AP_RangeFinder_LightWareSerial.h | 1 + .../AP_RangeFinder/AP_RangeFinder_MAVLink.cpp | 4 +- .../AP_RangeFinder/AP_RangeFinder_MAVLink.h | 2 +- .../AP_RangeFinder_MaxsonarI2CXL.cpp | 6 +- .../AP_RangeFinder_MaxsonarI2CXL.h | 2 + .../AP_RangeFinder_MaxsonarSerialLV.cpp | 3 +- .../AP_RangeFinder_MaxsonarSerialLV.h | 1 + .../AP_RangeFinder/AP_RangeFinder_NMEA.cpp | 3 +- .../AP_RangeFinder/AP_RangeFinder_NMEA.h | 1 + .../AP_RangeFinder/AP_RangeFinder_PWM.cpp | 33 +- libraries/AP_RangeFinder/AP_RangeFinder_PWM.h | 3 +- .../AP_RangeFinder/AP_RangeFinder_Params.cpp | 139 ++++ .../AP_RangeFinder/AP_RangeFinder_Params.h | 31 + .../AP_RangeFinder_PulsedLightLRF.cpp | 6 +- .../AP_RangeFinder_PulsedLightLRF.h | 2 + .../AP_RangeFinder_TeraRangerI2C.cpp | 8 +- .../AP_RangeFinder_TeraRangerI2C.h | 2 + .../AP_RangeFinder/AP_RangeFinder_VL53L0X.cpp | 8 +- .../AP_RangeFinder/AP_RangeFinder_VL53L0X.h | 4 +- .../AP_RangeFinder/AP_RangeFinder_VL53L1X.cpp | 8 +- .../AP_RangeFinder/AP_RangeFinder_VL53L1X.h | 4 +- .../AP_RangeFinder/AP_RangeFinder_Wasp.cpp | 3 +- .../AP_RangeFinder/AP_RangeFinder_Wasp.h | 1 + .../AP_RangeFinder/AP_RangeFinder_analog.cpp | 31 +- .../AP_RangeFinder/AP_RangeFinder_analog.h | 5 +- .../AP_RangeFinder_uLanding.cpp | 3 +- .../AP_RangeFinder/AP_RangeFinder_uLanding.h | 1 + libraries/AP_RangeFinder/RangeFinder.cpp | 782 ++++++------------ libraries/AP_RangeFinder/RangeFinder.h | 23 +- .../AP_RangeFinder/RangeFinder_Backend.cpp | 17 +- .../AP_RangeFinder/RangeFinder_Backend.h | 16 +- 40 files changed, 540 insertions(+), 643 deletions(-) create mode 100644 libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp create mode 100644 libraries/AP_RangeFinder/AP_RangeFinder_Params.h diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_BBB_PRU.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_BBB_PRU.cpp index 09d3b3d0de..513e7dfa81 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_BBB_PRU.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_BBB_PRU.cpp @@ -39,8 +39,8 @@ volatile struct range *rangerpru; constructor is not called until detect() returns true, so we already know that we should setup the rangefinder */ -AP_RangeFinder_BBB_PRU::AP_RangeFinder_BBB_PRU(RangeFinder::RangeFinder_State &_state) : - AP_RangeFinder_Backend(_state) +AP_RangeFinder_BBB_PRU::AP_RangeFinder_BBB_PRU(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params) : + AP_RangeFinder_Backend(_state, _params) { } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_BBB_PRU.h b/libraries/AP_RangeFinder/AP_RangeFinder_BBB_PRU.h index 5afe4ca4f0..5376dab6b6 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_BBB_PRU.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_BBB_PRU.h @@ -21,7 +21,7 @@ class AP_RangeFinder_BBB_PRU : public AP_RangeFinder_Backend { public: // constructor - AP_RangeFinder_BBB_PRU(RangeFinder::RangeFinder_State &_state); + AP_RangeFinder_BBB_PRU(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params); // static detection function static bool detect(); diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp index 3b746745f0..cc79085e69 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp @@ -49,10 +49,11 @@ extern const AP_HAL::HAL& hal; already know that we should setup the rangefinder */ AP_RangeFinder_Benewake::AP_RangeFinder_Benewake(RangeFinder::RangeFinder_State &_state, + AP_RangeFinder_Params &_params, AP_SerialManager &serial_manager, uint8_t serial_instance, benewake_model_type model) : - AP_RangeFinder_Backend(_state), + AP_RangeFinder_Backend(_state, _params), model_type(model) { uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Rangefinder, serial_instance); diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.h b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.h index 824f50ab16..30fe23ed95 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.h @@ -15,6 +15,7 @@ public: // constructor AP_RangeFinder_Benewake(RangeFinder::RangeFinder_State &_state, + AP_RangeFinder_Params &_params, AP_SerialManager &serial_manager, uint8_t serial_instance, benewake_model_type model); diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.cpp index 8ec49ca1af..dff49cce34 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.cpp @@ -25,9 +25,10 @@ extern const AP_HAL::HAL& hal; already know that we should setup the rangefinder */ AP_RangeFinder_LeddarOne::AP_RangeFinder_LeddarOne(RangeFinder::RangeFinder_State &_state, + AP_RangeFinder_Params &_params, AP_SerialManager &serial_manager, uint8_t serial_instance) : - AP_RangeFinder_Backend(_state) + AP_RangeFinder_Backend(_state, _params) { uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Rangefinder, serial_instance); if (uart != nullptr) { diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.h b/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.h index 0253ef0509..ea67a9707b 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.h @@ -43,6 +43,7 @@ class AP_RangeFinder_LeddarOne : public AP_RangeFinder_Backend public: // constructor AP_RangeFinder_LeddarOne(RangeFinder::RangeFinder_State &_state, + AP_RangeFinder_Params &_params, AP_SerialManager &serial_manager, uint8_t serial_instance); diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.cpp index ce84ab4c88..fa7320ff81 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.cpp @@ -26,8 +26,8 @@ extern const AP_HAL::HAL& hal; constructor is not called until detect() returns true, so we already know that we should setup the rangefinder */ -AP_RangeFinder_LightWareI2C::AP_RangeFinder_LightWareI2C(RangeFinder::RangeFinder_State &_state, AP_HAL::OwnPtr dev) - : AP_RangeFinder_Backend(_state) +AP_RangeFinder_LightWareI2C::AP_RangeFinder_LightWareI2C(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params, AP_HAL::OwnPtr dev) + : AP_RangeFinder_Backend(_state, _params) , _dev(std::move(dev)) {} /* @@ -35,14 +35,14 @@ AP_RangeFinder_LightWareI2C::AP_RangeFinder_LightWareI2C(RangeFinder::RangeFinde trying to take a reading on I2C. If we get a result the sensor is there. */ -AP_RangeFinder_Backend *AP_RangeFinder_LightWareI2C::detect(RangeFinder::RangeFinder_State &_state, AP_HAL::OwnPtr dev) +AP_RangeFinder_Backend *AP_RangeFinder_LightWareI2C::detect(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params, AP_HAL::OwnPtr dev) { if (!dev) { return nullptr; } AP_RangeFinder_LightWareI2C *sensor - = new AP_RangeFinder_LightWareI2C(_state, std::move(dev)); + = new AP_RangeFinder_LightWareI2C(_state, _params, std::move(dev)); if (!sensor) { delete sensor; @@ -76,7 +76,7 @@ bool AP_RangeFinder_LightWareI2C::get_reading(uint16_t &reading_cm) { be16_t val; - if (state.address == 0) { + if (params.address == 0) { return false; } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.h b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.h index ada0dac8b1..7655694cec 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.h @@ -10,6 +10,7 @@ class AP_RangeFinder_LightWareI2C : public AP_RangeFinder_Backend public: // static detection function static AP_RangeFinder_Backend *detect(RangeFinder::RangeFinder_State &_state, + AP_RangeFinder_Params &_params, AP_HAL::OwnPtr dev); // update state @@ -23,7 +24,7 @@ protected: private: // constructor - AP_RangeFinder_LightWareI2C(RangeFinder::RangeFinder_State &_state, AP_HAL::OwnPtr dev); + AP_RangeFinder_LightWareI2C(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params, AP_HAL::OwnPtr dev); void init(); void timer(); diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.cpp index 313ae05c68..944156c4b2 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.cpp @@ -26,9 +26,10 @@ extern const AP_HAL::HAL& hal; already know that we should setup the rangefinder */ AP_RangeFinder_LightWareSerial::AP_RangeFinder_LightWareSerial(RangeFinder::RangeFinder_State &_state, + AP_RangeFinder_Params &_params, AP_SerialManager &serial_manager, uint8_t serial_instance) : - AP_RangeFinder_Backend(_state) + AP_RangeFinder_Backend(_state, _params) { uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Rangefinder, serial_instance); if (uart != nullptr) { diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.h b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.h index a1b60898fd..a85b4105b1 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.h @@ -9,6 +9,7 @@ class AP_RangeFinder_LightWareSerial : public AP_RangeFinder_Backend public: // constructor AP_RangeFinder_LightWareSerial(RangeFinder::RangeFinder_State &_state, + AP_RangeFinder_Params &_params, AP_SerialManager &serial_manager, uint8_t serial_instance); diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.cpp index 7eef8875de..3941164134 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.cpp @@ -25,8 +25,8 @@ extern const AP_HAL::HAL& hal; constructor is not called until detect() returns true, so we already know that we should setup the rangefinder */ -AP_RangeFinder_MAVLink::AP_RangeFinder_MAVLink(RangeFinder::RangeFinder_State &_state) : - AP_RangeFinder_Backend(_state) +AP_RangeFinder_MAVLink::AP_RangeFinder_MAVLink(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params) : + AP_RangeFinder_Backend(_state, _params) { state.last_reading_ms = AP_HAL::millis(); distance_cm = 0; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.h b/libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.h index fcd0ba3685..f489a471ad 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.h @@ -11,7 +11,7 @@ class AP_RangeFinder_MAVLink : public AP_RangeFinder_Backend public: // constructor - AP_RangeFinder_MAVLink(RangeFinder::RangeFinder_State &_state); + AP_RangeFinder_MAVLink(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params); // static detection function static bool detect(); diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.cpp index f876353f12..5cb392b5a6 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.cpp @@ -37,8 +37,9 @@ extern const AP_HAL::HAL& hal; already know that we should setup the rangefinder */ AP_RangeFinder_MaxsonarI2CXL::AP_RangeFinder_MaxsonarI2CXL(RangeFinder::RangeFinder_State &_state, + AP_RangeFinder_Params &_params, AP_HAL::OwnPtr dev) - : AP_RangeFinder_Backend(_state) + : AP_RangeFinder_Backend(_state, _params) , _dev(std::move(dev)) { } @@ -49,6 +50,7 @@ AP_RangeFinder_MaxsonarI2CXL::AP_RangeFinder_MaxsonarI2CXL(RangeFinder::RangeFin there. */ AP_RangeFinder_Backend *AP_RangeFinder_MaxsonarI2CXL::detect(RangeFinder::RangeFinder_State &_state, + AP_RangeFinder_Params &_params, AP_HAL::OwnPtr dev) { if (!dev) { @@ -56,7 +58,7 @@ AP_RangeFinder_Backend *AP_RangeFinder_MaxsonarI2CXL::detect(RangeFinder::RangeF } AP_RangeFinder_MaxsonarI2CXL *sensor - = new AP_RangeFinder_MaxsonarI2CXL(_state, std::move(dev)); + = new AP_RangeFinder_MaxsonarI2CXL(_state, _params, std::move(dev)); if (!sensor) { return nullptr; } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.h b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.h index d9c4d74604..e0e7458acb 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.h @@ -12,6 +12,7 @@ class AP_RangeFinder_MaxsonarI2CXL : public AP_RangeFinder_Backend public: // static detection function static AP_RangeFinder_Backend *detect(RangeFinder::RangeFinder_State &_state, + AP_RangeFinder_Params &_params, AP_HAL::OwnPtr dev); // update state @@ -26,6 +27,7 @@ protected: private: // constructor AP_RangeFinder_MaxsonarI2CXL(RangeFinder::RangeFinder_State &_state, + AP_RangeFinder_Params &_params, AP_HAL::OwnPtr dev); bool _init(void); diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.cpp index 8d758a4c1e..5ae80569cd 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.cpp @@ -30,9 +30,10 @@ extern const AP_HAL::HAL& hal; already know that we should setup the rangefinder */ AP_RangeFinder_MaxsonarSerialLV::AP_RangeFinder_MaxsonarSerialLV(RangeFinder::RangeFinder_State &_state, + AP_RangeFinder_Params &_params, AP_SerialManager &serial_manager, uint8_t serial_instance) : - AP_RangeFinder_Backend(_state) + AP_RangeFinder_Backend(_state, _params) { uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Rangefinder, serial_instance); if (uart != nullptr) { diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.h b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.h index c254029222..f002fe5053 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.h @@ -9,6 +9,7 @@ class AP_RangeFinder_MaxsonarSerialLV : public AP_RangeFinder_Backend public: // constructor AP_RangeFinder_MaxsonarSerialLV(RangeFinder::RangeFinder_State &_state, + AP_RangeFinder_Params &_params, AP_SerialManager &serial_manager, uint8_t serial_instance); diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.cpp index 143a400bc2..2bf783d3ef 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.cpp @@ -25,9 +25,10 @@ extern const AP_HAL::HAL& hal; // Note this is called after detect() returns true, so we // already know that we should setup the rangefinder AP_RangeFinder_NMEA::AP_RangeFinder_NMEA(RangeFinder::RangeFinder_State &_state, + AP_RangeFinder_Params &_params, AP_SerialManager &serial_manager, uint8_t serial_instance) : - AP_RangeFinder_Backend(_state), + AP_RangeFinder_Backend(_state, _params), _distance_m(-1.0f) { uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Rangefinder, serial_instance); diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.h b/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.h index 7d19af3b22..9390e5ff06 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.h @@ -24,6 +24,7 @@ class AP_RangeFinder_NMEA : public AP_RangeFinder_Backend public: // constructor AP_RangeFinder_NMEA(RangeFinder::RangeFinder_State &_state, + AP_RangeFinder_Params &_params, AP_SerialManager &serial_manager, uint8_t serial_instance); diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_PWM.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_PWM.cpp index f14e5c6113..342febc4cb 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_PWM.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_PWM.cpp @@ -24,10 +24,9 @@ extern const AP_HAL::HAL& hal; The constructor also initialises the rangefinder. */ AP_RangeFinder_PWM::AP_RangeFinder_PWM(RangeFinder::RangeFinder_State &_state, - AP_Int16 &_powersave_range, + AP_RangeFinder_Params &_params, float &_estimated_terrain_height) : - AP_RangeFinder_Backend(_state), - powersave_range(_powersave_range), + AP_RangeFinder_Backend(_state, _params), estimated_terrain_height(_estimated_terrain_height) { } @@ -74,7 +73,7 @@ bool AP_RangeFinder_PWM::get_reading(uint16_t &reading_cm) void AP_RangeFinder_PWM::check_pin() { - if (state.pin == last_pin) { + if (params.pin == last_pin) { return; } @@ -88,19 +87,19 @@ void AP_RangeFinder_PWM::check_pin() } } - // set last pin to state.pin so we don't continually try to attach + // set last pin to params.pin so we don't continually try to attach // to it if the attach is failing - last_pin = state.pin; + last_pin = params.pin; - if (state.pin <= 0) { + if (params.pin <= 0) { // don't need to install handler return; } // install interrupt handler on rising and falling edge - hal.gpio->pinMode(state.pin, HAL_GPIO_INPUT); + hal.gpio->pinMode(params.pin, HAL_GPIO_INPUT); if (!hal.gpio->attach_interrupt( - state.pin, + params.pin, FUNCTOR_BIND_MEMBER(&AP_RangeFinder_PWM::irq_handler, void, uint8_t, @@ -110,20 +109,20 @@ void AP_RangeFinder_PWM::check_pin() // failed to attach interrupt gcs().send_text(MAV_SEVERITY_WARNING, "RangeFinder_PWM: Failed to attach to pin %u", - state.pin); + params.pin); return; } } void AP_RangeFinder_PWM::check_stop_pin() { - if (state.stop_pin == last_stop_pin) { + if (params.stop_pin == last_stop_pin) { return; } - hal.gpio->pinMode(state.stop_pin, HAL_GPIO_OUTPUT); + hal.gpio->pinMode(params.stop_pin, HAL_GPIO_OUTPUT); - last_stop_pin = state.stop_pin; + last_stop_pin = params.stop_pin; } void AP_RangeFinder_PWM::check_pins() @@ -146,12 +145,12 @@ void AP_RangeFinder_PWM::update(void) return; } - if (state.stop_pin != -1) { + if (params.stop_pin != -1) { const bool oor = out_of_range(); if (oor) { if (!was_out_of_range) { // we are above the power saving range. Disable the sensor - hal.gpio->write(state.stop_pin, false); + hal.gpio->write(params.stop_pin, false); set_status(RangeFinder::RangeFinder_NoData); state.distance_cm = 0; state.voltage_mv = 0; @@ -161,7 +160,7 @@ void AP_RangeFinder_PWM::update(void) } // re-enable the sensor: if (!oor && was_out_of_range) { - hal.gpio->write(state.stop_pin, true); + hal.gpio->write(params.stop_pin, true); was_out_of_range = oor; } } @@ -182,5 +181,5 @@ void AP_RangeFinder_PWM::update(void) // return true if we are beyond the power saving range bool AP_RangeFinder_PWM::out_of_range(void) const { - return powersave_range > 0 && estimated_terrain_height > powersave_range; + return params.powersave_range > 0 && estimated_terrain_height > params.powersave_range; } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_PWM.h b/libraries/AP_RangeFinder/AP_RangeFinder_PWM.h index 622d5ce6e5..54c8d395f3 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_PWM.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_PWM.h @@ -22,7 +22,7 @@ class AP_RangeFinder_PWM : public AP_RangeFinder_Backend public: // constructor AP_RangeFinder_PWM(RangeFinder::RangeFinder_State &_state, - AP_Int16 &_powersave_range, + AP_RangeFinder_Params &_params, float &_estimated_terrain_height); // destructor @@ -58,7 +58,6 @@ private: void check_pins(); uint8_t last_stop_pin = -1; - AP_Int16 &powersave_range; float &estimated_terrain_height; // return true if we are beyond the power saving range diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp new file mode 100644 index 0000000000..cd4a4d3c67 --- /dev/null +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp @@ -0,0 +1,139 @@ +#include +#include +#include "AP_RangeFinder_Params.h" +#include "AP_RangeFinder.h" + +// table of user settable parameters +const AP_Param::GroupInfo AP_RangeFinder_Params::var_info[] = { + // @Param: TYPE + // @DisplayName: Rangefinder type + // @Description: What type of rangefinder device that is connected + // @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLiteV2-I2C,5:PX4-PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:uLanding,12:LeddarOne,13:MaxbotixSerial,14:TeraRangerI2C,15:LidarLiteV3-I2C,16:VL53L0X,17:NMEA,18:WASP-LRF,19:BenewakeTF02,20:BenewakeTFmini + // @User: Standard + AP_GROUPINFO("TYPE", 1, AP_RangeFinder_Params, type, 0), + + // @Param: PIN + // @DisplayName: Rangefinder pin + // @Description: Analog pin that rangefinder is connected to. Set to 11 on PX4 for the analog 'airspeed' port. Set to 15 on the Pixhawk for the analog 'airspeed' port. + // @Values: -1:Not Used, 11:PX4-airspeed port, 15:Pixhawk-airspeed port + // @User: Standard + AP_GROUPINFO("PIN", 2, AP_RangeFinder_Params, pin, -1), + + // @Param: SCALING + // @DisplayName: Rangefinder scaling + // @Description: Scaling factor between rangefinder reading and distance. For the linear and inverted functions this is in meters per volt. For the hyperbolic function the units are meterVolts. + // @Units: m/V + // @Increment: 0.001 + // @User: Standard + AP_GROUPINFO("SCALING", 3, AP_RangeFinder_Params, scaling, 3.0f), + + // @Param: OFFSET + // @DisplayName: rangefinder offset + // @Description: Offset in volts for zero distance for analog rangefinders. Offset added to distance in centimeters for PWM and I2C Lidars + // @Units: V + // @Increment: 0.001 + // @User: Standard + AP_GROUPINFO("OFFSET", 4, AP_RangeFinder_Params, offset, 0.0f), + + // @Param: FUNCTION + // @DisplayName: Rangefinder function + // @Description: Control over what function is used to calculate distance. For a linear function, the distance is (voltage-offset)*scaling. For a inverted function the distance is (offset-voltage)*scaling. For a hyperbolic function the distance is scaling/(voltage-offset). The functions return the distance in meters. + // @Values: 0:Linear,1:Inverted,2:Hyperbolic + // @User: Standard + AP_GROUPINFO("FUNCTION", 5, AP_RangeFinder_Params, function, 0), + + // @Param: MIN_CM + // @DisplayName: Rangefinder minimum distance + // @Description: Minimum distance in centimeters that rangefinder can reliably read + // @Units: cm + // @Increment: 1 + // @User: Standard + AP_GROUPINFO("MIN_CM", 6, AP_RangeFinder_Params, min_distance_cm, 20), + + // @Param: MAX_CM + // @DisplayName: Rangefinder maximum distance + // @Description: Maximum distance in centimeters that rangefinder can reliably read + // @Units: cm + // @Increment: 1 + // @User: Standard + AP_GROUPINFO("MAX_CM", 7, AP_RangeFinder_Params, max_distance_cm, 700), + + // @Param: STOP_PIN + // @DisplayName: Rangefinder stop pin + // @Description: Digital pin that enables/disables rangefinder measurement for an analog rangefinder. A value of -1 means no pin. If this is set, then the pin is set to 1 to enable the rangefinder and set to 0 to disable it. This can be used to ensure that multiple sonar rangefinders don't interfere with each other. + // @Values: -1:Not Used,50:Pixhawk AUXOUT1,51:Pixhawk AUXOUT2,52:Pixhawk AUXOUT3,53:Pixhawk AUXOUT4,54:Pixhawk AUXOUT5,55:Pixhawk AUXOUT6,111:PX4 FMU Relay1,112:PX4 FMU Relay2,113:PX4IO Relay1,114:PX4IO Relay2,115:PX4IO ACC1,116:PX4IO ACC2 + // @User: Standard + AP_GROUPINFO("STOP_PIN", 8, AP_RangeFinder_Params, stop_pin, -1), + + // @Param: SETTLE + // @DisplayName: Rangefinder settle time + // @Description: The time in milliseconds that the rangefinder reading takes to settle. This is only used when a STOP_PIN is specified. It determines how long we have to wait for the rangefinder to give a reading after we set the STOP_PIN high. For a sonar rangefinder with a range of around 7m this would need to be around 50 milliseconds to allow for the sonar pulse to travel to the target and back again. + // @Units: ms + // @Increment: 1 + // @User: Standard + AP_GROUPINFO("SETTLE", 9, AP_RangeFinder_Params, settle_time_ms, 0), + + // @Param: RMETRIC + // @DisplayName: Ratiometric + // @Description: This parameter sets whether an analog rangefinder is ratiometric. Most analog rangefinders are ratiometric, meaning that their output voltage is influenced by the supply voltage. Some analog rangefinders (such as the SF/02) have their own internal voltage regulators so they are not ratiometric. + // @Values: 0:No,1:Yes + // @User: Standard + AP_GROUPINFO("RMETRIC", 10, AP_RangeFinder_Params, ratiometric, 1), + + // @Param: PWRRNG + // @DisplayName: Powersave range + // @Description: This parameter sets the estimated terrain distance in meters above which the sensor will be put into a power saving mode (if available). A value of zero means power saving is not enabled + // @Units: m + // @Range: 0 32767 + // @User: Standard + AP_GROUPINFO("PWRRNG", 11, AP_RangeFinder_Params, powersave_range, 0), + + // @Param: GNDCLEAR + // @DisplayName: Distance (in cm) from the range finder to the ground + // @Description: This parameter sets the expected range measurement(in cm) that the range finder should return when the vehicle is on the ground. + // @Units: cm + // @Range: 5 127 + // @Increment: 1 + // @User: Standard + AP_GROUPINFO("GNDCLEAR", 12, AP_RangeFinder_Params, ground_clearance_cm, RANGEFINDER_GROUND_CLEARANCE_CM_DEFAULT), + + // @Param: ADDR + // @DisplayName: Bus address of sensor + // @Description: This sets the bus address of the sensor, where applicable. Used for the LightWare I2C sensor to allow for multiple sensors on different addresses. A value of 0 disables the sensor. + // @Range: 0 127 + // @Increment: 1 + // @User: Standard + AP_GROUPINFO("ADDR", 23, AP_RangeFinder_Params, address, 0), + + // @Param: POS_X + // @DisplayName: X position offset + // @Description: X position of the first rangefinder in body frame. Positive X is forward of the origin. Use the zero range datum point if supplied. + // @Units: m + // @User: Advanced + + // @Param: POS_Y + // @DisplayName: Y position offset + // @Description: Y position of the first rangefinder in body frame. Positive Y is to the right of the origin. Use the zero range datum point if supplied. + // @Units: m + // @User: Advanced + + // @Param: POS_Z + // @DisplayName: Z position offset + // @Description: Z position of the first rangefinder in body frame. Positive Z is down from the origin. Use the zero range datum point if supplied. + // @Units: m + // @User: Advanced + AP_GROUPINFO("POS", 49, AP_RangeFinder_Params, pos_offset, 0.0f), + + // @Param: ORIENT + // @DisplayName: Rangefinder orientation + // @Description: Orientation of rangefinder + // @Values: 0:Forward, 1:Forward-Right, 2:Right, 3:Back-Right, 4:Back, 5:Back-Left, 6:Left, 7:Forward-Left, 24:Up, 25:Down + // @User: Advanced + AP_GROUPINFO("ORIENT", 53, AP_RangeFinder_Params, orientation, ROTATION_PITCH_270), + + AP_GROUPEND +}; + +AP_RangeFinder_Params::AP_RangeFinder_Params(void) { + AP_Param::setup_object_defaults(this, var_info); +} diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Params.h b/libraries/AP_RangeFinder/AP_RangeFinder_Params.h new file mode 100644 index 0000000000..6f5d3f0fc0 --- /dev/null +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Params.h @@ -0,0 +1,31 @@ +#pragma once + +#include +#include + +class AP_RangeFinder_Params { +public: + static const struct AP_Param::GroupInfo var_info[]; + + AP_RangeFinder_Params(void); + + /* Do not allow copies */ + AP_RangeFinder_Params(const AP_RangeFinder_Params &other) = delete; + AP_RangeFinder_Params &operator=(const AP_RangeFinder_Params&) = delete; + + AP_Int8 type; + AP_Int8 pin; + AP_Int8 ratiometric; + AP_Int16 powersave_range; + AP_Int8 stop_pin; + AP_Int16 settle_time_ms; + AP_Float scaling; + AP_Float offset; + AP_Int8 function; + AP_Int16 min_distance_cm; + AP_Int16 max_distance_cm; + AP_Int8 ground_clearance_cm; + AP_Int8 address; + AP_Vector3f pos_offset; // position offset in body frame + AP_Int8 orientation; +}; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp index 5c00837fba..5c2ab9f899 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp @@ -47,8 +47,9 @@ extern const AP_HAL::HAL& hal; */ AP_RangeFinder_PulsedLightLRF::AP_RangeFinder_PulsedLightLRF(uint8_t bus, RangeFinder::RangeFinder_State &_state, + AP_RangeFinder_Params &_params, RangeFinder::RangeFinder_Type _rftype) - : AP_RangeFinder_Backend(_state) + : AP_RangeFinder_Backend(_state, _params) , _dev(hal.i2c_mgr->get_device(bus, LL40LS_ADDR)) , rftype(_rftype) { @@ -60,10 +61,11 @@ AP_RangeFinder_PulsedLightLRF::AP_RangeFinder_PulsedLightLRF(uint8_t bus, */ AP_RangeFinder_Backend *AP_RangeFinder_PulsedLightLRF::detect(uint8_t bus, RangeFinder::RangeFinder_State &_state, + AP_RangeFinder_Params &_params, RangeFinder::RangeFinder_Type rftype) { AP_RangeFinder_PulsedLightLRF *sensor - = new AP_RangeFinder_PulsedLightLRF(bus, _state, rftype); + = new AP_RangeFinder_PulsedLightLRF(bus, _state, _params, rftype); if (!sensor || !sensor->init()) { delete sensor; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.h b/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.h index 7de2b81325..3de97915db 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.h @@ -21,6 +21,7 @@ public: // static detection function static AP_RangeFinder_Backend *detect(uint8_t bus, RangeFinder::RangeFinder_State &_state, + AP_RangeFinder_Params &_params, RangeFinder::RangeFinder_Type rftype); // update state @@ -36,6 +37,7 @@ private: // constructor AP_RangeFinder_PulsedLightLRF(uint8_t bus, RangeFinder::RangeFinder_State &_state, + AP_RangeFinder_Params &_params, RangeFinder::RangeFinder_Type rftype); // start a reading diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_TeraRangerI2C.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_TeraRangerI2C.cpp index 327851bc8c..723fac1e25 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_TeraRangerI2C.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_TeraRangerI2C.cpp @@ -35,8 +35,9 @@ extern const AP_HAL::HAL& hal; already know that we should setup the rangefinder */ AP_RangeFinder_TeraRangerI2C::AP_RangeFinder_TeraRangerI2C(RangeFinder::RangeFinder_State &_state, + AP_RangeFinder_Params &_params, AP_HAL::OwnPtr i2c_dev) - : AP_RangeFinder_Backend(_state) + : AP_RangeFinder_Backend(_state, _params) , dev(std::move(i2c_dev)) { } @@ -47,13 +48,14 @@ AP_RangeFinder_TeraRangerI2C::AP_RangeFinder_TeraRangerI2C(RangeFinder::RangeFin there. */ AP_RangeFinder_Backend *AP_RangeFinder_TeraRangerI2C::detect(RangeFinder::RangeFinder_State &_state, + AP_RangeFinder_Params &_params, AP_HAL::OwnPtr i2c_dev) { if (!i2c_dev) { return nullptr; } - AP_RangeFinder_TeraRangerI2C *sensor = new AP_RangeFinder_TeraRangerI2C(_state, std::move(i2c_dev)); + AP_RangeFinder_TeraRangerI2C *sensor = new AP_RangeFinder_TeraRangerI2C(_state, _params, std::move(i2c_dev)); if (!sensor) { return nullptr; } @@ -144,7 +146,7 @@ bool AP_RangeFinder_TeraRangerI2C::process_raw_measure(uint16_t raw_distance, ui return false; } else if (raw_distance == 0x0000) { // Too close - output_distance_cm = state.min_distance_cm; + output_distance_cm = params.min_distance_cm; return true; } else if (raw_distance == 0x0001) { // Unable to measure diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_TeraRangerI2C.h b/libraries/AP_RangeFinder/AP_RangeFinder_TeraRangerI2C.h index 3ee702e26b..f112605a4d 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_TeraRangerI2C.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_TeraRangerI2C.h @@ -9,6 +9,7 @@ class AP_RangeFinder_TeraRangerI2C : public AP_RangeFinder_Backend public: // static detection function static AP_RangeFinder_Backend *detect(RangeFinder::RangeFinder_State &_state, + AP_RangeFinder_Params &_params, AP_HAL::OwnPtr i2c_dev); // update state @@ -23,6 +24,7 @@ protected: private: // constructor AP_RangeFinder_TeraRangerI2C(RangeFinder::RangeFinder_State &_state, + AP_RangeFinder_Params &_params, AP_HAL::OwnPtr i2c_dev); bool measure(void); diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_VL53L0X.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_VL53L0X.cpp index 2476578362..a97b8dc3e4 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_VL53L0X.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_VL53L0X.cpp @@ -216,8 +216,8 @@ const AP_RangeFinder_VL53L0X::RegData AP_RangeFinder_VL53L0X::tuning_data[] = constructor is not called until detect() returns true, so we already know that we should setup the rangefinder */ -AP_RangeFinder_VL53L0X::AP_RangeFinder_VL53L0X(RangeFinder::RangeFinder_State &_state, AP_HAL::OwnPtr _dev) - : AP_RangeFinder_Backend(_state) +AP_RangeFinder_VL53L0X::AP_RangeFinder_VL53L0X(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params, AP_HAL::OwnPtr _dev) + : AP_RangeFinder_Backend(_state, _params) , dev(std::move(_dev)) {} @@ -226,13 +226,13 @@ AP_RangeFinder_VL53L0X::AP_RangeFinder_VL53L0X(RangeFinder::RangeFinder_State &_ trying to take a reading on I2C. If we get a result the sensor is there. */ -AP_RangeFinder_Backend *AP_RangeFinder_VL53L0X::detect(RangeFinder::RangeFinder_State &_state, AP_HAL::OwnPtr dev) +AP_RangeFinder_Backend *AP_RangeFinder_VL53L0X::detect(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params, AP_HAL::OwnPtr dev) { if(!dev){ return nullptr; } AP_RangeFinder_VL53L0X *sensor - = new AP_RangeFinder_VL53L0X(_state, std::move(dev)); + = new AP_RangeFinder_VL53L0X(_state, _params, std::move(dev)); if (!sensor) { delete sensor; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_VL53L0X.h b/libraries/AP_RangeFinder/AP_RangeFinder_VL53L0X.h index 1e12ee52e2..5f1a7bf1eb 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_VL53L0X.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_VL53L0X.h @@ -9,7 +9,7 @@ class AP_RangeFinder_VL53L0X : public AP_RangeFinder_Backend public: // static detection function - static AP_RangeFinder_Backend *detect(RangeFinder::RangeFinder_State &_state, AP_HAL::OwnPtr dev); + static AP_RangeFinder_Backend *detect(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params, AP_HAL::OwnPtr dev); // update state void update(void) override; @@ -22,7 +22,7 @@ protected: private: // constructor - AP_RangeFinder_VL53L0X(RangeFinder::RangeFinder_State &_state, AP_HAL::OwnPtr dev); + AP_RangeFinder_VL53L0X(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params, AP_HAL::OwnPtr dev); bool init(); void timer(); diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_VL53L1X.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_VL53L1X.cpp index 0476801ec4..f075d31009 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_VL53L1X.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_VL53L1X.cpp @@ -34,8 +34,8 @@ extern const AP_HAL::HAL& hal; constructor is not called until detect() returns true, so we already know that we should setup the rangefinder */ -AP_RangeFinder_VL53L1X::AP_RangeFinder_VL53L1X(RangeFinder::RangeFinder_State &_state, AP_HAL::OwnPtr _dev) - : AP_RangeFinder_Backend(_state) +AP_RangeFinder_VL53L1X::AP_RangeFinder_VL53L1X(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params, AP_HAL::OwnPtr _dev) + : AP_RangeFinder_Backend(_state, _params) , dev(std::move(_dev)) {} /* @@ -43,14 +43,14 @@ AP_RangeFinder_VL53L1X::AP_RangeFinder_VL53L1X(RangeFinder::RangeFinder_State &_ trying to take a reading on I2C. If we get a result the sensor is there. */ -AP_RangeFinder_Backend *AP_RangeFinder_VL53L1X::detect(RangeFinder::RangeFinder_State &_state, AP_HAL::OwnPtr dev) +AP_RangeFinder_Backend *AP_RangeFinder_VL53L1X::detect(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params, AP_HAL::OwnPtr dev) { if (!dev) { return nullptr; } AP_RangeFinder_VL53L1X *sensor - = new AP_RangeFinder_VL53L1X(_state, std::move(dev)); + = new AP_RangeFinder_VL53L1X(_state, _params, std::move(dev)); if (!sensor) { delete sensor; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_VL53L1X.h b/libraries/AP_RangeFinder/AP_RangeFinder_VL53L1X.h index 8d58523954..1979f47822 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_VL53L1X.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_VL53L1X.h @@ -9,7 +9,7 @@ class AP_RangeFinder_VL53L1X : public AP_RangeFinder_Backend public: // static detection function - static AP_RangeFinder_Backend *detect(RangeFinder::RangeFinder_State &_state, AP_HAL::OwnPtr dev); + static AP_RangeFinder_Backend *detect(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params, AP_HAL::OwnPtr _dev); // update state void update(void) override; @@ -1213,7 +1213,7 @@ private: }; // constructor - AP_RangeFinder_VL53L1X(RangeFinder::RangeFinder_State &_state, AP_HAL::OwnPtr dev); + AP_RangeFinder_VL53L1X(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params, AP_HAL::OwnPtr dev); bool init(); void timer(); diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Wasp.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Wasp.cpp index 258dfbb6ae..ff60b276e0 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Wasp.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Wasp.cpp @@ -67,9 +67,10 @@ const AP_Param::GroupInfo AP_RangeFinder_Wasp::var_info[] = { }; AP_RangeFinder_Wasp::AP_RangeFinder_Wasp(RangeFinder::RangeFinder_State &_state, + AP_RangeFinder_Params &_params, AP_SerialManager &serial_manager, uint8_t serial_instance) : - AP_RangeFinder_Backend(_state) { + AP_RangeFinder_Backend(_state, _params) { AP_Param::setup_object_defaults(this, var_info); uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Rangefinder, serial_instance); diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Wasp.h b/libraries/AP_RangeFinder/AP_RangeFinder_Wasp.h index 368e9b5c08..a38c015b96 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Wasp.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Wasp.h @@ -10,6 +10,7 @@ class AP_RangeFinder_Wasp : public AP_RangeFinder_Backend { public: AP_RangeFinder_Wasp(RangeFinder::RangeFinder_State &_state, + AP_RangeFinder_Params &_params, AP_SerialManager &serial_manager, uint8_t serial_instance); diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp index 1a781d615f..455a2a268c 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp @@ -22,6 +22,7 @@ #include #include #include "RangeFinder.h" +#include "AP_RangeFinder_Params.h" #include "AP_RangeFinder_analog.h" extern const AP_HAL::HAL& hal; @@ -31,17 +32,17 @@ extern const AP_HAL::HAL& hal; constructor is not called until detect() returns true, so we already know that we should setup the rangefinder */ -AP_RangeFinder_analog::AP_RangeFinder_analog(RangeFinder::RangeFinder_State &_state) : - AP_RangeFinder_Backend(_state) +AP_RangeFinder_analog::AP_RangeFinder_analog(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params) : + AP_RangeFinder_Backend(_state, _params) { - source = hal.analogin->channel(_state.pin); + source = hal.analogin->channel(_params.pin); if (source == nullptr) { // failed to allocate a ADC channel? This shouldn't happen set_status(RangeFinder::RangeFinder_NotConnected); return; } - source->set_stop_pin((uint8_t)_state.stop_pin); - source->set_settle_time((uint16_t)_state.settle_time_ms); + source->set_stop_pin((uint8_t)_params.stop_pin); + source->set_settle_time((uint16_t)_params.settle_time_ms); set_status(RangeFinder::RangeFinder_NoData); } @@ -50,9 +51,9 @@ AP_RangeFinder_analog::AP_RangeFinder_analog(RangeFinder::RangeFinder_State &_st can do is check if the pin number is valid. If it is, then assume that the device is connected */ -bool AP_RangeFinder_analog::detect(RangeFinder::RangeFinder_State &_state) +bool AP_RangeFinder_analog::detect(AP_RangeFinder_Params &_params) { - if (_state.pin != -1) { + if (_params.pin != -1) { return true; } return false; @@ -69,10 +70,10 @@ void AP_RangeFinder_analog::update_voltage(void) return; } // cope with changed settings - source->set_pin(state.pin); - source->set_stop_pin((uint8_t)state.stop_pin); - source->set_settle_time((uint16_t)state.settle_time_ms); - if (state.ratiometric) { + source->set_pin(params.pin); + source->set_stop_pin((uint8_t)params.stop_pin); + source->set_settle_time((uint16_t)params.settle_time_ms); + if (params.ratiometric) { state.voltage_mv = source->voltage_average_ratiometric() * 1000U; } else { state.voltage_mv = source->voltage_average() * 1000U; @@ -87,10 +88,10 @@ void AP_RangeFinder_analog::update(void) update_voltage(); float v = state.voltage_mv * 0.001f; float dist_m = 0; - float scaling = state.scaling; - float offset = state.offset; - RangeFinder::RangeFinder_Function function = (RangeFinder::RangeFinder_Function)state.function.get(); - int16_t _max_distance_cm = state.max_distance_cm; + float scaling = params.scaling; + float offset = params.offset; + RangeFinder::RangeFinder_Function function = (RangeFinder::RangeFinder_Function)params.function.get(); + int16_t _max_distance_cm = params.max_distance_cm; switch (function) { case RangeFinder::FUNCTION_LINEAR: diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_analog.h b/libraries/AP_RangeFinder/AP_RangeFinder_analog.h index 620e465c2e..aaa0651a61 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_analog.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_analog.h @@ -2,15 +2,16 @@ #include "RangeFinder.h" #include "RangeFinder_Backend.h" +#include "AP_RangeFinder_Params.h" class AP_RangeFinder_analog : public AP_RangeFinder_Backend { public: // constructor - AP_RangeFinder_analog(RangeFinder::RangeFinder_State &_state); + AP_RangeFinder_analog(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params); // static detection function - static bool detect(RangeFinder::RangeFinder_State &_state); + static bool detect(AP_RangeFinder_Params &_params); // update state void update(void) override; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_uLanding.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_uLanding.cpp index 74141ac275..1cf8038c3f 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_uLanding.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_uLanding.cpp @@ -33,9 +33,10 @@ extern const AP_HAL::HAL& hal; already know that we should setup the rangefinder */ AP_RangeFinder_uLanding::AP_RangeFinder_uLanding(RangeFinder::RangeFinder_State &_state, + AP_RangeFinder_Params &_params, AP_SerialManager &serial_manager, uint8_t serial_instance) : - AP_RangeFinder_Backend(_state) + AP_RangeFinder_Backend(_state, _params) { uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Rangefinder, serial_instance); if (uart != nullptr) { diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_uLanding.h b/libraries/AP_RangeFinder/AP_RangeFinder_uLanding.h index 240e004fa8..b57cda77b5 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_uLanding.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_uLanding.h @@ -9,6 +9,7 @@ class AP_RangeFinder_uLanding : public AP_RangeFinder_Backend public: // constructor AP_RangeFinder_uLanding(RangeFinder::RangeFinder_State &_state, + AP_RangeFinder_Params &_params, AP_SerialManager &serial_manager, uint8_t serial_instance); diff --git a/libraries/AP_RangeFinder/RangeFinder.cpp b/libraries/AP_RangeFinder/RangeFinder.cpp index c7734e543d..830932da70 100644 --- a/libraries/AP_RangeFinder/RangeFinder.cpp +++ b/libraries/AP_RangeFinder/RangeFinder.cpp @@ -37,527 +37,112 @@ #include "AP_RangeFinder_Benewake.h" #include "AP_RangeFinder_PWM.h" #include +#include +#include extern const AP_HAL::HAL &hal; // table of user settable parameters const AP_Param::GroupInfo RangeFinder::var_info[] = { - // @Param: _TYPE - // @DisplayName: Rangefinder type - // @Description: What type of rangefinder device that is connected - // @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLiteV2-I2C,5:PX4-PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:uLanding,12:LeddarOne,13:MaxbotixSerial,14:TeraRangerI2C,15:LidarLiteV3-I2C,16:VL53L0X,17:NMEA,18:WASP-LRF,19:BenewakeTF02,20:BenewakeTFmini,21:LidarLiteV3HP-I2C,22:PWM - // @User: Standard - AP_GROUPINFO("_TYPE", 0, RangeFinder, state[0].type, 0), - // @Param: _PIN - // @DisplayName: Rangefinder pin - // @Description: Pin that rangefinder is connected to, analogue pins for analogue sensors, GPIO pins for PWM sensors. Set to 11 on PX4 for the analog 'airspeed' port. Set to 15 on the Pixhawk for the analog 'airspeed' port. - // @Values: -1:Not Used, 11:PX4-airspeed port, 15:Pixhawk-airspeed port, 50:GPIO1,51:GPIO2,52:GPIO3,53:GPIO4,54:GPIO5,55:GPIO6 - // @User: Standard - AP_GROUPINFO("_PIN", 1, RangeFinder, state[0].pin, -1), + // @Group: 1_ + // @Path: AP_RangeFinder_Params.cpp + AP_SUBGROUPINFO_FLAGS(params[0], "1_", 25, RangeFinder, AP_RangeFinder_Params, AP_PARAM_FLAG_IGNORE_ENABLE), - // @Param: _SCALING - // @DisplayName: Rangefinder scaling - // @Description: Scaling factor between rangefinder reading and distance. For the linear and inverted functions this is in meters per volt. For the hyperbolic function the units are meterVolts. - // @Units: m/V - // @Increment: 0.001 - // @User: Standard - AP_GROUPINFO("_SCALING", 2, RangeFinder, state[0].scaling, 3.0f), - - // @Param: _OFFSET - // @DisplayName: rangefinder offset - // @Description: Offset in volts for zero distance for analog rangefinders. Offset added to distance in centimeters for PWM and I2C Lidars - // @Units: V - // @Increment: 0.001 - // @User: Standard - AP_GROUPINFO("_OFFSET", 3, RangeFinder, state[0].offset, 0.0f), - - // @Param: _FUNCTION - // @DisplayName: Rangefinder function - // @Description: Control over what function is used to calculate distance. For a linear function, the distance is (voltage-offset)*scaling. For a inverted function the distance is (offset-voltage)*scaling. For a hyperbolic function the distance is scaling/(voltage-offset). The functions return the distance in meters. - // @Values: 0:Linear,1:Inverted,2:Hyperbolic - // @User: Standard - AP_GROUPINFO("_FUNCTION", 4, RangeFinder, state[0].function, 0), - - // @Param: _MIN_CM - // @DisplayName: Rangefinder minimum distance - // @Description: Minimum distance in centimeters that rangefinder can reliably read - // @Units: cm - // @Increment: 1 - // @User: Standard - AP_GROUPINFO("_MIN_CM", 5, RangeFinder, state[0].min_distance_cm, 20), - - // @Param: _MAX_CM - // @DisplayName: Rangefinder maximum distance - // @Description: Maximum distance in centimeters that rangefinder can reliably read - // @Units: cm - // @Increment: 1 - // @User: Standard - AP_GROUPINFO("_MAX_CM", 6, RangeFinder, state[0].max_distance_cm, 700), - - // @Param: _STOP_PIN - // @DisplayName: Rangefinder stop pin - // @Description: Digital pin that enables/disables rangefinder measurement for an analog rangefinder. A value of -1 means no pin. If this is set, then the pin is set to 1 to enable the rangefinder and set to 0 to disable it. This can be used to ensure that multiple sonar rangefinders don't interfere with each other. - // @Values: -1:Not Used,50:Pixhawk AUXOUT1,51:Pixhawk AUXOUT2,52:Pixhawk AUXOUT3,53:Pixhawk AUXOUT4,54:Pixhawk AUXOUT5,55:Pixhawk AUXOUT6,111:PX4 FMU Relay1,112:PX4 FMU Relay2,113:PX4IO Relay1,114:PX4IO Relay2,115:PX4IO ACC1,116:PX4IO ACC2 - // @User: Standard - AP_GROUPINFO("_STOP_PIN", 7, RangeFinder, state[0].stop_pin, -1), - - // @Param: _SETTLE - // @DisplayName: Rangefinder settle time - // @Description: The time in milliseconds that the rangefinder reading takes to settle. This is only used when a STOP_PIN is specified. It determines how long we have to wait for the rangefinder to give a reading after we set the STOP_PIN high. For a sonar rangefinder with a range of around 7m this would need to be around 50 milliseconds to allow for the sonar pulse to travel to the target and back again. - // @Units: ms - // @Increment: 1 - // @User: Standard - AP_GROUPINFO("_SETTLE", 8, RangeFinder, state[0].settle_time_ms, 0), - - // @Param: _RMETRIC - // @DisplayName: Ratiometric - // @Description: This parameter sets whether an analog rangefinder is ratiometric. Most analog rangefinders are ratiometric, meaning that their output voltage is influenced by the supply voltage. Some analog rangefinders (such as the SF/02) have their own internal voltage regulators so they are not ratiometric. - // @Values: 0:No,1:Yes - // @User: Standard - AP_GROUPINFO("_RMETRIC", 9, RangeFinder, state[0].ratiometric, 1), - - // @Param: _PWRRNG - // @DisplayName: Powersave range - // @Description: This parameter sets the estimated terrain distance in meters above which the sensor will be put into a power saving mode (if available). A value of zero means power saving is not enabled - // @Units: m - // @Range: 0 32767 - // @User: Standard - AP_GROUPINFO("_PWRRNG", 10, RangeFinder, _powersave_range, 0), - - // @Param: _GNDCLEAR - // @DisplayName: Distance (in cm) from the range finder to the ground - // @Description: This parameter sets the expected range measurement(in cm) that the range finder should return when the vehicle is on the ground. - // @Units: cm - // @Range: 5 127 - // @Increment: 1 - // @User: Standard - AP_GROUPINFO("_GNDCLEAR", 11, RangeFinder, state[0].ground_clearance_cm, RANGEFINDER_GROUND_CLEARANCE_CM_DEFAULT), - - // @Param: _ADDR - // @DisplayName: Bus address of sensor - // @Description: This sets the bus address of the sensor, where applicable. Used for the LightWare I2C sensor to allow for multiple sensors on different addresses. A value of 0 disables the sensor. - // @Range: 0 127 - // @Increment: 1 - // @User: Standard - AP_GROUPINFO("_ADDR", 23, RangeFinder, state[0].address, 0), - - // @Param: _POS_X - // @DisplayName: X position offset - // @Description: X position of the first rangefinder in body frame. Positive X is forward of the origin. Use the zero range datum point if supplied. - // @Units: m - // @Range: -10 10 - // @User: Advanced - - // @Param: _POS_Y - // @DisplayName: Y position offset - // @Description: Y position of the first rangefinder in body frame. Positive Y is to the right of the origin. Use the zero range datum point if supplied. - // @Units: m - // @Range: -10 10 - // @User: Advanced - - // @Param: _POS_Z - // @DisplayName: Z position offset - // @Description: Z position of the first rangefinder in body frame. Positive Z is down from the origin. Use the zero range datum point if supplied. - // @Units: m - // @Range: -10 10 - // @User: Advanced - AP_GROUPINFO("_POS", 49, RangeFinder, state[0].pos_offset, 0.0f), - - // @Param: _ORIENT - // @DisplayName: Rangefinder orientation - // @Description: Orientation of rangefinder - // @Values: 0:Forward, 1:Forward-Right, 2:Right, 3:Back-Right, 4:Back, 5:Back-Left, 6:Left, 7:Forward-Left, 24:Up, 25:Down - // @User: Advanced - AP_GROUPINFO("_ORIENT", 53, RangeFinder, state[0].orientation, ROTATION_PITCH_270), - - // @Group: _ + // @Group: 1_ // @Path: AP_RangeFinder_Wasp.cpp - AP_SUBGROUPVARPTR(drivers[0], "_", 57, RangeFinder, backend_var_info[0]), + AP_SUBGROUPVARPTR(drivers[0], "1_", 57, RangeFinder, backend_var_info[0]), #if RANGEFINDER_MAX_INSTANCES > 1 - // @Param: 2_TYPE - // @DisplayName: Second Rangefinder type - // @Description: What type of rangefinder device that is connected - // @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLiteV2-I2C,5:PX4-PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:uLanding,12:LeddarOne,13:MaxbotixSerial,14:TeraRangerI2C,15:LidarLiteV3-I2C,16:VL53L0X,17:NMEA,18:WASP-LRF,19:BenewakeTF02,20:BenewakeTFmini,21:LidarLiteV3HP-I2C,22:PWM - // @User: Advanced - AP_GROUPINFO("2_TYPE", 12, RangeFinder, state[1].type, 0), - - // @Param: 2_PIN - // @DisplayName: Rangefinder pin - // @Description: Pin that rangefinder is connected to, analogue pins for analogue sensors, GPIO pins for PWM sensors. Set to 11 on PX4 for the analog 'airspeed' port. Set to 15 on the Pixhawk for the analog 'airspeed' port. - // @Values: -1:Not Used, 11:PX4-airspeed port, 15:Pixhawk-airspeed port - // @User: Advanced - AP_GROUPINFO("2_PIN", 13, RangeFinder, state[1].pin, -1), - - // @Param: 2_SCALING - // @DisplayName: Rangefinder scaling - // @Description: Scaling factor between rangefinder reading and distance. For the linear and inverted functions this is in meters per volt. For the hyperbolic function the units are meterVolts. - // @Units: m/V - // @Increment: 0.001 - // @User: Advanced - AP_GROUPINFO("2_SCALING", 14, RangeFinder, state[1].scaling, 3.0f), - - // @Param: 2_OFFSET - // @DisplayName: rangefinder offset - // @Description: Offset in volts for zero distance - // @Units: V - // @Increment: 0.001 - // @User: Advanced - AP_GROUPINFO("2_OFFSET", 15, RangeFinder, state[1].offset, 0.0f), - - // @Param: 2_FUNCTION - // @DisplayName: Rangefinder function - // @Description: Control over what function is used to calculate distance. For a linear function, the distance is (voltage-offset)*scaling. For a inverted function the distance is (offset-voltage)*scaling. For a hyperbolic function the distance is scaling/(voltage-offset). The functions return the distance in meters. - // @Values: 0:Linear,1:Inverted,2:Hyperbolic - // @User: Advanced - AP_GROUPINFO("2_FUNCTION", 16, RangeFinder, state[1].function, 0), - - // @Param: 2_MIN_CM - // @DisplayName: Rangefinder minimum distance - // @Description: Minimum distance in centimeters that rangefinder can reliably read - // @Units: cm - // @Increment: 1 - // @User: Advanced - AP_GROUPINFO("2_MIN_CM", 17, RangeFinder, state[1].min_distance_cm, 20), - - // @Param: 2_MAX_CM - // @DisplayName: Rangefinder maximum distance - // @Description: Maximum distance in centimeters that rangefinder can reliably read - // @Units: cm - // @Increment: 1 - // @User: Advanced - AP_GROUPINFO("2_MAX_CM", 18, RangeFinder, state[1].max_distance_cm, 700), - - // @Param: 2_STOP_PIN - // @DisplayName: Rangefinder stop pin - // @Description: Digital pin that enables/disables rangefinder measurement for an analog rangefinder. A value of -1 means no pin. If this is set, then the pin is set to 1 to enable the rangefinder and set to 0 to disable it. This can be used to ensure that multiple sonar rangefinders don't interfere with each other. - // @Values: -1:Not Used,50:Pixhawk AUXOUT1,51:Pixhawk AUXOUT2,52:Pixhawk AUXOUT3,53:Pixhawk AUXOUT4,54:Pixhawk AUXOUT5,55:Pixhawk AUXOUT6,111:PX4 FMU Relay1,112:PX4 FMU Relay2,113:PX4IO Relay1,114:PX4IO Relay2,115:PX4IO ACC1,116:PX4IO ACC2 - // @User: Advanced - AP_GROUPINFO("2_STOP_PIN", 19, RangeFinder, state[1].stop_pin, -1), - - // @Param: 2_SETTLE - // @DisplayName: Sonar settle time - // @Description: The time in milliseconds that the rangefinder reading takes to settle. This is only used when a STOP_PIN is specified. It determines how long we have to wait for the rangefinder to give a reading after we set the STOP_PIN high. For a sonar rangefinder with a range of around 7m this would need to be around 50 milliseconds to allow for the sonar pulse to travel to the target and back again. - // @Units: ms - // @Increment: 1 - // @User: Advanced - AP_GROUPINFO("2_SETTLE", 20, RangeFinder, state[1].settle_time_ms, 0), - - // @Param: 2_RMETRIC - // @DisplayName: Ratiometric - // @Description: This parameter sets whether an analog rangefinder is ratiometric. Most analog rangefinders are ratiometric, meaning that their output voltage is influenced by the supply voltage. Some analog rangefinders (such as the SF/02) have their own internal voltage regulators so they are not ratiometric. - // @Values: 0:No,1:Yes - // @User: Advanced - AP_GROUPINFO("2_RMETRIC", 21, RangeFinder, state[1].ratiometric, 1), - - // @Param: 2_GNDCLEAR - // @DisplayName: Distance (in cm) from the second range finder to the ground - // @Description: This parameter sets the expected range measurement(in cm) that the second range finder should return when the vehicle is on the ground. - // @Units: cm - // @Range: 0 127 - // @Increment: 1 - // @User: Advanced - AP_GROUPINFO("2_GNDCLEAR", 22, RangeFinder, state[1].ground_clearance_cm, RANGEFINDER_GROUND_CLEARANCE_CM_DEFAULT), - - // @Param: 2_ADDR - // @DisplayName: Bus address of second rangefinder - // @Description: This sets the bus address of the sensor, where applicable. Used for the LightWare I2C sensor to allow for multiple sensors on different addresses. A value of 0 disables the sensor. - // @Range: 0 127 - // @Increment: 1 - // @User: Advanced - AP_GROUPINFO("2_ADDR", 24, RangeFinder, state[1].address, 0), - - // @Param: 2_POS_X - // @DisplayName: X position offset - // @Description: X position of the second rangefinder in body frame. Positive X is forward of the origin. Use the zero range datum point if supplied. - // @Units: m - // @Range: -10 10 - // @User: Advanced - - // @Param: 2_POS_Y - // @DisplayName: Y position offset - // @Description: Y position of the second rangefinder in body frame. Positive Y is to the right of the origin. Use the zero range datum point if supplied. - // @Units: m - // @Range: -10 10 - // @User: Advanced - - // @Param: 2_POS_Z - // @DisplayName: Z position offset - // @Description: Z position of the second rangefinder in body frame. Positive Z is down from the origin. Use the zero range datum point if supplied. - // @Units: m - // @Range: -10 10 - // @User: Advanced - AP_GROUPINFO("2_POS", 50, RangeFinder, state[1].pos_offset, 0.0f), - - // @Param: 2_ORIENT - // @DisplayName: Rangefinder 2 orientation - // @Description: Orientation of 2nd rangefinder - // @Values: 0:Forward, 1:Forward-Right, 2:Right, 3:Back-Right, 4:Back, 5:Back-Left, 6:Left, 7:Forward-Left, 24:Up, 25:Down - // @User: Advanced - AP_GROUPINFO("2_ORIENT", 54, RangeFinder, state[1].orientation, ROTATION_PITCH_270), + // @Group: 2_ + // @Path: AP_RangeFinder_Params.cpp + AP_SUBGROUPINFO(params[1], "2_", 27, RangeFinder, AP_RangeFinder_Params), // @Group: 2_ // @Path: AP_RangeFinder_Wasp.cpp - AP_SUBGROUPVARPTR(drivers[1], "2_", 58, RangeFinder, backend_var_info[1]), + AP_SUBGROUPVARPTR(drivers[1], "2_", 58, RangeFinder, backend_var_info[1]), #endif #if RANGEFINDER_MAX_INSTANCES > 2 - - // @Param: 3_TYPE - // @DisplayName: Third Rangefinder type - // @Description: What type of rangefinder device that is connected - // @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLiteV2-I2C,5:PX4-PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:uLanding,12:LeddarOne,13:MaxbotixSerial,14:TeraRangerI2C,15:LidarLiteV3-I2C,16:VL53L0X,17:NMEA,18:WASP-LRF,19:BenewakeTF02,20:BenewakeTFmini,21:LidarLiteV3HP-I2C,22:PWM - // @User: Advanced - AP_GROUPINFO("3_TYPE", 25, RangeFinder, state[2].type, 0), - - // @Param: 3_PIN - // @DisplayName: Rangefinder pin - // @Description: Pin that rangefinder is connected to, analogue pins for analogue sensors, GPIO pins for PWM sensors. Set to 11 on PX4 for the analog 'airspeed' port. Set to 15 on the Pixhawk for the analog 'airspeed' port. - // @Values: -1:Not Used, 11:PX4-airspeed port, 15:Pixhawk-airspeed port - // @User: Advanced - AP_GROUPINFO("3_PIN", 26, RangeFinder, state[2].pin, -1), - - // @Param: 3_SCALING - // @DisplayName: Rangefinder scaling - // @Description: Scaling factor between rangefinder reading and distance. For the linear and inverted functions this is in meters per volt. For the hyperbolic function the units are meterVolts. - // @Units: m/V - // @Increment: 0.001 - // @User: Advanced - AP_GROUPINFO("3_SCALING", 27, RangeFinder, state[2].scaling, 3.0f), - - // @Param: 3_OFFSET - // @DisplayName: rangefinder offset - // @Description: Offset in volts for zero distance - // @Units: V - // @Increment: 0.001 - // @User: Advanced - AP_GROUPINFO("3_OFFSET", 28, RangeFinder, state[2].offset, 0.0f), - - // @Param: 3_FUNCTION - // @DisplayName: Rangefinder function - // @Description: Control over what function is used to calculate distance. For a linear function, the distance is (voltage-offset)*scaling. For a inverted function the distance is (offset-voltage)*scaling. For a hyperbolic function the distance is scaling/(voltage-offset). The functions return the distance in meters. - // @Values: 0:Linear,1:Inverted,2:Hyperbolic - // @User: Advanced - AP_GROUPINFO("3_FUNCTION", 29, RangeFinder, state[2].function, 0), - - // @Param: 3_MIN_CM - // @DisplayName: Rangefinder minimum distance - // @Description: Minimum distance in centimeters that rangefinder can reliably read - // @Units: cm - // @Increment: 1 - // @User: Advanced - AP_GROUPINFO("3_MIN_CM", 30, RangeFinder, state[2].min_distance_cm, 20), - - // @Param: 3_MAX_CM - // @DisplayName: Rangefinder maximum distance - // @Description: Maximum distance in centimeters that rangefinder can reliably read - // @Units: cm - // @Increment: 1 - // @User: Advanced - AP_GROUPINFO("3_MAX_CM", 31, RangeFinder, state[2].max_distance_cm, 700), - - // @Param: 3_STOP_PIN - // @DisplayName: Rangefinder stop pin - // @Description: Digital pin that enables/disables rangefinder measurement for an analog rangefinder. A value of -1 means no pin. If this is set, then the pin is set to 1 to enable the rangefinder and set to 0 to disable it. This can be used to ensure that multiple sonar rangefinders don't interfere with each other. - // @Values: -1:Not Used,50:Pixhawk AUXOUT1,51:Pixhawk AUXOUT2,52:Pixhawk AUXOUT3,53:Pixhawk AUXOUT4,54:Pixhawk AUXOUT5,55:Pixhawk AUXOUT6,111:PX4 FMU Relay1,112:PX4 FMU Relay2,113:PX4IO Relay1,114:PX4IO Relay2,115:PX4IO ACC1,116:PX4IO ACC2 - // @User: Advanced - AP_GROUPINFO("3_STOP_PIN", 32, RangeFinder, state[2].stop_pin, -1), - - // @Param: 3_SETTLE - // @DisplayName: Sonar settle time - // @Description: The time in milliseconds that the rangefinder reading takes to settle. This is only used when a STOP_PIN is specified. It determines how long we have to wait for the rangefinder to give a reading after we set the STOP_PIN high. For a sonar rangefinder with a range of around 7m this would need to be around 50 milliseconds to allow for the sonar pulse to travel to the target and back again. - // @Units: ms - // @Increment: 1 - // @User: Advanced - AP_GROUPINFO("3_SETTLE", 33, RangeFinder, state[2].settle_time_ms, 0), - - // @Param: 3_RMETRIC - // @DisplayName: Ratiometric - // @Description: This parameter sets whether an analog rangefinder is ratiometric. Most analog rangefinders are ratiometric, meaning that their output voltage is influenced by the supply voltage. Some analog rangefinders (such as the SF/02) have their own internal voltage regulators so they are not ratiometric. - // @Values: 0:No,1:Yes - // @User: Advanced - AP_GROUPINFO("3_RMETRIC", 34, RangeFinder, state[2].ratiometric, 1), - - // @Param: 3_GNDCLEAR - // @DisplayName: Distance (in cm) from the third range finder to the ground - // @Description: This parameter sets the expected range measurement(in cm) that the third range finder should return when the vehicle is on the ground. - // @Units: cm - // @Range: 0 127 - // @Increment: 1 - // @User: Advanced - AP_GROUPINFO("3_GNDCLEAR", 35, RangeFinder, state[2].ground_clearance_cm, RANGEFINDER_GROUND_CLEARANCE_CM_DEFAULT), - - // @Param: 3_ADDR - // @DisplayName: Bus address of third rangefinder - // @Description: This sets the bus address of the sensor, where applicable. Used for the LightWare I2C sensor to allow for multiple sensors on different addresses. A value of 0 disables the sensor. - // @Range: 0 127 - // @Increment: 1 - // @User: Advanced - AP_GROUPINFO("3_ADDR", 36, RangeFinder, state[2].address, 0), - - // @Param: 3_POS_X - // @DisplayName: X position offset - // @Description: X position of the third rangefinder in body frame. Positive X is forward of the origin. Use the zero range datum point if supplied. - // @Units: m - // @Range: -10 10 - // @User: Advanced - - // @Param: 3_POS_Y - // @DisplayName: Y position offset - // @Description: Y position of the third rangefinder in body frame. Positive Y is to the right of the origin. Use the zero range datum point if supplied. - // @Units: m - // @Range: -10 10 - // @User: Advanced - - // @Param: 3_POS_Z - // @DisplayName: Z position offset - // @Description: Z position of the third rangefinder in body frame. Positive Z is down from the origin. Use the zero range datum point if supplied. - // @Units: m - // @Range: -10 10 - // @User: Advanced - AP_GROUPINFO("3_POS", 51, RangeFinder, state[2].pos_offset, 0.0f), - - // @Param: 3_ORIENT - // @DisplayName: Rangefinder 3 orientation - // @Description: Orientation of 3rd rangefinder - // @Values: 0:Forward, 1:Forward-Right, 2:Right, 3:Back-Right, 4:Back, 5:Back-Left, 6:Left, 7:Forward-Left, 24:Up, 25:Down - // @User: Advanced - AP_GROUPINFO("3_ORIENT", 55, RangeFinder, state[2].orientation, ROTATION_PITCH_270), + // @Group: 3_ + // @Path: AP_RangeFinder_Params.cpp + AP_SUBGROUPINFO(params[2], "3_", 29, RangeFinder, AP_RangeFinder_Params), // @Group: 3_ // @Path: AP_RangeFinder_Wasp.cpp - AP_SUBGROUPVARPTR(drivers[2], "3_", 59, RangeFinder, backend_var_info[2]), + AP_SUBGROUPVARPTR(drivers[2], "3_", 59, RangeFinder, backend_var_info[2]), #endif #if RANGEFINDER_MAX_INSTANCES > 3 - - // @Param: 4_TYPE - // @DisplayName: Fourth Rangefinder type - // @Description: What type of rangefinder device that is connected - // @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLiteV2-I2C,5:PX4-PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:uLanding,12:LeddarOne,13:MaxbotixSerial,14:TeraRangerI2C,15:LidarLiteV3-I2C,16:VL53L0X,17:NMEA,18:WASP-LRF,19:BenewakeTF02,20:BenewakeTFmini,21:LidarLiteV3HP-I2C,22:PWM - // @User: Advanced - AP_GROUPINFO("4_TYPE", 37, RangeFinder, state[3].type, 0), - - // @Param: 4_PIN - // @DisplayName: Rangefinder pin - // @Description: Pin that rangefinder is connected to, analogue pins for analogue sensors, GPIO pins for PWM sensors. Set to 11 on PX4 for the analog 'airspeed' port. Set to 15 on the Pixhawk for the analog 'airspeed' port. - // @Values: -1:Not Used, 11:PX4-airspeed port, 15:Pixhawk-airspeed port - // @User: Advanced - AP_GROUPINFO("4_PIN", 38, RangeFinder, state[3].pin, -1), - - // @Param: 4_SCALING - // @DisplayName: Rangefinder scaling - // @Description: Scaling factor between rangefinder reading and distance. For the linear and inverted functions this is in meters per volt. For the hyperbolic function the units are meterVolts. - // @Units: m/V - // @Increment: 0.001 - // @User: Advanced - AP_GROUPINFO("4_SCALING", 39, RangeFinder, state[3].scaling, 3.0f), - - // @Param: 4_OFFSET - // @DisplayName: rangefinder offset - // @Description: Offset in volts for zero distance - // @Units: V - // @Increment: 0.001 - // @User: Advanced - AP_GROUPINFO("4_OFFSET", 40, RangeFinder, state[3].offset, 0.0f), - - // @Param: 4_FUNCTION - // @DisplayName: Rangefinder function - // @Description: Control over what function is used to calculate distance. For a linear function, the distance is (voltage-offset)*scaling. For a inverted function the distance is (offset-voltage)*scaling. For a hyperbolic function the distance is scaling/(voltage-offset). The functions return the distance in meters. - // @Values: 0:Linear,1:Inverted,2:Hyperbolic - // @User: Advanced - AP_GROUPINFO("4_FUNCTION", 41, RangeFinder, state[3].function, 0), - - // @Param: 4_MIN_CM - // @DisplayName: Rangefinder minimum distance - // @Description: Minimum distance in centimeters that rangefinder can reliably read - // @Units: cm - // @Increment: 1 - // @User: Advanced - AP_GROUPINFO("4_MIN_CM", 42, RangeFinder, state[3].min_distance_cm, 20), - - // @Param: 4_MAX_CM - // @DisplayName: Rangefinder maximum distance - // @Description: Maximum distance in centimeters that rangefinder can reliably read - // @Units: cm - // @Increment: 1 - // @User: Advanced - AP_GROUPINFO("4_MAX_CM", 43, RangeFinder, state[3].max_distance_cm, 700), - - // @Param: 4_STOP_PIN - // @DisplayName: Rangefinder stop pin - // @Description: Digital pin that enables/disables rangefinder measurement for an analog rangefinder. A value of -1 means no pin. If this is set, then the pin is set to 1 to enable the rangefinder and set to 0 to disable it. This can be used to ensure that multiple sonar rangefinders don't interfere with each other. - // @Values: -1:Not Used,50:Pixhawk AUXOUT1,51:Pixhawk AUXOUT2,52:Pixhawk AUXOUT3,53:Pixhawk AUXOUT4,54:Pixhawk AUXOUT5,55:Pixhawk AUXOUT6,111:PX4 FMU Relay1,112:PX4 FMU Relay2,113:PX4IO Relay1,114:PX4IO Relay2,115:PX4IO ACC1,116:PX4IO ACC2 - // @User: Advanced - AP_GROUPINFO("4_STOP_PIN", 44, RangeFinder, state[3].stop_pin, -1), - - // @Param: 4_SETTLE - // @DisplayName: Sonar settle time - // @Description: The time in milliseconds that the rangefinder reading takes to settle. This is only used when a STOP_PIN is specified. It determines how long we have to wait for the rangefinder to give a reading after we set the STOP_PIN high. For a sonar rangefinder with a range of around 7m this would need to be around 50 milliseconds to allow for the sonar pulse to travel to the target and back again. - // @Units: ms - // @Increment: 1 - // @User: Advanced - AP_GROUPINFO("4_SETTLE", 45, RangeFinder, state[3].settle_time_ms, 0), - - // @Param: 4_RMETRIC - // @DisplayName: Ratiometric - // @Description: This parameter sets whether an analog rangefinder is ratiometric. Most analog rangefinders are ratiometric, meaning that their output voltage is influenced by the supply voltage. Some analog rangefinders (such as the SF/02) have their own internal voltage regulators so they are not ratiometric. - // @Values: 0:No,1:Yes - // @User: Advanced - AP_GROUPINFO("4_RMETRIC", 46, RangeFinder, state[3].ratiometric, 1), - - // @Param: 4_GNDCLEAR - // @DisplayName: Distance (in cm) from the fourth range finder to the ground - // @Description: This parameter sets the expected range measurement(in cm) that the fourth range finder should return when the vehicle is on the ground. - // @Units: cm - // @Range: 0 127 - // @Increment: 1 - // @User: Advanced - AP_GROUPINFO("4_GNDCLEAR", 47, RangeFinder, state[3].ground_clearance_cm, RANGEFINDER_GROUND_CLEARANCE_CM_DEFAULT), - - // @Param: 4_ADDR - // @DisplayName: Bus address of fourth rangefinder - // @Description: This sets the bus address of the sensor, where applicable. Used for the LightWare I2C sensor to allow for multiple sensors on different addresses. A value of 0 disables the sensor. - // @Range: 0 127 - // @Increment: 1 - // @User: Advanced - AP_GROUPINFO("4_ADDR", 48, RangeFinder, state[3].address, 0), - - // @Param: 4_POS_X - // @DisplayName: X position offset - // @Description: X position of the fourth rangefinder in body frame. Use the zero range datum point if supplied. - // @Units: m - // @Range: -10 10 - // @User: Advanced - - // @Param: 4_POS_Y - // @DisplayName: Y position offset - // @Description: Y position of the fourth rangefinder in body frame. Use the zero range datum point if supplied. - // @Units: m - // @Range: -10 10 - // @User: Advanced - - // @Param: 4_POS_Z - // @DisplayName: Z position offset - // @Description: Z position of the fourth rangefinder in body frame. Use the zero range datum point if supplied. - // @Units: m - // @Range: -10 10 - // @User: Advanced - AP_GROUPINFO("4_POS", 52, RangeFinder, state[3].pos_offset, 0.0f), - - // @Param: 4_ORIENT - // @DisplayName: Rangefinder 4 orientation - // @Description: Orientation of 4th range finder - // @Values: 0:Forward, 1:Forward-Right, 2:Right, 3:Back-Right, 4:Back, 5:Back-Left, 6:Left, 7:Forward-Left, 24:Up, 25:Down - // @User: Advanced - AP_GROUPINFO("4_ORIENT", 56, RangeFinder, state[3].orientation, ROTATION_PITCH_270), + // @Group: 4_ + // @Path: AP_RangeFinder_Params.cpp + AP_SUBGROUPINFO(params[3], "4_", 31, RangeFinder, AP_RangeFinder_Params), // @Group: 4_ // @Path: AP_RangeFinder_Wasp.cpp - AP_SUBGROUPVARPTR(drivers[3], "4_", 60, RangeFinder, backend_var_info[3]), + AP_SUBGROUPVARPTR(drivers[0], "4_", 60, RangeFinder, backend_var_info[3]), #endif +#if RANGEFINDER_MAX_INSTANCES > 4 + // @Group: 5_ + // @Path: AP_RangeFinder_Params.cpp + AP_SUBGROUPINFO(params[4], "5_", 33, RangeFinder, AP_RangeFinder_Params), + + // @Group: 5_ + // @Path: AP_RangeFinder_Wasp.cpp + AP_SUBGROUPVARPTR(drivers[4], "5_", 34, RangeFinder, backend_var_info[4]), +#endif + +#if RANGEFINDER_MAX_INSTANCES > 5 + // @Group: 6_ + // @Path: AP_RangeFinder_Params.cpp + AP_SUBGROUPINFO(params[5], "6_", 35, RangeFinder, AP_RangeFinder_Params), + + // @Group: 6_ + // @Path: AP_RangeFinder_Wasp.cpp + AP_SUBGROUPVARPTR(drivers[5], "6_", 36, RangeFinder, backend_var_info[5]), +#endif + +#if RANGEFINDER_MAX_INSTANCES > 6 + // @Group: 7_ + // @Path: AP_RangeFinder_Params.cpp + AP_SUBGROUPINFO(params[6], "7_", 37, RangeFinder, AP_RangeFinder_Params), + + // @Group: 7_ + // @Path: AP_RangeFinder_Wasp.cpp + AP_SUBGROUPVARPTR(drivers[6], "7_", 38, RangeFinder, backend_var_info[6]), +#endif + +#if RANGEFINDER_MAX_INSTANCES > 7 + // @Group: 8_ + // @Path: AP_RangeFinder_Params.cpp + AP_SUBGROUPINFO(params[7], "8_", 39, RangeFinder, AP_RangeFinder_Params), + + // @Group: 8_ + // @Path: AP_RangeFinder_Wasp.cpp + AP_SUBGROUPVARPTR(drivers[7], "8_", 40, RangeFinder, backend_var_info[7]), +#endif + +#if RANGEFINDER_MAX_INSTANCES > 8 + // @Group: 9_ + // @Path: AP_RangeFinder_Params.cpp + AP_SUBGROUPINFO(params[8], "9_", 41, RangeFinder, AP_RangeFinder_Params), + + // @Group: 9_ + // @Path: AP_RangeFinder_Wasp.cpp + AP_SUBGROUPVARPTR(drivers[8], "9_", 42, RangeFinder, backend_var_info[8]), +#endif + +#if RANGEFINDER_MAX_INSTANCES > 9 + // @Group: A_ + // @Path: AP_RangeFinder_Params.cpp + AP_SUBGROUPINFO(params[9], "A_", 43, RangeFinder, AP_RangeFinder_Params), + + // @Group: A_ + // @Path: AP_RangeFinder_Wasp.cpp + AP_SUBGROUPVARPTR(drivers[9], "A_", 44, RangeFinder, backend_var_info[9]), +#endif + AP_GROUPEND }; @@ -570,7 +155,7 @@ RangeFinder::RangeFinder(AP_SerialManager &_serial_manager, enum Rotation orient // set orientation defaults for (uint8_t i=0; isnprintf(param_name, sizeof(param_name), "RNGFND%X_%s", param_instance, AP_RangeFinder_Params::var_info[destination_index].name); + param_name[sizeof(param_name)-1] = '\0'; + + AP_Param::convert_old_parameter(&info, 1.0f, 0); + } + + // force _params[0]._type into storage to flag that conversion has been done + params[0].type.save(true); +} + /* initialise the RangeFinder class. We do detection of attached range finders here. For now we won't allow for hot-plugging of @@ -592,6 +297,9 @@ void RangeFinder::init(void) // init called a 2nd time? return; } + + convert_params(); + for (uint8_t i=0, serial_instance = 0; iget_device(1, AP_RANGE_FINDER_MAXSONARI2CXL_DEFAULT_ADDR)))) { - _add_backend(AP_RangeFinder_MaxsonarI2CXL::detect(state[instance], + _add_backend(AP_RangeFinder_MaxsonarI2CXL::detect(state[instance], params[instance], hal.i2c_mgr->get_device(0, AP_RANGE_FINDER_MAXSONARI2CXL_DEFAULT_ADDR))); } break; case RangeFinder_TYPE_LWI2C: - if (state[instance].address) { + if (params[instance].address) { #ifdef HAL_RANGEFINDER_LIGHTWARE_I2C_BUS - _add_backend(AP_RangeFinder_LightWareI2C::detect(state[instance], - hal.i2c_mgr->get_device(HAL_RANGEFINDER_LIGHTWARE_I2C_BUS, state[instance].address))); + _add_backend(AP_RangeFinder_LightWareI2C::detect(state[instance], params[instance], + hal.i2c_mgr->get_device(HAL_RANGEFINDER_LIGHTWARE_I2C_BUS, params[instance].address))); #else - if (!_add_backend(AP_RangeFinder_LightWareI2C::detect(state[instance], - hal.i2c_mgr->get_device(1, state[instance].address)))) { - _add_backend(AP_RangeFinder_LightWareI2C::detect(state[instance], - hal.i2c_mgr->get_device(0, state[instance].address))); + if (!_add_backend(AP_RangeFinder_LightWareI2C::detect(state[instance], params[instance], + hal.i2c_mgr->get_device(1, params[instance].address)))) { + _add_backend(AP_RangeFinder_LightWareI2C::detect(state[instance], params[instance], + hal.i2c_mgr->get_device(0, params[instance].address))); } #endif } break; case RangeFinder_TYPE_TRI2C: - if (state[instance].address) { - if (!_add_backend(AP_RangeFinder_TeraRangerI2C::detect(state[instance], - hal.i2c_mgr->get_device(1, state[instance].address)))) { - _add_backend(AP_RangeFinder_TeraRangerI2C::detect(state[instance], - hal.i2c_mgr->get_device(0, state[instance].address))); + if (params[instance].address) { + if (!_add_backend(AP_RangeFinder_TeraRangerI2C::detect(state[instance], params[instance], + hal.i2c_mgr->get_device(1, params[instance].address)))) { + _add_backend(AP_RangeFinder_TeraRangerI2C::detect(state[instance], params[instance], + hal.i2c_mgr->get_device(0, params[instance].address))); } } break; case RangeFinder_TYPE_VL53L0X: - if (!_add_backend(AP_RangeFinder_VL53L0X::detect(state[instance], - hal.i2c_mgr->get_device(1, 0x29)))) { - if (!_add_backend(AP_RangeFinder_VL53L0X::detect(state[instance], - hal.i2c_mgr->get_device(0, 0x29)))) { - if (!_add_backend(AP_RangeFinder_VL53L1X::detect(state[instance], - hal.i2c_mgr->get_device(1, 0x29)))) { - _add_backend(AP_RangeFinder_VL53L1X::detect(state[instance], - hal.i2c_mgr->get_device(0, 0x29))); + if (!_add_backend(AP_RangeFinder_VL53L0X::detect(state[instance], params[instance], + hal.i2c_mgr->get_device(1, params[instance].address)))) { + if (!_add_backend(AP_RangeFinder_VL53L0X::detect(state[instance], params[instance], + hal.i2c_mgr->get_device(0, params[instance].address)))) { + if (!_add_backend(AP_RangeFinder_VL53L1X::detect(state[instance], params[instance], + hal.i2c_mgr->get_device(1, params[instance].address)))) { + _add_backend(AP_RangeFinder_VL53L1X::detect(state[instance], params[instance], + hal.i2c_mgr->get_device(0, params[instance].address))); } } } @@ -707,30 +415,30 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance) // to ease moving from PX4 to ChibiOS we'll lie a little about // the backend driver... if (AP_RangeFinder_PWM::detect()) { - drivers[instance] = new AP_RangeFinder_PWM(state[instance], _powersave_range, estimated_terrain_height); + drivers[instance] = new AP_RangeFinder_PWM(state[instance], params[instance], estimated_terrain_height); } break; #endif #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BBBMINI case RangeFinder_TYPE_BBB_PRU: if (AP_RangeFinder_BBB_PRU::detect()) { - drivers[instance] = new AP_RangeFinder_BBB_PRU(state[instance]); + drivers[instance] = new AP_RangeFinder_BBB_PRU(state[instance], params[instance]); } break; #endif case RangeFinder_TYPE_LWSER: if (AP_RangeFinder_LightWareSerial::detect(serial_manager, serial_instance)) { - drivers[instance] = new AP_RangeFinder_LightWareSerial(state[instance], serial_manager, serial_instance++); + drivers[instance] = new AP_RangeFinder_LightWareSerial(state[instance], params[instance], serial_manager, serial_instance++); } break; case RangeFinder_TYPE_LEDDARONE: if (AP_RangeFinder_LeddarOne::detect(serial_manager, serial_instance)) { - drivers[instance] = new AP_RangeFinder_LeddarOne(state[instance], serial_manager, serial_instance++); + drivers[instance] = new AP_RangeFinder_LeddarOne(state[instance], params[instance], serial_manager, serial_instance++); } break; case RangeFinder_TYPE_ULANDING: if (AP_RangeFinder_uLanding::detect(serial_manager, serial_instance)) { - drivers[instance] = new AP_RangeFinder_uLanding(state[instance], serial_manager, serial_instance++); + drivers[instance] = new AP_RangeFinder_uLanding(state[instance], params[instance], serial_manager, serial_instance++); } break; #if (CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP || \ @@ -743,43 +451,43 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance) #endif case RangeFinder_TYPE_MAVLink: if (AP_RangeFinder_MAVLink::detect()) { - drivers[instance] = new AP_RangeFinder_MAVLink(state[instance]); + drivers[instance] = new AP_RangeFinder_MAVLink(state[instance], params[instance]); } break; case RangeFinder_TYPE_MBSER: if (AP_RangeFinder_MaxsonarSerialLV::detect(serial_manager, serial_instance)) { - drivers[instance] = new AP_RangeFinder_MaxsonarSerialLV(state[instance], serial_manager, serial_instance++); + drivers[instance] = new AP_RangeFinder_MaxsonarSerialLV(state[instance], params[instance], serial_manager, serial_instance++); } break; case RangeFinder_TYPE_ANALOG: // note that analog will always come back as present if the pin is valid - if (AP_RangeFinder_analog::detect(state[instance])) { - drivers[instance] = new AP_RangeFinder_analog(state[instance]); + if (AP_RangeFinder_analog::detect(params[instance])) { + drivers[instance] = new AP_RangeFinder_analog(state[instance], params[instance]); } break; case RangeFinder_TYPE_NMEA: if (AP_RangeFinder_NMEA::detect(serial_manager, serial_instance)) { - drivers[instance] = new AP_RangeFinder_NMEA(state[instance], serial_manager, serial_instance++); + drivers[instance] = new AP_RangeFinder_NMEA(state[instance], params[instance], serial_manager, serial_instance++); } break; case RangeFinder_TYPE_WASP: if (AP_RangeFinder_Wasp::detect(serial_manager, serial_instance)) { - drivers[instance] = new AP_RangeFinder_Wasp(state[instance], serial_manager, serial_instance++); + drivers[instance] = new AP_RangeFinder_Wasp(state[instance], params[instance], serial_manager, serial_instance++); } break; case RangeFinder_TYPE_BenewakeTF02: if (AP_RangeFinder_Benewake::detect(serial_manager, serial_instance)) { - drivers[instance] = new AP_RangeFinder_Benewake(state[instance], serial_manager, serial_instance++, AP_RangeFinder_Benewake::BENEWAKE_TF02); + drivers[instance] = new AP_RangeFinder_Benewake(state[instance], params[instance], serial_manager, serial_instance++, AP_RangeFinder_Benewake::BENEWAKE_TF02); } break; case RangeFinder_TYPE_BenewakeTFmini: if (AP_RangeFinder_Benewake::detect(serial_manager, serial_instance)) { - drivers[instance] = new AP_RangeFinder_Benewake(state[instance], serial_manager, serial_instance++, AP_RangeFinder_Benewake::BENEWAKE_TFmini); + drivers[instance] = new AP_RangeFinder_Benewake(state[instance], params[instance], serial_manager, serial_instance++, AP_RangeFinder_Benewake::BENEWAKE_TFmini); } break; case RangeFinder_TYPE_PWM: if (AP_RangeFinder_PWM::detect()) { - drivers[instance] = new AP_RangeFinder_PWM(state[instance], _powersave_range, estimated_terrain_height); + drivers[instance] = new AP_RangeFinder_PWM(state[instance], params[instance], estimated_terrain_height); } break; default: @@ -819,7 +527,7 @@ void RangeFinder::handle_msg(mavlink_message_t *msg) { uint8_t i; for (i=0; ihandle_msg(msg); } } @@ -919,7 +627,7 @@ bool RangeFinder::pre_arm_check() const { for (uint8_t i=0; i #include #include +#include "AP_RangeFinder_Params.h" // Maximum number of range finder instances available on this platform -#define RANGEFINDER_MAX_INSTANCES 2 +#define RANGEFINDER_MAX_INSTANCES 10 #define RANGEFINDER_GROUND_CLEARANCE_CM_DEFAULT 10 #define RANGEFINDER_PREARM_ALT_MAX_CM 200 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL @@ -95,26 +96,10 @@ public: uint16_t pre_arm_distance_max; // max distance captured during pre-arm checks uint32_t last_reading_ms; // system time of last successful update from sensor - AP_Int8 type; - AP_Int8 pin; - AP_Int8 ratiometric; - AP_Int8 stop_pin; - AP_Int16 settle_time_ms; - AP_Float scaling; - AP_Float offset; - AP_Int8 function; - AP_Int16 min_distance_cm; - AP_Int16 max_distance_cm; - AP_Int8 ground_clearance_cm; - AP_Int8 address; - AP_Vector3f pos_offset; // position offset in body frame - AP_Int8 orientation; const struct AP_Param::GroupInfo *var_info; }; static const struct AP_Param::GroupInfo *backend_var_info[RANGEFINDER_MAX_INSTANCES]; - - AP_Int16 _powersave_range; // parameters for each instance static const struct AP_Param::GroupInfo var_info[]; @@ -173,6 +158,8 @@ public: static RangeFinder *get_singleton(void) { return _singleton; } +protected: + AP_RangeFinder_Params params[RANGEFINDER_MAX_INSTANCES]; private: static RangeFinder *_singleton; @@ -184,6 +171,8 @@ private: AP_SerialManager &serial_manager; Vector3f pos_offset_zero; // allows returning position offsets of zero for invalid requests + void convert_params(void); + void detect_instance(uint8_t instance, uint8_t& serial_instance); void update_instance(uint8_t instance); diff --git a/libraries/AP_RangeFinder/RangeFinder_Backend.cpp b/libraries/AP_RangeFinder/RangeFinder_Backend.cpp index db86eb5a15..901020a46d 100644 --- a/libraries/AP_RangeFinder/RangeFinder_Backend.cpp +++ b/libraries/AP_RangeFinder/RangeFinder_Backend.cpp @@ -24,20 +24,21 @@ extern const AP_HAL::HAL& hal; base class constructor. This incorporates initialisation as well. */ -AP_RangeFinder_Backend::AP_RangeFinder_Backend(RangeFinder::RangeFinder_State &_state) : - state(_state) +AP_RangeFinder_Backend::AP_RangeFinder_Backend(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params) : + state(_state), + params(_params) { } MAV_DISTANCE_SENSOR AP_RangeFinder_Backend::get_mav_distance_sensor_type() const { - if (state.type == RangeFinder::RangeFinder_TYPE_NONE) { + if (params.type == RangeFinder::RangeFinder_TYPE_NONE) { return MAV_DISTANCE_SENSOR_UNKNOWN; } return _get_mav_distance_sensor_type(); } RangeFinder::RangeFinder_Status AP_RangeFinder_Backend::status() const { - if (state.type == RangeFinder::RangeFinder_TYPE_NONE) { + if (params.type == RangeFinder::RangeFinder_TYPE_NONE) { // turned off at runtime? return RangeFinder::RangeFinder_NotConnected; } @@ -54,9 +55,9 @@ bool AP_RangeFinder_Backend::has_data() const { void AP_RangeFinder_Backend::update_status() { // check distance - if ((int16_t)state.distance_cm > state.max_distance_cm) { + if ((int16_t)state.distance_cm > params.max_distance_cm) { set_status(RangeFinder::RangeFinder_OutOfRangeHigh); - } else if ((int16_t)state.distance_cm < state.min_distance_cm) { + } else if ((int16_t)state.distance_cm < params.min_distance_cm) { set_status(RangeFinder::RangeFinder_OutOfRangeLow); } else { set_status(RangeFinder::RangeFinder_Good); @@ -98,8 +99,8 @@ void AP_RangeFinder_Backend::update_pre_arm_check() // Check that the range finder has been exercised through a realistic range of movement if (((state.pre_arm_distance_max - state.pre_arm_distance_min) >= RANGEFINDER_PREARM_REQUIRED_CHANGE_CM) && (state.pre_arm_distance_max < RANGEFINDER_PREARM_ALT_MAX_CM) && - ((int16_t)state.pre_arm_distance_min < (MAX(state.ground_clearance_cm,state.min_distance_cm) + 10)) && - ((int16_t)state.pre_arm_distance_min > (MIN(state.ground_clearance_cm,state.min_distance_cm) - 10))) { + ((int16_t)state.pre_arm_distance_min < (MAX(params.ground_clearance_cm,params.min_distance_cm) + 10)) && + ((int16_t)state.pre_arm_distance_min > (MIN(params.ground_clearance_cm,params.min_distance_cm) - 10))) { state.pre_arm_check = true; } } diff --git a/libraries/AP_RangeFinder/RangeFinder_Backend.h b/libraries/AP_RangeFinder/RangeFinder_Backend.h index 14ce4a6292..2792e2301a 100644 --- a/libraries/AP_RangeFinder/RangeFinder_Backend.h +++ b/libraries/AP_RangeFinder/RangeFinder_Backend.h @@ -22,7 +22,8 @@ class AP_RangeFinder_Backend { public: // constructor. This incorporates initialisation as well. - AP_RangeFinder_Backend(RangeFinder::RangeFinder_State &_state); + //AP_RangeFinder_Backend(RangeFinder::RangeFinder_State &_state); + AP_RangeFinder_Backend(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params); // we declare a virtual destructor so that RangeFinder drivers can // override with a custom destructor if need be @@ -35,15 +36,15 @@ public: void update_pre_arm_check(); - enum Rotation orientation() const { return (Rotation)state.orientation.get(); } + enum Rotation orientation() const { return (Rotation)params.orientation.get(); } uint16_t distance_cm() const { return state.distance_cm; } uint16_t voltage_mv() const { return state.voltage_mv; } - int16_t max_distance_cm() const { return state.max_distance_cm; } - int16_t min_distance_cm() const { return state.min_distance_cm; } - int16_t ground_clearance_cm() const { return state.ground_clearance_cm; } + int16_t max_distance_cm() const { return params.max_distance_cm; } + int16_t min_distance_cm() const { return params.min_distance_cm; } + int16_t ground_clearance_cm() const { return params.ground_clearance_cm; } MAV_DISTANCE_SENSOR get_mav_distance_sensor_type() const; RangeFinder::RangeFinder_Status status() const; - RangeFinder::RangeFinder_Type type() const { return (RangeFinder::RangeFinder_Type)state.type.get(); } + RangeFinder::RangeFinder_Type type() const { return (RangeFinder::RangeFinder_Type)params.type.get(); } // true if sensor is returning data bool has_data() const; @@ -53,7 +54,7 @@ public: // return a 3D vector defining the position offset of the sensor // in metres relative to the body frame origin - const Vector3f &get_pos_offset() const { return state.pos_offset; } + const Vector3f &get_pos_offset() const { return params.pos_offset; } // return system time of last successful read from the sensor uint32_t last_reading_ms() const { return state.last_reading_ms; } @@ -67,6 +68,7 @@ protected: void set_status(RangeFinder::RangeFinder_Status status); RangeFinder::RangeFinder_State &state; + AP_RangeFinder_Params ¶ms; // semaphore for access to shared frontend data HAL_Semaphore _sem;