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..8e0815bff561 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); @@ -75,12 +79,14 @@ class LightwareLaser : public device::I2C, public I2CSPIDriver private: // I2C (legacy) binary protocol command static constexpr uint8_t I2C_LEGACY_CMD_READ_ALTITUDE = 0; + static constexpr uint8_t I2C_LEGACY_CMD_WRITE_LASER_CONTROL = 5; enum class Register : uint8_t { // see http://support.lightware.co.za/sf20/#/commands ProductName = 0, DistanceOutput = 27, DistanceData = 44, + LaserFiring = 50, MeasurementMode = 93, ZeroOffset = 94, LostSignalCounter = 95, @@ -117,6 +123,8 @@ class LightwareLaser : public device::I2C, public I2CSPIDriver int enableI2CBinaryProtocol(); int collect(); + int updateRestriction(); + PX4Rangefinder _px4_rangefinder; int _conversion_interval{-1}; @@ -127,11 +135,22 @@ 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}; + bool _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 +166,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: @@ -292,8 +311,10 @@ int LightwareLaser::configure() { switch (_type) { case Type::Generic: { - uint8_t cmd = I2C_LEGACY_CMD_READ_ALTITUDE; - int ret = transfer(&cmd, 1, nullptr, 0); + uint8_t cmd1 = I2C_LEGACY_CMD_READ_ALTITUDE; + int ret = transfer(&cmd1, 1, nullptr, 0); + const uint8_t cmd2[] = {I2C_LEGACY_CMD_WRITE_LASER_CONTROL, (uint8_t)(_restriction ? 0 : 1)}; + ret |= transfer(cmd2, sizeof(cmd2), nullptr, 0); if (PX4_OK != ret) { perf_count(_comms_errors); @@ -318,6 +339,8 @@ int LightwareLaser::configure() ret |= transfer(cmd4, sizeof(cmd4), nullptr, 0); const uint8_t cmd5[] = {(uint8_t)Register::LostSignalCounter, 0, 0, 0, 0}; // immediately report lost signal ret |= transfer(cmd5, sizeof(cmd5), nullptr, 0); + const uint8_t cmd6[] = {(uint8_t)Register::LaserFiring, (uint8_t)(_restriction ? 0 : 1)}; + ret |= transfer(cmd6, sizeof(cmd6), nullptr, 0); return ret; break; @@ -386,8 +409,78 @@ void LightwareLaser::start() ScheduleDelayed(_conversion_interval); } +int LightwareLaser::updateRestriction() +{ + 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"); + + switch (_type) { + case Type::Generic: { + const uint8_t cmd[] = {I2C_LEGACY_CMD_WRITE_LASER_CONTROL, (uint8_t)(_restriction ? 0 : 1)}; + return transfer(cmd, sizeof(cmd), nullptr, 0); + } + + case Type::LW20c: { + const uint8_t cmd[] = {(uint8_t)Register::LaserFiring, (uint8_t)(_restriction ? 0 : 1)}; + return transfer(cmd, sizeof(cmd), nullptr, 0); + } + } + } + + return 0; +} + void LightwareLaser::RunImpl() { + if (PX4_OK != updateRestriction()) { + PX4_DEBUG("restriction error"); + perf_count(_comms_errors); + } + switch (_state) { case State::Configuring: { if (configure() == 0) { @@ -404,12 +497,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 (!_restriction) { + 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);