Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-25 00:31:36 +08:00
Code Issues Actions 7 Packages Projects Releases Wiki Activity

mavlink: DISTANCE_SENSOR populate all fields

Browse Source
This commit is contained in:
Daniel Agar
2022-04-28 17:36:22 -04:00
parent 9e0a8050a9
commit 46dba854b7
19 changed files with 107 additions and 52 deletions
Download Patch File Download Diff File Expand all files Collapse all files
msg/distance_sensor.msg
+5 -4
View File
@@ -11,10 +11,11 @@ float32 variance # Measurement variance (in m^2), 0 for unknown / invalid readi
int8 signal_quality # Signal quality in percent (0...100%), where 0 = invalid signal, 100 = perfect signal, and -1 = unknown signal quality.
uint8 type # Type from MAV_DISTANCE_SENSOR enum
uint8 MAV_DISTANCE_SENSOR_LASER = 0
uint8 MAV_DISTANCE_SENSOR_ULTRASOUND = 1
uint8 MAV_DISTANCE_SENSOR_INFRARED = 2
uint8 MAV_DISTANCE_SENSOR_RADAR = 3
uint8 TYPE_UNKNOWN = 0
uint8 TYPE_LASER = 1
uint8 TYPE_ULTRASOUND = 2
uint8 TYPE_INFRARED = 3
uint8 TYPE_RADAR = 4
float32 h_fov # Sensor horizontal field of view (rad)
float32 v_fov # Sensor vertical field of view (rad)
src/drivers/distance_sensor/cm8jl65/CM8JL65.cpp
+1 -1
View File
@@ -113,7 +113,7 @@ CM8JL65::CM8JL65(const char *port, uint8_t rotation) :
_px4_rangefinder.set_max_distance(7.9f); // Datasheet: 8.0m
_px4_rangefinder.set_fov(0.0488692f);
_px4_rangefinder.set_device_type(DRV_DIST_DEVTYPE_CM8JL65);
_px4_rangefinder.set_rangefinder_type(distance_sensor_s::MAV_DISTANCE_SENSOR_LASER);
_px4_rangefinder.set_rangefinder_type(distance_sensor_s::TYPE_LASER);
}
CM8JL65::~CM8JL65()
src/drivers/distance_sensor/mappydot/MappyDot.cpp
+1 -1
View File
@@ -272,7 +272,7 @@ MappyDot::collect()
report.orientation = _sensor_rotations[index];
report.signal_quality = -1;
report.timestamp = hrt_absolute_time();
report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
report.type = distance_sensor_s::TYPE_LASER;
report.variance = 0;
int instance_id;
src/drivers/distance_sensor/mb12xx/mb12xx.cpp
+1 -1
View File
@@ -227,7 +227,7 @@ MB12XX::collect()
report.orientation = _sensor_rotations[_sensor_index];
report.signal_quality = -1;
report.timestamp = hrt_absolute_time();
report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND;
report.type = distance_sensor_s::TYPE_ULTRASOUND;
report.variance = 0.0f;
int instance_id;
src/drivers/distance_sensor/pga460/pga460.cpp
+1 -1
View File
@@ -773,7 +773,7 @@ void PGA460::uORB_publish_results(const float object_distance)
struct distance_sensor_s report = {};
report.timestamp = hrt_absolute_time();
report.device_id = _device_id.devid;
report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND;
report.type = distance_sensor_s::TYPE_ULTRASOUND;
report.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
report.current_distance = object_distance;
report.min_distance = MIN_DETECTABLE_DISTANCE;
src/drivers/distance_sensor/srf02/SRF02.cpp
+1 -1
View File
@@ -39,7 +39,7 @@ SRF02::SRF02(const I2CSPIDriverConfig &config) :
_px4_rangefinder(get_device_id(), config.rotation)
{
_px4_rangefinder.set_device_type(DRV_DIST_DEVTYPE_SRF02);
_px4_rangefinder.set_rangefinder_type(distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND);
_px4_rangefinder.set_rangefinder_type(distance_sensor_s::TYPE_ULTRASOUND);
_px4_rangefinder.set_max_distance(SRF02_MAX_DISTANCE);
_px4_rangefinder.set_min_distance(SRF02_MIN_DISTANCE);
}
src/drivers/distance_sensor/srf05/SRF05.cpp
+1 -1
View File
@@ -51,7 +51,7 @@ SRF05::SRF05(const uint8_t rotation) :
_px4_rangefinder(0 /* no device type for GPIO input */, rotation)
{
_px4_rangefinder.set_device_type(DRV_DIST_DEVTYPE_SRF05);
_px4_rangefinder.set_rangefinder_type(distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND);
_px4_rangefinder.set_rangefinder_type(distance_sensor_s::TYPE_ULTRASOUND);
_px4_rangefinder.set_min_distance(HXSRX0X_MIN_DISTANCE);
_px4_rangefinder.set_max_distance(HXSRX0X_MAX_DISTANCE);
_px4_rangefinder.set_fov(0.261799); // 15 degree FOV
src/drivers/distance_sensor/teraranger/TERARANGER.cpp
+1 -1
View File
@@ -79,7 +79,7 @@ TERARANGER::TERARANGER(const I2CSPIDriverConfig &config) :
_px4_rangefinder(get_device_id(), config.rotation)
{
_px4_rangefinder.set_device_type(DRV_DIST_DEVTYPE_TERARANGER);
_px4_rangefinder.set_rangefinder_type(distance_sensor_s::MAV_DISTANCE_SENSOR_LASER);
_px4_rangefinder.set_rangefinder_type(distance_sensor_s::TYPE_LASER);
}
TERARANGER::~TERARANGER()
src/drivers/distance_sensor/tfmini/TFMINI.cpp
+1 -1
View File
@@ -57,7 +57,7 @@ TFMINI::TFMINI(const char *port, uint8_t rotation) :
}
_px4_rangefinder.set_device_id(device_id.devid);
_px4_rangefinder.set_rangefinder_type(distance_sensor_s::MAV_DISTANCE_SENSOR_LASER);
_px4_rangefinder.set_rangefinder_type(distance_sensor_s::TYPE_LASER);
}
TFMINI::~TFMINI()
src/drivers/distance_sensor/ulanding_radar/AerotennaULanding.cpp
+1 -1
View File
@@ -56,7 +56,7 @@ AerotennaULanding::AerotennaULanding(const char *port, uint8_t rotation) :
_px4_rangefinder.set_device_id(device_id.devid);
_px4_rangefinder.set_device_type(DRV_DIST_DEVTYPE_ULANDING);
_px4_rangefinder.set_rangefinder_type(distance_sensor_s::MAV_DISTANCE_SENSOR_RADAR);
_px4_rangefinder.set_rangefinder_type(distance_sensor_s::TYPE_RADAR);
_px4_rangefinder.set_min_distance(ULANDING_MIN_DISTANCE);
_px4_rangefinder.set_max_distance(ULANDING_MAX_DISTANCE);
src/drivers/optical_flow/px4flow/px4flow.cpp
+1 -1
View File
@@ -319,7 +319,7 @@ PX4FLOW::collect()
distance_report.current_distance = report.ground_distance_m;
distance_report.variance = 0.0f;
distance_report.signal_quality = -1;
distance_report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND;
distance_report.type = distance_sensor_s::TYPE_ULTRASOUND;
distance_report.device_id = device_id.devid;
distance_report.orientation = _sonar_rotation;
src/drivers/uavcan/sensors/rangefinder.cpp
+3 -3
View File
@@ -86,17 +86,17 @@ void UavcanRangefinderBridge::range_sub_cb(const
switch (msg.sensor_type) {
case uavcan::equipment::range_sensor::Measurement::SENSOR_TYPE_SONAR:
rangefinder_type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND;
rangefinder_type = distance_sensor_s::TYPE_ULTRASOUND;
break;
case uavcan::equipment::range_sensor::Measurement::SENSOR_TYPE_RADAR:
rangefinder_type = distance_sensor_s::MAV_DISTANCE_SENSOR_RADAR;
rangefinder_type = distance_sensor_s::TYPE_RADAR;
break;
case uavcan::equipment::range_sensor::Measurement::SENSOR_TYPE_LIDAR:
case uavcan::equipment::range_sensor::Measurement::SENSOR_TYPE_UNDEFINED:
default:
rangefinder_type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
rangefinder_type = distance_sensor_s::TYPE_LASER;
break;
}
src/drivers/uavcannode/Publishers/RangeSensorMeasurement.hpp
+4 -4
View File
@@ -81,19 +81,19 @@ public:
// sensor type
switch (dist.type) {
case distance_sensor_s::MAV_DISTANCE_SENSOR_LASER:
case distance_sensor_s::TYPE_LASER:
range_sensor.sensor_type = uavcan::equipment::range_sensor::Measurement::SENSOR_TYPE_LIDAR;
break;
case distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND:
case distance_sensor_s::TYPE_ULTRASOUND:
range_sensor.sensor_type = uavcan::equipment::range_sensor::Measurement::SENSOR_TYPE_SONAR;
break;
case distance_sensor_s::MAV_DISTANCE_SENSOR_RADAR:
case distance_sensor_s::TYPE_RADAR:
range_sensor.sensor_type = uavcan::equipment::range_sensor::Measurement::SENSOR_TYPE_RADAR;
break;
case distance_sensor_s::MAV_DISTANCE_SENSOR_INFRARED:
case distance_sensor_s::TYPE_INFRARED:
default:
range_sensor.sensor_type = uavcan::equipment::range_sensor::Measurement::SENSOR_TYPE_UNDEFINED;
break;
src/lib/collision_prevention/CollisionPreventionTest.cpp
+1 -1
View File
@@ -220,7 +220,7 @@ TEST_F(CollisionPreventionTest, testBehaviorOnWithDistanceMessage)
message.variance = 0.1f;
message.signal_quality = 100;
message.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
message.type = distance_sensor_s::TYPE_LASER;
message.orientation = distance_sensor_s::ROTATION_FORWARD_FACING;
message.h_fov = math::radians(50.f);
message.v_fov = math::radians(30.f);
src/lib/drivers/rangefinder/PX4Rangefinder.cpp
+1 -1
View File
@@ -39,7 +39,7 @@ PX4Rangefinder::PX4Rangefinder(const uint32_t device_id, const uint8_t device_or
{
set_device_id(device_id);
set_orientation(device_orientation);
set_rangefinder_type(distance_sensor_s::MAV_DISTANCE_SENSOR_LASER); // Default to type LASER
set_rangefinder_type(distance_sensor_s::TYPE_LASER); // Default to type LASER
}
PX4Rangefinder::~PX4Rangefinder()
src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
+2 -2
View File
@@ -206,13 +206,13 @@ void BlockLocalPositionEstimator::Run()
if (s->get().timestamp == 0) { continue; }
if (s->get().type == distance_sensor_s::MAV_DISTANCE_SENSOR_LASER &&
if (s->get().type == distance_sensor_s::TYPE_LASER &&
s->get().orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING &&
_sub_lidar == nullptr) {
_sub_lidar = s;
mavlink_log_info(&mavlink_log_pub, "%sDownward-facing Lidar detected with ID %zu", msg_label, i);
} else if (s->get().type == distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND &&
} else if (s->get().type == distance_sensor_s::TYPE_ULTRASOUND &&
s->get().orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING &&
_sub_sonar == nullptr) {
_sub_sonar = s;
src/modules/mavlink/mavlink_receiver.cpp
+39 -9
View File
@@ -841,7 +841,7 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg)
d.min_distance = _param_ekf2_min_rng;
d.max_distance = _param_ekf2_rng_a_hmax;
d.current_distance = flow.distance; /* both are in m */
d.type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND;
d.type = distance_sensor_s::TYPE_ULTRASOUND;
d.device_id = device_id.devid;
d.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
d.variance = 0.01;
@@ -887,7 +887,7 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg)
d.min_distance = 0.3f;
d.max_distance = 5.0f;
d.current_distance = flow.distance; /* both are in m */
d.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
d.type = distance_sensor_s::TYPE_LASER;
d.device_id = device_id.devid;
d.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
d.variance = 0.01;
@@ -939,25 +939,55 @@ MavlinkReceiver::handle_message_distance_sensor(mavlink_message_t *msg)
device_id.devid_s.address = dist_sensor.id;
ds.timestamp = hrt_absolute_time(); /* Use system time for now, don't trust sender to attach correct timestamp */
ds.device_id = device_id.devid;
ds.min_distance = static_cast<float>(dist_sensor.min_distance) * 1e-2f; /* cm to m */
ds.max_distance = static_cast<float>(dist_sensor.max_distance) * 1e-2f; /* cm to m */
ds.current_distance = static_cast<float>(dist_sensor.current_distance) * 1e-2f; /* cm to m */
ds.variance = dist_sensor.covariance * 1e-4f; /* cm^2 to m^2 */
if (dist_sensor.signal_quality <= 0) {
// Mavlink DISTANCE_SENSOR: 0 = unknown/unset signal quality
ds.signal_quality = -1;
} else if (dist_sensor.signal_quality == 1) {
// Mavlink DISTANCE_SENSOR: 1 = invalid signal
ds.signal_quality = 0;
} else {
ds.signal_quality = math::constrain(dist_sensor.signal_quality, (uint8_t)1, (uint8_t)100);
}
switch (dist_sensor.type) {
case MAV_DISTANCE_SENSOR_LASER:
ds.type = distance_sensor_s::TYPE_LASER;
break;
case MAV_DISTANCE_SENSOR_ULTRASOUND:
ds.type = distance_sensor_s::TYPE_ULTRASOUND;
break;
case MAV_DISTANCE_SENSOR_INFRARED:
ds.type = distance_sensor_s::TYPE_INFRARED;
break;
case MAV_DISTANCE_SENSOR_RADAR:
ds.type = distance_sensor_s::TYPE_RADAR;
break;
case MAV_DISTANCE_SENSOR_UNKNOWN:
default:
ds.type = distance_sensor_s::TYPE_UNKNOWN;
break;
}
ds.h_fov = dist_sensor.horizontal_fov;
ds.v_fov = dist_sensor.vertical_fov;
ds.q[0] = dist_sensor.quaternion[0];
ds.q[1] = dist_sensor.quaternion[1];
ds.q[2] = dist_sensor.quaternion[2];
ds.q[3] = dist_sensor.quaternion[3];
ds.type = dist_sensor.type;
ds.device_id = device_id.devid;
ds.orientation = dist_sensor.orientation;
// MAVLink DISTANCE_SENSOR signal_quality value of 0 means unset/unknown
// quality value. Also it comes normalised between 1 and 100 while the uORB
// signal quality is normalised between 0 and 100.
ds.signal_quality = dist_sensor.signal_quality == 0 ? -1 : 100 * (dist_sensor.signal_quality - 1) / 99;
_distance_sensor_pub.publish(ds);
}
src/modules/mavlink/streams/DISTANCE_SENSOR.hpp
+41 -17
View File
@@ -63,37 +63,61 @@ private:
bool updated = false;
for (int i = 0; i < _distance_sensor_subs.size(); i++) {
distance_sensor_s dist_sensor;
distance_sensor_s distance_sensor;
if (_distance_sensor_subs[i].update(&dist_sensor)) {
if (_distance_sensor_subs[i].update(&distance_sensor)) {
mavlink_distance_sensor_t msg{};
msg.time_boot_ms = dist_sensor.timestamp / 1000; /* us to ms */
msg.time_boot_ms = distance_sensor.timestamp / 1000; // us -> ms
switch (dist_sensor.type) {
case MAV_DISTANCE_SENSOR_ULTRASOUND:
switch (distance_sensor.type) {
default:
case distance_sensor_s::TYPE_UNKNOWN:
msg.type = MAV_DISTANCE_SENSOR_UNKNOWN;
break;
case distance_sensor_s::TYPE_LASER:
msg.type = MAV_DISTANCE_SENSOR_LASER;
break;
case distance_sensor_s::TYPE_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:
case distance_sensor_s::TYPE_INFRARED:
msg.type = MAV_DISTANCE_SENSOR_INFRARED;
break;
default:
msg.type = MAV_DISTANCE_SENSOR_LASER;
case distance_sensor_s::TYPE_RADAR:
msg.type = MAV_DISTANCE_SENSOR_RADAR;
break;
}
msg.current_distance = dist_sensor.current_distance * 1e2f; // m to cm
msg.min_distance = math::constrain(roundf(distance_sensor.min_distance * 1e2f), 0.f,
static_cast<float>(UINT16_MAX)); // m to cm
msg.max_distance = math::constrain(roundf(distance_sensor.max_distance * 1e2f), 0.f,
static_cast<float>(UINT16_MAX)); // m to cm
msg.current_distance = math::constrain(roundf(distance_sensor.current_distance * 1e2f), 0.f,
static_cast<float>(UINT16_MAX)); // m to cm
msg.id = i;
msg.max_distance = dist_sensor.max_distance * 1e2f; // m to cm
msg.min_distance = dist_sensor.min_distance * 1e2f; // m to cm
msg.orientation = dist_sensor.orientation;
msg.covariance = dist_sensor.variance * 1e4f; // m^2 to cm^2
msg.orientation = distance_sensor.orientation;
msg.covariance = distance_sensor.variance * 1e4f; // m^2 to cm^2
msg.horizontal_fov = distance_sensor.h_fov;
msg.vertical_fov = distance_sensor.v_fov;
msg.quaternion[0] = distance_sensor.q[0];
msg.quaternion[1] = distance_sensor.q[1];
msg.quaternion[2] = distance_sensor.q[2];
msg.quaternion[3] = distance_sensor.q[3];
if (distance_sensor.signal_quality < 0) {
msg.signal_quality = 0; // 0 = unknown/unset
} else if (distance_sensor.signal_quality == 0) {
msg.signal_quality = 1; // 1 = invalid signal
} else if (distance_sensor.signal_quality >= 0) {
msg.signal_quality = math::constrain(distance_sensor.signal_quality, (int8_t)2, (int8_t)100);
}
mavlink_msg_distance_sensor_send_struct(_mavlink->get_channel(), &msg);
src/modules/sih/sih.cpp
+1 -1
View File
@@ -535,7 +535,7 @@ void Sih::send_airspeed()
void Sih::send_dist_snsr()
{
_distance_snsr.timestamp = _now;
_distance_snsr.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
_distance_snsr.type = distance_sensor_s::TYPE_LASER;
_distance_snsr.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
_distance_snsr.min_distance = _distance_snsr_min;
_distance_snsr.max_distance = _distance_snsr_max;