Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Proximity: reorder method declarations and implementations

This commit is contained in:
Randy Mackay 2022-07-05 16:58:38 +09:00 committed by Andrew Tridgell
parent b6d133e285
commit eff86c88ab
2 changed files with 279 additions and 268 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

443
libraries/AP_Proximity/AP_Proximity.cpp
View File

@ -103,7 +103,7 @@ AP_Proximity::AP_Proximity()
// initialise the Proximity class. We do detection of attached sensors here
// we don't allow for hot-plugging of sensors (i.e. reboot required)
void AP_Proximity::init(void)
void AP_Proximity::init()
{
if (num_instances != 0) {
// init called a 2nd time?
@ -123,7 +123,7 @@ void AP_Proximity::init(void)
}
// update Proximity state for all instances. This should be called at a high rate by the main loop
void AP_Proximity::update(void)
void AP_Proximity::update()
{
for (uint8_t i=0; i<num_instances; i++) {
if (!valid_instance(i)) {
@ -141,6 +141,14 @@ void AP_Proximity::update(void)
}
}
AP_Proximity::Type AP_Proximity::get_type(uint8_t instance) const
{
if (instance < PROXIMITY_MAX_INSTANCES) {
return (Type)((uint8_t)params[instance].type);
}
return Type::None;
}
// return sensor orientation
uint8_t AP_Proximity::get_orientation(uint8_t instance) const
{
@ -177,6 +185,114 @@ AP_Proximity::Status AP_Proximity::get_status() const
return get_status(primary_instance);
}
// get maximum and minimum distances (in meters) of primary sensor
float AP_Proximity::distance_max() const
{
if (!valid_instance(primary_instance)) {
return 0.0f;
}
// get maximum distance from backend
return drivers[primary_instance]->distance_max();
}
float AP_Proximity::distance_min() const
{
if (!valid_instance(primary_instance)) {
return 0.0f;
}
// get minimum distance from backend
return drivers[primary_instance]->distance_min();
}
// get distances in 8 directions. used for sending distances to ground station
bool AP_Proximity::get_horizontal_distances(Proximity_Distance_Array &prx_dist_array) const
{
if (!valid_instance(primary_instance)) {
return false;
}
// get distances from backend
return drivers[primary_instance]->get_horizontal_distances(prx_dist_array);
}
// get number of layers.
uint8_t AP_Proximity::get_num_layers() const
{
if (!valid_instance(primary_instance)) {
return 0;
}
return drivers[primary_instance]->get_num_layers();
}
// get raw and filtered distances in 8 directions per layer. used for logging
bool AP_Proximity::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
{
if (!valid_instance(primary_instance)) {
return false;
}
// get distances from backend
return drivers[primary_instance]->get_active_layer_distances(layer, prx_dist_array, prx_filt_dist_array);
}
// get total number of obstacles, used in GPS based Simple Avoidance
uint8_t AP_Proximity::get_obstacle_count() const
{
if (!valid_instance(primary_instance)) {
return 0;
}
return drivers[primary_instance]->get_obstacle_count();
}
// get vector to obstacle based on obstacle_num passed, used in GPS based Simple Avoidance
bool AP_Proximity::get_obstacle(uint8_t obstacle_num, Vector3f& vec_to_obstacle) const
{
if (!valid_instance(primary_instance)) {
return false;
}
return drivers[primary_instance]->get_obstacle(obstacle_num, vec_to_obstacle);
}
// returns shortest distance to "obstacle_num" obstacle, from a line segment formed between "seg_start" and "seg_end"
// used in GPS based Simple Avoidance
bool AP_Proximity::closest_point_from_segment_to_obstacle(uint8_t obstacle_num, const Vector3f& seg_start, const Vector3f& seg_end, Vector3f& closest_point) const
{
if (!valid_instance(primary_instance)) {
return false;
}
return drivers[primary_instance]->closest_point_from_segment_to_obstacle(obstacle_num, seg_start, seg_end, closest_point);
}
// get distance and angle to closest object (used for pre-arm check)
// returns true on success, false if no valid readings
bool AP_Proximity::get_closest_object(float& angle_deg, float &distance) const
{
if (!valid_instance(primary_instance)) {
return false;
}
// get closest object from backend
return drivers[primary_instance]->get_closest_object(angle_deg, distance);
}
// get number of objects, used for non-GPS avoidance
uint8_t AP_Proximity::get_object_count() const
{
if (!valid_instance(primary_instance)) {
return 0;
}
// get count from backend
return drivers[primary_instance]->get_horizontal_object_count();
}
// get an object's angle and distance, used for non-GPS avoidance
// returns false if no angle or distance could be returned for some reason
bool AP_Proximity::get_object_angle_and_distance(uint8_t object_number, float& angle_deg, float &distance) const
{
if (!valid_instance(primary_instance)) {
return false;
}
// get angle and distance from backend
return drivers[primary_instance]->get_horizontal_object_angle_and_distance(object_number, angle_deg, distance);
}
// handle mavlink DISTANCE_SENSOR messages
void AP_Proximity::handle_msg(const mavlink_message_t &msg)
{
@ -187,6 +303,112 @@ void AP_Proximity::handle_msg(const mavlink_message_t &msg)
}
}
// methods for mavlink SYS_STATUS message (send_sys_status)
// these methods cover only the primary instance
bool AP_Proximity::sensor_present() const
{
return get_status() != Status::NotConnected;
}
bool AP_Proximity::sensor_enabled() const
{
return get_type(primary_instance) != Type::None;
}
bool AP_Proximity::sensor_failed() const
{
return get_status() != Status::Good;
}
// get distance in meters upwards, returns true on success
bool AP_Proximity::get_upward_distance(uint8_t instance, float &distance) const
{
if (!valid_instance(instance)) {
return false;
}
// get upward distance from backend
return drivers[instance]->get_upward_distance(distance);
}
bool AP_Proximity::get_upward_distance(float &distance) const
{
return get_upward_distance(primary_instance, distance);
}
// set alt as read from dowward facing rangefinder. Tilt is already adjusted for.
void AP_Proximity::set_rangefinder_alt(bool use, bool healthy, float alt_cm)
{
if (!valid_instance(primary_instance)) {
return;
}
// store alt at the backend
drivers[primary_instance]->set_rangefinder_alt(use, healthy, alt_cm);
}
#if HAL_LOGGING_ENABLED
// Write proximity sensor distances
void AP_Proximity::log()
{
// exit immediately if not enabled
if (get_status() == AP_Proximity::Status::NotConnected) {
return;
}
Proximity_Distance_Array dist_array{}; // raw distances stored here
Proximity_Distance_Array filt_dist_array{}; //filtered distances stored here
auto &logger { AP::logger() };
for (uint8_t i = 0; i < get_num_layers(); i++) {
const bool active = get_active_layer_distances(i, dist_array, filt_dist_array);
if (!active) {
// nothing on this layer
continue;
}
float dist_up;
if (!get_upward_distance(dist_up)) {
dist_up = 0.0f;
}
float closest_ang = 0.0f;
float closest_dist = 0.0f;
get_closest_object(closest_ang, closest_dist);
const struct log_Proximity pkt_proximity{
LOG_PACKET_HEADER_INIT(LOG_PROXIMITY_MSG),
time_us : AP_HAL::micros64(),
instance : i,
health : (uint8_t)get_status(),
dist0 : filt_dist_array.distance[0],
dist45 : filt_dist_array.distance[1],
dist90 : filt_dist_array.distance[2],
dist135 : filt_dist_array.distance[3],
dist180 : filt_dist_array.distance[4],
dist225 : filt_dist_array.distance[5],
dist270 : filt_dist_array.distance[6],
dist315 : filt_dist_array.distance[7],
distup : dist_up,
closest_angle : closest_ang,
closest_dist : closest_dist
};
logger.WriteBlock(&pkt_proximity, sizeof(pkt_proximity));
if (_raw_log_enable) {
const struct log_Proximity_raw pkt_proximity_raw{
LOG_PACKET_HEADER_INIT(LOG_RAW_PROXIMITY_MSG),
time_us : AP_HAL::micros64(),
instance : i,
raw_dist0 : dist_array.distance[0],
raw_dist45 : dist_array.distance[1],
raw_dist90 : dist_array.distance[2],
raw_dist135 : dist_array.distance[3],
raw_dist180 : dist_array.distance[4],
raw_dist225 : dist_array.distance[5],
raw_dist270 : dist_array.distance[6],
raw_dist315 : dist_array.distance[7],
};
logger.WriteBlock(&pkt_proximity_raw, sizeof(pkt_proximity_raw));
}
}
}
#endif
// return true if the given instance exists
bool AP_Proximity::valid_instance(uint8_t i) const
{
@ -279,224 +501,7 @@ void AP_Proximity::detect_instance(uint8_t instance)
}
}
// get distances in 8 directions. used for sending distances to ground station
bool AP_Proximity::get_horizontal_distances(Proximity_Distance_Array &prx_dist_array) const
{
if (!valid_instance(primary_instance)) {
return false;
}
// get distances from backend
return drivers[primary_instance]->get_horizontal_distances(prx_dist_array);
}
// get raw and filtered distances in 8 directions per layer. used for logging
bool AP_Proximity::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
{
if (!valid_instance(primary_instance)) {
return false;
}
// get distances from backend
return drivers[primary_instance]->get_active_layer_distances(layer, prx_dist_array, prx_filt_dist_array);
}
// get total number of obstacles, used in GPS based Simple Avoidance
uint8_t AP_Proximity::get_obstacle_count() const
{
if (!valid_instance(primary_instance)) {
return 0;
}
return drivers[primary_instance]->get_obstacle_count();
}
// get number of layers.
uint8_t AP_Proximity::get_num_layers() const
{
if (!valid_instance(primary_instance)) {
return 0;
}
return drivers[primary_instance]->get_num_layers();
}
// get vector to obstacle based on obstacle_num passed, used in GPS based Simple Avoidance
bool AP_Proximity::get_obstacle(uint8_t obstacle_num, Vector3f& vec_to_obstacle) const
{
if (!valid_instance(primary_instance)) {
return false;
}
return drivers[primary_instance]->get_obstacle(obstacle_num, vec_to_obstacle);
}
// returns shortest distance to "obstacle_num" obstacle, from a line segment formed between "seg_start" and "seg_end"
// used in GPS based Simple Avoidance
bool AP_Proximity::closest_point_from_segment_to_obstacle(uint8_t obstacle_num, const Vector3f& seg_start, const Vector3f& seg_end, Vector3f& closest_point) const
{
if (!valid_instance(primary_instance)) {
return false;
}
return drivers[primary_instance]->closest_point_from_segment_to_obstacle(obstacle_num, seg_start, seg_end, closest_point);
}
// get distance and angle to closest object (used for pre-arm check)
// returns true on success, false if no valid readings
bool AP_Proximity::get_closest_object(float& angle_deg, float &distance) const
{
if (!valid_instance(primary_instance)) {
return false;
}
// get closest object from backend
return drivers[primary_instance]->get_closest_object(angle_deg, distance);
}
// get number of objects, used for non-GPS avoidance
uint8_t AP_Proximity::get_object_count() const
{
if (!valid_instance(primary_instance)) {
return 0;
}
// get count from backend
return drivers[primary_instance]->get_horizontal_object_count();
}
// get an object's angle and distance, used for non-GPS avoidance
// returns false if no angle or distance could be returned for some reason
bool AP_Proximity::get_object_angle_and_distance(uint8_t object_number, float& angle_deg, float &distance) const
{
if (!valid_instance(primary_instance)) {
return false;
}
// get angle and distance from backend
return drivers[primary_instance]->get_horizontal_object_angle_and_distance(object_number, angle_deg, distance);
}
// get maximum and minimum distances (in meters) of primary sensor
float AP_Proximity::distance_max() const
{
if (!valid_instance(primary_instance)) {
return 0.0f;
}
// get maximum distance from backend
return drivers[primary_instance]->distance_max();
}
float AP_Proximity::distance_min() const
{
if (!valid_instance(primary_instance)) {
return 0.0f;
}
// get minimum distance from backend
return drivers[primary_instance]->distance_min();
}
// get distance in meters upwards, returns true on success
bool AP_Proximity::get_upward_distance(uint8_t instance, float &distance) const
{
if (!valid_instance(instance)) {
return false;
}
// get upward distance from backend
return drivers[instance]->get_upward_distance(distance);
}
bool AP_Proximity::get_upward_distance(float &distance) const
{
return get_upward_distance(primary_instance, distance);
}
AP_Proximity::Type AP_Proximity::get_type(uint8_t instance) const
{
if (instance < PROXIMITY_MAX_INSTANCES) {
return (Type)((uint8_t)params[instance].type);
}
return Type::None;
}
bool AP_Proximity::sensor_present() const
{
return get_status() != Status::NotConnected;
}
bool AP_Proximity::sensor_enabled() const
{
return get_type(primary_instance) != Type::None;
}
bool AP_Proximity::sensor_failed() const
{
return get_status() != Status::Good;
}
// set alt as read from dowward facing rangefinder. Tilt is already adjusted for.
void AP_Proximity::set_rangefinder_alt(bool use, bool healthy, float alt_cm)
{
if (!valid_instance(primary_instance)) {
return;
}
// store alt at the backend
drivers[primary_instance]->set_rangefinder_alt(use, healthy, alt_cm);
}
#if HAL_LOGGING_ENABLED
// Write proximity sensor distances
void AP_Proximity::log()
{
// exit immediately if not enabled
if (get_status() == AP_Proximity::Status::NotConnected) {
return;
}
Proximity_Distance_Array dist_array{}; // raw distances stored here
Proximity_Distance_Array filt_dist_array{}; //filtered distances stored here
auto &logger { AP::logger() };
for (uint8_t i = 0; i < get_num_layers(); i++) {
const bool active = get_active_layer_distances(i, dist_array, filt_dist_array);
if (!active) {
// nothing on this layer
continue;
}
float dist_up;
if (!get_upward_distance(dist_up)) {
dist_up = 0.0f;
}
float closest_ang = 0.0f;
float closest_dist = 0.0f;
get_closest_object(closest_ang, closest_dist);
const struct log_Proximity pkt_proximity{
LOG_PACKET_HEADER_INIT(LOG_PROXIMITY_MSG),
time_us : AP_HAL::micros64(),
instance : i,
health : (uint8_t)get_status(),
dist0 : filt_dist_array.distance[0],
dist45 : filt_dist_array.distance[1],
dist90 : filt_dist_array.distance[2],
dist135 : filt_dist_array.distance[3],
dist180 : filt_dist_array.distance[4],
dist225 : filt_dist_array.distance[5],
dist270 : filt_dist_array.distance[6],
dist315 : filt_dist_array.distance[7],
distup : dist_up,
closest_angle : closest_ang,
closest_dist : closest_dist
};
logger.WriteBlock(&pkt_proximity, sizeof(pkt_proximity));
if (_raw_log_enable) {
const struct log_Proximity_raw pkt_proximity_raw{
LOG_PACKET_HEADER_INIT(LOG_RAW_PROXIMITY_MSG),
time_us : AP_HAL::micros64(),
instance : i,
raw_dist0 : dist_array.distance[0],
raw_dist45 : dist_array.distance[1],
raw_dist90 : dist_array.distance[2],
raw_dist135 : dist_array.distance[3],
raw_dist180 : dist_array.distance[4],
raw_dist225 : dist_array.distance[5],
raw_dist270 : dist_array.distance[6],
raw_dist315 : dist_array.distance[7],
};
logger.WriteBlock(&pkt_proximity_raw, sizeof(pkt_proximity_raw));
}
}
}
#endif
AP_Proximity *AP_Proximity::_singleton;

