mirror of https://github.com/ArduPilot/ardupilot
145 lines
4.5 KiB
C++
145 lines
4.5 KiB
C++
/*
|
|
This program is free software: you can redistribute it and/or modify
|
|
it under the terms of the GNU General Public License as published by
|
|
the Free Software Foundation, either version 3 of the License, or
|
|
(at your option) any later version.
|
|
|
|
This program is distributed in the hope that it will be useful,
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
GNU General Public License for more details.
|
|
|
|
You should have received a copy of the GNU General Public License
|
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
#include "AP_Proximity_LightWareSerial.h"
|
|
|
|
#if HAL_PROXIMITY_ENABLED
|
|
#include <AP_Common/AP_Common.h>
|
|
#include <AP_HAL/AP_HAL.h>
|
|
#include <AP_HAL/utility/sparse-endian.h>
|
|
#include <AP_Math/crc.h>
|
|
#include <GCS_MAVLink/GCS.h>
|
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
#define PROXIMITY_LIGHTWARE_HEADER 0xAA
|
|
|
|
// send message to sensor
|
|
void AP_Proximity_LightWareSerial::send_message(uint8_t msgid, bool write, const uint8_t *payload, uint16_t payload_len)
|
|
{
|
|
if ((_uart == nullptr) || (payload_len > PROXIMITY_LIGHTWARE_PAYLOAD_LEN_MAX)) {
|
|
return;
|
|
}
|
|
|
|
// check for sufficient space in outgoing buffer
|
|
if (_uart->txspace() < payload_len + 6U) {
|
|
return;
|
|
}
|
|
|
|
// write header
|
|
_uart->write((uint8_t)PROXIMITY_LIGHTWARE_HEADER);
|
|
uint16_t crc = crc_xmodem_update(0, PROXIMITY_LIGHTWARE_HEADER);
|
|
|
|
// write flags including payload length
|
|
const uint16_t flags = ((payload_len+1) << 6) | (write ? 0x01 : 0);
|
|
_uart->write(LOWBYTE(flags));
|
|
crc = crc_xmodem_update(crc, LOWBYTE(flags));
|
|
_uart->write(HIGHBYTE(flags));
|
|
crc = crc_xmodem_update(crc, HIGHBYTE(flags));
|
|
|
|
// msgid
|
|
_uart->write(msgid);
|
|
crc = crc_xmodem_update(crc, msgid);
|
|
|
|
// payload
|
|
if ((payload_len > 0) && (payload != nullptr)) {
|
|
for (uint16_t i = 0; i < payload_len; i++) {
|
|
_uart->write(payload[i]);
|
|
crc = crc_xmodem_update(crc, payload[i]);
|
|
}
|
|
}
|
|
|
|
// checksum
|
|
_uart->write(LOWBYTE(crc));
|
|
_uart->write(HIGHBYTE(crc));
|
|
}
|
|
|
|
// process one byte received on serial port
|
|
// returns true if a complete message has been received
|
|
// state is stored in _msg structure
|
|
bool AP_Proximity_LightWareSerial::parse_byte(uint8_t b)
|
|
{
|
|
// check that payload buffer is large enough
|
|
static_assert(ARRAY_SIZE(_msg.payload) == PROXIMITY_LIGHTWARE_PAYLOAD_LEN_MAX, "AP_Proximity_LightWareSerial: check _msg.payload array size");
|
|
|
|
// process byte depending upon current state
|
|
switch (_parse_state) {
|
|
|
|
case ParseState::HEADER:
|
|
if (b == PROXIMITY_LIGHTWARE_HEADER) {
|
|
_crc_expected = crc_xmodem_update(0, b);
|
|
_parse_state = ParseState::FLAGS_L;
|
|
}
|
|
break;
|
|
|
|
case ParseState::FLAGS_L:
|
|
_msg.flags_low = b;
|
|
_crc_expected = crc_xmodem_update(_crc_expected, b);
|
|
_parse_state = ParseState::FLAGS_H;
|
|
break;
|
|
|
|
case ParseState::FLAGS_H:
|
|
_msg.flags_high = b;
|
|
_crc_expected = crc_xmodem_update(_crc_expected, b);
|
|
_msg.payload_len = UINT16_VALUE(_msg.flags_high, _msg.flags_low) >> 6;
|
|
if ((_msg.payload_len == 0) || (_msg.payload_len > PROXIMITY_LIGHTWARE_PAYLOAD_LEN_MAX)) {
|
|
// invalid payload length, abandon message
|
|
_parse_state = ParseState::HEADER;
|
|
} else {
|
|
_parse_state = ParseState::MSG_ID;
|
|
}
|
|
break;
|
|
|
|
case ParseState::MSG_ID:
|
|
_msg.msgid = b;
|
|
_crc_expected = crc_xmodem_update(_crc_expected, b);
|
|
if (_msg.payload_len > 1) {
|
|
_parse_state = ParseState::PAYLOAD;
|
|
} else {
|
|
_parse_state = ParseState::CRC_L;
|
|
}
|
|
_payload_recv = 0;
|
|
break;
|
|
|
|
case ParseState::PAYLOAD:
|
|
if (_payload_recv < (_msg.payload_len - 1)) {
|
|
_msg.payload[_payload_recv] = b;
|
|
_payload_recv++;
|
|
_crc_expected = crc_xmodem_update(_crc_expected, b);
|
|
}
|
|
if (_payload_recv >= (_msg.payload_len - 1)) {
|
|
_parse_state = ParseState::CRC_L;
|
|
}
|
|
break;
|
|
|
|
case ParseState::CRC_L:
|
|
_msg.crc_low = b;
|
|
_parse_state = ParseState::CRC_H;
|
|
break;
|
|
|
|
case ParseState::CRC_H:
|
|
_parse_state = ParseState::HEADER;
|
|
_msg.crc_high = b;
|
|
if (_crc_expected == UINT16_VALUE(_msg.crc_high, _msg.crc_low)) {
|
|
return true;
|
|
}
|
|
break;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
#endif // HAL_PROXIMITY_ENABLED
|