DistanceSensorCheck: do not raise a distance sensor failure if the SF…

…XX_MODE is set to 2 and we are in a VTOL FX flight phase
chfriedrich98 · Sep 3, 2024 · 80d4fad · 80d4fad
1 parent a9cdb36
commit 80d4fad
Show file tree

Hide file tree

Showing 5 changed files with 22 additions and 2 deletions.
diff --git a/msg/DistanceSensor.msg b/msg/DistanceSensor.msg
@@ -40,3 +40,8 @@ uint8 ROTATION_UPWARD_FACING   = 24 # MAV_SENSOR_ROTATION_PITCH_90
 uint8 ROTATION_DOWNWARD_FACING = 25 # MAV_SENSOR_ROTATION_PITCH_270
 
 uint8 ROTATION_CUSTOM          = 100 # MAV_SENSOR_ROTATION_CUSTOM
+
+uint8 mode 			# mode of operation
+uint8 DISTANCE_SENSOR_MODE_UNKNOWN = 0 # Unknown mode
+uint8 DISTANCE_SENSOR_MODE_RUN    = 1 # sensor is running continuosly
+uint8 DISTANCE_SENSOR_MODE_DISABLED = 2 # sensor is disabled per request
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
@@ -145,6 +145,7 @@ class LightwareLaser : public device::I2C, public I2CSPIDriver<LightwareLaser>,
 	typeof(px4::msg::VehicleStatus::vehicle_type) _vehicle_type{px4::msg::VehicleStatus::VEHICLE_TYPE_UNKNOWN};
 	bool _restriction{false};
 	bool _auto_restriction{false};
+	bool _prev_restriction{false};
 };
 
 LightwareLaser::LightwareLaser(const I2CSPIDriverConfig &config) :
@@ -438,7 +439,7 @@ int LightwareLaser::updateRestriction()
 		updateParams();
 	}
 
-	bool _prev_restriction{_restriction};
+	_prev_restriction = _restriction;
 
 	switch (_param_sf1xx_mode.get()) {
 	case 0: // Sensor disabled
@@ -498,6 +499,8 @@ void LightwareLaser::RunImpl()
 
 	case State::Running:
 		if (!_restriction) {
+			_px4_rangefinder.set_mode(distance_sensor_s::DISTANCE_SENSOR_MODE_RUN);
+
 			if (PX4_OK != collect()) {
 				PX4_DEBUG("collection error");
 
@@ -506,6 +509,14 @@ void LightwareLaser::RunImpl()
 					_consecutive_errors = 0;
 				}
 			}
+
+		} else {
+			_px4_rangefinder.set_mode(distance_sensor_s::DISTANCE_SENSOR_MODE_DISABLED);
+
+			if (!_prev_restriction) { // Publish disabled status once
+				_px4_rangefinder.update(hrt_absolute_time(), -1.f, 0);
+			}
+
 		}
 
 		ScheduleDelayed(_conversion_interval);

diff --git a/src/lib/drivers/rangefinder/PX4Rangefinder.cpp b/src/lib/drivers/rangefinder/PX4Rangefinder.cpp
@@ -40,6 +40,7 @@ PX4Rangefinder::PX4Rangefinder(const uint32_t device_id, const uint8_t device_or
 	set_device_id(device_id);
 	set_orientation(device_orientation);
 	set_rangefinder_type(distance_sensor_s::MAV_DISTANCE_SENSOR_LASER); // Default to type LASER
+	set_mode(UINT8_C(0));
 }
 
 PX4Rangefinder::~PX4Rangefinder()

diff --git a/src/lib/drivers/rangefinder/PX4Rangefinder.hpp b/src/lib/drivers/rangefinder/PX4Rangefinder.hpp
@@ -60,6 +60,8 @@ class PX4Rangefinder
 
 	void set_orientation(const uint8_t device_orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
 
+	void set_mode(const uint8_t mode) {_distance_sensor_pub.get().mode = mode; }
+
 	void update(const hrt_abstime &timestamp_sample, const float distance, const int8_t quality = -1);
 
 	int get_instance() { return _distance_sensor_pub.get_instance(); };

diff --git a/src/modules/commander/HealthAndArmingChecks/checks/distanceSensorChecks.cpp b/src/modules/commander/HealthAndArmingChecks/checks/distanceSensorChecks.cpp
@@ -48,7 +48,8 @@ void DistanceSensorChecks::checkAndReport(const Context &context, Report &report
 
 		if (exists) {
 			distance_sensor_s dist_sens;
-			valid = _distance_sensor_sub[instance].copy(&dist_sens) && hrt_elapsed_time(&dist_sens.timestamp) < 1_s;
+			valid = _distance_sensor_sub[instance].copy(&dist_sens) && ((hrt_elapsed_time(&dist_sens.timestamp) < 1_s)
+					|| (dist_sens.mode == distance_sensor_s::DISTANCE_SENSOR_MODE_DISABLED));
 			reporter.setIsPresent(health_component_t::distance_sensor);
 		}