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_LightWareSerial.h"
      17             : 
      18             : #if AP_LIGHTWARESERIAL_ENABLED
      19             : 
      20             : #include <AP_Common/AP_Common.h>
      21             : #include <AP_HAL/utility/sparse-endian.h>
      22             : #include <AP_Math/crc.h>
      23             : #include <GCS_MAVLink/GCS.h>
      24             : 
      25             : extern const AP_HAL::HAL& hal;
      26             : 
      27             : #define LIGHTWARE_HEADER    0xAA
      28             : 
      29             : // send message to sensor
      30          32 : void AP_LightWareSerial::send_message(uint8_t msgid, bool write, const uint8_t *payload, uint16_t payload_len)
      31             : {
      32          32 :     if ((_uart == nullptr) || (payload_len > LIGHTWARE_PAYLOAD_LEN_MAX)) {
      33           0 :         return;
      34             :     }
      35             : 
      36             :     // check for sufficient space in outgoing buffer
      37          32 :     if (_uart->txspace() < payload_len + 6U) {
      38           0 :         return;
      39             :     }
      40             : 
      41             :     // write header
      42          32 :     _uart->write((uint8_t)LIGHTWARE_HEADER);
      43          32 :     uint16_t crc = crc_xmodem_update(0, LIGHTWARE_HEADER);
      44             : 
      45             :     // write flags including payload length
      46          32 :     const uint16_t flags = ((payload_len+1) << 6) | (write ? 0x01 : 0);
      47          32 :     _uart->write(LOWBYTE(flags));
      48          32 :     crc = crc_xmodem_update(crc, LOWBYTE(flags));
      49          32 :     _uart->write(HIGHBYTE(flags));
      50          32 :     crc = crc_xmodem_update(crc, HIGHBYTE(flags));
      51             : 
      52             :     // msgid
      53          32 :     _uart->write(msgid);
      54          32 :     crc = crc_xmodem_update(crc, msgid);
      55             : 
      56             :     // payload
      57          32 :     if ((payload_len > 0) && (payload != nullptr)) {
      58         126 :         for (uint16_t i = 0; i < payload_len; i++) {
      59          96 :             _uart->write(payload[i]);
      60          96 :             crc = crc_xmodem_update(crc, payload[i]);
      61             :         }
      62             :     }
      63             : 
      64             :     // checksum
      65          32 :     _uart->write(LOWBYTE(crc));
      66          32 :     _uart->write(HIGHBYTE(crc));
      67             : }
      68             : 
      69             : // process one byte received on serial port
      70             : // returns true if a complete message has been received
      71             : // state is stored in _msg structure
      72      107622 : bool AP_LightWareSerial::parse_byte(uint8_t b)
      73             : {
      74             :     // check that payload buffer is large enough
      75             :     static_assert(ARRAY_SIZE(_msg.payload) == LIGHTWARE_PAYLOAD_LEN_MAX, "AP_LightWareSerial: check _msg.payload array size");
      76             : 
      77             :     // process byte depending upon current state
      78      107622 :     switch (_parse_state) {
      79             : 
      80        9402 :     case ParseState::HEADER:
      81        9402 :         if (b == LIGHTWARE_HEADER) {
      82        9402 :             _crc_expected = crc_xmodem_update(0, b);
      83        9402 :             _parse_state = ParseState::FLAGS_L;
      84             :         }
      85        9402 :         break;
      86             : 
      87        9402 :     case ParseState::FLAGS_L:
      88        9402 :         _msg.flags_low = b;
      89        9402 :         _crc_expected = crc_xmodem_update(_crc_expected, b);
      90        9402 :         _parse_state = ParseState::FLAGS_H;
      91        9402 :         break;
      92             : 
      93        9402 :     case ParseState::FLAGS_H:
      94        9402 :         _msg.flags_high = b;
      95        9402 :         _crc_expected = crc_xmodem_update(_crc_expected, b);
      96        9402 :         _msg.payload_len = UINT16_VALUE(_msg.flags_high, _msg.flags_low) >> 6;
      97        9402 :         if ((_msg.payload_len == 0) || (_msg.payload_len > LIGHTWARE_PAYLOAD_LEN_MAX)) {
      98             :             // invalid payload length, abandon message
      99           0 :             _parse_state = ParseState::HEADER;
     100             :         } else {
     101        9402 :             _parse_state = ParseState::MSG_ID;
     102             :         }
     103        9402 :         break;
     104             : 
     105        9402 :     case ParseState::MSG_ID:
     106        9402 :         _msg.msgid = b;
     107        9402 :         _crc_expected = crc_xmodem_update(_crc_expected, b);
     108        9402 :         if (_msg.payload_len > 1) {
     109        9402 :             _parse_state = ParseState::PAYLOAD;
     110             :         } else {
     111           0 :             _parse_state = ParseState::CRC_L;
     112             :         }
     113        9402 :         _payload_recv = 0;
     114        9402 :         break;
     115             : 
     116       51210 :     case ParseState::PAYLOAD:
     117       51210 :         if (_payload_recv < (_msg.payload_len - 1)) {
     118       51210 :             _msg.payload[_payload_recv] = b;
     119       51210 :             _payload_recv++;
     120       51210 :             _crc_expected = crc_xmodem_update(_crc_expected, b);
     121             :         }
     122       51210 :         if (_payload_recv >= (_msg.payload_len - 1)) {
     123        9402 :             _parse_state = ParseState::CRC_L;
     124             :         }
     125       51210 :         break;
     126             : 
     127        9402 :     case ParseState::CRC_L:
     128        9402 :         _msg.crc_low = b;
     129        9402 :         _parse_state = ParseState::CRC_H;
     130        9402 :         break;
     131             : 
     132        9402 :     case ParseState::CRC_H:
     133        9402 :         _parse_state = ParseState::HEADER;
     134        9402 :         _msg.crc_high = b;
     135        9402 :         if (_crc_expected == UINT16_VALUE(_msg.crc_high, _msg.crc_low)) {
     136        9402 :             return true;
     137             :         }
     138           0 :         break;
     139             :     }
     140             : 
     141       98220 :     return false;
     142             : }
     143             : 
     144             : #endif //AP_LIGHTWARESERIAL_ENABLED
     145             :