/* 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_Beacon_Pozyx.h" #if AP_BEACON_POZYX_ENABLED #include <AP_HAL/AP_HAL.h> #include <ctype.h> #include <stdio.h> extern const AP_HAL::HAL& hal; // return true if sensor is basically healthy (we are receiving data) bool AP_Beacon_Pozyx::healthy() { // healthy if we have parsed a message within the past 300ms return ((AP_HAL::millis() - last_update_ms) < AP_BEACON_TIMEOUT_MS); } // update the state of the sensor void AP_Beacon_Pozyx::update(void) { if (uart == nullptr) { return; } // read any available characters int16_t nbytes = uart->available(); while (nbytes-- > 0) { char c = uart->read(); switch (parse_state) { default: case ParseState_WaitingForHeader: if (c == AP_BEACON_POZYX_HEADER) { parse_state = ParseState_WaitingForMsgId; linebuf_len = 0; } break; case ParseState_WaitingForMsgId: parse_msg_id = c; switch (parse_msg_id) { case AP_BEACON_POZYX_MSGID_BEACON_CONFIG: case AP_BEACON_POZYX_MSGID_BEACON_DIST: case AP_BEACON_POZYX_MSGID_POSITION: parse_state = ParseState_WaitingForLen; break; default: // invalid message id parse_state = ParseState_WaitingForHeader; break; } break; case ParseState_WaitingForLen: parse_msg_len = c; if (parse_msg_len > AP_BEACON_POZYX_MSG_LEN_MAX) { // invalid message length parse_state = ParseState_WaitingForHeader; } else { parse_state = ParseState_WaitingForContents; } break; case ParseState_WaitingForContents: // add to buffer linebuf[linebuf_len++] = c; if ((linebuf_len == parse_msg_len) || (linebuf_len == sizeof(linebuf))) { // process buffer parse_buffer(); // reset state for next message parse_state = ParseState_WaitingForHeader; } break; } } } // parse buffer void AP_Beacon_Pozyx::parse_buffer() { // check crc uint8_t checksum = 0; checksum ^= parse_msg_id; checksum ^= parse_msg_len; for (uint8_t i=0; i<linebuf_len; i++) { checksum ^= linebuf[i]; } // return if failed checksum check if (checksum != 0) { return; } bool parsed = false; switch (parse_msg_id) { case AP_BEACON_POZYX_MSGID_BEACON_CONFIG: { uint8_t beacon_id = linebuf[0]; //uint8_t beacon_count = linebuf[1]; int32_t beacon_x = (uint32_t)linebuf[5] << 24 | (uint32_t)linebuf[4] << 16 | (uint32_t)linebuf[3] << 8 | (uint32_t)linebuf[2]; int32_t beacon_y = (uint32_t)linebuf[9] << 24 | (uint32_t)linebuf[8] << 16 | (uint32_t)linebuf[7] << 8 | (uint32_t)linebuf[6]; int32_t beacon_z = (uint32_t)linebuf[13] << 24 | (uint32_t)linebuf[12] << 16 | (uint32_t)linebuf[11] << 8 | (uint32_t)linebuf[10]; Vector3f beacon_pos(beacon_x * 0.001f, beacon_y * 0.001f, -beacon_z * 0.001f); if (beacon_pos.length() <= AP_BEACON_DISTANCE_MAX) { set_beacon_position(beacon_id, beacon_pos); parsed = true; } } break; case AP_BEACON_POZYX_MSGID_BEACON_DIST: { uint8_t beacon_id = linebuf[0]; uint32_t beacon_distance = (uint32_t)linebuf[4] << 24 | (uint32_t)linebuf[3] << 16 | (uint32_t)linebuf[2] << 8 | (uint32_t)linebuf[1]; float beacon_dist = beacon_distance/1000.0f; if (beacon_dist <= AP_BEACON_DISTANCE_MAX) { set_beacon_distance(beacon_id, beacon_dist); parsed = true; } } break; case AP_BEACON_POZYX_MSGID_POSITION: { int32_t vehicle_x = (uint32_t)linebuf[3] << 24 | (uint32_t)linebuf[2] << 16 | (uint32_t)linebuf[1] << 8 | (uint32_t)linebuf[0]; int32_t vehicle_y = (uint32_t)linebuf[7] << 24 | (uint32_t)linebuf[6] << 16 | (uint32_t)linebuf[5] << 8 | (uint32_t)linebuf[4]; int32_t vehicle_z = (uint32_t)linebuf[11] << 24 | (uint32_t)linebuf[10] << 16 | (uint32_t)linebuf[9] << 8 | (uint32_t)linebuf[8]; int16_t position_error = (uint32_t)linebuf[13] << 8 | (uint32_t)linebuf[12]; Vector3f veh_pos(Vector3f(vehicle_x * 0.001f, vehicle_y * 0.001f, -vehicle_z * 0.001f)); if (veh_pos.length() <= AP_BEACON_DISTANCE_MAX) { set_vehicle_position(veh_pos, position_error); parsed = true; } } break; default: // unrecognised message id break; } // record success if (parsed) { last_update_ms = AP_HAL::millis(); } } #endif // AP_BEACON_POZYX_ENABLED