SF1xx: implement emission control

PX4 · Nov 24, 2023 · d8a5031 · d8a5031
1 parent 6a34b63
commit d8a5031
Show file tree

Hide file tree

Showing 2 changed files with 91 additions and 9 deletions.
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
@@ -46,18 +46,22 @@
 #include <px4_platform_common/getopt.h>
 #include <px4_platform_common/i2c_spi_buses.h>
 #include <px4_platform_common/module.h>
+#include <px4_platform_common/module_params.h>
 #include <drivers/device/i2c.h>
 #include <lib/parameters/param.h>
 #include <lib/perf/perf_counter.h>
 #include <drivers/drv_hrt.h>
 #include <drivers/rangefinder/PX4Rangefinder.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/parameter_update.h>
 
 using namespace time_literals;
 
 /* Configuration Constants */
 #define LIGHTWARE_LASER_BASEADDR		0x66
 
-class LightwareLaser : public device::I2C, public I2CSPIDriver<LightwareLaser>
+class LightwareLaser : public device::I2C, public I2CSPIDriver<LightwareLaser>, public ModuleParams
 {
 public:
 	LightwareLaser(const I2CSPIDriverConfig &config);
@@ -117,6 +121,8 @@ class LightwareLaser : public device::I2C, public I2CSPIDriver<LightwareLaser>
 	int enableI2CBinaryProtocol();
 	int collect();
 
+	bool isRestricted();
+
 	PX4Rangefinder _px4_rangefinder;
 
 	int _conversion_interval{-1};
@@ -127,11 +133,21 @@ class LightwareLaser : public device::I2C, public I2CSPIDriver<LightwareLaser>
 	Type _type{Type::Generic};
 	State _state{State::Configuring};
 	int _consecutive_errors{0};
+
+	DEFINE_PARAMETERS(
+		(ParamInt<px4::params::SENS_EN_SF1XX>) _param_sens_en_sf1xx,
+		(ParamInt<px4::params::SF1XX_MODE>) _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
@@ -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);