diff --git a/libraries/AP_Proximity/AP_Proximity.cpp b/libraries/AP_Proximity/AP_Proximity.cpp index ac1d6786e6..82dc97e791 100644 --- a/libraries/AP_Proximity/AP_Proximity.cpp +++ b/libraries/AP_Proximity/AP_Proximity.cpp @@ -30,6 +30,8 @@ #include "AP_Proximity_DroneCAN.h" #include "AP_Proximity_Scripting.h" #include "AP_Proximity_LD06.h" +#include "AP_Proximity_MR72_CAN.h" + #include @@ -81,27 +83,45 @@ const AP_Param::GroupInfo AP_Proximity::var_info[] = { // @Path: AP_Proximity_Params.cpp AP_SUBGROUPINFO(params[0], "1", 21, AP_Proximity, AP_Proximity_Params), + // @Group: 1_ + // @Path: AP_Proximity_MR72_CAN.cpp + AP_SUBGROUPVARPTR(drivers[0], "1_", 26, AP_Proximity, backend_var_info[0]), + #if PROXIMITY_MAX_INSTANCES > 1 // @Group: 2 // @Path: AP_Proximity_Params.cpp AP_SUBGROUPINFO(params[1], "2", 22, AP_Proximity, AP_Proximity_Params), + + // @Group: 2_ + // @Path: AP_Proximity_MR72_CAN.cpp + AP_SUBGROUPVARPTR(drivers[1], "2_", 27, AP_Proximity, backend_var_info[1]), #endif #if PROXIMITY_MAX_INSTANCES > 2 // @Group: 3 // @Path: AP_Proximity_Params.cpp AP_SUBGROUPINFO(params[2], "3", 23, AP_Proximity, AP_Proximity_Params), + + // @Group: 3_ + // @Path: AP_Proximity_MR72_CAN.cpp + AP_SUBGROUPVARPTR(drivers[2], "3_", 28, AP_Proximity, backend_var_info[2]), #endif #if PROXIMITY_MAX_INSTANCES > 3 // @Group: 4 // @Path: AP_Proximity_Params.cpp AP_SUBGROUPINFO(params[3], "4", 24, AP_Proximity, AP_Proximity_Params), + + // @Group: 4_ + // @Path: AP_Proximity_MR72_CAN.cpp + AP_SUBGROUPVARPTR(drivers[3], "4_", 29, AP_Proximity, backend_var_info[3]), #endif AP_GROUPEND }; +const AP_Param::GroupInfo *AP_Proximity::backend_var_info[PROXIMITY_MAX_INSTANCES]; + AP_Proximity::AP_Proximity() { AP_Param::setup_object_defaults(this, var_info); @@ -206,6 +226,12 @@ void AP_Proximity::init() drivers[instance] = new AP_Proximity_Scripting(*this, state[instance], params[instance]); break; #endif +#if AP_PROXIMITY_MR72_ENABLED + case Type::MR72: + state[instance].instance = instance; + drivers[instance] = new AP_Proximity_MR72_CAN(*this, state[instance], params[instance]); + break; +# endif #if AP_PROXIMITY_SITL_ENABLED case Type::SITL: state[instance].instance = instance; @@ -237,6 +263,12 @@ void AP_Proximity::init() // initialise status state[instance].status = Status::NotConnected; + + // if the backend has some local parameters then make those available in the tree + if (drivers[instance] && state[instance].var_info) { + backend_var_info[instance] = state[instance].var_info; + AP_Param::load_object_from_eeprom(drivers[instance], backend_var_info[instance]); + } } } diff --git a/libraries/AP_Proximity/AP_Proximity.h b/libraries/AP_Proximity/AP_Proximity.h index 0363fea8f4..92d6f123e0 100644 --- a/libraries/AP_Proximity/AP_Proximity.h +++ b/libraries/AP_Proximity/AP_Proximity.h @@ -86,6 +86,9 @@ public: #endif #if AP_PROXIMITY_LD06_ENABLED LD06 = 16, +#endif +#if AP_PROXIMITY_MR72_ENABLED + MR72 = 17, #endif }; @@ -179,8 +182,12 @@ public: struct Proximity_State { uint8_t instance; // the instance number of this proximity sensor Status status; // sensor status + + const struct AP_Param::GroupInfo *var_info; // stores extra parameter information for the sensor (if it exists) }; + static const struct AP_Param::GroupInfo *backend_var_info[PROXIMITY_MAX_INSTANCES]; + // parameter list static const struct AP_Param::GroupInfo var_info[]; diff --git a/libraries/AP_Proximity/AP_Proximity_MR72_CAN.cpp b/libraries/AP_Proximity/AP_Proximity_MR72_CAN.cpp new file mode 100644 index 0000000000..4f0225c65b --- /dev/null +++ b/libraries/AP_Proximity/AP_Proximity_MR72_CAN.cpp @@ -0,0 +1,146 @@ +/* + 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_config.h" + +#if AP_PROXIMITY_MR72_ENABLED + +#include +#include +#include "AP_Proximity_MR72_CAN.h" + +const AP_Param::GroupInfo AP_Proximity_MR72_CAN::var_info[] = { + + // @Param: RECV_ID + // @DisplayName: CAN receive ID + // @Description: The receive ID of the CAN frames. A value of zero means all IDs are accepted. + // @Range: 0 65535 + // @User: Advanced + AP_GROUPINFO("RECV_ID", 1, AP_Proximity_MR72_CAN, receive_id, 0), + + AP_GROUPEND +}; + +MR72_MultiCAN *AP_Proximity_MR72_CAN::multican; + +AP_Proximity_MR72_CAN::AP_Proximity_MR72_CAN(AP_Proximity &_frontend, + AP_Proximity::Proximity_State &_state, + AP_Proximity_Params& _params): + AP_Proximity_Backend(_frontend, _state, _params) +{ + if (multican == nullptr) { + multican = new MR72_MultiCAN(); + if (multican == nullptr) { + AP_BoardConfig::allocation_error("MR72_CAN"); + } + } + + { + // add to linked list of drivers + WITH_SEMAPHORE(multican->sem); + auto *prev = multican->drivers; + next = prev; + multican->drivers = this; + } + + AP_Param::setup_object_defaults(this, var_info); + state.var_info = var_info; +} + +// update state +void AP_Proximity_MR72_CAN::update(void) +{ + WITH_SEMAPHORE(_sem); + const uint32_t now = AP_HAL::millis(); + if (now - last_update_ms > 500) { + // no new data. + set_status(AP_Proximity::Status::NoData); + } else { + set_status(AP_Proximity::Status::Good); + } +} + +// handler for incoming frames. These come in at 100Hz +bool AP_Proximity_MR72_CAN::handle_frame(AP_HAL::CANFrame &frame) +{ + WITH_SEMAPHORE(_sem); + + + // check if message is coming from the right sensor ID + const uint16_t id = frame.id; + + if (receive_id > 0 && (get_radar_id(frame.id) != uint32_t(receive_id))) { + return false; + } + + switch (id & 0xFU) { + case 0xAU: + // number of objects + _object_count = frame.data[0]; + _current_object_index = 0; + _temp_boundary.update_3D_boundary(state.instance, frontend.boundary); + _temp_boundary.reset(); + last_update_ms = AP_HAL::millis(); + break; + case 0xBU: + // obstacle data + parse_distance_message(frame); + break; + default: + break; + } + + return true; + +} + +// parse a distance message from CAN frame +bool AP_Proximity_MR72_CAN::parse_distance_message(AP_HAL::CANFrame &frame) +{ + if (_current_object_index >= _object_count) { + // should never happen + return false; + } + _current_object_index++; + + Vector2f obstacle_fr; + // This parsing comes from the NanoRadar MR72 datasheet + obstacle_fr.x = ((frame.data[2] & 0x07U) * 256 + frame.data[3]) * 0.2 - 204.6; + obstacle_fr.y = (frame.data[1] * 32 + (frame.data[2] >> 3)) * 0.2 - 500; + const float yaw = correct_angle_for_orientation(wrap_360(degrees(atan2f(obstacle_fr.x, obstacle_fr.y)))); + + const float objects_dist = obstacle_fr.length(); + + if (ignore_reading(yaw, objects_dist)) { + // obstacle is probably near ground or out of range + return false; + } + + const AP_Proximity_Boundary_3D::Face face = frontend.boundary.get_face(yaw); + _temp_boundary.add_distance(face, yaw, objects_dist); + return true; +} + +// handle frames from CANSensor, passing to the drivers +void MR72_MultiCAN::handle_frame(AP_HAL::CANFrame &frame) +{ + WITH_SEMAPHORE(sem); + for (auto *d = drivers; d; d=d->next) { + if (d->handle_frame(frame)) { + break; + } + } +} + +#endif // HAL_PROXIMITY_ENABLED diff --git a/libraries/AP_Proximity/AP_Proximity_MR72_CAN.h b/libraries/AP_Proximity/AP_Proximity_MR72_CAN.h new file mode 100644 index 0000000000..e2a3dcbb9d --- /dev/null +++ b/libraries/AP_Proximity/AP_Proximity_MR72_CAN.h @@ -0,0 +1,68 @@ +#pragma once +#include "AP_Proximity_config.h" + +#if AP_PROXIMITY_MR72_ENABLED + +#include "AP_Proximity.h" +#include "AP_Proximity_Backend.h" +#include +#include + +#define MR72_MAX_RANGE_M 50.0f // max range of the sensor in meters +#define MR72_MIN_RANGE_M 0.2f // min range of the sensor in meters + +class MR72_MultiCAN; + +class AP_Proximity_MR72_CAN : public AP_Proximity_Backend { +public: + friend class MR72_MultiCAN; + + AP_Proximity_MR72_CAN(AP_Proximity &_frontend, AP_Proximity::Proximity_State &_state, AP_Proximity_Params& _params); + + void update() override; + + // handler for incoming frames. Return true if consumed + bool handle_frame(AP_HAL::CANFrame &frame); + + // parse a distance message from CAN frame + bool parse_distance_message(AP_HAL::CANFrame &frame); + + // get maximum and minimum distances (in meters) of sensor + float distance_max() const override { return MR72_MAX_RANGE_M; } + float distance_min() const override { return MR72_MIN_RANGE_M; } + + static const struct AP_Param::GroupInfo var_info[]; + + AP_Proximity_Temp_Boundary _temp_boundary; + +private: + + uint32_t get_radar_id(uint32_t id) const { return ((id & 0xF0U) >> 4U); } + + uint32_t _object_count; // total number of objects to read + uint32_t _current_object_index; // current object index + uint32_t last_update_ms; // last update time in ms + + AP_Int32 receive_id; // ID of the sensor + + static MR72_MultiCAN *multican; // linked list + AP_Proximity_MR72_CAN *next; +}; + +// a class to allow for multiple MR_72 backends with one +// CANSensor driver +class MR72_MultiCAN : public CANSensor { +public: + MR72_MultiCAN() : CANSensor("MR72") { + register_driver(AP_CAN::Protocol::MR72); + } + + // handler for incoming frames + void handle_frame(AP_HAL::CANFrame &frame) override; + + HAL_Semaphore sem; + AP_Proximity_MR72_CAN *drivers; + +}; + +#endif // HAL_PROXIMITY_ENABLED diff --git a/libraries/AP_Proximity/AP_Proximity_Params.cpp b/libraries/AP_Proximity/AP_Proximity_Params.cpp index 046a29c3b7..ad85936067 100644 --- a/libraries/AP_Proximity/AP_Proximity_Params.cpp +++ b/libraries/AP_Proximity/AP_Proximity_Params.cpp @@ -8,7 +8,7 @@ const AP_Param::GroupInfo AP_Proximity_Params::var_info[] = { // @Param: _TYPE // @DisplayName: Proximity type // @Description: What type of proximity sensor is connected - // @Values: 0:None,7:LightwareSF40c,2:MAVLink,3:TeraRangerTower,4:RangeFinder,5:RPLidarA2,6:TeraRangerTowerEvo,8:LightwareSF45B,10:SITL,12:AirSimSITL,13:CygbotD1, 14:DroneCAN, 15:Scripting, 16:LD06 + // @Values: 0:None,7:LightwareSF40c,2:MAVLink,3:TeraRangerTower,4:RangeFinder,5:RPLidarA2,6:TeraRangerTowerEvo,8:LightwareSF45B,10:SITL,12:AirSimSITL,13:CygbotD1, 14:DroneCAN, 15:Scripting, 16:LD06, 17: MR72_CAN // @RebootRequired: True // @User: Standard AP_GROUPINFO_FLAGS("_TYPE", 1, AP_Proximity_Params, type, 0, AP_PARAM_FLAG_ENABLE), diff --git a/libraries/AP_Proximity/AP_Proximity_config.h b/libraries/AP_Proximity/AP_Proximity_config.h index a2130b0b5c..a05e94b653 100644 --- a/libraries/AP_Proximity/AP_Proximity_config.h +++ b/libraries/AP_Proximity/AP_Proximity_config.h @@ -37,6 +37,11 @@ #define AP_PROXIMITY_LIGHTWARE_SF45B_ENABLED AP_PROXIMITY_BACKEND_DEFAULT_ENABLED #endif + +#ifndef AP_PROXIMITY_MR72_ENABLED +#define AP_PROXIMITY_MR72_ENABLED AP_PROXIMITY_BACKEND_DEFAULT_ENABLED && HAL_MAX_CAN_PROTOCOL_DRIVERS +#endif + #ifndef AP_PROXIMITY_SCRIPTING_ENABLED #define AP_PROXIMITY_SCRIPTING_ENABLED AP_PROXIMITY_BACKEND_DEFAULT_ENABLED && AP_SCRIPTING_ENABLED #endif