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

rangefinder update quaternion

Browse Source 
Signed-off-by: dirksavage88 <dirksavage88@gmail.com>
This commit is contained in:
dirksavage88 2025-12-04 10:34:40 -05:00 committed by Matthias Grob
parent e822da74b2
commit eb1c0322e3
2 changed files with 26 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
src/lib/drivers/rangefinder/PX4Rangefinder.cpp
View File

@ -35,6 +35,8 @@
#include <lib/drivers/device/Device.hpp>
using namespace time_literals;
PX4Rangefinder::PX4Rangefinder(const uint32_t device_id, const uint8_t device_orientation)
{
set_device_id(device_id);
@ -66,6 +68,14 @@ void PX4Rangefinder::set_orientation(const uint8_t device_orientation)
_distance_sensor_pub.get().orientation = device_orientation;
}
void PX4Rangefinder::update_quaternion(const float (&quat)[4])
{
memcpy(_quat, quat, sizeof(float) * 4);
_q_update_now = hrt_absolute_time();
_quat_updated = true;
}
void PX4Rangefinder::update(const hrt_abstime &timestamp_sample, const float distance, const int8_t quality)
{
distance_sensor_s &report = _distance_sensor_pub.get();
@ -81,5 +91,15 @@ void PX4Rangefinder::update(const hrt_abstime &timestamp_sample, const float dis
}
}
// Update the quaternion in the sample update if we have set the update flag to true and the quaternion is fresh
if (_quat_updated && (_q_update_now - timestamp_sample < 10_ms)) {
for (uint8_t i = 0; i < 4; ++i) {
report.q[i] = _quat[i];
}
}
_distance_sensor_pub.update();
_quat_updated = false;
}

6
src/lib/drivers/rangefinder/PX4Rangefinder.hpp
View File

@ -62,6 +62,9 @@ public:
void set_mode(const uint8_t mode) { _distance_sensor_pub.get().mode = mode; }
// update quaterion sensor orientation with respect to the vehicle body frame (only called explicitly via method, not by the constructor when object created)
void update_quaternion(const float (&quat)[4]);
void update(const hrt_abstime &timestamp_sample, const float distance, const int8_t quality = -1);
int get_instance() { return _distance_sensor_pub.get_instance(); };
@ -69,4 +72,7 @@ public:
private:
uORB::PublicationMultiData<distance_sensor_s> _distance_sensor_pub{ORB_ID(distance_sensor)};
bool _quat_updated{false};
float _quat[4] {1.0, 0.0, 0.0, 0.0};
hrt_abstime _q_update_now;
};