mirror of https://github.com/ArduPilot/ardupilot
44 lines
2.0 KiB
C
44 lines
2.0 KiB
C
|
#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
|
||
|
|
};
|