zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_Logger: Privatize AP_VisualOdom Logging

zr-v5.1
Josh Henderson 4 years ago committed by Randy Mackay
parent
9ace8ed216
commit
00e3bda2f5
3 changed files with 3 additions and 155 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 3
      libraries/AP_Logger/AP_Logger.h
  2. 58
      libraries/AP_Logger/LogFile.cpp
  3. 97
      libraries/AP_Logger/LogStructure.h

3
libraries/AP_Logger/AP_Logger.h
Unescape View File

@ -327,9 +327,6 @@ public: @@ -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
Unescape View File

@ -572,64 +572,6 @@ void AP_Logger::Write_RPM(const AP_RPM &rpm_sensor) @@ -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
Unescape View File

@ -124,6 +124,7 @@ const struct MultiplierStructure log_Multipliers[] = { @@ -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 { @@ -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 { @@ -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 \ @@ -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 { @@ -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 { @@ -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,

Write Preview
Loading…
Cancel
Save