mirror of https://github.com/ArduPilot/ardupilot
98 lines
4.3 KiB
C++
98 lines
4.3 KiB
C++
/*
|
|
This program is free software: you can redistribute it and/or modify
|
|
it under the terms of the GNU General Public License as published by
|
|
the Free Software Foundation, either version 3 of the License, or
|
|
(at your option) any later version.
|
|
|
|
This program is distributed in the hope that it will be useful,
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
GNU General Public License for more details.
|
|
|
|
You should have received a copy of the GNU General Public License
|
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
#pragma once
|
|
|
|
#include "AP_Proximity.h"
|
|
|
|
#if HAL_PROXIMITY_ENABLED
|
|
#include <AP_Common/AP_Common.h>
|
|
#include <AP_HAL/Semaphores.h>
|
|
|
|
class AP_Proximity_Backend
|
|
{
|
|
public:
|
|
// constructor. This incorporates initialisation as well.
|
|
AP_Proximity_Backend(AP_Proximity &_frontend, AP_Proximity::Proximity_State &_state, AP_Proximity_Params &_params);
|
|
|
|
// we declare a virtual destructor so that Proximity drivers can
|
|
// override with a custom destructor if need be
|
|
virtual ~AP_Proximity_Backend(void) {}
|
|
|
|
// update the state structure
|
|
virtual void update() = 0;
|
|
|
|
// get maximum and minimum distances (in meters) of sensor
|
|
virtual float distance_max() const = 0;
|
|
virtual float distance_min() const = 0;
|
|
|
|
// get distance upwards in meters. returns true on success
|
|
virtual bool get_upward_distance(float &distance) const { return false; }
|
|
|
|
// handle mavlink messages
|
|
virtual void handle_msg(const mavlink_message_t &msg) {}
|
|
|
|
#if AP_SCRIPTING_ENABLED
|
|
// handle scripting obstacle messages
|
|
virtual bool set_distance_min_max(float min, float max) { return false; }
|
|
// this is in body frame
|
|
virtual bool handle_script_distance_msg(float dist_m, float yaw_deg, float pitch_deg, bool push_to_boundary) { return false; }
|
|
virtual bool handle_script_3d_msg(const Vector3f &vec_to_obstacle, bool push_to_boundary) { return false; }
|
|
virtual bool update_virtual_boundary() { return false; }
|
|
#endif
|
|
|
|
// return the type of sensor
|
|
AP_Proximity::Type type() const { return (AP_Proximity::Type)params.type.get(); }
|
|
|
|
protected:
|
|
|
|
// set status and update valid_count
|
|
void set_status(AP_Proximity::Status status);
|
|
|
|
// correct an angle (in degrees) based on the orientation and yaw correction parameters
|
|
float correct_angle_for_orientation(float angle_degrees) const;
|
|
|
|
// check if a reading should be ignored because it falls into an ignore area (check_for_ign_area should be sent as false if this check is not needed)
|
|
// pitch is the vertical body-frame angle (in degrees) to the obstacle (0=directly ahead, 90 is above the vehicle)
|
|
// yaw is the horizontal body-frame angle (in degrees) to the obstacle (0=directly ahead of the vehicle, 90 is to the right of the vehicle)
|
|
// Also checks if obstacle is near land or out of range
|
|
// angles should be in degrees and in the range of 0 to 360, distance should be in meteres
|
|
bool ignore_reading(float pitch, float yaw, float distance_m, bool check_for_ign_area = true) const;
|
|
bool ignore_reading(float yaw, float distance_m, bool check_for_ign_area = true) const { return ignore_reading(0.0f, yaw, distance_m, check_for_ign_area); }
|
|
|
|
// database helpers. All angles are in degrees
|
|
static bool database_prepare_for_push(Vector3f ¤t_pos, Matrix3f &body_to_ned);
|
|
// Note: "angle" refers to yaw (in body frame) towards the obstacle
|
|
static void database_push(float angle, float pitch, float distance);
|
|
static void database_push(float angle, float distance) {
|
|
database_push(angle, 0.0f, distance);
|
|
}
|
|
|
|
static void database_push(float angle, float distance, uint32_t timestamp_ms, const Vector3f ¤t_pos, const Matrix3f &body_to_ned) {
|
|
database_push(angle, 0.0f, distance, timestamp_ms, current_pos, body_to_ned);
|
|
};
|
|
static void database_push(float angle, float pitch, float distance, uint32_t timestamp_ms, const Vector3f ¤t_pos, const Matrix3f &body_to_ned);
|
|
|
|
// semaphore for access to shared frontend data
|
|
HAL_Semaphore _sem;
|
|
|
|
AP_Proximity::Type _backend_type;
|
|
|
|
AP_Proximity &frontend;
|
|
AP_Proximity::Proximity_State &state; // reference to this instances state
|
|
AP_Proximity_Params ¶ms; // parameters for this backend
|
|
};
|
|
|
|
#endif // HAL_PROXIMITY_ENABLED
|