104
libraries/AP_Proximity/AP_Proximity.h
View File

@ -64,6 +64,35 @@ public:
Good
};
// detect and initialise any available proximity sensors
void init();
// update state of all proximity sensors. Should be called at high rate from main loop
void update();
// return the number of proximity sensor backends
uint8_t num_sensors() const { return num_instances; }
// return sensor type of a given instance
Type get_type(uint8_t instance) const;
// 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;
// get maximum and minimum distances (in meters) of primary sensor
float distance_max() const;
float distance_min() const;
//
// 3D boundary related methods
//
// 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)
@ -76,29 +105,12 @@ public:
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 number of layers in the 3D boundary
uint8_t get_num_layers() 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;
@ -120,50 +132,44 @@ public:
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;
//
// mavlink related methods
//
// 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;
// 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;
//
// support for upwards and downwards 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;
// set alt as read from downward facing rangefinder. Tilt is already adjusted for
void set_rangefinder_alt(bool use, bool healthy, float alt_cm);
// method called by vehicle to have AP_Proximity write onboard log
// messages:
// method called by vehicle to have AP_Proximity write onboard log messages
void log();
// 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
};
// parameter list
static const struct AP_Param::GroupInfo var_info[];
static AP_Proximity *get_singleton(void) { return _singleton; };
protected:
// parameters for backends