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_TERARANGERTOWEREVO_ENABLED
      19             : 
      20             : #include <AP_HAL/AP_HAL.h>
      21             : #include "AP_Proximity_TeraRangerTowerEvo.h"
      22             : 
      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           0 : void AP_Proximity_TeraRangerTowerEvo::update(void)
      31             : {
      32           0 :     if (_uart == nullptr) {
      33           0 :         return;
      34             :     }
      35           0 :     if (_last_request_sent_ms == 0) {
      36           0 :         _last_request_sent_ms = AP_HAL::millis();
      37             :     }
      38             :     //initialize the sensor
      39           0 :     if(_current_init_state != InitState::InitState_Finished)
      40             :     {
      41           0 :         initialise_modes();
      42             :     }
      43             : 
      44             :     // process incoming messages
      45           0 :     read_sensor_data();
      46             : 
      47             :     // check for timeout and set health status
      48           0 :     if ((_last_distance_received_ms == 0) || (AP_HAL::millis() - _last_distance_received_ms > PROXIMITY_TRTOWER_TIMEOUT_MS)) {
      49           0 :         set_status(AP_Proximity::Status::NoData);
      50             :     } else {
      51           0 :         set_status(AP_Proximity::Status::Good);
      52             :     }
      53             : }
      54             : 
      55             : // get maximum and minimum distances (in meters) of primary sensor
      56           0 : float AP_Proximity_TeraRangerTowerEvo::distance_max_m() const
      57             : {
      58           0 :     return 60.0f;
      59             : }
      60           0 : float AP_Proximity_TeraRangerTowerEvo::distance_min_m() const
      61             : {
      62           0 :     return 0.50f;
      63             : }
      64             : 
      65           0 : void AP_Proximity_TeraRangerTowerEvo::initialise_modes()
      66             : {
      67           0 :     if((AP_HAL::millis() - _last_request_sent_ms) < _mode_request_delay) {
      68           0 :         return;
      69             :     }
      70             : 
      71           0 :     if (_current_init_state == InitState_Printout) {
      72           0 :         set_mode(BINARY_MODE, 4);
      73           0 :     } else if (_current_init_state == InitState_Sequence) {
      74             :         //set tower mode - 4 sensors are triggered at once with 90 deg angle between each sensor
      75           0 :         set_mode(TOWER_MODE, 4);
      76           0 :     } else if (_current_init_state == InitState_Rate) {
      77             :         //set update rate of the sensor.
      78           0 :         set_mode(REFRESH_100_HZ, 5);
      79           0 :     } else if (_current_init_state == InitState_StreamStart) {
      80           0 :         set_mode(ACTIVATE_STREAM, 5);
      81             :     }
      82             : }
      83             : 
      84           0 : void AP_Proximity_TeraRangerTowerEvo::set_mode(const uint8_t *c, int length)
      85             : {
      86           0 :     _uart->write(c, length);
      87           0 :     _last_request_sent_ms = AP_HAL::millis();
      88           0 : }
      89             : 
      90             : // check for replies from sensor, returns true if at least one message was processed
      91           0 : bool AP_Proximity_TeraRangerTowerEvo::read_sensor_data()
      92             : {
      93           0 :     if (_uart == nullptr) {
      94           0 :         return false;
      95             :     }
      96             : 
      97           0 :     uint16_t message_count = 0;
      98           0 :     int16_t nbytes = _uart->available();
      99             : 
     100           0 :     if(_current_init_state != InitState_Finished && nbytes == 4) {
     101             : 
     102             :         //Increment _current_init_state only when we receive 4 ack bytes
     103           0 :         switch (_current_init_state) {
     104           0 :             case InitState_Printout:
     105           0 :                 _current_init_state = InitState_Sequence;
     106           0 :                 break;
     107           0 :             case InitState_Sequence:
     108           0 :                 _current_init_state = InitState_Rate;
     109           0 :                 break;
     110           0 :             case InitState_Rate:
     111           0 :                 _current_init_state = InitState_StreamStart;
     112           0 :                 break;
     113           0 :             case InitState_StreamStart:
     114           0 :                 _current_init_state = InitState_Finished;
     115           0 :                 break;
     116           0 :             case InitState_Finished:
     117           0 :                 break;
     118             :         }
     119             :     }
     120             : 
     121           0 :     while (nbytes-- > 0) {
     122           0 :         int16_t c = _uart->read();
     123           0 :         if (c==-1) {
     124           0 :             return false;
     125             :         }
     126           0 :         if (char(c) == 'T' ) {
     127           0 :             buffer_count = 0;
     128             :         }
     129           0 :         buffer[buffer_count++] = c;
     130             : 
     131             :         // we should always read 19 bytes THxxxxxxxxxxxxxxxxMC
     132           0 :         if (buffer_count >= 20){
     133           0 :             buffer_count = 0;
     134             : 
     135             :             //check if message has right CRC
     136           0 :             if (crc_crc8(buffer, 19) == buffer[19]){
     137           0 :                 update_sector_data(0,   UINT16_VALUE(buffer[2],  buffer[3]));   // d1
     138           0 :                 update_sector_data(45,  UINT16_VALUE(buffer[4],  buffer[5]));   // d2
     139           0 :                 update_sector_data(90,  UINT16_VALUE(buffer[6],  buffer[7]));   // d3
     140           0 :                 update_sector_data(135, UINT16_VALUE(buffer[8],  buffer[9]));   // d4
     141           0 :                 update_sector_data(180, UINT16_VALUE(buffer[10], buffer[11]));  // d5
     142           0 :                 update_sector_data(225, UINT16_VALUE(buffer[12], buffer[13]));  // d6
     143           0 :                 update_sector_data(270, UINT16_VALUE(buffer[14], buffer[15]));  // d7
     144           0 :                 update_sector_data(315, UINT16_VALUE(buffer[16], buffer[17]));  // d8
     145             : 
     146           0 :                 message_count++;
     147             :             }
     148             :         }
     149             :     }
     150           0 :     return (message_count > 0);
     151             : }
     152             : 
     153             : // process reply
     154           0 : void AP_Proximity_TeraRangerTowerEvo::update_sector_data(int16_t angle_deg, uint16_t distance_mm)
     155             : {
     156             :     // Get location on 3-D boundary based on angle to the object
     157           0 :     const AP_Proximity_Boundary_3D::Face face = frontend.boundary.get_face(angle_deg);
     158             :     //check for target too far, target too close and sensor not connected
     159           0 :     const bool valid = (distance_mm != 0xffff) && (distance_mm > 0x0001);
     160           0 :     if (valid && !ignore_reading(angle_deg, distance_mm * 0.001f, false)) {
     161           0 :         frontend.boundary.set_face_attributes(face, angle_deg, ((float) distance_mm) / 1000, state.instance);
     162             :         // update OA database
     163           0 :         database_push(angle_deg, ((float) distance_mm) / 1000);
     164             :     } else {
     165           0 :         frontend.boundary.reset_face(face, state.instance);
     166             :     }
     167           0 :     _last_distance_received_ms = AP_HAL::millis();
     168           0 : }
     169             : 
     170             : #endif // AP_PROXIMITY_TERARANGERTOWEREVO_ENABLED