Line data    Source code

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