consolidated into update method; use default value in declaration

Signed-off-by: dirksavage88 <dirksavage88@gmail.com>

src/drivers/distance_sensor/vl53l1x/vl53l1x.cpp

@@ -199,12 +199,13 @@ int VL53L1X::collect()
 	uint8_t ret = 0;
 	uint8_t rangeStatus = VL53L1X_RANGE_STATUS_OK;
 	uint16_t distance_mm = 0;
-
+	static constexpr float DOWNWARD_FACING_THETA = -1.57f;
+	static constexpr float RIGHT_PSI = 0.117809725;
+	static constexpr float LEFT_PSI = -0.117809725;
 	perf_begin(_sample_perf);
 
 
-
-	ret = VL53L1X_GetRangeStatus(&rangeStatus);
+	const int8_t quality = VL53L1X_GetRangeStatus(&rangeStatus);
 
 	if ((ret != PX4_OK) | (rangeStatus == VL53L1X_RANGE_STATUS_OUT_OF_BOUNDS)) {
 		perf_count(_comms_errors);
@@ -236,8 +237,8 @@ int VL53L1X::collect()
 	float psi = 0.f;
 	float theta = 0.f;
 
-	if (_sensor_orientation == (Rotation)distance_sensor_s::ROTATION_DOWNWARD_FACING) {
-		theta = -1.57f;
+	if (_sensor_orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING) {
+		theta = DOWNWARD_FACING_THETA;
 	}
 
 	float phi = 0.f;
@@ -245,11 +246,11 @@ int VL53L1X::collect()
 	switch (roi_center[_zone_index]) {
 
 	case VL53L1X_ROI_MID_RIGHT:
-		psi = 0.117809725; // Approximately 7 degrees;
+		psi = RIGHT_PSI; // Approximately 7 degrees;
 		break;
 
 	case VL53L1X_ROI_MID_LEFT:
-		psi = -0.117809725;
+		psi = LEFT_PSI;
 		break;
 
 	default:
@@ -259,8 +260,8 @@ int VL53L1X::collect()
 	Quaternionf quat_e = Eulerf(phi, theta, psi);
 
 	float dist_q[] {quat_e(0), quat_e(1), quat_e(2), quat_e(3)};
-	_px4_rangefinder.update_quaternion(dist_q);
-	_px4_rangefinder.update(timestamp_sample, distance_m);
+
+	_px4_rangefinder.update(timestamp_sample, distance_m, quality, dist_q);
 
 	return PX4_OK;
 }
@@ -312,11 +313,9 @@ int VL53L1X::init()
 	}
 
 	// Spad width (x) & height (y)
-
 	uint8_t x = 8;
 	uint8_t y = 16;
 
-
 	ret |= VL53L1X_SensorInit();
 	ret |= VL53L1X_ConfigBig(_distance_mode, VL53L1X_SAMPLE_RATE);
 	ret |= VL53L1X_SetROI(x, y);

src/drivers/distance_sensor/vl53l1x/vl53l1x.hpp

@@ -157,7 +157,7 @@ private:
 	int8_t VL53L1X_SetROICenter(uint8_t data);
 	int8_t VL53L1X_SetROI(uint16_t x, uint16_t y);
 	PX4Rangefinder    _px4_rangefinder;
-	uint8_t _sensor_orientation{25};
+	Rotation _sensor_orientation{ROTATION_PITCH_270};
 	perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": com_err")};
 	perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")};
 };

src/lib/drivers/rangefinder/PX4Rangefinder.cpp

@@ -68,18 +68,10 @@ void PX4Rangefinder::set_orientation(const uint8_t device_orientation)
 	_distance_sensor_pub.get().orientation = device_orientation;
 }
 
-void PX4Rangefinder::update_quaternion(const float (&quat)[4])
-{
-	memcpy(_quat, quat, sizeof(float) * 4);
-	_q_update_now = hrt_absolute_time();
-	_quat_updated = true;
 
-}
-
-void PX4Rangefinder::update(const hrt_abstime &timestamp_sample, const float distance, const int8_t quality)
+void PX4Rangefinder::update(const hrt_abstime &timestamp_sample, const float distance, const int8_t quality, const float (&quat)[4])
 {
 	distance_sensor_s &report = _distance_sensor_pub.get();
-
 	report.timestamp = timestamp_sample;
 	report.current_distance = distance;
 	report.signal_quality = quality;
@@ -91,15 +83,8 @@ void PX4Rangefinder::update(const hrt_abstime &timestamp_sample, const float dis
 		}
 	}
 
-	// Update the quaternion in the sample update if we have set the update flag to true and the quaternion is fresh
-	if (_quat_updated && (_q_update_now - timestamp_sample < 10_ms)) {
-		for (uint8_t i = 0; i < 4; ++i) {
-			report.q[i] = _quat[i];
-
-		}
-
-	}
+	// Update the quaternion in the sample update
+	memcpy(report.q, quat, sizeof(float) * 4);
 
 	_distance_sensor_pub.update();
-	_quat_updated = false;
 }

src/lib/drivers/rangefinder/PX4Rangefinder.hpp

@@ -62,17 +62,13 @@ public:
 
 	void set_mode(const uint8_t mode) { _distance_sensor_pub.get().mode = mode; }
 
-	// update quaterion sensor orientation with respect to the vehicle body frame (only called explicitly via method, not by the constructor when object created)
-	void update_quaternion(const float (&quat)[4]);
-
-	void update(const hrt_abstime &timestamp_sample, const float distance, const int8_t quality = -1);
+	// update with quaterion sensor orientation with respect to the vehicle body frame
+	void update(const hrt_abstime &timestamp_sample, const float distance, const int8_t quality = -1, const float (&quat)[4] = {1.0, 0.0, 0.0, 0.0});
 
 	int get_instance() { return _distance_sensor_pub.get_instance(); };
 	uint32_t get_device_id() { return _distance_sensor_pub.get().device_id; };
 
 private:
 	uORB::PublicationMultiData<distance_sensor_s> _distance_sensor_pub{ORB_ID(distance_sensor)};
-	bool _quat_updated{false};
-	float _quat[4] {1.0, 0.0, 0.0, 0.0};
 	hrt_abstime _q_update_now;
 };