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

AP_Logger: Privatize AP_VisualOdom Logging

Browse Source
This commit is contained in:
Josh Henderson 2021-02-02 21:12:58 -05:00 committed by Randy Mackay
parent 9ace8ed216
commit 00e3bda2f5
3 changed files with 3 additions and 155 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
libraries/AP_Logger/AP_Logger.h
View File

@ -327,9 +327,6 @@ public:
void Write_RallyPoint(uint8_t total,
uint8_t sequence,
const RallyLocation &rally_point);
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, float vel_err, uint8_t reset_counter, bool ignored);
void Write_Beacon(AP_Beacon &beacon);
void Write_Proximity(AP_Proximity &proximity);
void Write_SRTL(bool active, uint16_t num_points, uint16_t max_points, uint8_t action, const Vector3f& point);

58
libraries/AP_Logger/LogFile.cpp
View File

@ -572,64 +572,6 @@ void AP_Logger::Write_RPM(const AP_RPM &rpm_sensor)
WriteBlock(&pkt, sizeof(pkt));
}
// Write visual odometry sensor data
void AP_Logger::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
};
WriteBlock(&pkt_visualodom, sizeof(log_VisualOdom));
}
// Write visual position sensor data. x,y,z are in meters, angles are in degrees
void AP_Logger::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
};
WriteBlock(&pkt_visualpos, sizeof(log_VisualPosition));
}
// Write visual velocity sensor data, velocity in NED meters per second
void AP_Logger::Write_VisualVelocity(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, float vel_err, 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 : vel_err,
reset_counter : reset_counter,
ignored : (uint8_t)ignored
};
WriteBlock(&pkt_visualvel, sizeof(log_VisualVelocity));
}
// Write beacon sensor (position) data
void AP_Logger::Write_Beacon(AP_Beacon &beacon)
{

97
libraries/AP_Logger/LogStructure.h
View File

@ -124,6 +124,7 @@ const struct MultiplierStructure log_Multipliers[] = {
#include <AP_AHRS/LogStructure.h>
#include <AP_Camera/LogStructure.h>
#include <AP_Baro/LogStructure.h>
#include <AP_VisualOdom/LogStructure.h>
// structure used to define logging format
struct LogStructure {
@ -374,51 +375,6 @@ struct PACKED log_POWR {
uint8_t safety_and_arm;
};
// visual odometry sensor data
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;
};
// visual position data
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;
};
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;
};
struct PACKED log_Cmd {
LOG_PACKET_HEADER;
uint64_t time_us;
@ -1576,46 +1532,6 @@ struct PACKED log_PSC {
// @Field: VibeZ: Primary accelerometer filtered vibration, z-axis
// @Field: Clip: Number of clipping events on 1st accelerometer
// @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
// @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
// @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
// @LoggerMessage: WENC
// @Description: Wheel encoder measurements
// @Field: TimeUS: Time since system startup
@ -1771,12 +1687,7 @@ LOG_STRUCTURE_FROM_AHRS \
"RALY", "QBBLLh", "TimeUS,Tot,Seq,Lat,Lng,Alt", "s--DUm", "F--GGB" }, \
{ LOG_MAV_MSG, sizeof(log_MAV), \
"MAV", "QBHHHBHH", "TimeUS,chan,txp,rxp,rxdp,flags,ss,tf", "s#----s-", "F-000-C-" }, \
{ 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--" }, \
LOG_STRUCTURE_FROM_VISUALODOM \
{ LOG_OPTFLOW_MSG, sizeof(log_Optflow), \
"OF", "QBffff", "TimeUS,Qual,flowX,flowY,bodyX,bodyY", "s-EEnn", "F-0000" }, \
{ LOG_WHEELENCODER_MSG, sizeof(log_WheelEncoder), \
@ -1894,8 +1805,7 @@ enum LogMessages : uint8_t {
LOG_MSG_SBPEVENT,
LOG_RALLY_MSG,
LOG_VISUALODOM_MSG,
LOG_VISUALPOS_MSG,
LOG_IDS_FROM_VISUALODOM,
LOG_BEACON_MSG,
LOG_PROXIMITY_MSG,
LOG_DF_FILE_STATS,
@ -1912,7 +1822,6 @@ enum LogMessages : uint8_t {
LOG_ARM_DISARM_MSG,
LOG_OA_BENDYRULER_MSG,
LOG_OA_DIJKSTRA_MSG,
LOG_VISUALVEL_MSG,
LOG_SIMPLE_AVOID_MSG,
LOG_WINCH_MSG,
LOG_PSC_MSG,