慣性航法で用いられる座標変換をプログラムにしました。ECI座標の初期位置を設定した後、ECI,ECEF,NED,機体座標系の変換を行います。行列計算の方法や値の設定などは、ヘッダーファイル内の記述を見れば分かると思います。 また計算結果はTeratermで確認する事が出来ます。 (行列を見る場合はtoString関数、ベクトルを見る場合はtoString_V関数を使用します)
myConstants.h@0:6a28eb668082, 2019-01-30 (annotated)
- Committer:
- Joeatsumi
- Date:
- Wed Jan 30 11:39:03 2019 +0000
- Revision:
- 0:6a28eb668082
Direction cosine matrix and it's calculation.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Joeatsumi | 0:6a28eb668082 | 1 | #pragma once |
Joeatsumi | 0:6a28eb668082 | 2 | |
Joeatsumi | 0:6a28eb668082 | 3 | /* Math Constants */ |
Joeatsumi | 0:6a28eb668082 | 4 | #define NEARLY_ZERO 0.000000001f |
Joeatsumi | 0:6a28eb668082 | 5 | #define ZERO_TOLERANCE 0.000001f |
Joeatsumi | 0:6a28eb668082 | 6 | #define RAD_TO_DEG 57.2957795f // 180 / π |
Joeatsumi | 0:6a28eb668082 | 7 | #define DEG_TO_RAD 0.0174532925f // π / 180 |
Joeatsumi | 0:6a28eb668082 | 8 | |
Joeatsumi | 0:6a28eb668082 | 9 | /* Accelerometer */ |
Joeatsumi | 0:6a28eb668082 | 10 | #define ACC_LSB_TO_G 0.0000610351562f // g/LSB (1/2^14 |
Joeatsumi | 0:6a28eb668082 | 11 | #define G_TO_MPSS 9.8f // (m/s^2)/g |
Joeatsumi | 0:6a28eb668082 | 12 | |
Joeatsumi | 0:6a28eb668082 | 13 | /* Gyro Sensor */ |
Joeatsumi | 0:6a28eb668082 | 14 | //#define GYRO_LSB_TO_DEG 0.0304878048f // deg/LSB (1/32.8 |
Joeatsumi | 0:6a28eb668082 | 15 | #define GYRO_LSB_TO_DEG 0.0152671755f // deg/LSB (1/65.5 |
Joeatsumi | 0:6a28eb668082 | 16 | //#define GYRO_LSB_TO_DEG 0.00763358778f // deg/LSB (1/131 |
Joeatsumi | 0:6a28eb668082 | 17 | |
Joeatsumi | 0:6a28eb668082 | 18 | /* Pressure Sensor */ |
Joeatsumi | 0:6a28eb668082 | 19 | #define PRES_LSB_TO_HPA 0.000244140625f // hPa/LSB (1/4096 |
Joeatsumi | 0:6a28eb668082 | 20 | |
Joeatsumi | 0:6a28eb668082 | 21 | inline float TempLsbToDeg(short int temp) { |
Joeatsumi | 0:6a28eb668082 | 22 | return (42.5f + (float)temp * 0.00208333333f); // degree_C = 42.5 + temp / 480; |
Joeatsumi | 0:6a28eb668082 | 23 | } |
Joeatsumi | 0:6a28eb668082 | 24 | |
Joeatsumi | 0:6a28eb668082 | 25 | /* GPS */ |
Joeatsumi | 0:6a28eb668082 | 26 | #define GPS_SQ_E 0.00669437999f // (第一離心率)^2 |
Joeatsumi | 0:6a28eb668082 | 27 | #define GPS_A 6378137.0f // 長半径(赤道半径)(m) |
Joeatsumi | 0:6a28eb668082 | 28 | #define GPS_B 6356752.3f // 短半径(極半径)(m) |
Joeatsumi | 0:6a28eb668082 | 29 | |
Joeatsumi | 0:6a28eb668082 | 30 | /* Geomagnetic Sensor */ |
Joeatsumi | 0:6a28eb668082 | 31 | #define MAG_LSB_TO_GAUSS 0.00092f // Gauss/LSB |
Joeatsumi | 0:6a28eb668082 | 32 | #define MAG_MAGNITUDE 0.46f // Magnitude of GeoMagnetism (Gauss) |
Joeatsumi | 0:6a28eb668082 | 33 | #define MAG_SIN -0.754709580f // Sin-Value of Inclination |
Joeatsumi | 0:6a28eb668082 | 34 | #define MAG_DECLINATION 7.5f // declination (deg) |
Joeatsumi | 0:6a28eb668082 | 35 | |
Joeatsumi | 0:6a28eb668082 | 36 | /* ADC */ |
Joeatsumi | 0:6a28eb668082 | 37 | #define ADC_LSB_TO_V 0.000050354f // 3.3(V)/65535(LSB) |