mirror of https://github.com/ArduPilot/ardupilot
67 lines
2.6 KiB
C++
67 lines
2.6 KiB
C++
#include "AP_VisualOdom_Backend.h"
|
|
#include <AP_Logger/AP_Logger_config.h>
|
|
|
|
#if HAL_VISUALODOM_ENABLED && HAL_LOGGING_ENABLED
|
|
|
|
#include <AP_Logger/AP_Logger.h>
|
|
|
|
// Write visual odometry sensor data
|
|
void AP_VisualOdom_Backend::Write_VisualOdom(float time_delta, const Vector3f &angle_delta, const Vector3f &position_delta, float confidence)
|
|
{
|
|
const struct log_VisualOdom pkt_visualodom {
|
|
LOG_PACKET_HEADER_INIT(LOG_VISUALODOM_MSG),
|
|
time_us : AP_HAL::micros64(),
|
|
time_delta : time_delta,
|
|
angle_delta_x : angle_delta.x,
|
|
angle_delta_y : angle_delta.y,
|
|
angle_delta_z : angle_delta.z,
|
|
position_delta_x : position_delta.x,
|
|
position_delta_y : position_delta.y,
|
|
position_delta_z : position_delta.z,
|
|
confidence : confidence
|
|
};
|
|
AP::logger().WriteBlock(&pkt_visualodom, sizeof(log_VisualOdom));
|
|
}
|
|
|
|
// Write visual position sensor data. x,y,z are in meters, angles are in degrees
|
|
void AP_VisualOdom_Backend::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)
|
|
{
|
|
const struct log_VisualPosition pkt_visualpos {
|
|
LOG_PACKET_HEADER_INIT(LOG_VISUALPOS_MSG),
|
|
time_us : AP_HAL::micros64(),
|
|
remote_time_us : remote_time_us,
|
|
time_ms : time_ms,
|
|
pos_x : x,
|
|
pos_y : y,
|
|
pos_z : z,
|
|
roll : roll,
|
|
pitch : pitch,
|
|
yaw : yaw,
|
|
pos_err : pos_err,
|
|
ang_err : ang_err,
|
|
reset_counter : reset_counter,
|
|
ignored : (uint8_t)ignored
|
|
};
|
|
AP::logger().WriteBlock(&pkt_visualpos, sizeof(log_VisualPosition));
|
|
}
|
|
|
|
// Write visual velocity sensor data, velocity in NED meters per second
|
|
void AP_VisualOdom_Backend::Write_VisualVelocity(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter, bool ignored)
|
|
{
|
|
const struct log_VisualVelocity pkt_visualvel {
|
|
LOG_PACKET_HEADER_INIT(LOG_VISUALVEL_MSG),
|
|
time_us : AP_HAL::micros64(),
|
|
remote_time_us : remote_time_us,
|
|
time_ms : time_ms,
|
|
vel_x : vel.x,
|
|
vel_y : vel.y,
|
|
vel_z : vel.z,
|
|
vel_err : _frontend.get_vel_noise(),
|
|
reset_counter : reset_counter,
|
|
ignored : (uint8_t)ignored
|
|
};
|
|
AP::logger().WriteBlock(&pkt_visualvel, sizeof(log_VisualVelocity));
|
|
}
|
|
|
|
#endif // HAL_VISUALODOM_ENABLED
|