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             : #include "AP_Proximity_config.h"
      16             : 
      17             : #if AP_PROXIMITY_MR72_DRIVER_ENABLED
      18             : 
      19             : #include <AP_HAL/AP_HAL.h>
      20             : #include <AP_BoardConfig/AP_BoardConfig.h>
      21             : #include "AP_Proximity_MR72_CAN.h"
      22             : 
      23             : const AP_Param::GroupInfo AP_Proximity_MR72_CAN::var_info[] = {
      24             : 
      25             :     // @Param: RECV_ID
      26             :     // @DisplayName: CAN receive ID
      27             :     // @Description: The receive ID of the CAN frames. A value of zero means all IDs are accepted.
      28             :     // @Range: 0 65535
      29             :     // @User: Advanced
      30             :     AP_GROUPINFO("RECV_ID", 1, AP_Proximity_MR72_CAN, receive_id, 0),
      31             : 
      32             :     AP_GROUPEND
      33             : };
      34             : 
      35           0 : AP_Proximity_MR72_CAN::AP_Proximity_MR72_CAN(AP_Proximity &_frontend,
      36             :                                      AP_Proximity::Proximity_State &_state,
      37           0 :                                      AP_Proximity_Params& _params):
      38           0 :     AP_Proximity_Backend(_frontend, _state, _params)
      39             : {
      40           0 :     multican_MR72 = NEW_NOTHROW MultiCAN{FUNCTOR_BIND_MEMBER(&AP_Proximity_MR72_CAN::handle_frame, bool, AP_HAL::CANFrame &), AP_CAN::Protocol::RadarCAN, "MR72 MultiCAN"};
      41           0 :     if (multican_MR72 == nullptr) {
      42           0 :         AP_BoardConfig::allocation_error("Failed to create proximity multican");
      43             :     }
      44             : 
      45           0 :     AP_Param::setup_object_defaults(this, var_info);
      46           0 :     state.var_info = var_info;
      47           0 : }
      48             : 
      49             : // update state
      50           0 : void AP_Proximity_MR72_CAN::update(void)
      51             : {
      52           0 :     WITH_SEMAPHORE(_sem);
      53           0 :     const uint32_t now = AP_HAL::millis();
      54           0 :     if (now - last_update_ms > 500) {
      55             :         // no new data.
      56           0 :         set_status(AP_Proximity::Status::NoData);
      57             :     } else {
      58           0 :         set_status(AP_Proximity::Status::Good);
      59             :     }
      60           0 : }
      61             : 
      62             : // handler for incoming frames. These come in at 100Hz
      63           0 : bool AP_Proximity_MR72_CAN::handle_frame(AP_HAL::CANFrame &frame)
      64             : {
      65           0 :     WITH_SEMAPHORE(_sem);
      66             : 
      67             : 
      68             :     // check if message is coming from the right sensor ID
      69           0 :     const uint16_t id = frame.id;
      70             : 
      71           0 :     if (receive_id > 0 && (get_radar_id(frame.id) != uint32_t(receive_id))) {
      72           0 :         return false;
      73             :     }
      74             : 
      75           0 :     switch (id & 0xFU) {
      76           0 :     case 0xAU:
      77             :         // number of objects
      78           0 :         _object_count = frame.data[0];
      79           0 :         _current_object_index = 0;
      80           0 :         _temp_boundary.update_3D_boundary(state.instance, frontend.boundary);
      81           0 :         _temp_boundary.reset();
      82           0 :         last_update_ms = AP_HAL::millis();
      83           0 :         break;
      84           0 :     case 0xBU:
      85             :         // obstacle data
      86           0 :         parse_distance_message(frame);
      87           0 :         break;
      88           0 :     default:
      89           0 :         break;
      90             :     }
      91             : 
      92           0 :     return true;
      93             : 
      94           0 : }
      95             : 
      96             : // parse a distance message from CAN frame
      97           0 : bool AP_Proximity_MR72_CAN::parse_distance_message(AP_HAL::CANFrame &frame)
      98             : {
      99           0 :     if (_current_object_index >= _object_count) {
     100             :         // should never happen
     101           0 :         return false;
     102             :     }
     103           0 :     _current_object_index++;
     104             : 
     105           0 :     Vector2f obstacle_fr;
     106             :     // This parsing comes from the NanoRadar MR72 datasheet
     107           0 :     obstacle_fr.x = ((frame.data[2] & 0x07U) * 256 + frame.data[3]) * 0.2 - 204.6;
     108           0 :     obstacle_fr.y = (frame.data[1] * 32 + (frame.data[2] >> 3)) * 0.2 - 500;
     109           0 :     const float yaw = correct_angle_for_orientation(wrap_360(degrees(atan2f(obstacle_fr.x, obstacle_fr.y))));
     110             : 
     111           0 :     const float objects_dist = obstacle_fr.length();
     112             : 
     113           0 :     if (ignore_reading(yaw, objects_dist)) {
     114             :         // obstacle is probably near ground or out of range
     115           0 :         return false;
     116             :     }
     117             : 
     118           0 :     const AP_Proximity_Boundary_3D::Face face = frontend.boundary.get_face(yaw);
     119           0 :     _temp_boundary.add_distance(face, yaw, objects_dist);
     120           0 :     database_push(yaw, objects_dist);
     121           0 :     return true;
     122             : }
     123             : 
     124             : #endif  // AP_PROXIMITY_MR72_DRIVER_ENABLED