bool _MavlinkInterface::readMessage(mavlink_message_t &message)
{
uint8_t cp;
mavlink_status_t status;
uint8_t result;
while (m_pSerialPort->Read((char*)&cp, 1))
{
// if (mavlink_parse_char(MAVLINK_COMM_0, cp, &message, &status))
result = mavlink_frame_char(MAVLINK_COMM_0, cp, &message, &status);
if (result == 1)
{
//Good message decoded
last_status = status;
return true;
}
else if(result == 2)
{
//Bad CRC
// printf("ERROR: DROPPED %d PACKETS\n", status.packet_rx_drop_count);
}
// check for dropped packets
/* if ((last_status.packet_rx_drop_count != status.packet_rx_drop_count) &&
status.packet_rx_drop_count > 0)
{
printf("ERROR: DROPPED %d PACKETS\n", status.packet_rx_drop_count);
}
last_status = status;
*/
}
return false;
}
mavlink_message_t parsePacket(JNIEnv *env, jbyteArray packet) {
mavlink_message_t msg;
mavlink_status_t status;
jbyte* packetBytes = (*env)->GetByteArrayElements(env, packet, NULL);
jsize packetLength = (*env)->GetArrayLength(env, packet);
int chan = 0;
int i;
for (i = 0; i < packetLength; i++) {
uint8_t byte = packetBytes[i] & 0xff;
uint8_t framingStatus = mavlink_frame_char(chan, byte, &msg, &status);
}
(*env)->ReleaseByteArrayElements(env, packet, packetBytes, 0);
return msg;
}
JNIEXPORT jboolean JNICALL
Java_io_dronefleet_mavlink_testtool_CLibraryTestTool_crcCheck(
JNIEnv *env,
jobject obj,
jbyteArray packet) {
mavlink_message_t msg;
mavlink_status_t status;
jbyte* packetBytes = (*env)->GetByteArrayElements(env, packet, NULL);
jsize packetLength = (*env)->GetArrayLength(env, packet);
int chan = 0;
int i;
for (i = 0; i < packetLength; i++) {
uint8_t byte = packetBytes[i] & 0xff;
uint8_t framingStatus = mavlink_frame_char(chan, byte, &msg, &status);
if (framingStatus == MAVLINK_FRAMING_BAD_CRC) {
return false;
}
}
(*env)->ReleaseByteArrayElements(env, packet, packetBytes, 0);
return true;
}
