diff --git a/libraries/AP_Logger/AP_Logger.h b/libraries/AP_Logger/AP_Logger.h index b080042948..2cc17f70a3 100644 --- a/libraries/AP_Logger/AP_Logger.h +++ b/libraries/AP_Logger/AP_Logger.h @@ -289,6 +289,7 @@ public: void Write_SRTL(bool active, uint16_t num_points, uint16_t max_points, uint8_t action, const Vector3f& point); void Write_OABendyRuler(bool active, float target_yaw, bool ignore_chg, float margin, const Location &final_dest, const Location &oa_dest); void Write_OADijkstra(uint8_t state, uint8_t error_id, uint8_t curr_point, uint8_t tot_points, const Location &final_dest, const Location &oa_dest); + void Write_SimpleAvoidance(uint8_t state, const Vector2f& desired_vel, const Vector2f& modified_vel, bool back_up); void Write(const char *name, const char *labels, const char *fmt, ...); void Write(const char *name, const char *labels, const char *units, const char *mults, const char *fmt, ...); diff --git a/libraries/AP_Logger/LogFile.cpp b/libraries/AP_Logger/LogFile.cpp index b1caec9cdd..7ca031ddf2 100644 --- a/libraries/AP_Logger/LogFile.cpp +++ b/libraries/AP_Logger/LogFile.cpp @@ -1078,3 +1078,18 @@ void AP_Logger::Write_OADijkstra(uint8_t state, uint8_t error_id, uint8_t curr_p }; WriteBlock(&pkt, sizeof(pkt)); } + +void AP_Logger::Write_SimpleAvoidance(uint8_t state, const Vector2f& desired_vel, const Vector2f& modified_vel, bool back_up) +{ + struct log_SimpleAvoid pkt{ + LOG_PACKET_HEADER_INIT(LOG_SIMPLE_AVOID_MSG), + time_us : AP_HAL::micros64(), + state : state, + desired_vel_x : desired_vel.x * 0.01f, + desired_vel_y : desired_vel.y * 0.01f, + modified_vel_x : modified_vel.x * 0.01f, + modified_vel_y : modified_vel.y * 0.01f, + backing_up : back_up, + }; + WriteBlock(&pkt, sizeof(pkt)); +} diff --git a/libraries/AP_Logger/LogStructure.h b/libraries/AP_Logger/LogStructure.h index 3cc8761314..0f73f0e247 100644 --- a/libraries/AP_Logger/LogStructure.h +++ b/libraries/AP_Logger/LogStructure.h @@ -1194,6 +1194,17 @@ struct PACKED log_OADijkstra { int32_t oa_lng; }; +struct PACKED log_SimpleAvoid { + LOG_PACKET_HEADER; + uint64_t time_us; + uint8_t state; + float desired_vel_x; + float desired_vel_y; + float modified_vel_x; + float modified_vel_y; + uint8_t backing_up; +}; + struct PACKED log_DSTL { LOG_PACKET_HEADER; uint64_t time_us; @@ -2107,6 +2118,16 @@ struct PACKED log_Arm_Disarm { // @Field: E: point associated with most recent action (East component) // @Field: D: point associated with most recent action (Down component) +// @LoggerMessage: SA +// @Description: Simple Avoidance messages +// @Field: TimeUS: Time since system startup +// @Field: State: True if Simple Avoidance is active +// @Field: DVelX: Desired velocity, X-Axis (Velocity before Avoidance) +// @Field: DVelY: Desired velocity, Y-Axis (Velocity before Avoidance) +// @Field: MVelX: Modified velocity, X-Axis (Velocity after Avoidance) +// @Field: MVelY: Modified velocity, Y-Axis (Velocity after Avoidance) +// @Field: Back: True if vehicle is backing away + // @LoggerMessage: TERR // @Description: Terrain database infomration // @Field: TimeUS: Time since system startup @@ -2436,6 +2457,8 @@ struct PACKED log_Arm_Disarm { "OABR","QBHHBfLLLL","TimeUS,Active,DesYaw,Yaw,ResChg,Mar,DLat,DLng,OALat,OALng", "sbdd-mDUDU", "F-----GGGG" }, \ { LOG_OA_DIJKSTRA_MSG, sizeof(log_OADijkstra), \ "OADJ","QBBBBLLLL","TimeUS,State,Err,CurrPoint,TotPoints,DLat,DLng,OALat,OALng", "sbbbbDUDU", "F----GGGG" }, \ + { LOG_SIMPLE_AVOID_MSG, sizeof(log_SimpleAvoid), \ + "SA", "QBffffB","TimeUS,State,DVelX,DVelY,MVelX,MVelY,Back", "sbnnnnb", "F------"}, \ { LOG_IMU2_MSG, sizeof(log_IMU), \ "IMU2", IMU_FMT, IMU_LABELS, IMU_UNITS, IMU_MULTS }, \ { LOG_IMU3_MSG, sizeof(log_IMU), \ @@ -2728,6 +2751,7 @@ enum LogMessages : uint8_t { LOG_OA_BENDYRULER_MSG, LOG_OA_DIJKSTRA_MSG, LOG_VISUALVEL_MSG, + LOG_SIMPLE_AVOID_MSG, _LOG_LAST_MSG_ };