zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_VisualOdom: Privatize Logging

c415-sdk
Josh Henderson 4 years ago committed by Randy Mackay
parent
00e3bda2f5
commit
8135d9e60a
6 changed files with 172 additions and 5 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      libraries/AP_VisualOdom/AP_VisualOdom_Backend.cpp
  2. 5
      libraries/AP_VisualOdom/AP_VisualOdom_Backend.h
  3. 4
      libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp
  4. 65
      libraries/AP_VisualOdom/AP_VisualOdom_Logging.cpp
  5. 4
      libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp
  6. 97
      libraries/AP_VisualOdom/LogStructure.h

2
libraries/AP_VisualOdom/AP_VisualOdom_Backend.cpp
Unescape View File

@ -66,7 +66,7 @@ void AP_VisualOdom_Backend::handle_vision_position_delta_msg(const mavlink_messa
_frontend.get_pos_offset()); _frontend.get_pos_offset());
// log sensor data // log sensor data
AP::logger().Write_VisualOdom(time_delta_sec, Write_VisualOdom(time_delta_sec,
angle_delta, angle_delta,
position_delta, position_delta,
packet.confidence); packet.confidence);

5
libraries/AP_VisualOdom/AP_VisualOdom_Backend.h
Unescape View File

@ -51,6 +51,11 @@ protected:
// updates the reset timestamp to the current system time if the reset_counter has changed // 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); 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 AP_VisualOdom &_frontend; // reference to frontend
uint32_t _last_update_ms; // system time of last update from sensor (used by health checks) uint32_t _last_update_ms; // system time of last update from sensor (used by health checks)

4
libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp
Unescape View File

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

65
libraries/AP_VisualOdom/AP_VisualOdom_Logging.cpp
Unescape 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
Unescape 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); attitude.to_euler(roll, pitch, yaw);
// log sensor data // 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 // record time for health monitoring
_last_update_ms = AP_HAL::millis(); _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 // record time for health monitoring
_last_update_ms = AP_HAL::millis(); _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 #endif

97
libraries/AP_VisualOdom/LogStructure.h
Unescape 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--" },
Write Preview
Loading…
Cancel
Save