Line data    Source code

       1             : #include "AP_Proximity_config.h"
       2             : 
       3             : #if AP_PROXIMITY_CYGBOT_ENABLED
       4             : 
       5             : #include "AP_Proximity_Cygbot_D1.h"
       6             : 
       7             : // update the state of the sensor
       8           0 : void AP_Proximity_Cygbot_D1::update()
       9             : {
      10           0 :     if (!_initialized) {
      11           0 :         send_sensor_start();
      12           0 :         _temp_boundary.reset();
      13           0 :         _initialized = true;
      14           0 :         _last_init_ms = AP_HAL::millis();
      15             :     }
      16             : 
      17           0 :     if ((AP_HAL::millis() - _last_init_ms) < CYGBOT_INIT_TIMEOUT_MS) {
      18             :         // just initialized
      19           0 :         set_status(AP_Proximity::Status::NoData);
      20           0 :         return;
      21             :     }
      22             : 
      23             :     // read data
      24           0 :     read_sensor_data();
      25             : 
      26           0 :     if (AP_HAL::millis() - _last_distance_received_ms < CYGBOT_TIMEOUT_MS) {
      27           0 :         set_status(AP_Proximity::Status::Good);
      28             :     } else {
      29             :         // long time since we received any valid sensor data
      30             :         // try sending the sensor the "send data" message
      31           0 :         _initialized = false;
      32           0 :         set_status(AP_Proximity::Status::NoData);
      33             :     }
      34             : }
      35             : 
      36             : // send message to the sensor to start streaming 2-D data
      37           0 : void AP_Proximity_Cygbot_D1::send_sensor_start()
      38             : {
      39             :     // this message corresponds to "start message"
      40           0 :     const uint8_t packet_start_2d[8] = { CYGBOT_PACKET_HEADER_0, CYGBOT_PACKET_HEADER_1, CYGBOT_PACKET_HEADER_2, 0x02, 0x00, 0x01, 0x00, 0x03 };
      41           0 :     _uart->write(packet_start_2d, 8);
      42           0 : }
      43             : 
      44           0 : void AP_Proximity_Cygbot_D1::read_sensor_data()
      45             : {
      46           0 :     uint32_t nbytes = _uart->available();
      47           0 :     while (nbytes-- > 0) {
      48           0 :         int16_t byte = _uart->read();
      49           0 :         if (!parse_byte(byte)) {
      50             :             // reset
      51           0 :             reset();
      52             :         }
      53             :     }
      54           0 : }
      55             : 
      56             : // parse one byte from the sensor. Return false on error.
      57             : // Message format is: header1 + header2 + header3 + length1 + length2 + PayloadCommand + checksum
      58           0 : bool AP_Proximity_Cygbot_D1::parse_byte(uint8_t data)
      59             : {
      60           0 :     switch (_parse_state) {
      61           0 :     case Header1:
      62           0 :         if (data == CYGBOT_PACKET_HEADER_0) {
      63           0 :             _parse_state = Header2;
      64           0 :             return true;
      65             :         }
      66             : 
      67           0 :         return false;
      68             : 
      69           0 :     case Header2:
      70           0 :         if (data == CYGBOT_PACKET_HEADER_1) {
      71           0 :             _parse_state = Header3;
      72           0 :             return true;
      73             :         }
      74           0 :         return false;
      75             : 
      76           0 :     case Header3:
      77           0 :         if (data == CYGBOT_PACKET_HEADER_2) {
      78           0 :             _parse_state = Length1;
      79           0 :             return true;
      80             :         }
      81           0 :         return false;
      82             : 
      83           0 :     case Length1:
      84           0 :         _msg.payload_len_flags_low = data;
      85           0 :         _parse_state = Length2;
      86           0 :         return true;
      87             : 
      88           0 :     case Length2:
      89           0 :         _msg.payload_len_flags_high = data;
      90           0 :         _msg.payload_len = UINT16_VALUE(data, _msg.payload_len_flags_low);
      91           0 :         if (_msg.payload_len > CYGBOT_MAX_MSG_SIZE) {
      92           0 :             return false;
      93             :         }
      94           0 :         _parse_state = Payload_Header;
      95           0 :         return true;
      96             : 
      97           0 :     case Payload_Header:
      98           0 :         if (data == CYGBOT_PAYLOAD_HEADER) {
      99           0 :             _parse_state = Payload_Data;
     100           0 :             _msg.payload_counter = 1;
     101           0 :             _msg.payload[_msg.payload_counter] = data;
     102           0 :             return true;
     103             :         }
     104           0 :         return false;
     105             : 
     106           0 :     case Payload_Data:
     107           0 :         if (_msg.payload_counter < (_msg.payload_len)) {
     108           0 :             _msg.payload_counter++;
     109           0 :             _msg.payload[_msg.payload_counter] = data;
     110           0 :             return true;
     111             :         }
     112           0 :         _parse_state = CheckSum;
     113             :         FALLTHROUGH;
     114             : 
     115           0 :     case CheckSum: {
     116           0 :         const uint8_t checksum_num = calc_checksum(_msg.payload, _msg.payload_len);
     117           0 :         if (data != checksum_num) {
     118           0 :             return false;
     119             :         }
     120             :         // checksum is valid, parse payload
     121           0 :         _last_distance_received_ms = AP_HAL::millis();
     122           0 :         parse_payload();
     123           0 :         _temp_boundary.update_3D_boundary(state.instance, frontend.boundary);
     124           0 :         reset();
     125           0 :         return true;
     126             :     }
     127             :     break;
     128             : 
     129           0 :     default:
     130           0 :         return false;
     131             :     }
     132             : 
     133             :     return false;
     134             : }
     135             : 
     136             : // parse payload, to pick out distances, and feed them to the correct faces
     137           0 : void AP_Proximity_Cygbot_D1::parse_payload()
     138             : {
     139             :     // current horizontal angle in the payload
     140           0 :     float sampled_angle = CYGBOT_2D_START_ANGLE;
     141             : 
     142             :     // start from second byte as first byte is part of the header
     143           0 :     for (uint16_t i = 2; i < _msg.payload_len; i += 2) {
     144           0 :         const float corrected_angle = correct_angle_for_orientation(sampled_angle);
     145           0 :         const uint16_t distance_mm = UINT16_VALUE(_msg.payload[i], _msg.payload[i+1]);
     146           0 :         float distance_m = distance_mm * 0.001f;
     147           0 :         if (distance_m > distance_min_m() && distance_m < distance_max_m()) {
     148           0 :             if (ignore_reading(corrected_angle, distance_m)) {
     149             :                 // ignore this angle
     150           0 :                 sampled_angle += CYGBOT_2D_ANGLE_STEP;
     151           0 :                 continue;
     152             :             }
     153             :             // convert angle to face
     154           0 :             const AP_Proximity_Boundary_3D::Face face = frontend.boundary.get_face(corrected_angle);
     155             : 
     156             :             // push face to temp boundary
     157           0 :             _temp_boundary.add_distance(face, corrected_angle, distance_m);
     158             :             // push to OA_DB
     159           0 :             database_push(corrected_angle, distance_m);
     160             :         }
     161             :         // increment sampled angle
     162           0 :         sampled_angle += CYGBOT_2D_ANGLE_STEP;
     163             :     }
     164           0 : }
     165             : 
     166             : // Checksum
     167           0 : uint8_t AP_Proximity_Cygbot_D1::calc_checksum(uint8_t *buff, int buffSize)
     168             : {
     169           0 :     uint8_t check_sum_num = 0;
     170           0 :     check_sum_num ^= _msg.payload_len_flags_high;
     171           0 :     check_sum_num ^= _msg.payload_len_flags_low;
     172           0 :     for (uint16_t i = 0; i <= buffSize; i++) {
     173           0 :         check_sum_num ^= buff[i];
     174             :     }
     175           0 :     return check_sum_num;
     176             : }
     177             : 
     178             : // reset all variables and flags
     179           0 : void AP_Proximity_Cygbot_D1::reset()
     180             : {
     181           0 :     _parse_state = Header1;
     182           0 :     _msg.payload_counter = 0;
     183           0 :     _msg.payload_len = 0;
     184           0 :     _temp_boundary.reset();
     185           0 : }
     186             : 
     187             : #endif // AP_PROXIMITY_CYGBOT_ENABLED