libraries/AP_VisualOdom/AP_VisualOdom_Logging.cpp

   1 #include "AP_VisualOdom_Backend.h"
   2 #include <AP_Logger/AP_Logger_config.h>
   3
   4 #if HAL_VISUALODOM_ENABLED && HAL_LOGGING_ENABLED
   5
   6 #include <AP_Logger/AP_Logger.h>
   7
   8 // Write visual odometry sensor data
   9 void AP_VisualOdom_Backend::Write_VisualOdom(float time_delta, const Vector3f &angle_delta, const Vector3f &position_delta, float confidence)
  10 {
  11     const struct log_VisualOdom pkt_visualodom {
  12         LOG_PACKET_HEADER_INIT(LOG_VISUALODOM_MSG),
  13         time_us             : AP_HAL::micros64(),
  14         time_delta          : time_delta,
  15         angle_delta_x       : angle_delta.x,
  16         angle_delta_y       : angle_delta.y,
  17         angle_delta_z       : angle_delta.z,
  18         position_delta_x    : position_delta.x,
  19         position_delta_y    : position_delta.y,
  20         position_delta_z    : position_delta.z,
  21         confidence          : confidence
  22     };
  23     AP::logger().WriteBlock(&pkt_visualodom, sizeof(log_VisualOdom));
  24 }
  25
  26 // Write visual position sensor data.  x,y,z are in meters, angles are in degrees
  27 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, int8_t quality)
  28 {
  29     const struct log_VisualPosition pkt_visualpos {
  30         LOG_PACKET_HEADER_INIT(LOG_VISUALPOS_MSG),
  31         time_us         : AP_HAL::micros64(),
  32         remote_time_us  : remote_time_us,
  33         time_ms         : time_ms,
  34         pos_x           : x,
  35         pos_y           : y,
  36         pos_z           : z,
  37         roll            : roll,
  38         pitch           : pitch,
  39         yaw             : yaw,
  40         pos_err         : pos_err,
  41         ang_err         : ang_err,
  42         reset_counter   : reset_counter,
  43         ignored         : (uint8_t)ignored,
  44         quality         : quality
  45     };
  46     AP::logger().WriteBlock(&pkt_visualpos, sizeof(log_VisualPosition));
  47 }
  48
  49 // Write visual velocity sensor data, velocity in NED meters per second
  50 void AP_VisualOdom_Backend::Write_VisualVelocity(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter, bool ignored, int8_t quality)
  51 {
  52     const struct log_VisualVelocity pkt_visualvel {
  53         LOG_PACKET_HEADER_INIT(LOG_VISUALVEL_MSG),
  54         time_us         : AP_HAL::micros64(),
  55         remote_time_us  : remote_time_us,
  56         time_ms         : time_ms,
  57         vel_x           : vel.x,
  58         vel_y           : vel.y,
  59         vel_z           : vel.z,
  60         vel_err         : _frontend.get_vel_noise(),
  61         reset_counter   : reset_counter,
  62         ignored         : (uint8_t)ignored,
  63         quality         : quality
  64     };
  65     AP::logger().WriteBlock(&pkt_visualvel, sizeof(log_VisualVelocity));
  66 }
  67
  68 #endif // HAL_VISUALODOM_ENABLED