aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_VisualOdom: Change name from position to pose

Browse Source 
* The function takes position (linear) + orientation (angular), therefore it's a pose, not a position

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>
This commit is contained in:
Ryan Friedman 2023-08-06 22:05:25 -06:00 committed by Randy Mackay
parent 4226371f76
commit 1615038e57
7 changed files with 16 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_VisualOdom/AP_VisualOdom.cpp
View File

@ -179,7 +179,7 @@ void AP_VisualOdom::handle_vision_position_delta_msg(const mavlink_message_t &ms
// general purpose method to consume position estimate data and send to EKF
// distances in meters, roll, pitch and yaw are in radians
void AP_VisualOdom::handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw, float posErr, float angErr, uint8_t reset_counter)
void AP_VisualOdom::handle_pose_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw, float posErr, float angErr, uint8_t reset_counter)
{
// exit immediately if not enabled
if (!enabled()) {
@ -191,12 +191,12 @@ void AP_VisualOdom::handle_vision_position_estimate(uint64_t remote_time_us, uin
// convert attitude to quaternion and call backend
Quaternion attitude;
attitude.from_euler(roll, pitch, yaw);
_driver->handle_vision_position_estimate(remote_time_us, time_ms, x, y, z, attitude, posErr, angErr, reset_counter);
_driver->handle_pose_estimate(remote_time_us, time_ms, x, y, z, attitude, posErr, angErr, reset_counter);
}
}
// general purpose method to consume position estimate data and send to EKF
void AP_VisualOdom::handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter)
void AP_VisualOdom::handle_pose_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter)
{
// exit immediately if not enabled
if (!enabled()) {
@ -205,7 +205,7 @@ void AP_VisualOdom::handle_vision_position_estimate(uint64_t remote_time_us, uin
// call backend
if (_driver != nullptr) {
_driver->handle_vision_position_estimate(remote_time_us, time_ms, x, y, z, attitude, posErr, angErr, reset_counter);
_driver->handle_pose_estimate(remote_time_us, time_ms, x, y, z, attitude, posErr, angErr, reset_counter);
}
}

4
libraries/AP_VisualOdom/AP_VisualOdom.h
View File

@ -90,8 +90,8 @@ public:
// general purpose methods to consume position estimate data and send to EKF
// distances in meters, roll, pitch and yaw are in radians
void handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw, float posErr, float angErr, uint8_t reset_counter);
void handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter);
void handle_pose_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw, float posErr, float angErr, uint8_t reset_counter);
void handle_pose_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter);
// general purpose methods to consume velocity estimate data and send to EKF
// velocity in NED meters per second

4
libraries/AP_VisualOdom/AP_VisualOdom_Backend.h
View File

@ -30,8 +30,8 @@ public:
// consume vision_position_delta mavlink messages
void handle_vision_position_delta_msg(const mavlink_message_t &msg);
// consume vision position estimate data and send to EKF. distances in meters
virtual void handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter) = 0;
// consume vision pose estimate data and send to EKF. distances in meters
virtual void handle_pose_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter) = 0;
// consume vision velocity estimate data and send to EKF, velocity in NED meters per second
virtual void handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter) = 0;

4
libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp
View File

@ -27,8 +27,8 @@
extern const AP_HAL::HAL& hal;
// consume vision position estimate data and send to EKF. distances in meters
void AP_VisualOdom_IntelT265::handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter)
// consume vision pose estimate data and send to EKF. distances in meters
void AP_VisualOdom_IntelT265::handle_pose_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter)
{
const float scale_factor = _frontend.get_pos_scale();
Vector3f pos{x * scale_factor, y * scale_factor, z * scale_factor};

4
libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.h
View File

@ -13,8 +13,8 @@ public:
using AP_VisualOdom_Backend::AP_VisualOdom_Backend;
// consume vision position estimate data and send to EKF. distances in meters
void handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter) override;
// consume vision pose estimate data and send to EKF. distances in meters
void handle_pose_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter) override;
// consume vision velocity estimate data and send to EKF, velocity in NED meters per second
void handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter) override;

4
libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp
View File

@ -22,8 +22,8 @@
#include <AP_HAL/AP_HAL.h>
#include <AP_AHRS/AP_AHRS.h>
// consume vision position estimate data and send to EKF. distances in meters
void AP_VisualOdom_MAV::handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter)
// consume vision pose estimate data and send to EKF. distances in meters
void AP_VisualOdom_MAV::handle_pose_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter)
{
const float scale_factor = _frontend.get_pos_scale();
Vector3f pos{x * scale_factor, y * scale_factor, z * scale_factor};

4
libraries/AP_VisualOdom/AP_VisualOdom_MAV.h
View File

@ -13,8 +13,8 @@ public:
// constructor
using AP_VisualOdom_Backend::AP_VisualOdom_Backend;
// consume vision position estimate data and send to EKF. distances in meters
void handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter) override;
// consume vision pose estimate data and send to EKF. distances in meters
void handle_pose_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter) override;
// consume vision velocity estimate data and send to EKF, velocity in NED meters per second
void handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter) override;