public static int decodeAHRSPosUpdate(byte[] buffer,
int offset,
int length,
AHRSPosUpdate u)
{
if (length < AHRSPOS_UPDATE_MESSAGE_LENGTH)
{
return(0);
}
if ((buffer[offset + 0] == PACKET_START_CHAR) &&
(buffer[offset + 1] == BINARY_PACKET_INDICATOR_CHAR) &&
(buffer[offset + 2] == AHRSPOS_UPDATE_MESSAGE_LENGTH - 2) &&
(buffer[offset + 3] == MSGID_AHRSPOS_UPDATE))
{
if (!verifyChecksum(buffer, offset, AHRSPOS_UPDATE_MESSAGE_CHECKSUM_INDEX))
{
return(0);
}
u.yaw = decodeProtocolSignedHundredthsFloat(buffer, offset + AHRSPOS_UPDATE_YAW_VALUE_INDEX);
u.pitch = decodeProtocolSignedHundredthsFloat(buffer, offset + AHRSPOS_UPDATE_ROLL_VALUE_INDEX); /* FIXME */
u.roll = decodeProtocolSignedHundredthsFloat(buffer, offset + AHRSPOS_UPDATE_PITCH_VALUE_INDEX); /* FIXME */
u.compass_heading = decodeProtocolUnsignedHundredthsFloat(buffer, offset + AHRSPOS_UPDATE_HEADING_VALUE_INDEX);
u.altitude = decodeProtocol1616Float(buffer, offset + AHRSPOS_UPDATE_ALTITUDE_VALUE_INDEX);
u.fused_heading = decodeProtocolUnsignedHundredthsFloat(buffer, offset + AHRSPOS_UPDATE_FUSED_HEADING_VALUE_INDEX);
u.linear_accel_x = decodeProtocolSignedThousandthsFloat(buffer, offset + AHRSPOS_UPDATE_LINEAR_ACCEL_X_VALUE_INDEX);
u.linear_accel_y = decodeProtocolSignedThousandthsFloat(buffer, offset + AHRSPOS_UPDATE_LINEAR_ACCEL_Y_VALUE_INDEX);
u.linear_accel_z = decodeProtocolSignedThousandthsFloat(buffer, offset + AHRSPOS_UPDATE_LINEAR_ACCEL_Z_VALUE_INDEX);
u.vel_x = decodeProtocol1616Float(buffer, offset + AHRSPOS_UPDATE_VEL_X_VALUE_INDEX);
u.vel_y = decodeProtocol1616Float(buffer, offset + AHRSPOS_UPDATE_VEL_Y_VALUE_INDEX);
u.vel_z = decodeProtocol1616Float(buffer, offset + AHRSPOS_UPDATE_VEL_Z_VALUE_INDEX);
u.disp_x = decodeProtocol1616Float(buffer, offset + AHRSPOS_UPDATE_DISP_X_VALUE_INDEX);
u.disp_y = decodeProtocol1616Float(buffer, offset + AHRSPOS_UPDATE_DISP_Y_VALUE_INDEX);
u.disp_z = decodeProtocol1616Float(buffer, offset + AHRSPOS_UPDATE_DISP_Z_VALUE_INDEX);
u.mpu_temp = decodeProtocolSignedHundredthsFloat(buffer, offset + AHRSPOS_UPDATE_MPU_TEMP_VAUE_INDEX);
u.quat_w = decodeBinaryInt16(buffer, offset + AHRSPOS_UPDATE_QUAT_W_VALUE_INDEX) / 16384.0f;
u.quat_x = decodeBinaryInt16(buffer, offset + AHRSPOS_UPDATE_QUAT_X_VALUE_INDEX) / 16384.0f;
u.quat_y = decodeBinaryInt16(buffer, offset + AHRSPOS_UPDATE_QUAT_Y_VALUE_INDEX) / 16384.0f;
u.quat_z = decodeBinaryInt16(buffer, offset + AHRSPOS_UPDATE_QUAT_Z_VALUE_INDEX) / 16384.0f;
u.op_status = buffer[AHRSPOS_UPDATE_OPSTATUS_VALUE_INDEX];
u.sensor_status = buffer[AHRSPOS_UPDATE_SENSOR_STATUS_VALUE_INDEX];
u.cal_status = buffer[AHRSPOS_UPDATE_CAL_STATUS_VALUE_INDEX];
u.selftest_status = buffer[AHRSPOS_UPDATE_SELFTEST_STATUS_VALUE_INDEX];
return(AHRSPOS_UPDATE_MESSAGE_LENGTH);
}
return(0);
}
public static int decodeAHRSPosUpdate(byte[] buffer,
int offset,
int length,
AHRSPosUpdate u)
{
if (length < AHRSPOS_UPDATE_MESSAGE_LENGTH)
{
return 0;
}
if ((buffer[offset + 0] == PACKET_START_CHAR) &&
(buffer[offset + 1] == BINARY_PACKET_INDICATOR_CHAR) &&
(buffer[offset + 2] == AHRSPOS_UPDATE_MESSAGE_LENGTH - 2) &&
(buffer[offset + 3] == MSGID_AHRSPOS_UPDATE))
{
if (!verifyChecksum(buffer, offset, AHRSPOS_UPDATE_MESSAGE_CHECKSUM_INDEX))
{
return 0;
}
u.yaw = decodeProtocolSignedHundredthsFloat(buffer, offset + AHRSPOS_UPDATE_YAW_VALUE_INDEX);
u.pitch = decodeProtocolSignedHundredthsFloat(buffer, offset + AHRSPOS_UPDATE_ROLL_VALUE_INDEX); /* FIXME */
u.roll = decodeProtocolSignedHundredthsFloat(buffer, offset + AHRSPOS_UPDATE_PITCH_VALUE_INDEX); /* FIXME */
u.compass_heading = decodeProtocolUnsignedHundredthsFloat(buffer, offset + AHRSPOS_UPDATE_HEADING_VALUE_INDEX);
u.altitude = decodeProtocol1616Float(buffer, offset + AHRSPOS_UPDATE_ALTITUDE_VALUE_INDEX);
u.fused_heading = decodeProtocolUnsignedHundredthsFloat(buffer, offset + AHRSPOS_UPDATE_FUSED_HEADING_VALUE_INDEX);
u.linear_accel_x = decodeProtocolSignedThousandthsFloat(buffer, offset + AHRSPOS_UPDATE_LINEAR_ACCEL_X_VALUE_INDEX);
u.linear_accel_y = decodeProtocolSignedThousandthsFloat(buffer, offset + AHRSPOS_UPDATE_LINEAR_ACCEL_Y_VALUE_INDEX);
u.linear_accel_z = decodeProtocolSignedThousandthsFloat(buffer, offset + AHRSPOS_UPDATE_LINEAR_ACCEL_Z_VALUE_INDEX);
u.vel_x = decodeProtocol1616Float(buffer, offset + AHRSPOS_UPDATE_VEL_X_VALUE_INDEX);
u.vel_y = decodeProtocol1616Float(buffer, offset + AHRSPOS_UPDATE_VEL_Y_VALUE_INDEX);
u.vel_z = decodeProtocol1616Float(buffer, offset + AHRSPOS_UPDATE_VEL_Z_VALUE_INDEX);
u.disp_x = decodeProtocol1616Float(buffer, offset + AHRSPOS_UPDATE_DISP_X_VALUE_INDEX);
u.disp_y = decodeProtocol1616Float(buffer, offset + AHRSPOS_UPDATE_DISP_Y_VALUE_INDEX);
u.disp_z = decodeProtocol1616Float(buffer, offset + AHRSPOS_UPDATE_DISP_Z_VALUE_INDEX);
u.mpu_temp = decodeProtocolSignedHundredthsFloat(buffer, offset + AHRSPOS_UPDATE_MPU_TEMP_VAUE_INDEX);
u.quat_w = decodeBinaryInt16(buffer, offset + AHRSPOS_UPDATE_QUAT_W_VALUE_INDEX);
u.quat_x = decodeBinaryInt16(buffer, offset + AHRSPOS_UPDATE_QUAT_X_VALUE_INDEX);
u.quat_y = decodeBinaryInt16(buffer, offset + AHRSPOS_UPDATE_QUAT_Y_VALUE_INDEX);
u.quat_z = decodeBinaryInt16(buffer, offset + AHRSPOS_UPDATE_QUAT_Z_VALUE_INDEX);
u.op_status = buffer[AHRSPOS_UPDATE_OPSTATUS_VALUE_INDEX];
u.sensor_status = buffer[AHRSPOS_UPDATE_SENSOR_STATUS_VALUE_INDEX];
u.cal_status = buffer[AHRSPOS_UPDATE_CAL_STATUS_VALUE_INDEX];
u.selftest_status = buffer[AHRSPOS_UPDATE_SELFTEST_STATUS_VALUE_INDEX];
return AHRSPOS_UPDATE_MESSAGE_LENGTH;
}
return 0;
}