Line data    Source code

       1             : /*
       2             :    This program is free software: you can redistribute it and/or modify
       3             :    it under the terms of the GNU General Public License as published by
       4             :    the Free Software Foundation, either version 3 of the License, or
       5             :    (at your option) any later version.
       6             : 
       7             :    This program is distributed in the hope that it will be useful,
       8             :    but WITHOUT ANY WARRANTY; without even the implied warranty of
       9             :    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
      10             :    GNU General Public License for more details.
      11             : 
      12             :    You should have received a copy of the GNU General Public License
      13             :    along with this program.  If not, see <http://www.gnu.org/licenses/>.
      14             :  */
      15             : 
      16             : #include "AP_Proximity_DroneCAN.h"
      17             : 
      18             : #if AP_PROXIMITY_DRONECAN_ENABLED
      19             : 
      20             : #include <AP_HAL/AP_HAL.h>
      21             : #include <AP_CANManager/AP_CANManager.h>
      22             : #include <AP_DroneCAN/AP_DroneCAN.h>
      23             : #include <AP_BoardConfig/AP_BoardConfig.h>
      24             : #include <GCS_MAVLink/GCS.h>
      25             : 
      26             : extern const AP_HAL::HAL& hal;
      27             : 
      28             : 
      29             : ObjectBuffer_TS<AP_Proximity_DroneCAN::ObstacleItem> AP_Proximity_DroneCAN::items(50);
      30             : 
      31             : #define PROXIMITY_TIMEOUT_MS    500 // distance messages must arrive within this many milliseconds
      32             : 
      33             : 
      34             : //links the Proximity DroneCAN message to this backend
      35           5 : bool AP_Proximity_DroneCAN::subscribe_msgs(AP_DroneCAN* ap_dronecan)
      36             : {
      37           5 :     const auto driver_index = ap_dronecan->get_driver_index();
      38             : 
      39           5 :     return (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_measurement, driver_index) != nullptr);
      40             : }
      41             : 
      42             : //Method to find the backend relating to the node id
      43           0 : AP_Proximity_DroneCAN* AP_Proximity_DroneCAN::get_dronecan_backend(AP_DroneCAN* ap_dronecan, uint8_t node_id, uint8_t address, bool create_new)
      44             : {
      45           0 :     if (ap_dronecan == nullptr) {
      46           0 :         return nullptr;
      47             :     }
      48             : 
      49           0 :     AP_Proximity *prx = AP::proximity();
      50           0 :     if (prx == nullptr) {
      51           0 :         return nullptr;
      52             :     }
      53             : 
      54           0 :     AP_Proximity_DroneCAN* driver = nullptr;
      55             :     //Scan through the proximity type params to find DroneCAN with matching address.
      56           0 :     for (uint8_t i = 0; i < AP_PROXIMITY_MAX_INSTANCES; i++) {
      57           0 :         if ((AP_Proximity::Type)prx->params[i].type.get() == AP_Proximity::Type::DroneCAN &&
      58           0 :             prx->params[i].address == address) {
      59           0 :             driver = (AP_Proximity_DroneCAN*)prx->drivers[i];
      60             :         }
      61             :         //Double check if the driver was initialised as DroneCAN Type
      62           0 :         if (driver != nullptr && (driver->_backend_type == AP_Proximity::Type::DroneCAN)) {
      63           0 :             if (driver->_ap_dronecan == ap_dronecan &&
      64           0 :                 driver->_node_id == node_id) {
      65           0 :                 return driver;
      66             :             } else {
      67             :                 //we found a possible duplicate addressed sensor
      68             :                 //we return nothing in such scenario
      69           0 :                 return nullptr;
      70             :             }
      71             :         }
      72             :     }
      73             : 
      74           0 :     if (create_new) {
      75           0 :         for (uint8_t i = 0; i < AP_PROXIMITY_MAX_INSTANCES; i++) {
      76           0 :             if ((AP_Proximity::Type)prx->params[i].type.get() == AP_Proximity::Type::DroneCAN &&
      77           0 :                 prx->params[i].address == address) {
      78           0 :                 WITH_SEMAPHORE(prx->detect_sem);
      79           0 :                 if (prx->drivers[i] != nullptr) {
      80             :                     //we probably initialised this driver as something else, reboot is required for setting
      81             :                     //it up as DroneCAN type
      82           0 :                     return nullptr;
      83             :                 }
      84           0 :                 prx->drivers[i] = NEW_NOTHROW AP_Proximity_DroneCAN(*prx, prx->state[i], prx->params[i]);
      85           0 :                 driver = (AP_Proximity_DroneCAN*)prx->drivers[i];
      86           0 :                 if (driver == nullptr) {
      87           0 :                     break;
      88             :                 }
      89           0 :                 GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Prx[%u]: added DroneCAN node %u addr %u",
      90             :                                 unsigned(i), unsigned(node_id), unsigned(address));
      91             : 
      92           0 :                 if (is_zero(prx->params[i].max_m) && is_zero(prx->params[i].min_m)) {
      93             :                     // GCS reporting will be incorrect if min/max are not set
      94           0 :                     GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Configure PRX%u_MIN and PRX%u_MAX",
      95             :                                 unsigned(i), unsigned(i));
      96             :                 }
      97             :                 //Assign node id and respective dronecan driver, for identification
      98           0 :                 if (driver->_ap_dronecan == nullptr) {
      99           0 :                     driver->_ap_dronecan = ap_dronecan;
     100           0 :                     driver->_node_id = node_id;
     101           0 :                     break;
     102             :                 }
     103           0 :             }
     104             :         }
     105             :     }
     106             : 
     107           0 :     return driver;
     108             : }
     109             : 
     110             : 
     111             : // update the state of the sensor
     112           0 : void AP_Proximity_DroneCAN::update(void)
     113             : {
     114             :     // check for timeout and set health status
     115           0 :     if ((_last_update_ms == 0 || (AP_HAL::millis() - _last_update_ms > PROXIMITY_TIMEOUT_MS))) {
     116           0 :         set_status(AP_Proximity::Status::NoData);
     117             :     } else {
     118           0 :         set_status(_status);
     119             :     }
     120             : 
     121           0 :     if (_status == AP_Proximity::Status::Good) {
     122             :         ObstacleItem object_item;
     123           0 :         WITH_SEMAPHORE(_sem);
     124           0 :         while (items.pop(object_item)) {
     125           0 :             const AP_Proximity_Boundary_3D::Face face = frontend.boundary.get_face(object_item.pitch_deg, object_item.yaw_deg);
     126           0 :             if (!is_zero(object_item.distance_m) && !ignore_reading(object_item.pitch_deg, object_item.yaw_deg, object_item.distance_m, false)) {
     127             :                 // update boundary used for avoidance
     128           0 :                 frontend.boundary.set_face_attributes(face, object_item.pitch_deg, object_item.yaw_deg, object_item.distance_m, state.instance);
     129             :                 // update OA database
     130           0 :                 database_push(object_item.pitch_deg, object_item.yaw_deg, object_item.distance_m);
     131             :             }
     132             :         }
     133           0 :     }
     134           0 : }
     135             : 
     136             : // get maximum and minimum distances (in meters)
     137           0 : float AP_Proximity_DroneCAN::distance_max_m() const
     138             : {
     139           0 :     if (is_zero(params.max_m)) {
     140             :         // GCS will not report correct correct value if max isn't set properly
     141             :         // This is a arbitrary value to prevent the above issue
     142           0 :         return 100.0f;
     143             :     }
     144           0 :     return params.max_m;
     145             : }
     146             : 
     147           0 : float AP_Proximity_DroneCAN::distance_min_m() const
     148             : {
     149           0 :     return params.min_m;
     150             : }
     151             : 
     152             : //Proximity message handler
     153           0 : void AP_Proximity_DroneCAN::handle_measurement(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const ardupilot_equipment_proximity_sensor_Proximity &msg)
     154             : {
     155             :     //fetch the matching DroneCAN driver, node id and sensor id backend instance
     156           0 :     AP_Proximity_DroneCAN* driver = get_dronecan_backend(ap_dronecan, transfer.source_node_id, msg.sensor_id, true);
     157           0 :     if (driver == nullptr) {
     158           0 :         return;
     159             :     }
     160           0 :     WITH_SEMAPHORE(driver->_sem);
     161           0 :     switch (msg.reading_type) {
     162           0 :         case ARDUPILOT_EQUIPMENT_PROXIMITY_SENSOR_PROXIMITY_READING_TYPE_GOOD: {
     163             :             //update the states in backend instance
     164           0 :             driver->_last_update_ms = AP_HAL::millis();
     165           0 :             driver->_status = AP_Proximity::Status::Good;
     166           0 :             const ObstacleItem item = {msg.yaw, msg.pitch, msg.distance};
     167             : 
     168           0 :             if (driver->items.space()) {
     169             :                 // ignore reading if no place to put it in the queue
     170           0 :                 driver->items.push(item);
     171             :             }
     172           0 :             break;
     173             :         }
     174             :         //Additional states supported by Proximity message
     175           0 :         case ARDUPILOT_EQUIPMENT_PROXIMITY_SENSOR_PROXIMITY_READING_TYPE_NOT_CONNECTED: {
     176           0 :             driver->_last_update_ms = AP_HAL::millis();
     177           0 :             driver->_status = AP_Proximity::Status::NotConnected;
     178           0 :             break;
     179             :         }
     180           0 :         case ARDUPILOT_EQUIPMENT_PROXIMITY_SENSOR_PROXIMITY_READING_TYPE_NO_DATA: {
     181           0 :             driver->_last_update_ms = AP_HAL::millis();
     182           0 :             driver->_status = AP_Proximity::Status::NoData;
     183           0 :             break;
     184             :         }
     185           0 :         default:
     186           0 :             break;
     187             :     }
     188           0 : }
     189             : 
     190             : #endif // AP_PROXIMITY_DRONECAN_ENABLED