mirror of https://github.com/ArduPilot/ardupilot
AC_Avoidance: move AC_Avoidance defines into libraries
This commit is contained in:
parent
dc97899ce8
commit
6e5ed88087
|
@ -13,6 +13,10 @@
|
||||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
|
#include "AC_Avoidance_config.h"
|
||||||
|
|
||||||
|
#if AP_AVOIDANCE_ENABLED
|
||||||
|
|
||||||
#include "AC_Avoid.h"
|
#include "AC_Avoid.h"
|
||||||
#include <AP_AHRS/AP_AHRS.h> // AHRS library
|
#include <AP_AHRS/AP_AHRS.h> // AHRS library
|
||||||
#include <AC_Fence/AC_Fence.h> // Failsafe fence library
|
#include <AC_Fence/AC_Fence.h> // Failsafe fence library
|
||||||
|
@ -1477,3 +1481,5 @@ AC_Avoid *ac_avoid()
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif // !APM_BUILD_Arduplane
|
#endif // !APM_BUILD_Arduplane
|
||||||
|
|
||||||
|
#endif // AP_AVOIDANCE_ENABLED
|
||||||
|
|
|
@ -1,5 +1,9 @@
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
|
#include "AC_Avoidance_config.h"
|
||||||
|
|
||||||
|
#if AP_AVOIDANCE_ENABLED
|
||||||
|
|
||||||
#include <AP_Common/AP_Common.h>
|
#include <AP_Common/AP_Common.h>
|
||||||
#include <AP_Param/AP_Param.h>
|
#include <AP_Param/AP_Param.h>
|
||||||
#include <AP_Math/AP_Math.h>
|
#include <AP_Math/AP_Math.h>
|
||||||
|
@ -228,3 +232,5 @@ private:
|
||||||
namespace AP {
|
namespace AP {
|
||||||
AC_Avoid *ac_avoid();
|
AC_Avoid *ac_avoid();
|
||||||
};
|
};
|
||||||
|
|
||||||
|
#endif // AP_AVOIDANCE_ENABLED
|
||||||
|
|
|
@ -7,6 +7,7 @@
|
||||||
#include "AP_OABendyRuler.h"
|
#include "AP_OABendyRuler.h"
|
||||||
#include <AP_Logger/AP_Logger.h>
|
#include <AP_Logger/AP_Logger.h>
|
||||||
|
|
||||||
|
#if AP_OAPATHPLANNER_BENDYRULER_ENABLED
|
||||||
void AP_OABendyRuler::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 AP_OABendyRuler::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
|
||||||
{
|
{
|
||||||
int32_t oa_dest_alt, final_alt;
|
int32_t oa_dest_alt, final_alt;
|
||||||
|
@ -32,7 +33,9 @@ void AP_OABendyRuler::Write_OABendyRuler(const uint8_t type, const bool active,
|
||||||
};
|
};
|
||||||
AP::logger().WriteBlock(&pkt, sizeof(pkt));
|
AP::logger().WriteBlock(&pkt, sizeof(pkt));
|
||||||
}
|
}
|
||||||
|
#endif // AP_OAPATHPLANNER_BENDYRULER_ENABLED
|
||||||
|
|
||||||
|
#if AP_OAPATHPLANNER_DIJKSTRA_ENABLED
|
||||||
void AP_OADijkstra::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 AP_OADijkstra::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
|
||||||
{
|
{
|
||||||
const struct log_OADijkstra pkt{
|
const struct log_OADijkstra pkt{
|
||||||
|
@ -49,7 +52,9 @@ void AP_OADijkstra::Write_OADijkstra(const uint8_t state, const uint8_t error_id
|
||||||
};
|
};
|
||||||
AP::logger().WriteBlock(&pkt, sizeof(pkt));
|
AP::logger().WriteBlock(&pkt, sizeof(pkt));
|
||||||
}
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#if AP_OAPATHPLANNER_ENABLED
|
||||||
void AP_OADijkstra::Write_Visgraph_point(const uint8_t version, const uint8_t point_num, const int32_t Lat, const int32_t Lon) const
|
void AP_OADijkstra::Write_Visgraph_point(const uint8_t version, const uint8_t point_num, const int32_t Lat, const int32_t Lon) const
|
||||||
{
|
{
|
||||||
const struct log_OD_Visgraph pkt{
|
const struct log_OD_Visgraph pkt{
|
||||||
|
@ -62,8 +67,9 @@ void AP_OADijkstra::Write_Visgraph_point(const uint8_t version, const uint8_t po
|
||||||
};
|
};
|
||||||
AP::logger().WriteBlock(&pkt, sizeof(pkt));
|
AP::logger().WriteBlock(&pkt, sizeof(pkt));
|
||||||
}
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#if AP_AVOIDANCE_ENABLED
|
||||||
void AC_Avoid::Write_SimpleAvoidance(const uint8_t state, const Vector3f& desired_vel, const Vector3f& 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{
|
const struct log_SimpleAvoid pkt{
|
||||||
|
@ -80,5 +86,6 @@ 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
|
||||||
|
|
||||||
#endif // HAL_LOGGING_ENABLED
|
#endif // HAL_LOGGING_ENABLED
|
||||||
|
|
|
@ -0,0 +1,30 @@
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <AP_HAL/AP_HAL_Boards.h>
|
||||||
|
#include <AC_Fence/AC_Fence_config.h>
|
||||||
|
|
||||||
|
#ifndef AP_AVOIDANCE_ENABLED
|
||||||
|
#define AP_AVOIDANCE_ENABLED AP_FENCE_ENABLED
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#ifndef AP_OAPATHPLANNER_ENABLED
|
||||||
|
#define AP_OAPATHPLANNER_ENABLED AP_FENCE_ENABLED
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#ifndef AP_OAPATHPLANNER_BACKEND_DEFAULT_ENABLED
|
||||||
|
#define AP_OAPATHPLANNER_BACKEND_DEFAULT_ENABLED AP_OAPATHPLANNER_ENABLED
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#ifndef AP_OAPATHPLANNER_BENDYRULER_ENABLED
|
||||||
|
#define AP_OAPATHPLANNER_BENDYRULER_ENABLED AP_OAPATHPLANNER_BACKEND_DEFAULT_ENABLED
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#ifndef AP_OAPATHPLANNER_DIJKSTRA_ENABLED
|
||||||
|
#define AP_OAPATHPLANNER_DIJKSTRA_ENABLED AP_OAPATHPLANNER_BACKEND_DEFAULT_ENABLED
|
||||||
|
#endif
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
#ifndef AP_OADATABASE_ENABLED
|
||||||
|
#define AP_OADATABASE_ENABLED AP_OAPATHPLANNER_ENABLED
|
||||||
|
#endif
|
|
@ -13,6 +13,10 @@
|
||||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
|
#include "AC_Avoidance_config.h"
|
||||||
|
|
||||||
|
#if AP_OAPATHPLANNER_BENDYRULER_ENABLED
|
||||||
|
|
||||||
#include "AP_OABendyRuler.h"
|
#include "AP_OABendyRuler.h"
|
||||||
#include <AC_Avoidance/AP_OADatabase.h>
|
#include <AC_Avoidance/AP_OADatabase.h>
|
||||||
#include <AC_Fence/AC_Fence.h>
|
#include <AC_Fence/AC_Fence.h>
|
||||||
|
@ -708,3 +712,5 @@ bool AP_OABendyRuler::calc_margin_from_object_database(const Location &start, co
|
||||||
|
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#endif // AP_OAPATHPLANNER_BENDYRULER_ENABLED
|
||||||
|
|
|
@ -1,5 +1,9 @@
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
|
#include "AC_Avoidance_config.h"
|
||||||
|
|
||||||
|
#if AP_OAPATHPLANNER_BENDYRULER_ENABLED
|
||||||
|
|
||||||
#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>
|
||||||
|
@ -88,3 +92,5 @@ private:
|
||||||
float _bearing_prev; // stored bearing in degrees
|
float _bearing_prev; // stored bearing in degrees
|
||||||
Location _destination_prev; // previous destination, to check if there has been a change in destination
|
Location _destination_prev; // previous destination, to check if there has been a change in destination
|
||||||
};
|
};
|
||||||
|
|
||||||
|
#endif // AP_OAPATHPLANNER_BENDYRULER_ENABLED
|
||||||
|
|
|
@ -11,6 +11,10 @@
|
||||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
|
#include "AC_Avoidance_config.h"
|
||||||
|
|
||||||
|
#if AP_OADATABASE_ENABLED
|
||||||
|
|
||||||
#include "AP_OADatabase.h"
|
#include "AP_OADatabase.h"
|
||||||
|
|
||||||
#include <AP_AHRS/AP_AHRS.h>
|
#include <AP_AHRS/AP_AHRS.h>
|
||||||
|
@ -482,3 +486,5 @@ AP_OADatabase *oadatabase()
|
||||||
}
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#endif // AP_OADATABASE_ENABLED
|
||||||
|
|
|
@ -1,5 +1,9 @@
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
|
#include "AC_Avoidance_config.h"
|
||||||
|
|
||||||
|
#if AP_OADATABASE_ENABLED
|
||||||
|
|
||||||
#include <AP_HAL/Semaphores.h>
|
#include <AP_HAL/Semaphores.h>
|
||||||
#include <AP_Math/AP_Math.h>
|
#include <AP_Math/AP_Math.h>
|
||||||
#include <GCS_MAVLink/GCS_MAVLink.h>
|
#include <GCS_MAVLink/GCS_MAVLink.h>
|
||||||
|
@ -115,4 +119,4 @@ namespace AP {
|
||||||
AP_OADatabase *oadatabase();
|
AP_OADatabase *oadatabase();
|
||||||
};
|
};
|
||||||
|
|
||||||
|
#endif // AP_OADATABASE_ENABLED
|
||||||
|
|
|
@ -13,6 +13,10 @@
|
||||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
|
#include "AC_Avoidance_config.h"
|
||||||
|
|
||||||
|
#if AP_OAPATHPLANNER_DIJKSTRA_ENABLED
|
||||||
|
|
||||||
#include "AP_OADijkstra.h"
|
#include "AP_OADijkstra.h"
|
||||||
#include "AP_OAPathPlanner.h"
|
#include "AP_OAPathPlanner.h"
|
||||||
|
|
||||||
|
@ -1011,3 +1015,5 @@ bool AP_OADijkstra::convert_node_to_point(const AP_OAVisGraph::OAItemID& id, Vec
|
||||||
}
|
}
|
||||||
#endif // AP_FENCE_ENABLED
|
#endif // AP_FENCE_ENABLED
|
||||||
|
|
||||||
|
|
||||||
|
#endif // AP_OAPATHPLANNER_DIJKSTRA_ENABLED
|
||||||
|
|
|
@ -1,5 +1,9 @@
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
|
#include "AC_Avoidance_config.h"
|
||||||
|
|
||||||
|
#if AP_OAPATHPLANNER_DIJKSTRA_ENABLED
|
||||||
|
|
||||||
#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>
|
||||||
|
@ -219,3 +223,5 @@ private:
|
||||||
// reference to AP_OAPathPlanner options param
|
// reference to AP_OAPathPlanner options param
|
||||||
AP_Int16 &_options;
|
AP_Int16 &_options;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
#endif // AP_OAPATHPLANNER_DIJKSTRA_ENABLED
|
||||||
|
|
|
@ -13,6 +13,10 @@
|
||||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
|
#include "AC_Avoidance_config.h"
|
||||||
|
|
||||||
|
#if AP_OAPATHPLANNER_ENABLED
|
||||||
|
|
||||||
#include "AP_OAPathPlanner.h"
|
#include "AP_OAPathPlanner.h"
|
||||||
#include <AP_Math/AP_Math.h>
|
#include <AP_Math/AP_Math.h>
|
||||||
#include <AP_AHRS/AP_AHRS.h>
|
#include <AP_AHRS/AP_AHRS.h>
|
||||||
|
@ -432,3 +436,5 @@ AP_OAPathPlanner *ap_oapathplanner()
|
||||||
}
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#endif // AP_OAPATHPLANNER_ENABLED
|
||||||
|
|
|
@ -1,5 +1,9 @@
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
|
#include "AC_Avoidance_config.h"
|
||||||
|
|
||||||
|
#if AP_OAPATHPLANNER_ENABLED
|
||||||
|
|
||||||
#include <AP_Common/AP_Common.h>
|
#include <AP_Common/AP_Common.h>
|
||||||
#include <AP_Common/Location.h>
|
#include <AP_Common/Location.h>
|
||||||
#include <AP_Param/AP_Param.h>
|
#include <AP_Param/AP_Param.h>
|
||||||
|
@ -135,3 +139,5 @@ private:
|
||||||
namespace AP {
|
namespace AP {
|
||||||
AP_OAPathPlanner *ap_oapathplanner();
|
AP_OAPathPlanner *ap_oapathplanner();
|
||||||
};
|
};
|
||||||
|
|
||||||
|
#endif // AP_OAPATHPLANNER_ENABLED
|
||||||
|
|
|
@ -13,6 +13,10 @@
|
||||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
|
#include "AC_Avoidance_config.h"
|
||||||
|
|
||||||
|
#if AP_OAPATHPLANNER_ENABLED
|
||||||
|
|
||||||
#include "AP_OAVisGraph.h"
|
#include "AP_OAVisGraph.h"
|
||||||
|
|
||||||
// constructor initialises expanding array to use 20 elements per chunk
|
// constructor initialises expanding array to use 20 elements per chunk
|
||||||
|
@ -39,3 +43,5 @@ bool AP_OAVisGraph::add_item(const OAItemID &id1, const OAItemID &id2, float dis
|
||||||
_num_items++;
|
_num_items++;
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#endif // AP_OAPATHPLANNER_ENABLED
|
||||||
|
|
|
@ -1,5 +1,9 @@
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
|
#include "AC_Avoidance_config.h"
|
||||||
|
|
||||||
|
#if AP_OAPATHPLANNER_ENABLED
|
||||||
|
|
||||||
#include <AP_Common/AP_Common.h>
|
#include <AP_Common/AP_Common.h>
|
||||||
#include <AP_Common/AP_ExpandingArray.h>
|
#include <AP_Common/AP_ExpandingArray.h>
|
||||||
|
|
||||||
|
@ -54,3 +58,5 @@ private:
|
||||||
AP_ExpandingArray<VisGraphItem> _items;
|
AP_ExpandingArray<VisGraphItem> _items;
|
||||||
uint16_t _num_items;
|
uint16_t _num_items;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
#endif // AP_OAPATHPLANNER_ENABLED
|
||||||
|
|
Loading…
Reference in New Issue