// 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(); }