libraries/AC_Avoidance/LogStructure.h

   1 #pragma once
   2
   3 #include <AP_Logger/LogStructure.h>
   4 #include "AC_Avoidance_config.h"
   5
   6 #define LOG_IDS_FROM_AVOIDANCE \
   7     LOG_OA_BENDYRULER_MSG, \
   8     LOG_OA_DIJKSTRA_MSG, \
   9     LOG_SIMPLE_AVOID_MSG, \
  10     LOG_OD_VISGRAPH_MSG
  11
  12 // @LoggerMessage: OABR
  13 // @Description: Object avoidance (Bendy Ruler) diagnostics
  14 // @Field: TimeUS: Time since system startup
  15 // @Field: Type: Type of BendyRuler currently active
  16 // @Field: Act: True if Bendy Ruler avoidance is being used
  17 // @Field: DYaw: Best yaw chosen to avoid obstacle
  18 // @Field: Yaw: Current vehicle yaw
  19 // @Field: DP: Desired pitch chosen to avoid obstacle
  20 // @Field: RChg: True if BendyRuler resisted changing bearing and continued in last calculated bearing
  21 // @Field: Mar: Margin from path to obstacle on best yaw chosen
  22 // @Field: DLt: Destination latitude
  23 // @Field: DLg: Destination longitude
  24 // @Field: DAlt: Desired alt above EKF Origin
  25 // @Field: OLt: Intermediate location chosen for avoidance
  26 // @Field: OLg: Intermediate location chosen for avoidance
  27 // @Field: OAlt: Intermediate alt chosen for avoidance above EKF origin
  28 struct PACKED log_OABendyRuler {
  29     LOG_PACKET_HEADER;
  30     uint64_t time_us;
  31     uint8_t type;
  32     uint8_t active;
  33     uint16_t target_yaw;
  34     uint16_t yaw;
  35     uint16_t target_pitch;
  36     bool resist_chg;
  37     float margin;
  38     int32_t final_lat;
  39     int32_t final_lng;
  40     int32_t final_alt;
  41     int32_t oa_lat;
  42     int32_t oa_lng;
  43     int32_t oa_alt;
  44 };
  45
  46 // @LoggerMessage: OADJ
  47 // @Description: Object avoidance (Dijkstra) diagnostics
  48 // @Field: TimeUS: Time since system startup
  49 // @Field: State: Dijkstra avoidance library state
  50 // @Field: Err: Dijkstra library error condition
  51 // @Field: CurrPoint: Destination point in calculated path to destination
  52 // @Field: TotPoints: Number of points in path to destination
  53 // @Field: DLat: Destination latitude
  54 // @Field: DLng: Destination longitude
  55 // @Field: OALat: Object Avoidance chosen destination point latitude
  56 // @Field: OALng: Object Avoidance chosen destination point longitude
  57 struct PACKED log_OADijkstra {
  58     LOG_PACKET_HEADER;
  59     uint64_t time_us;
  60     uint8_t state;
  61     uint8_t error_id;
  62     uint8_t curr_point;
  63     uint8_t tot_points;
  64     int32_t final_lat;
  65     int32_t final_lng;
  66     int32_t oa_lat;
  67     int32_t oa_lng;
  68 };
  69
  70 // @LoggerMessage: SA
  71 // @Description: Simple Avoidance messages
  72 // @Field: TimeUS: Time since system startup
  73 // @Field: State: True if Simple Avoidance is active
  74 // @Field: DVelX: Desired velocity, X-Axis (Velocity before Avoidance)
  75 // @Field: DVelY: Desired velocity, Y-Axis (Velocity before Avoidance)
  76 // @Field: DVelZ: Desired velocity, Z-Axis (Velocity before Avoidance)
  77 // @Field: MVelX: Modified velocity, X-Axis (Velocity after Avoidance)
  78 // @Field: MVelY: Modified velocity, Y-Axis (Velocity after Avoidance)
  79 // @Field: MVelZ: Modified velocity, Z-Axis (Velocity after Avoidance)
  80 // @Field: Back: True if vehicle is backing away
  81 struct PACKED log_SimpleAvoid {
  82   LOG_PACKET_HEADER;
  83   uint64_t time_us;
  84   uint8_t state;
  85   float desired_vel_x;
  86   float desired_vel_y;
  87   float desired_vel_z;
  88   float modified_vel_x;
  89   float modified_vel_y;
  90   float modified_vel_z;
  91   uint8_t backing_up;
  92 };
  93
  94 // @LoggerMessage: OAVG
  95 // @Description: Object avoidance path planning visgraph points
  96 // @Field: TimeUS: Time since system startup
  97 // @Field: version: Visgraph version, increments each time the visgraph is re-generated
  98 // @Field: point_num: point number in visgraph
  99 // @Field: Lat: Latitude
 100 // @Field: Lon: longitude
 101 struct PACKED log_OD_Visgraph {
 102   LOG_PACKET_HEADER;
 103   uint64_t time_us;
 104   uint8_t version;
 105   uint8_t point_num;
 106   int32_t Lat;
 107   int32_t Lon;
 108 };
 109
 110 #if AP_AVOIDANCE_ENABLED
 111 #define LOG_STRUCTURE_FROM_AVOIDANCE \
 112     { LOG_OA_BENDYRULER_MSG, sizeof(log_OABendyRuler), \
 113       "OABR","QBBHHHBfLLiLLi","TimeUS,Type,Act,DYaw,Yaw,DP,RChg,Mar,DLt,DLg,DAlt,OLt,OLg,OAlt", "s--ddd-mDUmDUm", "F-------GGBGGB" , true }, \
 114     { LOG_OA_DIJKSTRA_MSG, sizeof(log_OADijkstra), \
 115       "OADJ","QBBBBLLLL","TimeUS,State,Err,CurrPoint,TotPoints,DLat,DLng,OALat,OALng", "s----DUDU", "F----GGGG" , true }, \
 116     { LOG_SIMPLE_AVOID_MSG, sizeof(log_SimpleAvoid), \
 117       "SA",  "QBffffffB","TimeUS,State,DVelX,DVelY,DVelZ,MVelX,MVelY,MVelZ,Back", "s-nnnnnn-", "F--------", true }, \
 118      { LOG_OD_VISGRAPH_MSG, sizeof(log_OD_Visgraph), \
 119       "OAVG", "QBBLL", "TimeUS,version,point_num,Lat,Lon", "s--DU", "F--GG", true},
 120 #else
 121 #define LOG_STRUCTURE_FROM_AVOIDANCE
 122 #endif // AP_AVOIDANCE_ENABLED