From 7e79999b86855f91efcb12882d6f54aeebf49a9a Mon Sep 17 00:00:00 2001 From: Rishabh Date: Sun, 9 May 2021 23:17:47 +0530 Subject: [PATCH] AC_Avoid: reenable avoidance logging --- libraries/AC_Avoidance/AC_Avoid.cpp | 17 +++++++++++++++++ libraries/AC_Avoidance/AC_Avoid.h | 2 +- libraries/AC_Avoidance/AC_Avoidance_Logging.cpp | 4 +++- libraries/AC_Avoidance/LogStructure.h | 6 +++++- 4 files changed, 26 insertions(+), 3 deletions(-) diff --git a/libraries/AC_Avoidance/AC_Avoid.cpp b/libraries/AC_Avoidance/AC_Avoid.cpp index 97af9b5dd9..756cc4b2e5 100644 --- a/libraries/AC_Avoidance/AC_Avoid.cpp +++ b/libraries/AC_Avoidance/AC_Avoid.cpp @@ -241,6 +241,23 @@ void AC_Avoid::adjust_velocity(Vector3f &desired_vel_cms, bool &backing_up, floa if (desired_vel_cms_original != desired_vel_cms) { _last_limit_time = AP_HAL::millis(); } + + if (limits_active()) { + // log at not more than 10hz (adjust_velocity method can be potentially called at 400hz!) + uint32_t now = AP_HAL::millis(); + if ((now - _last_log_ms) > 100) { + _last_log_ms = now; + Write_SimpleAvoidance(true, desired_vel_cms_original, desired_vel_cms, backing_up); + } + } else { + // avoidance isn't active anymore + // log once so that it registers in logs + if (_last_log_ms) { + Write_SimpleAvoidance(false, desired_vel_cms_original, desired_vel_cms, backing_up); + // this makes sure logging won't run again till it is active + _last_log_ms = 0; + } + } } /* diff --git a/libraries/AC_Avoidance/AC_Avoid.h b/libraries/AC_Avoidance/AC_Avoid.h index 55f3ceb4ff..89a9cf1dc2 100644 --- a/libraries/AC_Avoidance/AC_Avoid.h +++ b/libraries/AC_Avoidance/AC_Avoid.h @@ -205,7 +205,7 @@ private: void get_proximity_roll_pitch_pct(float &roll_positive, float &roll_negative, float &pitch_positive, float &pitch_negative); // Logging function - void Write_SimpleAvoidance(const uint8_t state, const Vector2f& desired_vel, const Vector2f& modified_vel, const bool back_up) const; + void Write_SimpleAvoidance(const uint8_t state, const Vector3f& desired_vel, const Vector3f& modified_vel, const bool back_up) const; // parameters AP_Int8 _enabled; diff --git a/libraries/AC_Avoidance/AC_Avoidance_Logging.cpp b/libraries/AC_Avoidance/AC_Avoidance_Logging.cpp index 35a2bb76b9..a904f4cd6e 100644 --- a/libraries/AC_Avoidance/AC_Avoidance_Logging.cpp +++ b/libraries/AC_Avoidance/AC_Avoidance_Logging.cpp @@ -46,7 +46,7 @@ void AP_OADijkstra::Write_OADijkstra(const uint8_t state, const uint8_t error_id AP::logger().WriteBlock(&pkt, sizeof(pkt)); } -void AC_Avoid::Write_SimpleAvoidance(const uint8_t state, const Vector2f& desired_vel, const Vector2f& modified_vel, const bool back_up) const +void AC_Avoid::Write_SimpleAvoidance(const uint8_t state, const Vector3f& desired_vel, const Vector3f& modified_vel, const bool back_up) const { const struct log_SimpleAvoid pkt{ LOG_PACKET_HEADER_INIT(LOG_SIMPLE_AVOID_MSG), @@ -54,8 +54,10 @@ void AC_Avoid::Write_SimpleAvoidance(const uint8_t state, const Vector2f& desire state : state, desired_vel_x : desired_vel.x * 0.01f, desired_vel_y : desired_vel.y * 0.01f, + desired_vel_z : desired_vel.z * 0.01f, modified_vel_x : modified_vel.x * 0.01f, modified_vel_y : modified_vel.y * 0.01f, + modified_vel_z : modified_vel.z * 0.01f, backing_up : back_up, }; AP::logger().WriteBlock(&pkt, sizeof(pkt)); diff --git a/libraries/AC_Avoidance/LogStructure.h b/libraries/AC_Avoidance/LogStructure.h index 17ad807224..e9e393684d 100644 --- a/libraries/AC_Avoidance/LogStructure.h +++ b/libraries/AC_Avoidance/LogStructure.h @@ -71,8 +71,10 @@ struct PACKED log_OADijkstra { // @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: DVelZ: Desired velocity, Z-Axis (Velocity before Avoidance) // @Field: MVelX: Modified velocity, X-Axis (Velocity after Avoidance) // @Field: MVelY: Modified velocity, Y-Axis (Velocity after Avoidance) +// @Field: MVelZ: Modified velocity, Z-Axis (Velocity after Avoidance) // @Field: Back: True if vehicle is backing away struct PACKED log_SimpleAvoid { LOG_PACKET_HEADER; @@ -80,8 +82,10 @@ struct PACKED log_SimpleAvoid { uint8_t state; float desired_vel_x; float desired_vel_y; + float desired_vel_z; float modified_vel_x; float modified_vel_y; + float modified_vel_z; uint8_t backing_up; }; @@ -91,4 +95,4 @@ struct PACKED log_SimpleAvoid { { 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------"}, + "SA", "QBffffffB","TimeUS,State,DVelX,DVelY,DVelZ,MVelX,MVelY,MVelZ,Back", "sbnnnnnnb", "F--------"},