fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues 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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp
View File

@ -92,6 +92,11 @@ int SF45LaserSerial::init()
param_get(param_find("SF45_ORIENT_CFG"), &_orient_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();
return PX4_OK;
}
@ -594,7 +599,6 @@ void SF45LaserSerial::sf45_process_replies()
case SF_DISTANCE_DATA_CM: {
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 scaled_yaw = 0;
// The sensor scans from 0 to -160, so extract negative angle from int16 and represent as if a float
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}"
scaled_yaw = raw_yaw * SF45_SCALE_FACTOR;
float scaled_yaw = raw_yaw * SF45_SCALE_FACTOR;
switch (_yaw_cfg) {
case ROTATION_FORWARD_FACING:
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;
}
// Adjust for sensor orientation
scaled_yaw = sf45_wrap_360(scaled_yaw + _obstacle_distance.angle_offset);
// Convert to meters for the debug message
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);
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);
if (_vehicle_attitude_sub.updated()) {
@ -726,7 +707,7 @@ uint8_t SF45LaserSerial::sf45_convert_angle(const int16_t yaw)
{
uint8_t mapped_sector = 0;
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;
}