Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_VisualOdom: Privatize Logging

This commit is contained in:
Josh Henderson 2021-01-22 13:30:14 -05:00 committed by Randy Mackay
parent 00e3bda2f5
commit 8135d9e60a
6 changed files with 172 additions and 5 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,7 +66,7 @@ void AP_VisualOdom_Backend::handle_vision_position_delta_msg(const mavlink_messa
_frontend.get_pos_offset());
// log sensor data
AP::logger().Write_VisualOdom(time_delta_sec,
Write_VisualOdom(time_delta_sec,
angle_delta,
position_delta,
packet.confidence);

5
libraries/AP_VisualOdom/AP_VisualOdom_Backend.h
View File

@ -51,6 +51,11 @@ protected:
// updates the reset timestamp to the current system time if the reset_counter has changed
uint32_t get_reset_timestamp_ms(uint8_t reset_counter);
// 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);
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

@ -65,7 +65,7 @@ void AP_VisualOdom_IntelT265::handle_vision_position_estimate(uint64_t remote_ti
att.to_euler(roll, pitch, yaw);
// log sensor data
AP::logger().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);
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);
// store corrected attitude for use in pre-arm checks
_attitude_last = att;
@ -91,7 +91,7 @@ void AP_VisualOdom_IntelT265::handle_vision_speed_estimate(uint64_t remote_time_
// record time for health monitoring
_last_update_ms = AP_HAL::millis();
AP::logger().Write_VisualVelocity(remote_time_us, time_ms, vel_corrected, _frontend.get_vel_noise(), reset_counter, !consume);
Write_VisualVelocity(remote_time_us, time_ms, vel_corrected, reset_counter, !consume);
}
// apply rotation and correction to position

65
libraries/AP_VisualOdom/AP_VisualOdom_Logging.cpp Normal file
View File

@ -0,0 +1,65 @@
#include "AP_VisualOdom_Backend.h"
#if HAL_VISUALODOM_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

4
libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp
View File

@ -47,7 +47,7 @@ void AP_VisualOdom_MAV::handle_vision_position_estimate(uint64_t remote_time_us,
attitude.to_euler(roll, pitch, yaw);
// log sensor data
AP::logger().Write_VisualPosition(remote_time_us, time_ms, pos.x, pos.y, pos.z, degrees(roll), degrees(pitch), degrees(yaw), posErr, angErr, reset_counter, false);
Write_VisualPosition(remote_time_us, time_ms, pos.x, pos.y, pos.z, degrees(roll), degrees(pitch), degrees(yaw), posErr, angErr, reset_counter, false);
// record time for health monitoring
_last_update_ms = AP_HAL::millis();
@ -61,7 +61,7 @@ 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();
AP::logger().Write_VisualVelocity(remote_time_us, time_ms, vel, _frontend.get_vel_noise(), reset_counter, false);
Write_VisualVelocity(remote_time_us, time_ms, vel, reset_counter, false);
}
#endif

97
libraries/AP_VisualOdom/LogStructure.h Normal file
View File

@ -0,0 +1,97 @@
#pragma once
#include <AP_Logger/LogStructure.h>
#define LOG_IDS_FROM_VISUALODOM \
LOG_VISUALODOM_MSG, \
LOG_VISUALPOS_MSG, \
LOG_VISUALVEL_MSG
// @LoggerMessage: VISO
// @Description: Visual Odometry
// @Field: TimeUS: System time
// @Field: dt: Time period this data covers
// @Field: AngDX: Angular change for body-frame roll axis
// @Field: AngDY: Angular change for body-frame pitch axis
// @Field: AngDZ: Angular change for body-frame z axis
// @Field: PosDX: Position change for body-frame X axis (Forward-Back)
// @Field: PosDY: Position change for body-frame Y axis (Right-Left)
// @Field: PosDZ: Position change for body-frame Z axis (Down-Up)
// @Field: conf: Confidence
struct PACKED log_VisualOdom {
LOG_PACKET_HEADER;
uint64_t time_us;
float time_delta;
float angle_delta_x;
float angle_delta_y;
float angle_delta_z;
float position_delta_x;
float position_delta_y;
float position_delta_z;
float confidence;
};
// @LoggerMessage: VISP
// @Description: Vision Position
// @Field: TimeUS: System time
// @Field: RTimeUS: Remote system time
// @Field: CTimeMS: Corrected system time
// @Field: PX: Position X-axis (North-South)
// @Field: PY: Position Y-axis (East-West)
// @Field: PZ: Position Z-axis (Down-Up)
// @Field: Roll: Roll lean angle
// @Field: Pitch: Pitch lean angle
// @Field: Yaw: Yaw angle
// @Field: PErr: Position estimate error
// @Field: AErr: Attitude estimate error
// @Field: Rst: Position reset counter
// @Field: Ign: Ignored
struct PACKED log_VisualPosition {
LOG_PACKET_HEADER;
uint64_t time_us;
uint64_t remote_time_us;
uint32_t time_ms;
float pos_x;
float pos_y;
float pos_z;
float roll; // degrees
float pitch; // degrees
float yaw; // degrees
float pos_err; // meters
float ang_err; // radians
uint8_t reset_counter;
uint8_t ignored;
};
// @LoggerMessage: VISV
// @Description: Vision Velocity
// @Field: TimeUS: System time
// @Field: RTimeUS: Remote system time
// @Field: CTimeMS: Corrected system time
// @Field: VX: Velocity X-axis (North-South)
// @Field: VY: Velocity Y-axis (East-West)
// @Field: VZ: Velocity Z-axis (Down-Up)
// @Field: VErr: Velocity estimate error
// @Field: Rst: Velocity reset counter
// @Field: Ign: Ignored
struct PACKED log_VisualVelocity {
LOG_PACKET_HEADER;
uint64_t time_us;
uint64_t remote_time_us;
uint32_t time_ms;
float vel_x;
float vel_y;
float vel_z;
float vel_err;
uint8_t reset_counter;
uint8_t ignored;
};
#define LOG_STRUCTURE_FROM_VISUALODOM \
{ LOG_VISUALODOM_MSG, sizeof(log_VisualOdom), \
"VISO", "Qffffffff", "TimeUS,dt,AngDX,AngDY,AngDZ,PosDX,PosDY,PosDZ,conf", "ssrrrmmm-", "FF000000-" }, \
{ LOG_VISUALPOS_MSG, sizeof(log_VisualPosition), \
"VISP", "QQIffffffffBB", "TimeUS,RTimeUS,CTimeMS,PX,PY,PZ,Roll,Pitch,Yaw,PErr,AErr,Rst,Ign", "sssmmmddhmd--", "FFC00000000--" }, \
{ LOG_VISUALVEL_MSG, sizeof(log_VisualVelocity), \
"VISV", "QQIffffBB", "TimeUS,RTimeUS,CTimeMS,VX,VY,VZ,VErr,Rst,Ign", "sssnnnn--", "FFC0000--" },