public static int decodeAHRSUpdate(byte[] buffer,
int offset,
int length,
AHRSUpdate u)
{
if (length < AHRS_UPDATE_MESSAGE_LENGTH)
{
return(0);
}
if ((buffer[offset + 0] == PACKET_START_CHAR) &&
(buffer[offset + 1] == BINARY_PACKET_INDICATOR_CHAR) &&
(buffer[offset + 2] == AHRS_UPDATE_MESSAGE_LENGTH - 2) &&
(buffer[offset + 3] == MSGID_AHRS_UPDATE))
{
if (!verifyChecksum(buffer, offset, AHRS_UPDATE_MESSAGE_CHECKSUM_INDEX))
{
return(0);
}
u.yaw = decodeProtocolSignedHundredthsFloat(buffer, offset + AHRS_UPDATE_YAW_VALUE_INDEX);
u.pitch = decodeProtocolSignedHundredthsFloat(buffer, offset + AHRS_UPDATE_ROLL_VALUE_INDEX); /* FIXME */
u.roll = decodeProtocolSignedHundredthsFloat(buffer, offset + AHRS_UPDATE_PITCH_VALUE_INDEX); /* FIXME */
u.compass_heading = decodeProtocolUnsignedHundredthsFloat(buffer, offset + AHRS_UPDATE_HEADING_VALUE_INDEX);
u.altitude = decodeProtocol1616Float(buffer, offset + AHRS_UPDATE_ALTITUDE_VALUE_INDEX);
u.fused_heading = decodeProtocolUnsignedHundredthsFloat(buffer, offset + AHRS_UPDATE_FUSED_HEADING_VALUE_INDEX);
u.linear_accel_x = decodeProtocolSignedThousandthsFloat(buffer, offset + AHRS_UPDATE_LINEAR_ACCEL_X_VALUE_INDEX);
u.linear_accel_y = decodeProtocolSignedThousandthsFloat(buffer, offset + AHRS_UPDATE_LINEAR_ACCEL_Y_VALUE_INDEX);
u.linear_accel_z = decodeProtocolSignedThousandthsFloat(buffer, offset + AHRS_UPDATE_LINEAR_ACCEL_Z_VALUE_INDEX);
u.cal_mag_x = decodeBinaryInt16(buffer, offset + AHRS_UPDATE_CAL_MAG_X_VALUE_INDEX);
u.cal_mag_y = decodeBinaryInt16(buffer, offset + AHRS_UPDATE_CAL_MAG_Y_VALUE_INDEX);
u.cal_mag_z = decodeBinaryInt16(buffer, offset + AHRS_UPDATE_CAL_MAG_Z_VALUE_INDEX);
u.mag_field_norm_ratio = decodeProtocolUnsignedHundredthsFloat(buffer, offset + AHRS_UPDATE_CAL_MAG_NORM_RATIO_VALUE_INDEX);
u.mag_field_norm_scalar = decodeProtocol1616Float(buffer, offset + AHRS_UPDATE_CAL_MAG_SCALAR_VALUE_INDEX);
u.mpu_temp = decodeProtocolSignedHundredthsFloat(buffer, offset + AHRS_UPDATE_MPU_TEMP_VAUE_INDEX);
u.raw_mag_x = decodeBinaryInt16(buffer, offset + AHRS_UPDATE_RAW_MAG_X_VALUE_INDEX);
u.raw_mag_y = decodeBinaryInt16(buffer, offset + AHRS_UPDATE_RAW_MAG_Y_VALUE_INDEX);
u.raw_mag_z = decodeBinaryInt16(buffer, offset + AHRS_UPDATE_RAW_MAG_Z_VALUE_INDEX);
u.quat_w = decodeBinaryInt16(buffer, offset + AHRS_UPDATE_QUAT_W_VALUE_INDEX) / 16384.0f;
u.quat_x = decodeBinaryInt16(buffer, offset + AHRS_UPDATE_QUAT_X_VALUE_INDEX) / 16384.0f;
u.quat_y = decodeBinaryInt16(buffer, offset + AHRS_UPDATE_QUAT_Y_VALUE_INDEX) / 16384.0f;
u.quat_z = decodeBinaryInt16(buffer, offset + AHRS_UPDATE_QUAT_Z_VALUE_INDEX) / 16384.0f;
u.barometric_pressure = decodeProtocol1616Float(buffer, offset + AHRS_UPDATE_BARO_PRESSURE_VALUE_INDEX);
u.baro_temp = decodeProtocolSignedHundredthsFloat(buffer, offset + AHRS_UPDATE_BARO_TEMP_VAUE_INDEX);
u.op_status = buffer[AHRS_UPDATE_OPSTATUS_VALUE_INDEX];
u.sensor_status = buffer[AHRS_UPDATE_SENSOR_STATUS_VALUE_INDEX];
u.cal_status = buffer[AHRS_UPDATE_CAL_STATUS_VALUE_INDEX];
u.selftest_status = buffer[AHRS_UPDATE_SELFTEST_STATUS_VALUE_INDEX];
return(AHRS_UPDATE_MESSAGE_LENGTH);
}
return(0);
}
public static int decodeAHRSUpdate(byte[] buffer,
int offset,
int length,
AHRSUpdate u)
{
if (length < AHRS_UPDATE_MESSAGE_LENGTH)
{
return 0;
}
if ((buffer[offset + 0] == PACKET_START_CHAR) &&
(buffer[offset + 1] == BINARY_PACKET_INDICATOR_CHAR) &&
(buffer[offset + 2] == AHRS_UPDATE_MESSAGE_LENGTH - 2) &&
(buffer[offset + 3] == MSGID_AHRS_UPDATE))
{
if (!verifyChecksum(buffer, offset, AHRS_UPDATE_MESSAGE_CHECKSUM_INDEX))
{
return 0;
}
u.yaw = decodeProtocolSignedHundredthsFloat(buffer, offset + AHRS_UPDATE_YAW_VALUE_INDEX);
u.pitch = decodeProtocolSignedHundredthsFloat(buffer, offset + AHRS_UPDATE_ROLL_VALUE_INDEX); /* FIXME */
u.roll = decodeProtocolSignedHundredthsFloat(buffer, offset + AHRS_UPDATE_PITCH_VALUE_INDEX); /* FIXME */
u.compass_heading = decodeProtocolUnsignedHundredthsFloat(buffer, offset + AHRS_UPDATE_HEADING_VALUE_INDEX);
u.altitude = decodeProtocol1616Float(buffer, offset + AHRS_UPDATE_ALTITUDE_VALUE_INDEX);
u.fused_heading = decodeProtocolUnsignedHundredthsFloat(buffer, offset + AHRS_UPDATE_FUSED_HEADING_VALUE_INDEX);
u.linear_accel_x = decodeProtocolSignedThousandthsFloat(buffer, offset + AHRS_UPDATE_LINEAR_ACCEL_X_VALUE_INDEX);
u.linear_accel_y = decodeProtocolSignedThousandthsFloat(buffer, offset + AHRS_UPDATE_LINEAR_ACCEL_Y_VALUE_INDEX);
u.linear_accel_z = decodeProtocolSignedThousandthsFloat(buffer, offset + AHRS_UPDATE_LINEAR_ACCEL_Z_VALUE_INDEX);
u.cal_mag_x = decodeBinaryInt16(buffer, offset + AHRS_UPDATE_CAL_MAG_X_VALUE_INDEX);
u.cal_mag_y = decodeBinaryInt16(buffer, offset + AHRS_UPDATE_CAL_MAG_Y_VALUE_INDEX);
u.cal_mag_z = decodeBinaryInt16(buffer, offset + AHRS_UPDATE_CAL_MAG_Z_VALUE_INDEX);
u.mag_field_norm_ratio = decodeProtocolUnsignedHundredthsFloat(buffer, offset + AHRS_UPDATE_CAL_MAG_NORM_RATIO_VALUE_INDEX);
u.mag_field_norm_scalar = decodeProtocol1616Float(buffer, offset + AHRS_UPDATE_CAL_MAG_SCALAR_VALUE_INDEX);
u.mpu_temp = decodeProtocolSignedHundredthsFloat(buffer, offset + AHRS_UPDATE_MPU_TEMP_VAUE_INDEX);
u.raw_mag_x = decodeBinaryInt16(buffer, offset + AHRS_UPDATE_RAW_MAG_X_VALUE_INDEX);
u.raw_mag_y = decodeBinaryInt16(buffer, offset + AHRS_UPDATE_RAW_MAG_Y_VALUE_INDEX);
u.raw_mag_z = decodeBinaryInt16(buffer, offset + AHRS_UPDATE_RAW_MAG_Z_VALUE_INDEX);
u.quat_w = decodeBinaryInt16(buffer, offset + AHRS_UPDATE_QUAT_W_VALUE_INDEX);
u.quat_x = decodeBinaryInt16(buffer, offset + AHRS_UPDATE_QUAT_X_VALUE_INDEX);
u.quat_y = decodeBinaryInt16(buffer, offset + AHRS_UPDATE_QUAT_Y_VALUE_INDEX);
u.quat_z = decodeBinaryInt16(buffer, offset + AHRS_UPDATE_QUAT_Z_VALUE_INDEX);
u.barometric_pressure = decodeProtocol1616Float(buffer, offset + AHRS_UPDATE_BARO_PRESSURE_VALUE_INDEX);
u.baro_temp = decodeProtocolSignedHundredthsFloat(buffer, offset + AHRS_UPDATE_BARO_TEMP_VAUE_INDEX);
u.op_status = buffer[AHRS_UPDATE_OPSTATUS_VALUE_INDEX];
u.sensor_status = buffer[AHRS_UPDATE_SENSOR_STATUS_VALUE_INDEX];
u.cal_status = buffer[AHRS_UPDATE_CAL_STATUS_VALUE_INDEX];
u.selftest_status = buffer[AHRS_UPDATE_SELFTEST_STATUS_VALUE_INDEX];
return AHRS_UPDATE_MESSAGE_LENGTH;
}
return 0;
}
