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

Merge pull request #2224 from bansiesta/lidarlite_merge

Browse Source 
Lidarlite merge and bringup
This commit is contained in:
Lorenz Meier
2015-05-24 23:18:52 +02:00
parent 5dfa3d29bd 9f314dd191
commit c5f14ef785
43 changed files with 2222 additions and 1285 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_messages.cpp
+27 -18
View File
@@ -68,9 +68,9 @@
#include <uORB/topics/airspeed.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/navigation_capabilities.h>
#include <uORB/topics/distance_sensor.h>
#include <drivers/drv_rc_input.h>
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_range_finder.h>
#include <systemlib/err.h>
#include <mavlink/mavlink_log.h>
@@ -2178,7 +2178,6 @@ protected:
}
};
class MavlinkStreamDistanceSensor : public MavlinkStream
{
public:
@@ -2204,12 +2203,12 @@ public:
unsigned get_size()
{
return _range_sub->is_published() ? (MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
return _distance_sensor_sub->is_published() ? (MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
}
private:
MavlinkOrbSubscription *_range_sub;
uint64_t _range_time;
MavlinkOrbSubscription *_distance_sensor_sub;
uint64_t _dist_sensor_time;
/* do not allow top copying this class */
MavlinkStreamDistanceSensor(MavlinkStreamDistanceSensor &);
@@ -2217,36 +2216,46 @@ private:
protected:
explicit MavlinkStreamDistanceSensor(Mavlink *mavlink) : MavlinkStream(mavlink),
_range_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_range_finder))),
_range_time(0)
_distance_sensor_sub(_mavlink->add_orb_subscription(ORB_ID(distance_sensor))),
_dist_sensor_time(0)
{}
void send(const hrt_abstime t)
{
struct range_finder_report range;
struct distance_sensor_s dist_sensor;
if (_range_sub->update(&_range_time, &range)) {
if (_distance_sensor_sub->update(&_dist_sensor_time, &dist_sensor)) {
mavlink_distance_sensor_t msg;
msg.time_boot_ms = range.timestamp / 1000;
msg.time_boot_ms = dist_sensor.timestamp / 1000; /* us to ms */
switch (range.type) {
case RANGE_FINDER_TYPE_LASER:
/* TODO: use correct ID here */
msg.id = 0;
switch (dist_sensor.type) {
case MAV_DISTANCE_SENSOR_ULTRASOUND:
msg.type = MAV_DISTANCE_SENSOR_ULTRASOUND;
break;
case MAV_DISTANCE_SENSOR_LASER:
msg.type = MAV_DISTANCE_SENSOR_LASER;
break;
case MAV_DISTANCE_SENSOR_INFRARED:
msg.type = MAV_DISTANCE_SENSOR_INFRARED;
break;
default:
msg.type = MAV_DISTANCE_SENSOR_LASER;
break;
}
msg.id = 0;
msg.orientation = 0;
msg.min_distance = range.minimum_distance * 100;
msg.max_distance = range.maximum_distance * 100;
msg.current_distance = range.distance * 100;
msg.covariance = 20;
msg.orientation = dist_sensor.orientation;
msg.min_distance = dist_sensor.min_distance * 100.0f; /* m to cm */
msg.max_distance = dist_sensor.max_distance * 100.0f; /* m to cm */
msg.current_distance = dist_sensor.current_distance * 100.0f; /* m to cm */
msg.covariance = dist_sensor.covariance;
_mavlink->send_message(MAVLINK_MSG_ID_DISTANCE_SENSOR, &msg);
}
src/modules/mavlink/mavlink_receiver.cpp
+67 -15
View File
@@ -56,7 +56,6 @@
#include <drivers/drv_gyro.h>
#include <drivers/drv_mag.h>
#include <drivers/drv_baro.h>
#include <drivers/drv_range_finder.h>
#include <time.h>
#include <float.h>
#include <unistd.h>
@@ -109,7 +108,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_battery_pub(-1),
_cmd_pub(-1),
_flow_pub(-1),
_range_pub(-1),
_distance_sensor_pub(-1),
_offboard_control_mode_pub(-1),
_actuator_controls_pub(-1),
_global_vel_sp_pub(-1),
@@ -211,6 +210,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
handle_message_timesync(msg);
break;
case MAVLINK_MSG_ID_DISTANCE_SENSOR:
handle_message_distance_sensor(msg);
break;
default:
break;
}
@@ -412,6 +415,25 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg)
} else {
orb_publish(ORB_ID(optical_flow), _flow_pub, &f);
}
/* Use distance value for distance sensor topic */
struct distance_sensor_s d;
memset(&d, 0, sizeof(d));
d.timestamp = flow.integration_time_us * 1000; /* ms to us */
d.min_distance = 0.3f;
d.max_distance = 5.0f;
d.current_distance = flow.distance; /* both are in m */
d.type = 1;
d.id = 0;
d.orientation = 8;
d.covariance = 0.0;
if (_distance_sensor_pub < 0) {
_distance_sensor_pub = orb_advertise(ORB_ID(distance_sensor), &d);
} else {
orb_publish(ORB_ID(distance_sensor), _distance_sensor_pub, &d);
}
}
void
@@ -444,22 +466,23 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg)
orb_publish(ORB_ID(optical_flow), _flow_pub, &f);
}
/* Use distance value for range finder report */
struct range_finder_report r;
memset(&r, 0, sizeof(r));
/* Use distance value for distance sensor topic */
struct distance_sensor_s d;
memset(&d, 0, sizeof(d));
r.timestamp = hrt_absolute_time();
r.error_count = 0;
r.type = RANGE_FINDER_TYPE_LASER;
r.distance = flow.distance;
r.minimum_distance = 0.0f;
r.maximum_distance = 40.0f; // this is set to match the typical range of real sensors, could be made configurable
r.valid = (r.distance > r.minimum_distance) && (r.distance < r.maximum_distance);
d.timestamp = hrt_absolute_time();
d.min_distance = 0.3f;
d.max_distance = 5.0f;
d.current_distance = flow.distance; /* both are in m */
d.type = 1;
d.id = 0;
d.orientation = 8;
d.covariance = 0.0;
if (_range_pub < 0) {
_range_pub = orb_advertise(ORB_ID(sensor_range_finder), &r);
if (_distance_sensor_pub < 0) {
_distance_sensor_pub = orb_advertise(ORB_ID(distance_sensor), &d);
} else {
orb_publish(ORB_ID(sensor_range_finder), _range_pub, &r);
orb_publish(ORB_ID(distance_sensor), _distance_sensor_pub, &d);
}
}
@@ -497,6 +520,35 @@ MavlinkReceiver::handle_message_set_mode(mavlink_message_t *msg)
}
}
void
MavlinkReceiver::handle_message_distance_sensor(mavlink_message_t *msg)
{
/* distance sensor */
mavlink_distance_sensor_t dist_sensor;
mavlink_msg_distance_sensor_decode(msg, &dist_sensor);
struct distance_sensor_s d;
memset(&d, 0, sizeof(d));
d.timestamp = dist_sensor.time_boot_ms * 1000; /* ms to us */
d.min_distance = float(dist_sensor.min_distance) * 1e-2f; /* cm to m */
d.max_distance = float(dist_sensor.max_distance) * 1e-2f; /* cm to m */
d.current_distance = float(dist_sensor.current_distance) * 1e-2f; /* cm to m */
d.type = dist_sensor.type;
d.id = dist_sensor.id;
d.orientation = dist_sensor.orientation;
d.covariance = dist_sensor.covariance;
/// TODO Add sensor rotation according to MAV_SENSOR_ORIENTATION enum
if (_distance_sensor_pub < 0) {
_distance_sensor_pub = orb_advertise(ORB_ID(distance_sensor), &d);
} else {
orb_publish(ORB_ID(distance_sensor), _distance_sensor_pub, &d);
}
}
void
MavlinkReceiver::handle_message_vicon_position_estimate(mavlink_message_t *msg)
{
src/modules/mavlink/mavlink_receiver.h
+3 -1
View File
@@ -73,6 +73,7 @@
#include <uORB/topics/battery_status.h>
#include <uORB/topics/vehicle_force_setpoint.h>
#include <uORB/topics/time_offset.h>
#include <uORB/topics/distance_sensor.h>
#include "mavlink_ftp.h"
@@ -134,6 +135,7 @@ private:
void handle_message_hil_sensor(mavlink_message_t *msg);
void handle_message_hil_gps(mavlink_message_t *msg);
void handle_message_hil_state_quaternion(mavlink_message_t *msg);
void handle_message_distance_sensor(mavlink_message_t *msg);
void *receive_thread(void *arg);
@@ -164,7 +166,7 @@ private:
orb_advert_t _battery_pub;
orb_advert_t _cmd_pub;
orb_advert_t _flow_pub;
orb_advert_t _range_pub;
orb_advert_t _distance_sensor_pub;
orb_advert_t _offboard_control_mode_pub;
orb_advert_t _actuator_controls_pub;
orb_advert_t _global_vel_sp_pub;