You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
43 lines
2.0 KiB
43 lines
2.0 KiB
#pragma once |
|
|
|
#include <AC_WPNav/AC_WPNav.h> |
|
#include <AC_Avoidance/AP_OAPathPlanner.h> |
|
|
|
class AC_WPNav_OA : public AC_WPNav |
|
{ |
|
|
|
public: |
|
/// Constructor |
|
AC_WPNav_OA(const AP_InertialNav& inav, const AP_AHRS_View& ahrs, AC_PosControl& pos_control, const AC_AttitudeControl& attitude_control); |
|
|
|
// returns object avoidance adjusted wp location using location class |
|
// returns false if unable to convert from target vector to global coordinates |
|
bool get_oa_wp_destination(Location& destination) const override; |
|
|
|
/// set origin and destination waypoints using position vectors (distance from ekf origin in cm) |
|
/// terrain_alt should be true if origin.z and destination.z are desired altitudes above terrain (false if these are alt-above-ekf-origin) |
|
/// returns false on failure (likely caused by missing terrain data) |
|
bool set_wp_origin_and_destination(const Vector3f& origin, const Vector3f& destination, bool terrain_alt = false) override; |
|
|
|
/// get horizontal distance to destination in cm |
|
/// always returns distance to final destination (i.e. does not use oa adjusted destination) |
|
float get_wp_distance_to_destination() const override; |
|
|
|
/// get bearing to next waypoint in centi-degrees |
|
/// always returns bearing to final destination (i.e. does not use oa adjusted destination) |
|
int32_t get_wp_bearing_to_destination() const override; |
|
|
|
/// true when we have come within RADIUS cm of the final destination |
|
bool reached_wp_destination() const override; |
|
|
|
/// run the wp controller |
|
bool update_wpnav() override; |
|
|
|
protected: |
|
|
|
// oa path planning variables |
|
AP_OAPathPlanner::OA_RetState _oa_state; // state of object avoidance, if OA_SUCCESS we use _oa_destination to avoid obstacles |
|
Vector3f _origin_oabak; // backup of _origin so it can be restored when oa completes |
|
Vector3f _destination_oabak; // backup of _destination so it can be restored when oa completes |
|
Location _oa_destination; // intermediate destination during avoidance |
|
};
|
|
|