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_config.h"
      17             : 
      18             : #if AP_PROXIMITY_TERARANGERTOWER_ENABLED
      19             : 
      20             : #include "AP_Proximity_TeraRangerTower.h"
      21             : 
      22             : #include <AP_HAL/AP_HAL.h>
      23             : #include <AP_Math/crc.h>
      24             : #include <ctype.h>
      25             : #include <stdio.h>
      26             : 
      27             : extern const AP_HAL::HAL& hal;
      28             : 
      29             : // update the state of the sensor
      30        8366 : void AP_Proximity_TeraRangerTower::update(void)
      31             : {
      32        8366 :     if (_uart == nullptr) {
      33           0 :         return;
      34             :     }
      35             : 
      36             :     // process incoming messages
      37        8366 :     read_sensor_data();
      38             : 
      39             :     // check for timeout and set health status
      40        8366 :     if ((_last_distance_received_ms == 0) || (AP_HAL::millis() - _last_distance_received_ms > PROXIMITY_TRTOWER_TIMEOUT_MS)) {
      41           0 :         set_status(AP_Proximity::Status::NoData);
      42             :     } else {
      43        8366 :         set_status(AP_Proximity::Status::Good);
      44             :     }
      45             : }
      46             : 
      47             : // get maximum and minimum distances (in meters) of primary sensor
      48        2510 : float AP_Proximity_TeraRangerTower::distance_max_m() const
      49             : {
      50        2510 :     return 4.5f;
      51             : }
      52         210 : float AP_Proximity_TeraRangerTower::distance_min_m() const
      53             : {
      54         210 :     return 0.20f;
      55             : }
      56             : 
      57             : // check for replies from sensor, returns true if at least one message was processed
      58        8366 : bool AP_Proximity_TeraRangerTower::read_sensor_data()
      59             : {
      60        8366 :     if (_uart == nullptr) {
      61           0 :         return false;
      62             :     }
      63             : 
      64        8366 :     uint16_t message_count = 0;
      65        8366 :     int16_t nbytes = _uart->available();
      66             : 
      67       12394 :     while (nbytes-- > 0) {
      68             :         uint8_t c;
      69        4028 :         if (!_uart->read(c)) {
      70           0 :             return false;
      71             :         }
      72        4028 :         if (char(c) == 'T' ) {
      73         212 :             buffer_count = 0;
      74             :         }
      75             : 
      76        4028 :         buffer[buffer_count++] = c;
      77             : 
      78             :         // we should always read 19 bytes THxxxxxxxxxxxxxxxxC
      79        4028 :         if (buffer_count >= 19){
      80         212 :             buffer_count = 0;
      81             : 
      82             :             // check if message has right CRC
      83         212 :             if (crc_crc8(buffer, 18) == buffer[18]){
      84         212 :                 update_sector_data(0,   UINT16_VALUE(buffer[2],  buffer[3]));   // d1
      85         212 :                 update_sector_data(45,  UINT16_VALUE(buffer[16], buffer[17]));  // d8
      86         212 :                 update_sector_data(90,  UINT16_VALUE(buffer[14], buffer[15]));  // d7
      87         212 :                 update_sector_data(135, UINT16_VALUE(buffer[12], buffer[13]));  // d6
      88         212 :                 update_sector_data(180, UINT16_VALUE(buffer[10], buffer[11]));  // d5
      89         212 :                 update_sector_data(225, UINT16_VALUE(buffer[8],  buffer[9]));   // d4
      90         212 :                 update_sector_data(270, UINT16_VALUE(buffer[6],  buffer[7]));   // d3
      91         212 :                 update_sector_data(315, UINT16_VALUE(buffer[4],  buffer[5]));   // d2
      92             : 
      93         212 :                 message_count++;
      94             :             }
      95             :         }
      96             :     }
      97        8366 :     return (message_count > 0);
      98             : }
      99             : 
     100             : // process reply
     101        1696 : void AP_Proximity_TeraRangerTower::update_sector_data(int16_t angle_deg, uint16_t distance_mm)
     102             : {
     103             :     // Get location on 3-D boundary based on angle to the object
     104        1696 :     const AP_Proximity_Boundary_3D::Face face = frontend.boundary.get_face(angle_deg);
     105        1696 :     if ((distance_mm != 0xffff) && !ignore_reading(angle_deg, distance_mm * 0.001f, false)) {
     106         849 :         frontend.boundary.set_face_attributes(face, angle_deg, ((float) distance_mm) / 1000, state.instance);
     107             :         // update OA database
     108         849 :         database_push(angle_deg, ((float) distance_mm) / 1000);
     109             :     } else {
     110         847 :         frontend.boundary.reset_face(face, state.instance);
     111             :     }
     112        1696 :     _last_distance_received_ms = AP_HAL::millis();
     113        1696 : }
     114             : 
     115             : #endif // AP_PROXIMITY_TERARANGERTOWER_ENABLED