/* Mag Cal Data Storage Request */
public static int encodeMagCalDataSetRequest(byte[] buffer,
MagCalData d)
{
// Header
buffer[0] = PACKET_START_CHAR;
buffer[1] = (byte)BINARY_PACKET_INDICATOR_CHAR;
buffer[2] = MAG_CAL_CMD_MESSAGE_LENGTH - 2;
buffer[3] = MSGID_MAG_CAL_CMD;
// Data
buffer[MAG_CAL_DATA_ACTION_VALUE_INDEX] = d.action;
for (int i = 0; i < 3; i++)
{
encodeBinaryInt16(d.mag_bias[i],
buffer, MAG_X_BIAS_VALUE_INDEX + (i * 2));
}
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
encodeProtocol1616Float(d.mag_xform[i][j],
buffer, MAG_XFORM_1_1_VALUE_INDEX + (i * 6) + (j * 2));
}
}
encodeProtocol1616Float(d.earth_mag_field_norm, buffer, MAG_CAL_EARTH_MAG_FIELD_NORM_VALUE_INDEX);
// Footer
encodeTermination(buffer, MAG_CAL_CMD_MESSAGE_LENGTH, MAG_CAL_CMD_MESSAGE_LENGTH - 4);
return(MAG_CAL_CMD_MESSAGE_LENGTH);
}
/* Mag Cal Data Retrieval Response */
public static int decodeMagCalDataGetResponse(byte[] buffer,
int offset,
int length,
MagCalData d)
{
if (length < MAG_CAL_CMD_MESSAGE_LENGTH)
{
return(0);
}
if ((buffer[0] == PACKET_START_CHAR) &&
(buffer[1] == BINARY_PACKET_INDICATOR_CHAR) &&
(buffer[2] == MAG_CAL_CMD_MESSAGE_LENGTH - 2) &&
(buffer[3] == MSGID_MAG_CAL_CMD))
{
if (!verifyChecksum(buffer, offset, MAG_CAL_CMD_MESSAGE_CHECKSUM_INDEX))
{
return(0);
}
d.action = buffer[MAG_CAL_DATA_ACTION_VALUE_INDEX];
for (int i = 0; i < 3; i++)
{
d.mag_bias[i] = decodeBinaryInt16(buffer, MAG_X_BIAS_VALUE_INDEX + (i * 2));
}
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
d.mag_xform[i][j] = decodeProtocol1616Float(buffer, MAG_XFORM_1_1_VALUE_INDEX + (i * 6) + (j * 2));
}
}
d.earth_mag_field_norm = decodeProtocol1616Float(buffer, MAG_CAL_EARTH_MAG_FIELD_NORM_VALUE_INDEX);
return(MAG_CAL_CMD_MESSAGE_LENGTH);
}
return(0);
}
/* Mag Cal Data Retrieval Response */
public static int decodeMagCalDataGetResponse(byte[] buffer,
int offset,
int length,
MagCalData d)
{
if (length < MAG_CAL_CMD_MESSAGE_LENGTH) return 0;
if ((buffer[0] == PACKET_START_CHAR) &&
(buffer[1] == BINARY_PACKET_INDICATOR_CHAR) &&
(buffer[2] == MAG_CAL_CMD_MESSAGE_LENGTH - 2) &&
(buffer[3] == MSGID_MAG_CAL_CMD))
{
if (!verifyChecksum(buffer, offset, MAG_CAL_CMD_MESSAGE_CHECKSUM_INDEX)) return 0;
d.action = buffer[MAG_CAL_DATA_ACTION_VALUE_INDEX];
for (int i = 0; i < 3; i++)
{
d.mag_bias[i] = decodeBinaryInt16(buffer, MAG_X_BIAS_VALUE_INDEX + (i * 2));
}
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
d.mag_xform[i][j] = decodeProtocol1616Float(buffer, MAG_XFORM_1_1_VALUE_INDEX + (i * 6) + (j * 2));
}
}
d.earth_mag_field_norm = decodeProtocol1616Float(buffer, MAG_CAL_EARTH_MAG_FIELD_NORM_VALUE_INDEX);
return MAG_CAL_CMD_MESSAGE_LENGTH;
}
return 0;
}
/* Mag Cal Data Storage Request */
public static int encodeMagCalDataSetRequest(byte[] buffer,
MagCalData d)
{
// Header
buffer[0] = PACKET_START_CHAR;
buffer[1] = (byte)BINARY_PACKET_INDICATOR_CHAR;
buffer[2] = MAG_CAL_CMD_MESSAGE_LENGTH - 2;
buffer[3] = MSGID_MAG_CAL_CMD;
// Data
buffer[MAG_CAL_DATA_ACTION_VALUE_INDEX] = d.action;
for (int i = 0; i < 3; i++)
{
encodeBinaryInt16(d.mag_bias[i],
buffer, MAG_X_BIAS_VALUE_INDEX + (i * 2));
}
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
encodeProtocol1616Float(d.mag_xform[i][j],
buffer, MAG_XFORM_1_1_VALUE_INDEX + (i * 6) + (j * 2));
}
}
encodeProtocol1616Float(d.earth_mag_field_norm, buffer, MAG_CAL_EARTH_MAG_FIELD_NORM_VALUE_INDEX);
// Footer
encodeTermination(buffer, MAG_CAL_CMD_MESSAGE_LENGTH, MAG_CAL_CMD_MESSAGE_LENGTH - 4);
return MAG_CAL_CMD_MESSAGE_LENGTH;
}
