Atsumi Toda
慣性航法で用いられる座標変換をプログラムにしました。ECI座標の初期位置を設定した後、ECI,ECEF,NED,機体座標系の変換を行います。行列計算の方法や値の設定などは、ヘッダーファイル内の記述を見れば分かると思います。 また計算結果はTeratermで確認する事が出来ます。 （行列を見る場合はtoString関数、ベクトルを見る場合はtoString_V関数を使用します）
Embed:
(wiki syntax)
Show/hide line numbers
myConstants.h
00001 #pragma once 00002 00003 /* Math Constants */ 00004 #define NEARLY_ZERO 0.000000001f 00005 #define ZERO_TOLERANCE 0.000001f 00006 #define RAD_TO_DEG 57.2957795f // 180 / π 00007 #define DEG_TO_RAD 0.0174532925f // π / 180 00008 00009 /* Accelerometer */ 00010 #define ACC_LSB_TO_G 0.0000610351562f // g/LSB (1/2^14 00011 #define G_TO_MPSS 9.8f // (m/s^2)/g 00012 00013 /* Gyro Sensor */ 00014 //#define GYRO_LSB_TO_DEG 0.0304878048f // deg/LSB (1/32.8 00015 #define GYRO_LSB_TO_DEG 0.0152671755f // deg/LSB (1/65.5 00016 //#define GYRO_LSB_TO_DEG 0.00763358778f // deg/LSB (1/131 00017 00018 /* Pressure Sensor */ 00019 #define PRES_LSB_TO_HPA 0.000244140625f // hPa/LSB (1/4096 00020 00021 inline float TempLsbToDeg(short int temp) { 00022 return (42.5f + (float)temp * 0.00208333333f); // degree_C = 42.5 + temp / 480; 00023 } 00024 00025 /* GPS */ 00026 #define GPS_SQ_E 0.00669437999f // （第一離心率）^2 00027 #define GPS_A 6378137.0f // 長半径(赤道半径)(m) 00028 #define GPS_B 6356752.3f // 短半径(極半径)(m) 00029 00030 /* Geomagnetic Sensor */ 00031 #define MAG_LSB_TO_GAUSS 0.00092f // Gauss/LSB 00032 #define MAG_MAGNITUDE 0.46f // Magnitude of GeoMagnetism (Gauss) 00033 #define MAG_SIN -0.754709580f // Sin-Value of Inclination 00034 #define MAG_DECLINATION 7.5f // declination (deg) 00035 00036 /* ADC */ 00037 #define ADC_LSB_TO_V 0.000050354f // 3.3(V)/65535(LSB)
Generated on Wed Jul 20 2022 18:18:03 by
1.7.2