Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_Avoidance: allow compilation with HAL_LOGGING_ENABLED false

This commit is contained in:
Peter Barker 2023-07-14 10:58:04 +10:00 committed by Andrew Tridgell
parent 2eede45f3a
commit b1c29c5033
5 changed files with 20 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AC_Avoidance/AC_Avoid.cpp
View File

@ -259,6 +259,7 @@ void AC_Avoid::adjust_velocity(Vector3f &desired_vel_cms, bool &backing_up, floa
_last_limit_time = AP_HAL::millis(); _last_limit_time = AP_HAL::millis();
} }
#if HAL_LOGGING_ENABLED
if (limits_active()) { if (limits_active()) {
// log at not more than 10hz (adjust_velocity method can be potentially called at 400hz!) // log at not more than 10hz (adjust_velocity method can be potentially called at 400hz!)
uint32_t now = AP_HAL::millis(); uint32_t now = AP_HAL::millis();
@ -275,6 +276,7 @@ void AC_Avoid::adjust_velocity(Vector3f &desired_vel_cms, bool &backing_up, floa
_last_log_ms = 0; _last_log_ms = 0;
} }
} }
#endif
} }
/* /*

6
libraries/AC_Avoidance/AC_Avoidance_Logging.cpp
View File

@ -1,3 +1,7 @@
#include <AP_Logger/AP_Logger_config.h>
#if HAL_LOGGING_ENABLED
#include "AC_Avoid.h" #include "AC_Avoid.h"
#include "AP_OADijkstra.h" #include "AP_OADijkstra.h"
#include "AP_OABendyRuler.h" #include "AP_OABendyRuler.h"
@ -76,3 +80,5 @@ void AC_Avoid::Write_SimpleAvoidance(const uint8_t state, const Vector3f& desire
}; };
AP::logger().WriteBlock(&pkt, sizeof(pkt)); AP::logger().WriteBlock(&pkt, sizeof(pkt));
} }
#endif // HAL_LOGGING_ENABLED

5
libraries/AC_Avoidance/AP_OABendyRuler.h
View File

@ -3,6 +3,7 @@
#include <AP_Common/AP_Common.h> #include <AP_Common/AP_Common.h>
#include <AP_Common/Location.h> #include <AP_Common/Location.h>
#include <AP_Math/AP_Math.h> #include <AP_Math/AP_Math.h>
#include <AP_Logger/AP_Logger_config.h>
/* /*
* BendyRuler avoidance algorithm for avoiding the polygon and circular fence and dynamic objects detected by the proximity sensor * BendyRuler avoidance algorithm for avoiding the polygon and circular fence and dynamic objects detected by the proximity sensor
@ -67,7 +68,11 @@ private:
bool calc_margin_from_object_database(const Location &start, const Location &end, float &margin) const; bool calc_margin_from_object_database(const Location &start, const Location &end, float &margin) const;
// Logging function // Logging function
#if HAL_LOGGING_ENABLED
void 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; void 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;
#else
void 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 {}
#endif
// OA common parameters // OA common parameters
float _margin_max; // object avoidance will ignore objects more than this many meters from vehicle float _margin_max; // object avoidance will ignore objects more than this many meters from vehicle

6
libraries/AC_Avoidance/AP_OADijkstra.h
View File

@ -4,6 +4,7 @@
#include <AP_Common/Location.h> #include <AP_Common/Location.h>
#include <AP_Math/AP_Math.h> #include <AP_Math/AP_Math.h>
#include "AP_OAVisGraph.h" #include "AP_OAVisGraph.h"
#include <AP_Logger/AP_Logger_config.h>
/* /*
* Dijkstra's algorithm for path planning around polygon fence * Dijkstra's algorithm for path planning around polygon fence
@ -204,9 +205,14 @@ private:
AP_OADijkstra_Error _error_last_id; // last error id sent to GCS AP_OADijkstra_Error _error_last_id; // last error id sent to GCS
uint32_t _error_last_report_ms; // last time an error message was sent to GCS uint32_t _error_last_report_ms; // last time an error message was sent to GCS
#if HAL_LOGGING_ENABLED
// Logging functions // Logging functions
void 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; void 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;
void Write_Visgraph_point(const uint8_t version, const uint8_t point_num, const int32_t Lat, const int32_t Lon) const; void Write_Visgraph_point(const uint8_t version, const uint8_t point_num, const int32_t Lat, const int32_t Lon) const;
#else
void 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 {}
void Write_Visgraph_point(const uint8_t version, const uint8_t point_num, const int32_t Lat, const int32_t Lon) const {}
#endif
uint8_t _log_num_points; uint8_t _log_num_points;
uint8_t _log_visgraph_version; uint8_t _log_visgraph_version;