void __mip_interface_command_response_handler(void *user_ptr, u8 *packet, u16 packet_size, u8 callback_type)
{
u16 payload_size;
global_ack_nack_field *ack_nack_field_ptr;
mip_field_header *field_header_ptr;
u8 *field_data_ptr;
u16 field_offset = 0;
mip_interface *device_interface = (mip_interface *)user_ptr;
if(device_interface == NULL)
return;
//Flag that the response was received
device_interface->command_response_received = 1;
///
//Successful Packet
///
if(callback_type == MIP_INTERFACE_CALLBACK_VALID_PACKET)
{
payload_size = mip_get_payload_size(packet);
//Get the ACK/NACK Field
if(mip_get_first_field(packet, &field_header_ptr, &field_data_ptr, &field_offset) == MIP_OK)
{
ack_nack_field_ptr = (global_ack_nack_field *)field_data_ptr;
device_interface->command_id = ack_nack_field_ptr->command_echo_byte;
device_interface->command_acknack_response = ack_nack_field_ptr->error_code;
}
//Get the remaining data & subtract the size of the first field
if(mip_get_next_field(packet, &field_header_ptr, &field_data_ptr, &field_offset) == MIP_OK)
{
device_interface->command_response_data = field_header_ptr;
device_interface->command_response_data_size = payload_size - (sizeof(mip_field_header) + sizeof(global_ack_nack_field));
}
}
///
// Error
///
else
{
device_interface->command_id = 0;
device_interface->command_acknack_response = 0;
device_interface->command_response_data = 0;
device_interface->command_response_data_size = 0;
}
}
// 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 gps_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:
{
gps_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)
{
///
// LLH Position
///
case MIP_GPS_DATA_LLH_POS:
{
memcpy(&curr_llh_pos, field_data, sizeof(mip_gps_llh_pos));
//For little-endian targets, byteswap the data field
mip_gps_llh_pos_byteswap(&curr_llh_pos);
}break;
///
// NED Velocity
///
case MIP_GPS_DATA_NED_VELOCITY:
{
memcpy(&curr_ned_vel, field_data, sizeof(mip_gps_ned_vel));
//For little-endian targets, byteswap the data field
mip_gps_ned_vel_byteswap(&curr_ned_vel);
}break;
///
// GPS Time
///
case MIP_GPS_DATA_GPS_TIME:
{
memcpy(&curr_gps_time, field_data, sizeof(mip_gps_time));
//For little-endian targets, byteswap the data field
mip_gps_time_byteswap(&curr_gps_time);
}break;
default: break;
}
}
}break;
///
//Handle checksum error packets
///
case MIP_INTERFACE_CALLBACK_CHECKSUM_ERROR:
{
gps_checksum_error_packet_count++;
}break;
///
//Handle timeout packets
///
case MIP_INTERFACE_CALLBACK_TIMEOUT:
{
gps_timeout_packet_count++;
}break;
default: break;
}
print_packet_stats();
}
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();
}
