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

Use Rangefinder

Browse Source
This commit is contained in:
Claudio Chies
2024-12-05 14:56:34 +01:00
parent c8b9fb0d58
commit b7a8b64a6e
1 changed files with 11 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp
+11 -7
View File
@@ -645,17 +645,21 @@ void SF45LaserSerial::sf45_process_replies(float &distance_m)
break;
}
// Convert to meters for rangefinder update
distance_m = raw_distance * SF45_SCALE_FACTOR;
Quatf quaternion(Eulerf{0, 0, sf45_wrap_360(scaled_yaw)*M_DEG_TO_RAD_F});
float q[4];
if (raw_distance < 65436) { // Discard invalid readings
// Convert to meters for rangefinder update
distance_m = raw_distance * SF45_SCALE_FACTOR;
for (int i = 0; i < 4; i++) {
q[i] = quaternion(i);
Quatf quaternion(Eulerf{0, 0, sf45_wrap_360(scaled_yaw)*M_DEG_TO_RAD_F});
float q[4];
for (int i = 0; i < 4; i++) {
q[i] = quaternion(i);
}
_px4_rangefinder.update(hrt_absolute_time(), distance_m, -1, q);
}
_px4_rangefinder.update(hrt_absolute_time(), distance_m, -1, q);
break;
}