From a47e215a8e7bec295874feaf402396b32d7bcced Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 16 Jan 2017 15:58:14 +0900 Subject: [PATCH] AP_Proximity: add get_upward_distance This allow support for upward facing proximity sensor received through a DISTANCE_SENSOR message. Also added SITL test --- libraries/AP_Proximity/AP_Proximity.cpp | 15 +++++++++++++++ libraries/AP_Proximity/AP_Proximity.h | 8 ++++++++ libraries/AP_Proximity/AP_Proximity_Backend.h | 3 +++ libraries/AP_Proximity/AP_Proximity_MAV.cpp | 16 ++++++++++++++++ libraries/AP_Proximity/AP_Proximity_MAV.h | 8 ++++++++ libraries/AP_Proximity/AP_Proximity_SITL.cpp | 12 ++++++++++++ libraries/AP_Proximity/AP_Proximity_SITL.h | 4 ++++ 7 files changed, 66 insertions(+) diff --git a/libraries/AP_Proximity/AP_Proximity.cpp b/libraries/AP_Proximity/AP_Proximity.cpp index 38966a93d1..fbd8c7f8f1 100644 --- a/libraries/AP_Proximity/AP_Proximity.cpp +++ b/libraries/AP_Proximity/AP_Proximity.cpp @@ -385,3 +385,18 @@ float AP_Proximity::distance_min() const // 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 ((drivers[instance] == nullptr) || (_type[instance] == Proximity_Type_None)) { + 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); +} diff --git a/libraries/AP_Proximity/AP_Proximity.h b/libraries/AP_Proximity/AP_Proximity.h index 1908edda29..77171f5fde 100644 --- a/libraries/AP_Proximity/AP_Proximity.h +++ b/libraries/AP_Proximity/AP_Proximity.h @@ -107,6 +107,14 @@ public: enum Proximity_Status status; // sensor status }; + // + // support for upwardward 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; + // parameter list static const struct AP_Param::GroupInfo var_info[]; diff --git a/libraries/AP_Proximity/AP_Proximity_Backend.h b/libraries/AP_Proximity/AP_Proximity_Backend.h index 2410232664..f8b50e8a87 100644 --- a/libraries/AP_Proximity/AP_Proximity_Backend.h +++ b/libraries/AP_Proximity/AP_Proximity_Backend.h @@ -39,6 +39,9 @@ public: 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 DISTANCE_SENSOR messages virtual void handle_msg(mavlink_message_t *msg) {} diff --git a/libraries/AP_Proximity/AP_Proximity_MAV.cpp b/libraries/AP_Proximity/AP_Proximity_MAV.cpp index 19fef8ca5a..27a1aa2446 100644 --- a/libraries/AP_Proximity/AP_Proximity_MAV.cpp +++ b/libraries/AP_Proximity/AP_Proximity_MAV.cpp @@ -43,6 +43,16 @@ void AP_Proximity_MAV::update(void) } } +// get distance upwards in meters. returns true on success +bool AP_Proximity_MAV::get_upward_distance(float &distance) const +{ + if ((_last_upward_update_ms != 0) && (AP_HAL::millis() - _last_upward_update_ms <= PROXIMITY_MAV_TIMEOUT_MS)) { + distance = _distance_upward; + return true; + } + return false; +} + // handle mavlink DISTANCE_SENSOR messages void AP_Proximity_MAV::handle_msg(mavlink_message_t *msg) { @@ -60,4 +70,10 @@ void AP_Proximity_MAV::handle_msg(mavlink_message_t *msg) _last_update_ms = AP_HAL::millis(); update_boundary_for_sector(sector); } + + // store upward distance + if (packet.orientation == MAV_SENSOR_ROTATION_PITCH_90) { + _distance_upward = packet.current_distance / 100.0f; + _last_upward_update_ms = AP_HAL::millis(); + } } diff --git a/libraries/AP_Proximity/AP_Proximity_MAV.h b/libraries/AP_Proximity/AP_Proximity_MAV.h index d898b543c3..6e75f7c5f3 100644 --- a/libraries/AP_Proximity/AP_Proximity_MAV.h +++ b/libraries/AP_Proximity/AP_Proximity_MAV.h @@ -19,6 +19,9 @@ public: float distance_max() const { return _distance_max; } float distance_min() const { return _distance_min; }; + // get distance upwards in meters. returns true on success + bool get_upward_distance(float &distance) const; + // handle mavlink DISTANCE_SENSOR messages void handle_msg(mavlink_message_t *msg) override; @@ -27,7 +30,12 @@ private: // initialise sensor (returns true if sensor is succesfully initialised) bool initialise(); + // horizontal distance support uint32_t _last_update_ms; // system time of last DISTANCE_SENSOR message received float _distance_max; // max range of sensor in meters float _distance_min; // min range of sensor in meters + + // upward distance support + uint32_t _last_upward_update_ms; // system time of last update distance + float _distance_upward; // upward distance in meters }; diff --git a/libraries/AP_Proximity/AP_Proximity_SITL.cpp b/libraries/AP_Proximity/AP_Proximity_SITL.cpp index 7289b4f8c0..acf3747390 100644 --- a/libraries/AP_Proximity/AP_Proximity_SITL.cpp +++ b/libraries/AP_Proximity/AP_Proximity_SITL.cpp @@ -38,6 +38,10 @@ AP_Proximity_SITL::AP_Proximity_SITL(AP_Proximity &_frontend, if (fence_count == nullptr || ptype != AP_PARAM_INT8) { AP_HAL::panic("Proximity_SITL: Failed to find FENCE_TOTAL"); } + fence_alt_max = (AP_Float *)AP_Param::find("FENCE_ALT_MAX", &ptype); + if (fence_alt_max == nullptr || ptype != AP_PARAM_FLOAT) { + AP_HAL::panic("Proximity_SITL: Failed to find FENCE_ALT_MAX"); + } } // update the state of the sensor @@ -125,4 +129,12 @@ float AP_Proximity_SITL::distance_min() const return 0.0f; } +// get distance upwards in meters. returns true on success +bool AP_Proximity_SITL::get_upward_distance(float &distance) const +{ + // return distance to fence altitude + distance = MAX(0.0f, fence_alt_max->get() - sitl->height_agl); + return true; +} + #endif // CONFIG_HAL_BOARD diff --git a/libraries/AP_Proximity/AP_Proximity_SITL.h b/libraries/AP_Proximity/AP_Proximity_SITL.h index 81da426627..dd20a2a856 100644 --- a/libraries/AP_Proximity/AP_Proximity_SITL.h +++ b/libraries/AP_Proximity/AP_Proximity_SITL.h @@ -20,10 +20,14 @@ public: float distance_max() const; float distance_min() const; + // get distance upwards in meters. returns true on success + bool get_upward_distance(float &distance) const; + private: SITL::SITL *sitl; Vector2l *fence; AP_Int8 *fence_count; + AP_Float *fence_alt_max; uint32_t last_load_ms; AC_PolyFence_loader fence_loader; Location current_loc;