// AHRS Packet Callback
void ahrs_packet_callback(void *user_ptr, u8 *packet, u16 packet_size, u8 callback_type)
{
mip_field_header *field_header;
u8 *field_data;
u16 field_offset = 0;
float percent;
//The packet callback can have several types, process them all
switch(callback_type)
{
//Handle valid packets
case MIP_INTERFACE_CALLBACK_VALID_PACKET:
{
ahrs_valid_packet_count++;
//Loop through all of the data fields
while(mip_get_next_field(packet, &field_header, &field_data, &field_offset) == MIP_OK)
{
switch(field_header->descriptor)
{
case MIP_AHRS_DATA_EULER_ANGLES:
{
//mip_ahrs_euler_angles_byteswap(mip_ahrs_euler_angles *euler_angles
memcpy(&curr_ahrs_euler, field_data, sizeof(mip_ahrs_euler_angles));
mip_ahrs_euler_angles_byteswap(&curr_ahrs_euler);
convertRadiansToDegrees(&curr_ahrs_euler);
}break;
case MIP_AHRS_DATA_ACCEL_SCALED:
{
memcpy(&curr_ahrs_accel, field_data, sizeof(mip_ahrs_scaled_accel));
//For little-endian targets, byteswap the data field
mip_ahrs_scaled_accel_byteswap(&curr_ahrs_accel);
}break;
case MIP_AHRS_DATA_GYRO_SCALED:
{
memcpy(&curr_ahrs_gyro, field_data, sizeof(mip_ahrs_scaled_gyro));
//For little-endian targets, byteswap the data field
mip_ahrs_scaled_gyro_byteswap(&curr_ahrs_gyro);
}break;
default:
{
printf("Unknown case: %u\n", field_header->descriptor);
}break;
}
}
// printf("%lf, %lf, %lf\t%lf, %lf, %lf\t%lf, %lf, %lf\n", curr_ahrs_euler.yaw, curr_ahrs_euler.pitch, curr_ahrs_euler.roll, curr_ahrs_gyro.scaled_gyro[0],
// curr_ahrs_gyro.scaled_gyro[1], curr_ahrs_gyro.scaled_gyro[2], curr_ahrs_accel.scaled_accel[0], curr_ahrs_accel.scaled_accel[1], curr_ahrs_accel.scaled_accel[2]);
}break;
case MIP_INTERFACE_CALLBACK_CHECKSUM_ERROR:
{
ahrs_checksum_error_packet_count++;
percent = ((float)(ahrs_checksum_error_packet_count)/ (float)(ahrs_checksum_error_packet_count + ahrs_valid_packet_count + ahrs_timeout_packet_count))*100;
fprintf(stderr, "FAILED: error count %u: percent %f\n", ahrs_checksum_error_packet_count, percent);
}break;
case MIP_INTERFACE_CALLBACK_TIMEOUT:
{
ahrs_timeout_packet_count++;
}break;
default: break;
}
}
void ahrs_packet_callback(void *user_ptr, u8 *packet, u16 packet_size, u8 callback_type)
{
mip_field_header *field_header;
u8 *field_data;
u16 field_offset = 0;
//The packet callback can have several types, process them all
switch(callback_type)
{
///
//Handle valid packets
///
case MIP_INTERFACE_CALLBACK_VALID_PACKET:
{
ahrs_valid_packet_count++;
///
//Loop through all of the data fields
///
while(mip_get_next_field(packet, &field_header, &field_data, &field_offset) == MIP_OK)
{
///
// Decode the field
///
switch(field_header->descriptor)
{
///
// Scaled Accelerometer
///
case MIP_AHRS_DATA_ACCEL_SCALED:
{
memcpy(&curr_ahrs_accel, field_data, sizeof(mip_ahrs_scaled_accel));
//For little-endian targets, byteswap the data field
mip_ahrs_scaled_accel_byteswap(&curr_ahrs_accel);
}break;
///
// Scaled Gyro
///
case MIP_AHRS_DATA_GYRO_SCALED:
{
memcpy(&curr_ahrs_gyro, field_data, sizeof(mip_ahrs_scaled_gyro));
//For little-endian targets, byteswap the data field
mip_ahrs_scaled_gyro_byteswap(&curr_ahrs_gyro);
}break;
///
// Scaled Magnetometer
///
case MIP_AHRS_DATA_MAG_SCALED:
{
memcpy(&curr_ahrs_mag, field_data, sizeof(mip_ahrs_scaled_mag));
//For little-endian targets, byteswap the data field
mip_ahrs_scaled_mag_byteswap(&curr_ahrs_mag);
}break;
default: break;
}
}
}break;
///
//Handle checksum error packets
///
case MIP_INTERFACE_CALLBACK_CHECKSUM_ERROR:
{
ahrs_checksum_error_packet_count++;
}break;
///
//Handle timeout packets
///
case MIP_INTERFACE_CALLBACK_TIMEOUT:
{
ahrs_timeout_packet_count++;
}break;
default: break;
}
print_packet_stats();
}
