libraries/AC_Avoidance/AC_Avoidance_Logging.cpp

   1 #include <AP_Logger/AP_Logger_config.h>
   2
   3 #if HAL_LOGGING_ENABLED
   4
   5 #include "AC_Avoid.h"
   6 #include "AP_OADijkstra.h"
   7 #include "AP_OABendyRuler.h"
   8 #include <AP_Logger/AP_Logger.h>
   9 #include <AP_AHRS/AP_AHRS.h>
  10
  11 #if AP_OAPATHPLANNER_BENDYRULER_ENABLED
  12 void AP_OABendyRuler::Write_OABendyRuler(const uint8_t type, const bool active, const float target_yaw, const float target_pitch, const bool resist_chg, const float margin, const Location &final_dest, const Location &oa_dest) const
  13 {
  14     int32_t oa_dest_alt, final_alt;
  15     const bool got_oa_dest = oa_dest.get_alt_cm(Location::AltFrame::ABOVE_ORIGIN, oa_dest_alt);
  16     const bool got_final_dest = final_dest.get_alt_cm(Location::AltFrame::ABOVE_ORIGIN, final_alt);
  17
  18     const struct log_OABendyRuler pkt{
  19         LOG_PACKET_HEADER_INIT(LOG_OA_BENDYRULER_MSG),
  20         time_us     : AP_HAL::micros64(),
  21         type        : type,
  22         active      : active,
  23         target_yaw  : (uint16_t)wrap_360(target_yaw),
  24         yaw         : (uint16_t)wrap_360(AP::ahrs().yaw_sensor * 0.01f),
  25         target_pitch: (uint16_t)target_pitch,
  26         resist_chg  : resist_chg,
  27         margin      : margin,
  28         final_lat   : final_dest.lat,
  29         final_lng   : final_dest.lng,
  30         final_alt   : got_final_dest ? final_alt : final_dest.alt,
  31         oa_lat      : oa_dest.lat,
  32         oa_lng      : oa_dest.lng,
  33         oa_alt      : got_oa_dest ? oa_dest_alt : oa_dest.alt
  34     };
  35     AP::logger().WriteBlock(&pkt, sizeof(pkt));
  36 }
  37 #endif  // AP_OAPATHPLANNER_BENDYRULER_ENABLED
  38
  39 #if AP_OAPATHPLANNER_DIJKSTRA_ENABLED
  40 void AP_OADijkstra::Write_OADijkstra(const uint8_t state, const uint8_t error_id, const uint8_t curr_point, const uint8_t tot_points, const Location &final_dest, const Location &oa_dest) const
  41 {
  42     const struct log_OADijkstra pkt{
  43         LOG_PACKET_HEADER_INIT(LOG_OA_DIJKSTRA_MSG),
  44         time_us     : AP_HAL::micros64(),
  45         state       : state,
  46         error_id    : error_id,
  47         curr_point  : curr_point,
  48         tot_points  : tot_points,
  49         final_lat   : final_dest.lat,
  50         final_lng   : final_dest.lng,
  51         oa_lat      : oa_dest.lat,
  52         oa_lng      : oa_dest.lng
  53     };
  54     AP::logger().WriteBlock(&pkt, sizeof(pkt));
  55 }
  56 #endif
  57
  58 #if AP_OAPATHPLANNER_ENABLED
  59 void AP_OADijkstra::Write_Visgraph_point(const uint8_t version, const uint8_t point_num, const int32_t Lat, const int32_t Lon) const
  60 {
  61     const struct log_OD_Visgraph pkt{
  62         LOG_PACKET_HEADER_INIT(LOG_OD_VISGRAPH_MSG),
  63         time_us     : AP_HAL::micros64(),
  64         version     : version,
  65         point_num   : point_num,
  66         Lat         : Lat,
  67         Lon         : Lon,
  68     };
  69     AP::logger().WriteBlock(&pkt, sizeof(pkt));
  70 }
  71 #endif
  72
  73 #if AP_AVOIDANCE_ENABLED
  74 void AC_Avoid::Write_SimpleAvoidance(const uint8_t state, const Vector3f& desired_vel, const Vector3f& modified_vel, const bool back_up) const
  75 {
  76     const struct log_SimpleAvoid pkt{
  77         LOG_PACKET_HEADER_INIT(LOG_SIMPLE_AVOID_MSG),
  78         time_us         : AP_HAL::micros64(),
  79         state           : state,
  80         desired_vel_x   : desired_vel.x * 0.01f,
  81         desired_vel_y   : desired_vel.y * 0.01f,
  82         desired_vel_z   : desired_vel.z * 0.01f,
  83         modified_vel_x  : modified_vel.x * 0.01f,
  84         modified_vel_y  : modified_vel.y * 0.01f,
  85         modified_vel_z  : modified_vel.z * 0.01f,
  86         backing_up      : back_up,
  87     };
  88     AP::logger().WriteBlock(&pkt, sizeof(pkt));
  89 }
  90 #endif
  91
  92 #endif  // HAL_LOGGING_ENABLED