Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-31 10:26:52 +08:00
Code Issues Actions 7 Packages Projects Releases Wiki Activity

sf45: refactor how sensor orientation (yaw_cfg) correction is applied to incoming sensor data.

Browse Source 
yaw_cfg is now read into the obstacle_distance message as the angle_offset. The offset is computed once at init and applied to each measurement.
This commit is contained in:
mahimayoga
2025-02-03 16:41:37 +01:00
committed by Silvan Fuhrer
parent 31bff3e5bb
commit 48c0992a7d
1 changed files with 10 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp
+10 -29
View File
@@ -92,6 +92,11 @@ int SF45LaserSerial::init()
param_get(param_find("SF45_ORIENT_CFG"), &_orient_cfg); param_get(param_find("SF45_ORIENT_CFG"), &_orient_cfg);
param_get(param_find("SF45_YAW_CFG"), &_yaw_cfg); param_get(param_find("SF45_YAW_CFG"), &_yaw_cfg);
// set the sensor orientation
const float yaw_cfg_angle = ObstacleMath::sensor_orientation_to_yaw_offset(static_cast<ObstacleMath::SensorOrientation>
(_yaw_cfg));
_obstacle_distance.angle_offset = math::degrees(matrix::wrap_2pi(yaw_cfg_angle));
start(); start();
return PX4_OK; return PX4_OK;
} }
@@ -594,7 +599,6 @@ void SF45LaserSerial::sf45_process_replies()
case SF_DISTANCE_DATA_CM: { case SF_DISTANCE_DATA_CM: {
const float raw_distance = (rx_field.data[0] << 0) | (rx_field.data[1] << 8); const float raw_distance = (rx_field.data[0] << 0) | (rx_field.data[1] << 8);
int16_t raw_yaw = ((rx_field.data[2] << 0) | (rx_field.data[3] << 8)); int16_t raw_yaw = ((rx_field.data[2] << 0) | (rx_field.data[3] << 8));
int16_t scaled_yaw = 0;
// The sensor scans from 0 to -160, so extract negative angle from int16 and represent as if a float // The sensor scans from 0 to -160, so extract negative angle from int16 and represent as if a float
if (raw_yaw > 32000) { if (raw_yaw > 32000) {
@@ -607,33 +611,10 @@ void SF45LaserSerial::sf45_process_replies()
} }
// SF45/B product guide {Data output bit: 8 Description: "Yaw angle [1/100 deg] size: int16}" // SF45/B product guide {Data output bit: 8 Description: "Yaw angle [1/100 deg] size: int16}"
scaled_yaw = raw_yaw * SF45_SCALE_FACTOR; float scaled_yaw = raw_yaw * SF45_SCALE_FACTOR;
switch (_yaw_cfg) { // Adjust for sensor orientation
case ROTATION_FORWARD_FACING: scaled_yaw = sf45_wrap_360(scaled_yaw + _obstacle_distance.angle_offset);
break;
case ROTATION_BACKWARD_FACING:
if (scaled_yaw > 180) {
scaled_yaw = scaled_yaw - 180;
} else {
scaled_yaw = scaled_yaw + 180; // rotation facing aft
}
break;
case ROTATION_RIGHT_FACING:
scaled_yaw = scaled_yaw + 90; // rotation facing right
break;
case ROTATION_LEFT_FACING:
scaled_yaw = scaled_yaw - 90; // rotation facing left
break;
default:
break;
}
// Convert to meters for the debug message // Convert to meters for the debug message
float distance_m = raw_distance * SF45_SCALE_FACTOR; float distance_m = raw_distance * SF45_SCALE_FACTOR;
@@ -642,7 +623,7 @@ void SF45LaserSerial::sf45_process_replies()
uint8_t current_bin = sf45_convert_angle(scaled_yaw); uint8_t current_bin = sf45_convert_angle(scaled_yaw);
if (current_bin != _previous_bin) { if (current_bin != _previous_bin) {
PX4_DEBUG("scaled_yaw: \t %d, \t current_bin: \t %d, \t distance: \t %8.4f\n", scaled_yaw, current_bin, PX4_DEBUG("scaled_yaw: \t %f, \t current_bin: \t %d, \t distance: \t %8.4f\n", (double)scaled_yaw, current_bin,
(double)distance_m); (double)distance_m);
if (_vehicle_attitude_sub.updated()) { if (_vehicle_attitude_sub.updated()) {
@@ -726,7 +707,7 @@ uint8_t SF45LaserSerial::sf45_convert_angle(const int16_t yaw)
{ {
uint8_t mapped_sector = 0; uint8_t mapped_sector = 0;
float adjusted_yaw = sf45_wrap_360(yaw - _obstacle_distance.angle_offset); float adjusted_yaw = sf45_wrap_360(yaw - _obstacle_distance.angle_offset);
mapped_sector = round(adjusted_yaw / _obstacle_distance.increment); mapped_sector = floor(adjusted_yaw / _obstacle_distance.increment);
return mapped_sector; return mapped_sector;
} }