Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-04 06:58:39 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_VisualOdom: front-end implements handle_vision_position_estimate with eulers

Browse Source
This commit is contained in:
Randy Mackay 2020-04-03 13:37:39 +09:00
parent 3530d5b348
commit a41b169635
3 changed files with 4 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/AP_VisualOdom/AP_VisualOdom.cpp
View File

@ -131,7 +131,10 @@ void AP_VisualOdom::handle_vision_position_estimate(uint64_t remote_time_us, uin
// call backend // call backend
if (_driver != nullptr) { if (_driver != nullptr) {
_driver->handle_vision_position_estimate(remote_time_us, time_ms, x, y, z, roll, pitch, yaw); // 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);
} }
} }

9
libraries/AP_VisualOdom/AP_VisualOdom_Backend.cpp
View File

@ -35,15 +35,6 @@ bool AP_VisualOdom_Backend::healthy() const
return ((AP_HAL::millis() - _last_update_ms) < AP_VISUALODOM_TIMEOUT_MS); return ((AP_HAL::millis() - _last_update_ms) < AP_VISUALODOM_TIMEOUT_MS);
} }
// general purpose method to send position estimate data to EKF
// distances in meters, roll, pitch and yaw are in radians
void AP_VisualOdom_Backend::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)
{
Quaternion attitude;
attitude.from_euler(roll, pitch, yaw);
handle_vision_position_estimate(remote_time_us, time_ms, x, y, z, attitude);
}
// general purpose method to send position estimate data to EKF // general purpose method to send position estimate data to EKF
void AP_VisualOdom_Backend::handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude) void AP_VisualOdom_Backend::handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude)
{ {

1
libraries/AP_VisualOdom/AP_VisualOdom_Backend.h
View File

@ -33,7 +33,6 @@ public:
// general purpose methods to consume position estimate data and send to EKF // general purpose methods to consume position estimate data and send to EKF
// distances in meters, roll, pitch and yaw are in radians // 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);
void handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude); void handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude);
// calibrate camera attitude to align with vehicle's AHRS/EKF attitude // calibrate camera attitude to align with vehicle's AHRS/EKF attitude