libraries/AP_Proximity/AP_Proximity_DroneCAN.cpp

   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 bool AP_Proximity_DroneCAN::subscribe_msgs(AP_DroneCAN* ap_dronecan)
  36 {
  37     const auto driver_index = ap_dronecan->get_driver_index();
  38
  39     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 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     if (ap_dronecan == nullptr) {
  46         return nullptr;
  47     }
  48
  49     AP_Proximity *prx = AP::proximity();
  50     if (prx == nullptr) {
  51         return nullptr;
  52     }
  53
  54     AP_Proximity_DroneCAN* driver = nullptr;
  55     //Scan through the proximity type params to find DroneCAN with matching address.
  56     for (uint8_t i = 0; i < PROXIMITY_MAX_INSTANCES; i++) {
  57         if ((AP_Proximity::Type)prx->params[i].type.get() == AP_Proximity::Type::DroneCAN &&
  58             prx->params[i].address == address) {
  59             driver = (AP_Proximity_DroneCAN*)prx->drivers[i];
  60         }
  61         //Double check if the driver was initialised as DroneCAN Type
  62         if (driver != nullptr && (driver->_backend_type == AP_Proximity::Type::DroneCAN)) {
  63             if (driver->_ap_dronecan == ap_dronecan &&
  64                 driver->_node_id == node_id) {
  65                 return driver;
  66             } else {
  67                 //we found a possible duplicate addressed sensor
  68                 //we return nothing in such scenario
  69                 return nullptr;
  70             }
  71         }
  72     }
  73
  74     if (create_new) {
  75         for (uint8_t i = 0; i < PROXIMITY_MAX_INSTANCES; i++) {
  76             if ((AP_Proximity::Type)prx->params[i].type.get() == AP_Proximity::Type::DroneCAN &&
  77                 prx->params[i].address == address) {
  78                 WITH_SEMAPHORE(prx->detect_sem);
  79                 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                     return nullptr;
  83                 }
  84                 prx->drivers[i] = NEW_NOTHROW AP_Proximity_DroneCAN(*prx, prx->state[i], prx->params[i]);
  85                 driver = (AP_Proximity_DroneCAN*)prx->drivers[i];
  86                 if (driver == nullptr) {
  87                     break;
  88                 }
  89                 GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Prx[%u]: added DroneCAN node %u addr %u",
  90                                 unsigned(i), unsigned(node_id), unsigned(address));
  91
  92                 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                     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                 if (driver->_ap_dronecan == nullptr) {
  99                     driver->_ap_dronecan = ap_dronecan;
 100                     driver->_node_id = node_id;
 101                     break;
 102                 }
 103             }
 104         }
 105     }
 106
 107     return driver;
 108 }
 109
 110
 111 // update the state of the sensor
 112 void AP_Proximity_DroneCAN::update(void)
 113 {
 114     // check for timeout and set health status
 115     if ((_last_update_ms == 0 || (AP_HAL::millis() - _last_update_ms > PROXIMITY_TIMEOUT_MS))) {
 116         set_status(AP_Proximity::Status::NoData);
 117     } else {
 118         set_status(_status);
 119     }
 120
 121     if (_status == AP_Proximity::Status::Good) {
 122         ObstacleItem object_item;
 123         WITH_SEMAPHORE(_sem);
 124         while (items.pop(object_item)) {
 125             const AP_Proximity_Boundary_3D::Face face = frontend.boundary.get_face(object_item.pitch_deg, object_item.yaw_deg);
 126             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                 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                 database_push(object_item.pitch_deg, object_item.yaw_deg, object_item.distance_m);
 131             }
 132         }
 133     }
 134 }
 135
 136 // get maximum and minimum distances (in meters)
 137 float AP_Proximity_DroneCAN::distance_max() const
 138 {
 139     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         return 100.0f;
 143     }
 144     return params.max_m;
 145 }
 146
 147 float AP_Proximity_DroneCAN::distance_min() const
 148 {
 149     return params.min_m;
 150 }
 151
 152 //Proximity message handler
 153 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     AP_Proximity_DroneCAN* driver = get_dronecan_backend(ap_dronecan, transfer.source_node_id, msg.sensor_id, true);
 157     if (driver == nullptr) {
 158         return;
 159     }
 160     WITH_SEMAPHORE(driver->_sem);
 161     switch (msg.reading_type) {
 162         case ARDUPILOT_EQUIPMENT_PROXIMITY_SENSOR_PROXIMITY_READING_TYPE_GOOD: {
 163             //update the states in backend instance
 164             driver->_last_update_ms = AP_HAL::millis();
 165             driver->_status = AP_Proximity::Status::Good;
 166             const ObstacleItem item = {msg.yaw, msg.pitch, msg.distance};
 167
 168             if (driver->items.space()) {
 169                 // ignore reading if no place to put it in the queue
 170                 driver->items.push(item);
 171             }
 172             break;
 173         }
 174         //Additional states supported by Proximity message
 175         case ARDUPILOT_EQUIPMENT_PROXIMITY_SENSOR_PROXIMITY_READING_TYPE_NOT_CONNECTED: {
 176             driver->_last_update_ms = AP_HAL::millis();
 177             driver->_status = AP_Proximity::Status::NotConnected;
 178             break;
 179         }
 180         case ARDUPILOT_EQUIPMENT_PROXIMITY_SENSOR_PROXIMITY_READING_TYPE_NO_DATA: {
 181             driver->_last_update_ms = AP_HAL::millis();
 182             driver->_status = AP_Proximity::Status::NoData;
 183             break;
 184         }
 185         default:
 186             break;
 187     }
 188 }
 189
 190 #endif // AP_PROXIMITY_DRONECAN_ENABLED