ardupilot/libraries/AC_WPNav/AC_WPNav_OA.h

45 lines
2.1 KiB
C++

#pragma once
#include <AC_WPNav/AC_WPNav.h>
#include <AC_Avoidance/AP_OAPathPlanner.h>
#include <AC_Avoidance/AP_OABendyRuler.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
};