mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-31 03:11:35 +08:00
refactor(lightware_laser): simplify device_information handling
- publish every 3 seconds - adjust to name instead of vendor and model - buffer size adjusted according to datasheet - switch to PublicationData
This commit is contained in:
@@ -54,10 +54,10 @@
|
|||||||
#include <drivers/rangefinder/PX4Rangefinder.hpp>
|
#include <drivers/rangefinder/PX4Rangefinder.hpp>
|
||||||
#include <uORB/Subscription.hpp>
|
#include <uORB/Subscription.hpp>
|
||||||
#include <uORB/Publication.hpp>
|
#include <uORB/Publication.hpp>
|
||||||
|
#include <uORB/topics/device_information.h>
|
||||||
#include <uORB/topics/vehicle_status.h>
|
#include <uORB/topics/vehicle_status.h>
|
||||||
#include <uORB/topics/parameter_update.h>
|
#include <uORB/topics/parameter_update.h>
|
||||||
#include <uORB/topics/distance_sensor_mode_change_request.h>
|
#include <uORB/topics/distance_sensor_mode_change_request.h>
|
||||||
#include <uORB/topics/device_information.h>
|
|
||||||
|
|
||||||
using namespace time_literals;
|
using namespace time_literals;
|
||||||
|
|
||||||
@@ -154,17 +154,14 @@ private:
|
|||||||
|
|
||||||
int updateRestriction();
|
int updateRestriction();
|
||||||
|
|
||||||
void readDeviceInformation();
|
|
||||||
void publishDeviceInformation();
|
|
||||||
|
|
||||||
PX4Rangefinder _px4_rangefinder;
|
PX4Rangefinder _px4_rangefinder;
|
||||||
uORB::Publication<device_information_s> _device_info_pub{ORB_ID(device_information)};
|
|
||||||
|
void readDeviceInformation();
|
||||||
|
void updateDeviceInformation();
|
||||||
|
uORB::PublicationData<device_information_s> _device_info_pub{ORB_ID(device_information)};
|
||||||
|
|
||||||
int _conversion_interval{-1};
|
int _conversion_interval{-1};
|
||||||
|
|
||||||
device_information_s _device_info{};
|
|
||||||
bool _device_info_valid{false};
|
|
||||||
|
|
||||||
perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")};
|
perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")};
|
||||||
perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": com err")};
|
perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": com err")};
|
||||||
|
|
||||||
@@ -185,7 +182,6 @@ private:
|
|||||||
bool _restriction{false};
|
bool _restriction{false};
|
||||||
bool _auto_restriction{false};
|
bool _auto_restriction{false};
|
||||||
bool _prev_restriction{false};
|
bool _prev_restriction{false};
|
||||||
hrt_abstime _last_device_info_pub{0};
|
|
||||||
};
|
};
|
||||||
|
|
||||||
LightwareLaser::LightwareLaser(const I2CSPIDriverConfig &config) :
|
LightwareLaser::LightwareLaser(const I2CSPIDriverConfig &config) :
|
||||||
@@ -428,59 +424,34 @@ int LightwareLaser::configure()
|
|||||||
|
|
||||||
void LightwareLaser::readDeviceInformation()
|
void LightwareLaser::readDeviceInformation()
|
||||||
{
|
{
|
||||||
_device_info = {};
|
_device_info_pub.get().device_type = device_information_s::DEVICE_TYPE_RANGEFINDER;
|
||||||
_device_info.device_type = device_information_s::DEVICE_TYPE_RANGEFINDER;
|
_device_info_pub.get().device_id = get_device_id();
|
||||||
_device_info.device_id = get_device_id();
|
|
||||||
|
|
||||||
strlcpy(_device_info.vendor_name, "LightWare", sizeof(_device_info.vendor_name));
|
char string_buffer[17] {};
|
||||||
|
readRegister(Register::ProductName, (uint8_t *)string_buffer, sizeof(string_buffer) - 1);
|
||||||
uint8_t buffer[32] = {};
|
snprintf(_device_info_pub.get().name, sizeof(_device_info_pub.get().name), "%s %s", "LightWare", string_buffer);
|
||||||
|
|
||||||
if (readRegister(Register::ProductName, buffer, sizeof(buffer) - 1) == PX4_OK) {
|
|
||||||
strlcpy(_device_info.model_name, (char *)buffer, sizeof(_device_info.model_name));
|
|
||||||
|
|
||||||
} else {
|
|
||||||
|
|
||||||
strlcpy(_device_info.model_name, "", sizeof(_device_info.model_name));
|
|
||||||
}
|
|
||||||
|
|
||||||
uint32_t hw_version = 0;
|
uint32_t hw_version = 0;
|
||||||
|
readRegister(Register::HardwareVersion, (uint8_t *)&hw_version, sizeof(hw_version));
|
||||||
|
snprintf(_device_info_pub.get().hardware_version, sizeof(_device_info_pub.get().hardware_version), "%" PRIu32, hw_version);
|
||||||
|
|
||||||
if (readRegister(Register::HardwareVersion, (uint8_t *)&hw_version, sizeof(hw_version)) == 0) {
|
uint8_t fw_bytes[4] {}; // Datasheet: 4 bytes but forth one is "Reserved"
|
||||||
snprintf(_device_info.hardware_version, sizeof(_device_info.hardware_version), "%u", (unsigned int)hw_version);
|
readRegister(Register::FirmwareVersion, fw_bytes, sizeof(fw_bytes));
|
||||||
|
snprintf(_device_info_pub.get().firmware_version, sizeof(_device_info_pub.get().firmware_version), "%u.%u.%u", fw_bytes[2], fw_bytes[1],
|
||||||
|
fw_bytes[0]);
|
||||||
|
|
||||||
} else {
|
readRegister(Register::SerialNumber, (uint8_t *)string_buffer, sizeof(string_buffer) - 1);
|
||||||
strlcpy(_device_info.hardware_version, "", sizeof(_device_info.hardware_version));
|
strlcpy(_device_info_pub.get().serial_number, string_buffer, sizeof(_device_info_pub.get().serial_number));
|
||||||
}
|
|
||||||
|
|
||||||
uint8_t fw_bytes[4] = {};
|
|
||||||
|
|
||||||
if (readRegister(Register::FirmwareVersion, fw_bytes, sizeof(fw_bytes)) == 0) {
|
|
||||||
snprintf(_device_info.firmware_version, sizeof(_device_info.firmware_version), "%u.%u.%u",
|
|
||||||
fw_bytes[2], fw_bytes[1], fw_bytes[0]);
|
|
||||||
|
|
||||||
} else {
|
|
||||||
strlcpy(_device_info.firmware_version, "", sizeof(_device_info.firmware_version));
|
|
||||||
}
|
|
||||||
|
|
||||||
if (readRegister(Register::SerialNumber, buffer, sizeof(buffer) - 1) == 0) {
|
|
||||||
strlcpy(_device_info.serial_number, (char *)buffer, sizeof(_device_info.serial_number));
|
|
||||||
|
|
||||||
} else {
|
|
||||||
strlcpy(_device_info.serial_number, "", sizeof(_device_info.serial_number));
|
|
||||||
}
|
|
||||||
|
|
||||||
_device_info_valid = true;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void LightwareLaser::publishDeviceInformation()
|
void LightwareLaser::updateDeviceInformation()
|
||||||
{
|
{
|
||||||
if (!_device_info_valid) {
|
const hrt_abstime now = hrt_absolute_time();
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
_device_info.timestamp = hrt_absolute_time();
|
if (_device_info_pub.get().name[0] != '\0' && now >= _device_info_pub.get().timestamp + 3_s) {
|
||||||
_device_info_pub.publish(_device_info);
|
_device_info_pub.get().timestamp = now;
|
||||||
|
_device_info_pub.update();
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
int LightwareLaser::collect()
|
int LightwareLaser::collect()
|
||||||
@@ -657,13 +628,7 @@ void LightwareLaser::RunImpl()
|
|||||||
perf_count(_comms_errors);
|
perf_count(_comms_errors);
|
||||||
}
|
}
|
||||||
|
|
||||||
// Publish device information (1 Hz)
|
updateDeviceInformation();
|
||||||
const hrt_abstime now = hrt_absolute_time();
|
|
||||||
|
|
||||||
if (now - _last_device_info_pub >= 1_s) {
|
|
||||||
publishDeviceInformation();
|
|
||||||
_last_device_info_pub = now;
|
|
||||||
}
|
|
||||||
|
|
||||||
switch (_state) {
|
switch (_state) {
|
||||||
case State::Configuring: {
|
case State::Configuring: {
|
||||||
|
|||||||
Reference in New Issue
Block a user