mirror of https://github.com/ArduPilot/ardupilot
AP_OADatabase: available regardless of hal-minimize-features
In Copter with minimize feature set the entire AP_PathPlanning feature including the database are unavailable Rover has enough space for the database even with minimise features set
This commit is contained in:
parent
50b4dd2599
commit
985a15709a
|
@ -355,7 +355,6 @@ bool AP_OABendyRuler::calc_margin_from_inclusion_and_exclusion_circles(const Loc
|
|||
// on success returns true and updates margin
|
||||
bool AP_OABendyRuler::calc_margin_from_object_database(const Location &start, const Location &end, float &margin)
|
||||
{
|
||||
#if !HAL_MINIMIZE_FEATURES
|
||||
// exit immediately if db is empty
|
||||
AP_OADatabase *oaDb = AP::oadatabase();
|
||||
if (oaDb == nullptr || !oaDb->healthy()) {
|
||||
|
@ -386,6 +385,5 @@ bool AP_OABendyRuler::calc_margin_from_object_database(const Location &start, co
|
|||
return true;
|
||||
}
|
||||
|
||||
#endif
|
||||
return false;
|
||||
}
|
||||
|
|
|
@ -13,8 +13,6 @@
|
|||
|
||||
#include "AP_OADatabase.h"
|
||||
|
||||
#if !HAL_MINIMIZE_FEATURES
|
||||
|
||||
#include <AP_AHRS/AP_AHRS.h>
|
||||
#include <GCS_MAVLink/GCS.h>
|
||||
#include <AP_Math/AP_Math.h>
|
||||
|
@ -439,7 +437,6 @@ void AP_OADatabase::send_adsb_vehicle(mavlink_channel_t chan, uint16_t interval_
|
|||
|
||||
// singleton instance
|
||||
AP_OADatabase *AP_OADatabase::_singleton;
|
||||
#endif //!HAL_MINIMIZE_FEATURES
|
||||
|
||||
namespace AP {
|
||||
AP_OADatabase *oadatabase()
|
||||
|
|
|
@ -3,8 +3,6 @@
|
|||
#include <AP_HAL/AP_HAL.h>
|
||||
#include <AP_Math/AP_Math.h>
|
||||
#include <GCS_MAVLink/GCS_MAVLink.h>
|
||||
|
||||
#if !HAL_MINIMIZE_FEATURES
|
||||
#include <AP_Param/AP_Param.h>
|
||||
|
||||
class AP_OADatabase {
|
||||
|
@ -111,16 +109,6 @@ private:
|
|||
|
||||
static AP_OADatabase *_singleton;
|
||||
};
|
||||
#else
|
||||
class AP_OADatabase {
|
||||
public:
|
||||
static AP_OADatabase *get_singleton() { return nullptr; }
|
||||
void init() {};
|
||||
void queue_push(const Vector2f &pos, uint32_t timestamp_ms, float distance) {};
|
||||
bool healthy() const { return false; }
|
||||
void send_adsb_vehicle(mavlink_channel_t chan, uint16_t interval_ms) {};
|
||||
};
|
||||
#endif // #if !HAL_MINIMIZE_FEATURES
|
||||
|
||||
namespace AP {
|
||||
AP_OADatabase *oadatabase();
|
||||
|
|
|
@ -57,11 +57,9 @@ const AP_Param::GroupInfo AP_OAPathPlanner::var_info[] = {
|
|||
// @User: Standard
|
||||
AP_GROUPINFO("MARGIN_MAX", 3, AP_OAPathPlanner, _margin_max, OA_MARGIN_MAX_DEFAULT),
|
||||
|
||||
#if !HAL_MINIMIZE_FEATURES
|
||||
// @Group: DB_
|
||||
// @Path: AP_OADatabase.cpp
|
||||
AP_SUBGROUPINFO(_oadatabase, "DB_", 4, AP_OAPathPlanner, AP_OADatabase),
|
||||
#endif
|
||||
|
||||
AP_GROUPEND
|
||||
};
|
||||
|
@ -195,7 +193,6 @@ AP_OAPathPlanner::OA_RetState AP_OAPathPlanner::mission_avoidance(const Location
|
|||
void AP_OAPathPlanner::avoidance_thread()
|
||||
{
|
||||
while (true) {
|
||||
#if !HAL_MINIMIZE_FEATURES
|
||||
|
||||
// if database queue needs attention, service it faster
|
||||
if (_oadatabase.process_queue()) {
|
||||
|
@ -211,10 +208,6 @@ void AP_OAPathPlanner::avoidance_thread()
|
|||
avoidance_latest_ms = now;
|
||||
|
||||
_oadatabase.update();
|
||||
#else
|
||||
hal.scheduler->delay(100);
|
||||
const uint32_t now = AP_HAL::millis();
|
||||
#endif
|
||||
|
||||
Location origin_new;
|
||||
Location destination_new;
|
||||
|
|
|
@ -91,9 +91,7 @@ private:
|
|||
AP_OABendyRuler *_oabendyruler; // Bendy Ruler algorithm
|
||||
AP_OADijkstra *_oadijkstra; // Dijkstra's algorithm
|
||||
AP_OADatabase _oadatabase; // Database of dynamic objects to avoid
|
||||
#if !HAL_MINIMIZE_FEATURES
|
||||
uint32_t avoidance_latest_ms; // last time Dijkstra's or BendyRuler algorithms ran
|
||||
#endif
|
||||
|
||||
static AP_OAPathPlanner *_singleton;
|
||||
};
|
||||
|
|
Loading…
Reference in New Issue