Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_VisualOdom: allow compilation with HAL_LOGGING_ENABLED false

Browse Source
This commit is contained in:
Peter Barker 2023-07-14 10:58:09 +10:00 committed by Andrew Tridgell
parent 1b288416e1
commit 03be6e13b7
5 changed files with 17 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_VisualOdom/AP_VisualOdom_Backend.cpp
View File

@ -66,11 +66,13 @@ void AP_VisualOdom_Backend::handle_vision_position_delta_msg(const mavlink_messa
_frontend.get_pos_offset());
#endif
#if HAL_LOGGING_ENABLED
// log sensor data
Write_VisualOdom(time_delta_sec,
angle_delta,
position_delta,
packet.confidence);
#endif
}
#endif // HAL_GCS_ENABLED

4
libraries/AP_VisualOdom/AP_VisualOdom_Backend.h
View File

@ -18,6 +18,8 @@
#if HAL_VISUALODOM_ENABLED
#include <AP_Logger/AP_Logger_config.h>
class AP_VisualOdom_Backend
{
public:
@ -57,10 +59,12 @@ protected:
return _frontend.get_type();
}
#if HAL_LOGGING_ENABLED
// Logging Functions
void Write_VisualOdom(float time_delta, const Vector3f &angle_delta, const Vector3f &position_delta, float confidence);
void Write_VisualPosition(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw, float pos_err, float ang_err, uint8_t reset_counter, bool ignored);
void Write_VisualVelocity(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter, bool ignored);
#endif
AP_VisualOdom &_frontend; // reference to frontend
uint32_t _last_update_ms; // system time of last update from sensor (used by health checks)

4
libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp
View File

@ -72,8 +72,10 @@ void AP_VisualOdom_IntelT265::handle_pose_estimate(uint64_t remote_time_us, uint
float yaw;
att.to_euler(roll, pitch, yaw);
#if HAL_LOGGING_ENABLED
// log sensor data
Write_VisualPosition(remote_time_us, time_ms, pos.x, pos.y, pos.z, degrees(roll), degrees(pitch), wrap_360(degrees(yaw)), posErr, angErr, reset_counter, !consume);
#endif
// store corrected attitude for use in pre-arm checks
_attitude_last = att;
@ -99,7 +101,9 @@ void AP_VisualOdom_IntelT265::handle_vision_speed_estimate(uint64_t remote_time_
// record time for health monitoring
_last_update_ms = AP_HAL::millis();
#if HAL_LOGGING_ENABLED
Write_VisualVelocity(remote_time_us, time_ms, vel_corrected, reset_counter, !consume);
#endif
}
// apply rotation and correction to position

3
libraries/AP_VisualOdom/AP_VisualOdom_Logging.cpp
View File

@ -1,6 +1,7 @@
#include "AP_VisualOdom_Backend.h"
#include <AP_Logger/AP_Logger_config.h>
#if HAL_VISUALODOM_ENABLED
#if HAL_VISUALODOM_ENABLED && HAL_LOGGING_ENABLED
#include <AP_Logger/AP_Logger.h>

5
libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp
View File

@ -21,6 +21,7 @@
#include <AP_HAL/AP_HAL.h>
#include <AP_AHRS/AP_AHRS.h>
#include <AP_Logger/AP_Logger.h>
// 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)
@ -39,8 +40,10 @@ void AP_VisualOdom_MAV::handle_pose_estimate(uint64_t remote_time_us, uint32_t t
float yaw;
attitude.to_euler(roll, pitch, yaw);
#if HAL_LOGGING_ENABLED
// log sensor data
Write_VisualPosition(remote_time_us, time_ms, pos.x, pos.y, pos.z, degrees(roll), degrees(pitch), degrees(yaw), posErr, angErr, reset_counter, false);
#endif
// record time for health monitoring
_last_update_ms = AP_HAL::millis();
@ -54,7 +57,9 @@ void AP_VisualOdom_MAV::handle_vision_speed_estimate(uint64_t remote_time_us, ui
// record time for health monitoring
_last_update_ms = AP_HAL::millis();
#if HAL_LOGGING_ENABLED
Write_VisualVelocity(remote_time_us, time_ms, vel, reset_counter, false);
#endif
}
#endif // AP_VISUALODOM_MAV_ENABLED