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

Merge pull request #721 from PX4/laser_rangefinder

Browse Source 
Laser rangefinder
This commit is contained in:
Lorenz Meier
2014-03-12 21:54:34 +01:00
parent 9153eee235 4840d5fbcb
commit 82e2619f1f
5 changed files with 1172 additions and 104 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sdlog2/sdlog2.c
+14
View File
@@ -58,6 +58,8 @@
#include <drivers/drv_hrt.h>
#include <math.h>
#include <drivers/drv_range_finder.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/sensor_combined.h>
@@ -791,6 +793,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct vehicle_global_velocity_setpoint_s global_vel_sp;
struct battery_status_s battery;
struct telemetry_status_s telemetry;
struct range_finder_report range_finder;
} buf;
memset(&buf, 0, sizeof(buf));
@@ -851,6 +854,7 @@ int sdlog2_thread_main(int argc, char *argv[])
int global_vel_sp_sub;
int battery_sub;
int telemetry_sub;
int range_finder_sub;
} subs;
subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
@@ -874,6 +878,7 @@ int sdlog2_thread_main(int argc, char *argv[])
subs.global_vel_sp_sub = orb_subscribe(ORB_ID(vehicle_global_velocity_setpoint));
subs.battery_sub = orb_subscribe(ORB_ID(battery_status));
subs.telemetry_sub = orb_subscribe(ORB_ID(telemetry_status));
subs.range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder));
thread_running = true;
@@ -1227,6 +1232,15 @@ int sdlog2_thread_main(int argc, char *argv[])
LOGBUFFER_WRITE_AND_COUNT(TELE);
}
/* --- BOTTOM DISTANCE --- */
if (copy_if_updated(ORB_ID(sensor_range_finder), subs.range_finder_sub, &buf.range_finder)) {
log_msg.msg_type = LOG_DIST_MSG;
log_msg.body.log_DIST.bottom = buf.range_finder.distance;
log_msg.body.log_DIST.bottom_rate = 0.0f;
log_msg.body.log_DIST.flags = (buf.range_finder.valid ? 1 : 0);
LOGBUFFER_WRITE_AND_COUNT(DIST);
}
/* signal the other thread new data, but not yet unlock */
if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) {
/* only request write if several packets can be written at once */