/* 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 "AP_Proximity_DroneCAN.h" #if AP_PROXIMITY_DRONECAN_ENABLED #include #include #include #include #include extern const AP_HAL::HAL& hal; ObjectBuffer_TS AP_Proximity_DroneCAN::items(50); #define PROXIMITY_TIMEOUT_MS 500 // distance messages must arrive within this many milliseconds //links the Proximity DroneCAN message to this backend void AP_Proximity_DroneCAN::subscribe_msgs(AP_DroneCAN* ap_dronecan) { if (ap_dronecan == nullptr) { return; } if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_measurement, ap_dronecan->get_driver_index()) == nullptr) { AP_BoardConfig::allocation_error("measurement_sub"); } } //Method to find the backend relating to the node id AP_Proximity_DroneCAN* AP_Proximity_DroneCAN::get_uavcan_backend(AP_DroneCAN* ap_dronecan, uint8_t node_id, uint8_t address, bool create_new) { if (ap_dronecan == nullptr) { return nullptr; } AP_Proximity *prx = AP::proximity(); if (prx == nullptr) { return nullptr; } AP_Proximity_DroneCAN* driver = nullptr; //Scan through the proximity type params to find DroneCAN with matching address. for (uint8_t i = 0; i < PROXIMITY_MAX_INSTANCES; i++) { if ((AP_Proximity::Type)prx->params[i].type.get() == AP_Proximity::Type::DroneCAN && prx->params[i].address == address) { driver = (AP_Proximity_DroneCAN*)prx->drivers[i]; } //Double check if the driver was initialised as DroneCAN Type if (driver != nullptr && (driver->_backend_type == AP_Proximity::Type::DroneCAN)) { if (driver->_ap_dronecan == ap_dronecan && driver->_node_id == node_id) { return driver; } else { //we found a possible duplicate addressed sensor //we return nothing in such scenario return nullptr; } } } if (create_new) { for (uint8_t i = 0; i < PROXIMITY_MAX_INSTANCES; i++) { if ((AP_Proximity::Type)prx->params[i].type.get() == AP_Proximity::Type::DroneCAN && prx->params[i].address == address) { WITH_SEMAPHORE(prx->detect_sem); if (prx->drivers[i] != nullptr) { //we probably initialised this driver as something else, reboot is required for setting //it up as DroneCAN type return nullptr; } prx->drivers[i] = new AP_Proximity_DroneCAN(*prx, prx->state[i], prx->params[i]); driver = (AP_Proximity_DroneCAN*)prx->drivers[i]; if (driver == nullptr) { break; } GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Prx[%u]: added DroneCAN node %u addr %u", unsigned(i), unsigned(node_id), unsigned(address)); if (is_zero(prx->params[i].max_m) && is_zero(prx->params[i].min_m)) { // GCS reporting will be incorrect if min/max are not set GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Configure PRX%u_MIN and PRX%u_MAX", unsigned(i), unsigned(i)); } //Assign node id and respective dronecan driver, for identification if (driver->_ap_dronecan == nullptr) { driver->_ap_dronecan = ap_dronecan; driver->_node_id = node_id; break; } } } } return driver; } // update the state of the sensor void AP_Proximity_DroneCAN::update(void) { // check for timeout and set health status if ((_last_update_ms == 0 || (AP_HAL::millis() - _last_update_ms > PROXIMITY_TIMEOUT_MS))) { set_status(AP_Proximity::Status::NoData); } else { set_status(_status); } if (_status == AP_Proximity::Status::Good) { ObstacleItem object_item; WITH_SEMAPHORE(_sem); while (items.pop(object_item)) { const AP_Proximity_Boundary_3D::Face face = frontend.boundary.get_face(object_item.pitch_deg, object_item.yaw_deg); if (!is_zero(object_item.distance_m) && !ignore_reading(object_item.pitch_deg, object_item.yaw_deg, object_item.distance_m, false)) { // update boundary used for avoidance frontend.boundary.set_face_attributes(face, object_item.pitch_deg, object_item.yaw_deg, object_item.distance_m, state.instance); // update OA database database_push(object_item.pitch_deg, object_item.yaw_deg, object_item.distance_m); } } } } // get maximum and minimum distances (in meters) float AP_Proximity_DroneCAN::distance_max() const { if (is_zero(params.max_m)) { // GCS will not report correct correct value if max isn't set properly // This is a arbitrary value to prevent the above issue return 100.0f; } return params.max_m; } float AP_Proximity_DroneCAN::distance_min() const { return params.min_m; } //Proximity message handler void AP_Proximity_DroneCAN::handle_measurement(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const ardupilot_equipment_proximity_sensor_Proximity &msg) { //fetch the matching DroneCAN driver, node id and sensor id backend instance AP_Proximity_DroneCAN* driver = get_uavcan_backend(ap_dronecan, transfer.source_node_id, msg.sensor_id, true); if (driver == nullptr) { return; } WITH_SEMAPHORE(driver->_sem); switch (msg.reading_type) { case ARDUPILOT_EQUIPMENT_PROXIMITY_SENSOR_PROXIMITY_READING_TYPE_GOOD: { //update the states in backend instance driver->_last_update_ms = AP_HAL::millis(); driver->_status = AP_Proximity::Status::Good; const ObstacleItem item = {msg.yaw, msg.pitch, msg.distance}; if (driver->items.space()) { // ignore reading if no place to put it in the queue driver->items.push(item); } break; } //Additional states supported by Proximity message case ARDUPILOT_EQUIPMENT_PROXIMITY_SENSOR_PROXIMITY_READING_TYPE_NOT_CONNECTED: { driver->_last_update_ms = AP_HAL::millis(); driver->_status = AP_Proximity::Status::NotConnected; break; } case ARDUPILOT_EQUIPMENT_PROXIMITY_SENSOR_PROXIMITY_READING_TYPE_NO_DATA: { driver->_last_update_ms = AP_HAL::millis(); driver->_status = AP_Proximity::Status::NoData; break; } default: break; } } #endif // AP_PROXIMITY_DRONECAN_ENABLED