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();
}
#if HAL_LOGGING_ENABLED
if (limits_active()) {
// log at not more than 10hz (adjust_velocity method can be potentially called at 400hz!)
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;
}
}
#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 "AP_OADijkstra.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));
}
#endif // HAL_LOGGING_ENABLED

2
libraries/AC_Avoidance/AP_OABendyRuler.cpp
View File

@ -318,7 +318,7 @@ bool AP_OABendyRuler::search_vertical_path(const Location &current_loc, const Lo
destination_new = current_loc;
destination_new.offset_bearing_and_pitch(bearing_to_dest, pitch_delta, distance_to_dest);
_current_lookahead = MIN(_lookahead, _current_lookahead * 1.1f);
Write_OABendyRuler((uint8_t)OABendyType::OA_BENDY_VERTICAL, active, bearing_to_dest, pitch_delta, false, margin, destination, destination_new);
return active;
}

5
libraries/AC_Avoidance/AP_OABendyRuler.h
View File

@ -3,6 +3,7 @@
#include <AP_Common/AP_Common.h>
#include <AP_Common/Location.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
@ -67,7 +68,11 @@ private:
bool calc_margin_from_object_database(const Location &start, const Location &end, float &margin) const;
// 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;
#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
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_Math/AP_Math.h>
#include "AP_OAVisGraph.h"
#include <AP_Logger/AP_Logger_config.h>
/*
* 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
uint32_t _error_last_report_ms; // last time an error message was sent to GCS
#if HAL_LOGGING_ENABLED
// 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_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_visgraph_version;