慣性航法で用いられる座標変換をプログラムにしました。ECI座標の初期位置を設定した後、ECI,ECEF,NED,機体座標系の変換を行います。行列計算の方法や値の設定などは、ヘッダーファイル内の記述を見れば分かると思います。 また計算結果はTeratermで確認する事が出来ます。 (行列を見る場合はtoString関数、ベクトルを見る場合はtoString_V関数を使用します)

Dependencies:   mbed

myConstants.h

Committer:
Joeatsumi
Date:
7 months ago
Revision:
0:6a28eb668082

File content as of revision 0:6a28eb668082:

#pragma once

/* Math Constants */
#define NEARLY_ZERO         0.000000001f
#define ZERO_TOLERANCE      0.000001f
#define RAD_TO_DEG          57.2957795f             // 180 / π
#define DEG_TO_RAD          0.0174532925f           // π / 180

/* Accelerometer */
#define ACC_LSB_TO_G        0.0000610351562f        // g/LSB (1/2^14
#define G_TO_MPSS           9.8f                    // (m/s^2)/g

/* Gyro Sensor */
//#define GYRO_LSB_TO_DEG     0.0304878048f           // deg/LSB (1/32.8
#define GYRO_LSB_TO_DEG     0.0152671755f           // deg/LSB (1/65.5
//#define GYRO_LSB_TO_DEG     0.00763358778f          // deg/LSB (1/131

/* Pressure Sensor */
#define PRES_LSB_TO_HPA     0.000244140625f         // hPa/LSB (1/4096

inline float TempLsbToDeg(short int temp) {
    return (42.5f + (float)temp * 0.00208333333f);  // degree_C = 42.5 + temp / 480;
}

/* GPS */
#define GPS_SQ_E                0.00669437999f      // (第一離心率)^2
#define GPS_A                   6378137.0f          // 長半径(赤道半径)(m)
#define GPS_B                   6356752.3f          // 短半径(極半径)(m)

/* Geomagnetic Sensor */
#define MAG_LSB_TO_GAUSS    0.00092f                // Gauss/LSB
#define MAG_MAGNITUDE       0.46f                   // Magnitude of GeoMagnetism (Gauss)
#define MAG_SIN             -0.754709580f           // Sin-Value of Inclination
#define MAG_DECLINATION     7.5f                    // declination (deg)

/* ADC */
#define ADC_LSB_TO_V        0.000050354f            // 3.3(V)/65535(LSB)