fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

lightware_laser: add option to listen to system to enable/disable distance sensor

Browse Source
This commit is contained in:
Konrad 2024-09-09 10:35:35 +02:00 committed by KonradRudin
parent 9ca0630376
commit e6f07bde2a
4 changed files with 22 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
msg/CMakeLists.txt
View File

@ -73,6 +73,7 @@ set(msg_files
DebugVect.msg
DifferentialPressure.msg
DistanceSensor.msg
DistanceSensorModeChangeRequest.msg
Ekf2Timestamps.msg
EscReport.msg
EscStatus.msg

5
msg/DistanceSensorModeChangeRequest.msg Normal file
View File

@ -0,0 +1,5 @@
uint64 timestamp # time since system start (microseconds)
uint8 req_mode # requested mode of operation
uint8 MODE_DISABLED = 0
uint8 MODE_ENABLED = 1

16
src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp
View File

@ -55,6 +55,7 @@
#include <uORB/Subscription.hpp>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/distance_sensor_mode_change_request.h>
using namespace time_literals;
@ -143,6 +144,8 @@ private:
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
typeof(px4::msg::VehicleStatus::vehicle_type) _vehicle_type{px4::msg::VehicleStatus::VEHICLE_TYPE_UNKNOWN};
uORB::Subscription _dist_sense_mode_change_sub{ORB_ID(distance_sensor_mode_change_request)};
typeof(px4::msg::DistanceSensorModeChangeRequest::req_mode) _req_mode{px4::msg::DistanceSensorModeChangeRequest::MODE_DISABLED};
bool _restriction{false};
bool _auto_restriction{false};
bool _prev_restriction{false};
@ -412,6 +415,17 @@ void LightwareLaser::start()
int LightwareLaser::updateRestriction()
{
if (_dist_sense_mode_change_sub.updated()) {
distance_sensor_mode_change_request_s dist_sense_mode_change;
if (_dist_sense_mode_change_sub.copy(&dist_sense_mode_change)) {
_req_mode = dist_sense_mode_change.req_mode;
} else {
_req_mode = distance_sensor_mode_change_request_s::MODE_DISABLED;
}
}
px4::msg::VehicleStatus vehicle_status;
if (_vehicle_status_sub.update(&vehicle_status)) {
@ -452,7 +466,7 @@ int LightwareLaser::updateRestriction()
break;
case 2:
_restriction = _auto_restriction;
_restriction = _auto_restriction && _req_mode != distance_sensor_mode_change_request_s::MODE_ENABLED;
break;
}

2
src/drivers/distance_sensor/lightware_laser_i2c/parameters.c
View File

@ -54,7 +54,7 @@ PARAM_DEFINE_INT32(SENS_EN_SF1XX, 0);
*
* @value 0 Disabled
* @value 1 Enabled
* @value 2 Disabled during VTOL fast forward flight
* @value 2 Enabled in VTOL MC mode, listen to request from system in FW mode
*
* @min 0
* @max 2