diff --git a/src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp b/src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp index aafa60576819..8a5564775af0 100644 --- a/src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp +++ b/src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp @@ -46,18 +46,22 @@ #include #include #include +#include #include #include #include #include #include +#include +#include +#include using namespace time_literals; /* Configuration Constants */ #define LIGHTWARE_LASER_BASEADDR 0x66 -class LightwareLaser : public device::I2C, public I2CSPIDriver +class LightwareLaser : public device::I2C, public I2CSPIDriver, public ModuleParams { public: LightwareLaser(const I2CSPIDriverConfig &config); @@ -117,6 +121,8 @@ class LightwareLaser : public device::I2C, public I2CSPIDriver int enableI2CBinaryProtocol(); int collect(); + bool isRestricted(); + PX4Rangefinder _px4_rangefinder; int _conversion_interval{-1}; @@ -127,11 +133,21 @@ class LightwareLaser : public device::I2C, public I2CSPIDriver Type _type{Type::Generic}; State _state{State::Configuring}; int _consecutive_errors{0}; + + DEFINE_PARAMETERS( + (ParamInt) _param_sens_en_sf1xx, + (ParamInt) _param_sf1xx_mode + ) + uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + typeof(px4::msg::VehicleStatus::vehicle_type) _vehicle_type{px4::msg::VehicleStatus::VEHICLE_TYPE_UNKNOWN}; + bool _restriction{false}, _auto_restriction{false}; }; LightwareLaser::LightwareLaser(const I2CSPIDriverConfig &config) : I2C(config), I2CSPIDriver(config), + ModuleParams(nullptr), _px4_rangefinder(get_device_id(), config.rotation) { _px4_rangefinder.set_device_type(DRV_DIST_DEVTYPE_LIGHTWARE_LASER); @@ -147,8 +163,8 @@ LightwareLaser::~LightwareLaser() int LightwareLaser::init() { int ret = PX4_ERROR; - int32_t hw_model = 0; - param_get(param_find("SENS_EN_SF1XX"), &hw_model); + updateParams(); + const int32_t hw_model = _param_sens_en_sf1xx.get(); switch (hw_model) { case 0: @@ -386,6 +402,58 @@ void LightwareLaser::start() ScheduleDelayed(_conversion_interval); } +bool LightwareLaser::isRestricted() +{ + px4::msg::VehicleStatus vehicle_status; + + if (_vehicle_status_sub.update(&vehicle_status)) { + // Check if vehicle type changed + if (vehicle_status.vehicle_type != _vehicle_type) { + // Transition VTOL -> Fixed Wing + if (_vehicle_type == px4::msg::VehicleStatus::VEHICLE_TYPE_ROTARY_WING && + vehicle_status.vehicle_type == px4::msg::VehicleStatus::VEHICLE_TYPE_FIXED_WING) { + _auto_restriction = true; + } + + // Transition Fixed Wing -> VTOL + else if (_vehicle_type == px4::msg::VehicleStatus::VEHICLE_TYPE_FIXED_WING && + vehicle_status.vehicle_type == px4::msg::VehicleStatus::VEHICLE_TYPE_ROTARY_WING) { + _auto_restriction = false; + } + + _vehicle_type = vehicle_status.vehicle_type; + } + } + + if (_parameter_update_sub.updated()) { + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); + updateParams(); + } + bool _prev_restriction{_restriction}; + + switch (_param_sf1xx_mode.get()) { + case 0: // Sensor disabled + _restriction = true; + break; + + case 1: // Sensor enabled + default: + _restriction = false; + break; + + case 2: + _restriction = _auto_restriction; + break; + } + + if (_prev_restriction != _restriction) { + PX4_INFO("Emission Control: %sabling sensor!", _restriction ? "dis" : "en"); + } + + return _restriction; +} + void LightwareLaser::RunImpl() { switch (_state) { @@ -404,12 +472,14 @@ void LightwareLaser::RunImpl() } case State::Running: - if (PX4_OK != collect()) { - PX4_DEBUG("collection error"); - - if (++_consecutive_errors > 3) { - _state = State::Configuring; - _consecutive_errors = 0; + if (!isRestricted()) { + if (PX4_OK != collect()) { + PX4_DEBUG("collection error"); + + if (++_consecutive_errors > 3) { + _state = State::Configuring; + _consecutive_errors = 0; + } } } diff --git a/src/drivers/distance_sensor/lightware_laser_i2c/parameters.c b/src/drivers/distance_sensor/lightware_laser_i2c/parameters.c index b0564780be97..defe335751f3 100644 --- a/src/drivers/distance_sensor/lightware_laser_i2c/parameters.c +++ b/src/drivers/distance_sensor/lightware_laser_i2c/parameters.c @@ -48,3 +48,15 @@ * @value 7 SF/LW30/d */ PARAM_DEFINE_INT32(SENS_EN_SF1XX, 0); + +/** + * Lightware SF1xx/SF20/LW20 Operation Mode + * + * @value 0 Disabled + * @value 1 Enabled + * @value 2 Disabled during VTOL fast forward flight + * + * @min 0 + * @max 2 + */ +PARAM_DEFINE_INT32(SF1XX_MODE, 1);