From e497dedf35233f5addc709d09a6149e7ad830698 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 23 Dec 2016 15:03:21 +0900 Subject: [PATCH] AP_Proximity: add MAV type This accepts DISTANCE_SENSOR messages from eight sectors around the vehicle --- libraries/AP_Proximity/AP_Proximity.cpp | 6 ++ libraries/AP_Proximity/AP_Proximity.h | 1 + libraries/AP_Proximity/AP_Proximity_MAV.cpp | 62 +++++++++++++++++++++ libraries/AP_Proximity/AP_Proximity_MAV.h | 33 +++++++++++ 4 files changed, 102 insertions(+) create mode 100644 libraries/AP_Proximity/AP_Proximity_MAV.cpp create mode 100644 libraries/AP_Proximity/AP_Proximity_MAV.h diff --git a/libraries/AP_Proximity/AP_Proximity.cpp b/libraries/AP_Proximity/AP_Proximity.cpp index 946315fa2a..3e426d5b7d 100644 --- a/libraries/AP_Proximity/AP_Proximity.cpp +++ b/libraries/AP_Proximity/AP_Proximity.cpp @@ -15,6 +15,7 @@ #include "AP_Proximity.h" #include "AP_Proximity_LightWareSF40C.h" +#include "AP_Proximity_MAV.h" #include "AP_Proximity_SITL.h" extern const AP_HAL::HAL &hal; @@ -252,6 +253,11 @@ void AP_Proximity::detect_instance(uint8_t instance) return; } } + if (type == Proximity_Type_MAV) { + state[instance].instance = instance; + drivers[instance] = new AP_Proximity_MAV(*this, state[instance]); + return; + } #if CONFIG_HAL_BOARD == HAL_BOARD_SITL if (type == Proximity_Type_SITL) { state[instance].instance = instance; diff --git a/libraries/AP_Proximity/AP_Proximity.h b/libraries/AP_Proximity/AP_Proximity.h index 16fd1fe051..e509c3ae1d 100644 --- a/libraries/AP_Proximity/AP_Proximity.h +++ b/libraries/AP_Proximity/AP_Proximity.h @@ -38,6 +38,7 @@ public: enum Proximity_Type { Proximity_Type_None = 0, Proximity_Type_SF40C = 1, + Proximity_Type_MAV = 2, Proximity_Type_SITL = 10, }; diff --git a/libraries/AP_Proximity/AP_Proximity_MAV.cpp b/libraries/AP_Proximity/AP_Proximity_MAV.cpp new file mode 100644 index 0000000000..3f3a2b6a93 --- /dev/null +++ b/libraries/AP_Proximity/AP_Proximity_MAV.cpp @@ -0,0 +1,62 @@ +/* + 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 . + */ + +#include +#include "AP_Proximity_MAV.h" +#include +#include +#include + +extern const AP_HAL::HAL& hal; + +/* + The constructor also initialises the proximity sensor. Note that this + constructor is not called until detect() returns true, so we + already know that we should setup the proximity sensor +*/ +AP_Proximity_MAV::AP_Proximity_MAV(AP_Proximity &_frontend, + AP_Proximity::Proximity_State &_state) : + AP_Proximity_Backend(_frontend, _state) +{ +} + +// update the state of the sensor +void AP_Proximity_MAV::update(void) +{ + // check for timeout and set health status + if ((_last_update_ms == 0) || (AP_HAL::millis() - _last_update_ms > PROXIMITY_MAV_TIMEOUT_MS)) { + set_status(AP_Proximity::Proximity_NoData); + } else { + set_status(AP_Proximity::Proximity_Good); + } +} + +// handle mavlink DISTANCE_SENSOR messages +void AP_Proximity_MAV::handle_msg(mavlink_message_t *msg) +{ + mavlink_distance_sensor_t packet; + mavlink_msg_distance_sensor_decode(msg, &packet); + + // store distance to appropriate sector based on orientation field + if (packet.orientation <= MAV_SENSOR_ROTATION_YAW_315) { + uint8_t sector = packet.orientation; + _angle[sector] = sector * 45; + _distance[sector] = packet.current_distance / 100.0f; + _distance_valid[sector] = true; + _distance_min = packet.min_distance / 100.0f; + _distance_max = packet.max_distance / 100.0f; + _last_update_ms = AP_HAL::millis(); + } +} diff --git a/libraries/AP_Proximity/AP_Proximity_MAV.h b/libraries/AP_Proximity/AP_Proximity_MAV.h new file mode 100644 index 0000000000..5adc1a76dc --- /dev/null +++ b/libraries/AP_Proximity/AP_Proximity_MAV.h @@ -0,0 +1,33 @@ +#pragma once + +#include "AP_Proximity.h" +#include "AP_Proximity_Backend.h" + +#define PROXIMITY_MAV_TIMEOUT_MS 200 // requests timeout after 0.2 seconds + +class AP_Proximity_MAV : public AP_Proximity_Backend +{ + +public: + // constructor + AP_Proximity_MAV(AP_Proximity &_frontend, AP_Proximity::Proximity_State &_state); + + // update state + void update(void); + + // get maximum and minimum distances (in meters) of sensor + float distance_max() const { return _distance_max; } + float distance_min() const { return _distance_min; }; + + // handle mavlink DISTANCE_SENSOR messages + void handle_msg(mavlink_message_t *msg); + +private: + + // initialise sensor (returns true if sensor is succesfully initialised) + bool initialise(); + + 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 +};