ardupilot/libraries/AP_Proximity/AP_Proximity.h

203 lines
7.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_HAL/AP_HAL.h>
#include <AP_HAL/AP_HAL_Boards.h>
#ifndef HAL_PROXIMITY_ENABLED
#define HAL_PROXIMITY_ENABLED (!HAL_MINIMIZE_FEATURES && BOARD_FLASH_SIZE > 1024)
#endif
#if HAL_PROXIMITY_ENABLED
#include <AP_Common/AP_Common.h>
#include <AP_Param/AP_Param.h>
#include <AP_Math/AP_Math.h>
#include <GCS_MAVLink/GCS_MAVLink.h>
#define PROXIMITY_MAX_INSTANCES 1 // Maximum number of proximity sensor instances available on this platform
#define PROXIMITY_MAX_IGNORE 6 // up to six areas can be ignored
#define PROXIMITY_MAX_DIRECTION 8
#define PROXIMITY_SENSOR_ID_START 10
class AP_Proximity_Backend;
class AP_Proximity
{
public:
friend class AP_Proximity_Backend;
AP_Proximity();
AP_Proximity(const AP_Proximity &other) = delete;
AP_Proximity &operator=(const AP_Proximity) = delete;
// Proximity driver types
enum class Type {
None = 0,
SF40C_v09 = 1,
MAV = 2,
TRTOWER = 3,
RangeFinder = 4,
RPLidarA2 = 5,
TRTOWEREVO = 6,
SF40C = 7,
SF45B = 8,
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
SITL = 10,
AirSimSITL = 12,
#endif
CYGBOT_D1 = 13,
};
enum class Status {
NotConnected = 0,
NoData,
Good
};
// structure holding distances in PROXIMITY_MAX_DIRECTION directions. used for sending distances to ground station
struct Proximity_Distance_Array {
uint8_t orientation[PROXIMITY_MAX_DIRECTION]; // orientation (i.e. rough direction) of the distance (see MAV_SENSOR_ORIENTATION)
float distance[PROXIMITY_MAX_DIRECTION]; // distance in meters
bool valid(uint8_t offset) const {
// returns true if the distance stored at offset is valid
return (offset < 8 && (offset_valid & (1U<<offset)));
};
uint8_t offset_valid; // bitmask
};
// detect and initialise any available proximity sensors
void init(void);
// update state of all proximity sensors. Should be called at high rate from main loop
void update(void);
// return sensor orientation and yaw correction
uint8_t get_orientation(uint8_t instance) const;
int16_t get_yaw_correction(uint8_t instance) const;
float get_filter_freq() const { return _filt_freq; }
// return sensor health
Status get_status(uint8_t instance) const;
Status get_status() const;
// Return the number of proximity sensors
uint8_t num_sensors(void) const {
return num_instances;
}
// get distances in PROXIMITY_MAX_DIRECTION directions. used for sending distances to ground station
bool get_horizontal_distances(Proximity_Distance_Array &prx_dist_array) const;
// get raw and filtered distances in 8 directions per layer. used for logging
bool get_active_layer_distances(uint8_t layer, AP_Proximity::Proximity_Distance_Array &prx_dist_array, AP_Proximity::Proximity_Distance_Array &prx_filt_dist_array) const;
// get total number of obstacles, used in GPS based Simple Avoidance
uint8_t get_obstacle_count() const;
// get vector to obstacle based on obstacle_num passed, used in GPS based Simple Avoidance
bool get_obstacle(uint8_t obstacle_num, Vector3f& vec_to_obstacle) const;
// returns shortest distance to "obstacle_num" obstacle, from a line segment formed between "seg_start" and "seg_end"
// returns FLT_MAX if it's an invalid instance.
bool closest_point_from_segment_to_obstacle(uint8_t obstacle_num, const Vector3f& seg_start, const Vector3f& seg_end, Vector3f& closest_point) const;
// get distance and angle to closest object (used for pre-arm check)
// returns true on success, false if no valid readings
bool get_closest_object(float& angle_deg, float &distance) const;
// get number of objects, angle and distance - used for non-GPS avoidance
uint8_t get_object_count() const;
bool get_object_angle_and_distance(uint8_t object_number, float& angle_deg, float &distance) const;
// get number of layers
uint8_t get_num_layers() const;
// get maximum and minimum distances (in meters) of primary sensor
float distance_max() const;
float distance_min() const;
// handle mavlink DISTANCE_SENSOR messages
void handle_msg(const mavlink_message_t &msg);
// The Proximity_State structure is filled in by the backend driver
struct Proximity_State {
uint8_t instance; // the instance number of this proximity sensor
Status status; // sensor status
};
//
// support for upward facing sensors
//
// get distance upwards in meters. returns true on success
bool get_upward_distance(uint8_t instance, float &distance) const;
bool get_upward_distance(float &distance) const;
Type get_type(uint8_t instance) const;
// true if raw distances should be logged
bool get_raw_log_enable() const { return _raw_log_enable; }
// parameter list
static const struct AP_Param::GroupInfo var_info[];
static AP_Proximity *get_singleton(void) { return _singleton; };
// methods for mavlink SYS_STATUS message (send_sys_status)
// these methods cover only the primary instance
bool sensor_present() const;
bool sensor_enabled() const;
bool sensor_failed() const;
// set alt as read from downward facing rangefinder. Tilt is already adjusted for
void set_rangefinder_alt(bool use, bool healthy, float alt_cm);
private:
static AP_Proximity *_singleton;
Proximity_State state[PROXIMITY_MAX_INSTANCES];
AP_Proximity_Backend *drivers[PROXIMITY_MAX_INSTANCES];
uint8_t primary_instance;
uint8_t num_instances;
bool valid_instance(uint8_t i) const {
if (drivers[i] == nullptr) {
return false;
}
return (Type)_type[i].get() != Type::None;
}
// parameters for all instances
AP_Int8 _type[PROXIMITY_MAX_INSTANCES];
AP_Int8 _orientation[PROXIMITY_MAX_INSTANCES];
AP_Int16 _yaw_correction[PROXIMITY_MAX_INSTANCES];
AP_Int16 _ignore_angle_deg[PROXIMITY_MAX_IGNORE]; // angle (in degrees) of area that should be ignored by sensor (i.e. leg shows up)
AP_Int8 _ignore_width_deg[PROXIMITY_MAX_IGNORE]; // width of beam (in degrees) that should be ignored
AP_Int8 _raw_log_enable; // enable logging raw distances
AP_Int8 _ign_gnd_enable; // true if land detection should be enabled
AP_Float _filt_freq; // cutoff frequency for low pass filter
void detect_instance(uint8_t instance);
};
namespace AP {
AP_Proximity *proximity();
};
#endif // HAL_PROXIMITY_ENABLED