Line data    Source code

       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        3633 : void AP_VisualOdom_Backend::Write_VisualOdom(float time_delta, const Vector3f &angle_delta, const Vector3f &position_delta, float confidence)
      10             : {
      11        3633 :     const struct log_VisualOdom pkt_visualodom {
      12             :         LOG_PACKET_HEADER_INIT(LOG_VISUALODOM_MSG),
      13        3633 :         time_us             : AP_HAL::micros64(),
      14             :         time_delta          : time_delta,
      15        3633 :         angle_delta_x       : angle_delta.x,
      16        3633 :         angle_delta_y       : angle_delta.y,
      17        3633 :         angle_delta_z       : angle_delta.z,
      18        3633 :         position_delta_x    : position_delta.x,
      19        3633 :         position_delta_y    : position_delta.y,
      20        3633 :         position_delta_z    : position_delta.z,
      21             :         confidence          : confidence
      22        3633 :     };
      23        3633 :     AP::logger().WriteBlock(&pkt_visualodom, sizeof(log_VisualOdom));
      24        3633 : }
      25             : 
      26             : // Write visual position sensor data.  x,y,z are in meters, angles are in degrees
      27       10069 : 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       10069 :     const struct log_VisualPosition pkt_visualpos {
      30             :         LOG_PACKET_HEADER_INIT(LOG_VISUALPOS_MSG),
      31       10069 :         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       10069 :         ignored         : (uint8_t)ignored,
      44             :         quality         : quality
      45       10069 :     };
      46       10069 :     AP::logger().WriteBlock(&pkt_visualpos, sizeof(log_VisualPosition));
      47       10069 : }
      48             : 
      49             : // Write visual velocity sensor data, velocity in NED meters per second
      50        4278 : void AP_VisualOdom_Backend::Write_VisualVelocity(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, float vel_err, uint8_t reset_counter, bool ignored, int8_t quality)
      51             : {
      52        4278 :     const struct log_VisualVelocity pkt_visualvel {
      53             :         LOG_PACKET_HEADER_INIT(LOG_VISUALVEL_MSG),
      54        4278 :         time_us         : AP_HAL::micros64(),
      55             :         remote_time_us  : remote_time_us,
      56             :         time_ms         : time_ms,
      57        4278 :         vel_x           : vel.x,
      58        4278 :         vel_y           : vel.y,
      59        4278 :         vel_z           : vel.z,
      60             :         vel_err         : vel_err,
      61             :         reset_counter   : reset_counter,
      62        4278 :         ignored         : (uint8_t)ignored,
      63             :         quality         : quality
      64        4278 :     };
      65        4278 :     AP::logger().WriteBlock(&pkt_visualvel, sizeof(log_VisualVelocity));
      66        4278 : }
      67             : 
      68             : #endif // HAL_VISUALODOM_ENABLED