fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-29 12:23:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

fix to orientation offsets for scaled yaw, removed unused param

Browse Source 
Signed-off-by: dirksavage88 <dirksavage88@gmail.com>
This commit is contained in:
dirksavage88
2024-04-24 15:58:12 -04:00
committed by Daniel Agar
parent 65c7e034dc
commit 0f6f4c5b07
1 changed files with 7 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp
+7 -8
View File
@@ -157,7 +157,6 @@ int SF45LaserSerial::collect()
int64_t read_elapsed = hrt_elapsed_time(&_last_read); int64_t read_elapsed = hrt_elapsed_time(&_last_read);
int ret; int ret;
/* the buffer for read chars is buflen minus null termination */ /* the buffer for read chars is buflen minus null termination */
param_get(param_find("SF45_CP_LIMIT"), &_collision_constraint);
uint8_t readbuf[SF45_MAX_PAYLOAD]; uint8_t readbuf[SF45_MAX_PAYLOAD];
float distance_m = -1.0f; float distance_m = -1.0f;
@@ -664,6 +663,8 @@ void SF45LaserSerial::sf45_process_replies(float *distance_m)
} }
scaled_yaw = raw_yaw * SF45_SCALE_FACTOR;
// The sensor is facing downward, so the sensor is flipped about it's x-axis -inverse of each yaw angle // The sensor is facing downward, so the sensor is flipped about it's x-axis -inverse of each yaw angle
if (_orient_cfg == 1) { if (_orient_cfg == 1) {
raw_yaw = raw_yaw * -1; raw_yaw = raw_yaw * -1;
@@ -674,29 +675,27 @@ void SF45LaserSerial::sf45_process_replies(float *distance_m)
break; break;
case 1: case 1:
if (raw_yaw > 180) { if (scaled_yaw > 180) {
raw_yaw = raw_yaw - 180; scaled_yaw = scaled_yaw - 180;
} else { } else {
raw_yaw = raw_yaw + 180; // rotation facing aft scaled_yaw = scaled_yaw + 180; // rotation facing aft
} }
break; break;
case 2: case 2:
raw_yaw = raw_yaw + 90; // rotation facing right scaled_yaw = scaled_yaw + 90; // rotation facing right
break; break;
case 3: case 3:
raw_yaw = raw_yaw - 90; // rotation facing left scaled_yaw = scaled_yaw - 90; // rotation facing left
break; break;
default: default:
break; break;
} }
scaled_yaw = raw_yaw * SF45_SCALE_FACTOR;
// Convert to meters for rangefinder update // Convert to meters for rangefinder update
*distance_m = raw_distance * SF45_SCALE_FACTOR; *distance_m = raw_distance * SF45_SCALE_FACTOR;
obstacle_dist_cm = (uint16_t)raw_distance; obstacle_dist_cm = (uint16_t)raw_distance;