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; }