C MIP-SDK
|
00001 00002 // 00006 // 00008 // 00009 // External dependencies: 00010 // 00011 // 00012 // 00014 // 00017 // 00027 // 00029 00030 00031 #ifndef _MIP_SDK_NAV_H 00032 #define _MIP_SDK_NAV_H 00033 00034 00036 // 00037 //Include Files 00038 // 00040 00041 #include "mip.h" 00042 #include "mip_sdk_interface.h" 00043 00045 // 00046 // Defines 00047 // 00050 00051 00053 // 00054 // Descriptor Set designators - used in the Desc Set field of the MIP header 00055 // 00057 00058 #define MIP_NAV_COMMAND_SET 0x0D 00059 #define MIP_NAV_DATA_SET 0x82 00060 00061 00063 // NAV COMMAND DESCRIPTORS (command desc are < 0x80) 00065 00066 #define MIP_NAV_CMD_RESET_FILTER 0x01 00067 #define MIP_NAV_CMD_SET_INITIAL_ATTITUDE 0x02 00068 #define MIP_NAV_CMD_SET_INITIAL_HEADING 0x03 00069 #define MIP_NAV_CMD_SET_INITIAL_HEADING_FROM_AHRS 0x04 00070 00071 #define MIP_NAV_CMD_VEHICLE_DYNAMICS_MODE 0x10 00072 #define MIP_NAV_CMD_SENSOR2VEHICLE_TRANSFORMATION 0x11 00073 #define MIP_NAV_CMD_SENSOR2VEHICLE_OFFSET 0x12 00074 #define MIP_NAV_CMD_ANTENNA_OFFSET 0x13 00075 #define MIP_NAV_CMD_BIAS_ESTIMATION_CONTROL 0x14 00076 00077 #define MIP_NAV_CMD_GPS_SOURCE_CONTROL 0x15 00078 #define MIP_NAV_CMD_EXTERNAL_GPS_UPDATE 0x16 00079 #define MIP_NAV_CMD_EXTERNAL_HEADING_UPDATE 0x17 00080 #define MIP_NAV_CMD_HEADING_UPDATE_CONTROL 0x18 00081 #define MIP_NAV_CMD_AUTOINIT_CONTROL 0x19 00082 #define MIP_NAV_CMD_ACCEL_NOISE 0x1A 00083 #define MIP_NAV_CMD_GYRO_NOISE 0x1B 00084 #define MIP_NAV_CMD_ACCEL_BIAS_MODEL 0x1C 00085 #define MIP_NAV_CMD_GYRO_BIAS_MODEL 0x1D 00086 00087 00089 // NAV REPLY DESCRIPTORS (reply desc are >= 0x80) 00091 00092 #define MIP_NAV_REPLY_VEHICLE_DYNAMICS_MODE 0x80 00093 #define MIP_NAV_REPLY_SENSOR2VEHICLE_TRANSFORMATION 0x81 00094 #define MIP_NAV_REPLY_SENSOR2VEHICLE_OFFSET 0x82 00095 #define MIP_NAV_REPLY_ANTENNA_OFFSET 0x83 00096 #define MIP_NAV_REPLY_BIAS_ESTIMATION_CONTROL 0x84 00097 #define MIP_NAV_REPLY_GPS_SOURCE_CONTROL 0x86 00098 #define MIP_NAV_REPLY_HEADING_UPDATE_CONTROL 0x87 00099 #define MIP_NAV_REPLY_AUTOINIT_CONTROL 0x88 00100 #define MIP_NAV_REPLY_ACCEL_NOISE 0x89 00101 #define MIP_NAV_REPLY_GYRO_NOISE 0x8A 00102 #define MIP_NAV_REPLY_ACCEL_BIAS_MODEL 0x8B 00103 #define MIP_NAV_REPLY_GYRO_BIAS_MODEL 0x8C 00104 00105 00107 // 00108 // NAV DATA DESCRIPTORS 00109 // 00111 00112 #define MIP_NAV_DATA_LLH_POS 0x01 00113 #define MIP_NAV_DATA_NED_VEL 0x02 00114 #define MIP_NAV_DATA_ATT_QUATERNION 0x03 00115 #define MIP_NAV_DATA_ATT_MATRIX 0x04 00116 #define MIP_NAV_DATA_ATT_EULER_ANGLES 0x05 00117 #define MIP_NAV_DATA_GYRO_BIAS 0x06 00118 #define MIP_NAV_DATA_ACCEL_BIAS 0x07 00119 #define MIP_NAV_DATA_POS_UNCERTAINTY 0x08 00120 #define MIP_NAV_DATA_VEL_UNCERTAINTY 0x09 00121 #define MIP_NAV_DATA_ATT_UNCERTAINTY_EULER 0x0A 00122 #define MIP_NAV_DATA_GYRO_BIAS_UNCERTAINTY 0x0B 00123 #define MIP_NAV_DATA_ACCEL_BIAS_UNCERTAINTY 0x0C 00124 #define MIP_NAV_DATA_ACCELERATION 0x0D 00125 #define MIP_NAV_DATA_ANGULAR_RATE 0x0E 00126 #define MIP_NAV_DATA_WGS84_GRAVITY 0x0F 00127 #define MIP_NAV_DATA_FILTER_STATUS 0x10 00128 #define MIP_NAV_DATA_FILTER_TIMESTAMP 0x11 00129 #define MIP_NAV_DATA_ATT_UNCERTAINTY_QUATERNION 0x12 00130 #define MIP_NAV_DATA_GRAVITY_VECTOR 0x13 00131 #define MIP_NAV_DATA_HEADING_UPDATE_STATE 0x14 00132 #define MIP_NAV_DATA_MAGNETIC_MODEL 0x15 00133 00134 00135 //NAV Data Descriptor Validation Macro 00136 #define IS_NAV_DATA_DESCRIPTOR(DESC) (((DESC) == MIP_NAV_DATA_LLH_POS) || \ 00137 ((DESC) == MIP_NAV_DATA_NED_VEL) || \ 00138 ((DESC) == MIP_NAV_DATA_ATT_QUATERNION) || \ 00139 ((DESC) == MIP_NAV_DATA_ATT_MATRIX) || \ 00140 ((DESC) == MIP_NAV_DATA_ATT_EULER_ANGLES) || \ 00141 ((DESC) == MIP_NAV_DATA_GYRO_BIAS) || \ 00142 ((DESC) == MIP_NAV_DATA_ACCEL_BIAS) || \ 00143 ((DESC) == MIP_NAV_DATA_POS_UNCERTAINTY) || \ 00144 ((DESC) == MIP_NAV_DATA_VEL_UNCERTAINTY) || \ 00145 ((DESC) == MIP_NAV_DATA_ATT_UNCERTAINTY_EULER) || \ 00146 ((DESC) == MIP_NAV_DATA_GYRO_BIAS_UNCERTAINTY) || \ 00147 ((DESC) == MIP_NAV_DATA_ACCEL_BIAS_UNCERTAINTY) || \ 00148 ((DESC) == MIP_NAV_DATA_ACCELERATION) || \ 00149 ((DESC) == MIP_NAV_DATA_ANGULAR_RATE) || \ 00150 ((DESC) == MIP_NAV_DATA_WGS84_GRAVITY) || \ 00151 ((DESC) == MIP_NAV_DATA_FILTER_STATUS) || \ 00152 ((DESC) == MIP_NAV_DATA_FILTER_TIMESTAMP) || \ 00153 ((DESC) == MIP_NAV_DATA_ATT_UNCERTAINTY_QUATERNION) || \ 00154 ((DESC) == MIP_NAV_DATA_GRAVITY_VECTOR) || \ 00155 ((DESC) == MIP_NAV_DATA_HEADING_UPDATE_STATE) || \ 00156 ((DESC) == MIP_NAV_DATA_MAGNETIC_MODEL)) 00157 00158 00160 // NAV PARAMETERS 00162 00163 //Bias Estimation Control 00164 #define MIP_NAV_BIAS_ESTIMATION_OFF 0x00 00165 #define MIP_NAV_BIAS_ESTIMATION_ON 0x01 00166 00167 00168 //Dynamics Modes 00169 #define MIP_NAV_DYNAMICS_MODE_PORTABLE 0x01 00170 #define MIP_NAV_DYNAMICS_MODE_AUTOMOTIVE 0x02 00171 #define MIP_NAV_DYNAMICS_MODE_AIRBORNE 0x03 00172 00173 00174 //Heading update sources 00175 #define MIP_NAV_HEADING_SOURCE_NONE 0x00 00176 #define MIP_NAV_HEADING_SOURCE_MAGNETOMETER 0x01 00177 #define MIP_NAV_HEADING_SOURCE_GPS_VELOCITY 0x02 00178 #define MIP_NAV_HEADING_SOURCE_EXTERNAL 0x03 00179 00180 #define IS_NAV_HEADING_SOURCE(SOURCE) (((SOURCE) == MIP_NAV_HEADING_SOURCE_NONE) || \ 00181 ((SOURCE) == MIP_NAV_HEADING_SOURCE_MAGNETOMETER) || \ 00182 ((SOURCE) == MIP_NAV_HEADING_SOURCE_GPS_VELOCITY) || \ 00183 ((SOURCE) == MIP_NAV_HEADING_SOURCE_EXTERNAL)) 00184 00185 //Heading update types 00186 #define MIP_NAV_HEADING_UPDATE_TYPE_TRUE_NORTH 0x01 00187 #define MIP_NAV_HEADING_UPDATE_TYPE_MAGNETIC_NORTH 0x02 00188 00189 #define IS_NAV_HEADING_UPDATE_TYPE(TYPE) (((TYPE) == MIP_NAV_HEADING_UPDATE_TYPE_TRUE_NORTH) || \ 00190 ((TYPE) == MIP_NAV_HEADING_UPDATE_TYPE_MAGNETIC_NORTH)) 00191 00192 00194 // 00195 // Flag Definitions 00196 // 00198 00200 //EKF Modes 00202 00203 #define MIP_NAV_EKF_STATE_STARTUP 0x00 00204 #define MIP_NAV_EKF_STATE_INIT 0x01 00205 #define MIP_NAV_EKF_STATE_RUN_SOLUTION_VALID 0x02 00206 #define MIP_NAV_EKF_STATE_RUN_SOLUTION_ERROR 0x03 00207 00209 //Dynamics Modes 00211 00212 #define MIP_NAV_EKF_DYNAMICS_MODE_PORTABLE 0x01 00213 #define MIP_NAV_EKF_DYNAMICS_MODE_AUTOMOTIVE 0x02 00214 #define MIP_NAV_EKF_DYNAMICS_MODE_AIRBORNE 0x03 00215 00216 #define IS_MIP_NAV_EKF_DYNAMICS_MODE(MODE) (((MODE) == MIP_NAV_EKF_DYNAMICS_MODE_PORTABLE) || \ 00217 ((MODE) == MIP_NAV_EKF_DYNAMICS_MODE_AUTOMOTIVE) || \ 00218 ((MODE) == MIP_NAV_EKF_DYNAMICS_MODE_AIRBORNE)) 00219 00221 //EKF Status Flags 00223 00224 #define MIP_NAV_EKF_STATUS_FLAG_INIT_NO_ATTITUDE 0x1000 00225 #define MIP_NAV_EKF_STATUS_FLAG_INIT_NO_POSITION_VELOCITY 0x2000 00226 00227 #define MIP_NAV_EKF_STATUS_FLAG_IMU_UNAVAILABLE 0x0001 00228 #define MIP_NAV_EKF_STATUS_FLAG_GPS_UNAVAILABLE 0x0002 00229 #define MIP_NAV_EKF_STATUS_FLAG_MATRIX_SINGULARITY 0x0008 00230 #define MIP_NAV_EKF_STATUS_FLAG_POSITION_COVARIANCE_WARNING 0x0010 00231 #define MIP_NAV_EKF_STATUS_FLAG_VELOCITY_COVARIANCE_WARNING 0x0020 00232 #define MIP_NAV_EKF_STATUS_FLAG_ATTITUDE_COVARIANCE_WARNING 0x0040 00233 #define MIP_NAV_EKF_STATUS_FLAG_NAN_IN_SOLUTION_WARNING 0x0080 00234 00235 00237 //Bias Estimation Control 00239 00240 #define MIP_NAV_EKF_BIAS_ESTIMATION_CONTROL_OFF 0x00 00241 #define MIP_NAV_EKF_BIAS_ESTIMATION_CONTROL_ON 0x01 00242 00243 #define IS_MIP_NAV_EKF_BIAS_ESTIMATION_CONTROL(CONTROL) (((CONTROL) == MIP_NAV_EKF_BIAS_ESTIMATION_CONTROL_OFF) || \ 00244 ((CONTROL) == MIP_NAV_EKF_BIAS_ESTIMATION_CONTROL_ON)) 00245 00246 00248 // Heading Update State 00250 00251 #define NAV_HEADING_UPDATE_STATE_SOURCE_VALID 0x0001 00252 #define NAV_HEADING_UPDATE_STATE_HEADING_VALID 0x0002 00253 #define NAV_HEADING_UPDATE_STATE_HEADING_1SIGMA_VALID 0x0004 00254 00255 #define NAV_HEADING_UPDATE_STATE_PACKET_VALID (NAV_HEADING_UPDATE_STATE_SOURCE_VALID | NAV_HEADING_UPDATE_STATE_HEADING_VALID | NAV_HEADING_UPDATE_STATE_HEADING_1SIGMA_VALID) 00256 00257 00258 00259 00260 00261 00263 // 00264 // Structures 00265 // 00267 00268 #pragma pack(1) 00269 00271 // 00272 // Commands 00273 // 00275 00277 // External GPS Update Information: GPS Time, Position, Velocity 00279 00280 typedef struct _mip_nav_external_gps_update_command 00281 { 00282 double tow; //(sec) 00283 u16 week_number; 00284 00285 double pos[3]; //Lat, Lon, Height (LLH) Position (deg, deg, m) 00286 float vel[3]; //NED Velocity, (m/s) 00287 00288 float pos_1sigma[3]; //NED position 1-sigma (m) 00289 float vel_1sigma[3]; //NED velocity 1-sigma (m/s) 00290 00291 }mip_nav_external_gps_update_command; 00292 00293 00295 // External Heading Update Information: Heading angle 00297 00298 typedef struct _mip_nav_external_heading_update_command 00299 { 00300 float heading_angle; //(deg, +-180) 00301 float heading_angle_1sigma; //(deg) 00302 u8 type; // 1 - true north, 2 - magnetic north 00303 }mip_nav_external_heading_update_command; 00304 00305 00306 00308 // 00309 // Data 00310 // 00312 00313 00314 //LLH Position 00315 typedef struct _mip_nav_llh_pos 00316 { 00317 double latitude, longitude; //Degrees 00318 double ellipsoid_height; //meters 00319 u16 valid_flags; 00320 }mip_nav_llh_pos; 00321 00322 00323 00324 //NED Velocity 00325 typedef struct _mip_nav_ned_velocity 00326 { 00327 float north, east, down; //meters/sec 00328 u16 valid_flags; 00329 }mip_nav_ned_velocity; 00330 00331 00332 //Attitude (quaternion) 00333 typedef struct _mip_nav_attitude_quaternion 00334 { 00335 float q[4]; 00336 u16 valid_flags; 00337 }mip_nav_attitude_quaternion; 00338 00339 00340 //Attitude (DCM) 00341 typedef struct _mip_nav_attitude_dcm 00342 { 00343 float dcm[3][3]; //Rows, Columns 00344 u16 valid_flags; 00345 }mip_nav_attitude_dcm; 00346 00347 00348 //Attitude (Euler Angles) 00349 typedef struct _mip_nav_attitude_euler_angles 00350 { 00351 float roll, pitch, yaw; //radians 00352 u16 valid_flags; 00353 }mip_nav_attitude_euler_angles; 00354 00355 00356 //Gyro Bias Estimates 00357 typedef struct _mip_nav_gyro_bias 00358 { 00359 float x, y, z; //sensor body frame (radians/sec) 00360 u16 valid_flags; 00361 }mip_nav_gyro_bias; 00362 00363 00364 //Accel Bias Estimates 00365 typedef struct _mip_nav_accel_bias 00366 { 00367 float x, y, z; //sensor body frame (m/s^2) 00368 u16 valid_flags; 00369 }mip_nav_accel_bias; 00370 00371 00372 //LLH Position Uncertainty 00373 typedef struct _mip_nav_llh_pos_uncertainty 00374 { 00375 float north, east, down; //1-sigma (meters) 00376 u16 valid_flags; 00377 }mip_nav_llh_pos_uncertainty; 00378 00379 00380 //NED Velocity Uncertainty 00381 typedef struct _mip_nav_ned_vel_uncertainty 00382 { 00383 float north, east, down; //1-sigma (meters/sec) 00384 u16 valid_flags; 00385 }mip_nav_ned_vel_uncertainty; 00386 00387 00388 //Attitude Uncertainty (Euler Angles) 00389 typedef struct _mip_nav_euler_attitude_uncertainty 00390 { 00391 float roll, pitch, yaw; //1-sigma (radians) 00392 u16 valid_flags; 00393 }mip_nav_euler_attitude_uncertainty; 00394 00395 00396 //Gyro Bias Uncertainty 00397 typedef struct _mip_nav_gyro_bias_uncertainty 00398 { 00399 float x, y, z; //sensor body frame (radians/sec) 00400 u16 valid_flags; 00401 }mip_nav_gyro_bias_uncertainty; 00402 00403 00404 //Accel Bias Uncertainty 00405 typedef struct _mip_nav_accel_bias_uncertainty 00406 { 00407 float x, y, z; //sensor body frame (m/s^2) 00408 u16 valid_flags; 00409 }mip_nav_accel_bias_uncertainty; 00410 00411 00412 //Navigation Solution Timetamp 00413 typedef struct _mip_nav_timestamp 00414 { 00415 double tow; //Time of Week (seconds) 00416 u16 week_number; 00417 u16 valid_flags; 00418 }mip_nav_timestamp; 00419 00420 00421 //Navigation Status 00422 typedef struct _mip_nav_status 00423 { 00424 u16 filter_state; 00425 u16 dynamics_mode; 00426 u16 status_flags; 00427 }mip_nav_status; 00428 00429 00430 //Nav Acceleration Estimate 00431 typedef struct _mip_nav_acceleration 00432 { 00433 float x, y, z; //sensor or vehicle frame (m/s^2) 00434 u16 valid_flags; 00435 }mip_nav_acceleration; 00436 00437 00438 //Nav Gravity Estimate 00439 typedef struct _mip_nav_gravity_vector 00440 { 00441 float x, y, z; //sensor or vehicle frame (m/s^2) 00442 u16 valid_flags; 00443 }mip_nav_gravity_vector; 00444 00445 00446 //Nav Angular Rate Estimate 00447 typedef struct _mip_nav_angular_rate 00448 { 00449 float x, y, z; //sensor or vehicle frame (rad/s) 00450 u16 valid_flags; 00451 }mip_nav_angular_rate; 00452 00453 00454 //Attitude Uncertainty (Quaternion Elements) 00455 typedef struct _mip_nav_quaternion_attitude_uncertainty 00456 { 00457 float q0, q1, q2, q3; //1-sigma (radians) 00458 u16 valid_flags; 00459 }mip_nav_quaternion_attitude_uncertainty; 00460 00461 00462 //WGS84 Gravity magnitude 00463 typedef struct _mip_nav_wgs84_gravity_mag 00464 { 00465 float magnitude; //m/s^2 00466 u16 valid_flags; 00467 }mip_nav_wgs84_gravity_mag; 00468 00469 00470 //Heading Update Source 00471 typedef struct _mip_nav_heading_update_state 00472 { 00473 float heading; 00474 float heading_1sigma; 00475 u16 source; 00476 u16 valid_flags; 00477 }mip_nav_heading_update_state; 00478 00479 00480 //Magnetic Model 00481 typedef struct _mip_nav_magnetic_model 00482 { 00483 float intensity_north, intensity_east, intensity_down; //nT 00484 float inclination, declination; //deg (AKA Dip, Magnetic Variation) 00485 u16 valid_flags; 00486 }mip_nav_magnetic_model; 00487 00488 00489 #pragma pack() 00490 00491 00492 00493 00494 00496 // 00497 // Function Prototypes 00498 // 00500 00501 00503 //NAV Data 00505 00506 void mip_nav_llh_pos_byteswap(mip_nav_llh_pos *llh_pos); 00507 void mip_nav_ned_velocity_byteswap(mip_nav_ned_velocity *ned_velocity); 00508 void mip_nav_attitude_quaternion_byteswap(mip_nav_attitude_quaternion *attitude_quaternion); 00509 void mip_nav_attitude_dcm_byteswap(mip_nav_attitude_dcm *attitude_dcm); 00510 void mip_nav_attitude_euler_angles_byteswap(mip_nav_attitude_euler_angles *attitude_euler_angles); 00511 void mip_nav_gyro_bias_byteswap(mip_nav_gyro_bias *gyro_bias); 00512 void mip_nav_accel_bias_byteswap(mip_nav_accel_bias *accel_bias); 00513 void mip_nav_llh_pos_uncertainty_byteswap(mip_nav_llh_pos_uncertainty *llh_pos_uncertainty); 00514 void mip_nav_ned_vel_uncertainty_byteswap(mip_nav_ned_vel_uncertainty *ned_vel_uncertainty); 00515 void mip_nav_euler_attitude_uncertainty_byteswap(mip_nav_euler_attitude_uncertainty *euler_attitude_uncertainty); 00516 void mip_nav_gyro_bias_uncertainty_byteswap(mip_nav_gyro_bias_uncertainty *gyro_bias_uncertainty); 00517 void mip_nav_accel_bias_uncertainty_byteswap(mip_nav_accel_bias_uncertainty *accel_bias_uncertainty); 00518 void mip_nav_timestamp_byteswap(mip_nav_timestamp *timestamp); 00519 void mip_nav_status_byteswap(mip_nav_status *status); 00520 void mip_nav_acceleration_byteswap(mip_nav_acceleration *acceleration); 00521 void mip_nav_gravity_vector_byteswap(mip_nav_gravity_vector *gravity_vector); 00522 void mip_nav_angular_rate_byteswap(mip_nav_angular_rate *angular_rate); 00523 void mip_nav_quaternion_attitude_uncertainty_byteswap(mip_nav_quaternion_attitude_uncertainty *quaternion_attitude_uncertainty); 00524 void mip_nav_wgs84_gravity_mag_byteswap(mip_nav_wgs84_gravity_mag *wgs84_gravity_mag); 00525 void mip_nav_heading_update_state_byteswap(mip_nav_heading_update_state *heading_update_state); 00526 void mip_nav_magnetic_model_byteswap(mip_nav_magnetic_model *magnetic_model); 00527 00528 00530 //NAV Commands 00532 00533 u16 mip_nav_reset_filter(mip_interface *device_interface); 00534 u16 mip_nav_set_init_attitude(mip_interface *device_interface, float euler_angles[3]); 00535 u16 mip_nav_set_init_heading(mip_interface *device_interface, float heading); 00536 u16 mip_nav_set_init_attitude_from_ahrs(mip_interface *device_interface, float declination); 00537 u16 mip_nav_vehicle_dynamics_mode(mip_interface *device_interface, u8 function_selector, u8 *dynamics_mode); 00538 u16 mip_nav_sensor2vehicle_tranformation(mip_interface *device_interface, u8 function_selector, float euler_angles[3]); 00539 u16 mip_nav_sensor2vehicle_offset(mip_interface *device_interface, u8 function_selector, float offset[3]); 00540 u16 mip_nav_antenna_offset(mip_interface *device_interface, u8 function_selector, float offset[3]); 00541 u16 mip_nav_bias_estimation(mip_interface *device_interface, u8 function_selector, u16 *bias_control); 00542 u16 mip_nav_gps_source(mip_interface *device_interface, u8 function_selector, u8 *gps_source); 00543 u16 mip_nav_external_gps_update(mip_interface *device_interface, mip_nav_external_gps_update_command *command); 00544 u16 mip_nav_external_heading_update(mip_interface *device_interface, mip_nav_external_heading_update_command *command); 00545 u16 mip_nav_heading_source(mip_interface *device_interface, u8 function_selector, u8 *heading_source); 00546 u16 mip_nav_auto_initialization(mip_interface *device_interface, u8 function_selector, u8 *enable); 00547 u16 mip_nav_accel_white_noise(mip_interface *device_interface, u8 function_selector, float noise_1sigma[3]); 00548 u16 mip_nav_gyro_white_noise(mip_interface *device_interface, u8 function_selector, float noise_1sigma[3]); 00549 u16 mip_nav_gyro_bias_model(mip_interface *device_interface, u8 function_selector, float bias_beta[3], float bias_noise_1sigma[3]); 00550 00551 00552 #endif