Eigen Revision
Dependencies: mbed LPS25HB_I2C LSM9DS1 PIDcontroller Autopilot_Eigen LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM
gps.cpp@100:7589b663d462, 2021-11-05 (annotated)
- Committer:
- NaotoMorita
- Date:
- Fri Nov 05 13:48:22 2021 +0000
- Revision:
- 100:7589b663d462
- Parent:
- 99:98b892ca70ec
- Child:
- 106:2d854e92cebb
- Child:
- 107:46e039e12182
gps pos vel simultaneous update
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
osaka | 96:3645f8f9bdd3 | 1 | #include "global.hpp" |
osaka | 96:3645f8f9bdd3 | 2 | |
osaka | 96:3645f8f9bdd3 | 3 | void getGPSval() |
osaka | 96:3645f8f9bdd3 | 4 | { |
NaotoMorita | 100:7589b663d462 | 5 | gpsUpdateFlag = false; |
osaka | 96:3645f8f9bdd3 | 6 | gps.Update(); |
osaka | 97:2b3242c2dd85 | 7 | if (gps.iTOW_VELNED != itow_velned) |
osaka | 97:2b3242c2dd85 | 8 | { |
osaka | 97:2b3242c2dd85 | 9 | itow_velned = gps.iTOW_VELNED; |
osaka | 97:2b3242c2dd85 | 10 | vi.x = gps.VelocityNED.x; |
osaka | 97:2b3242c2dd85 | 11 | vi.y = gps.VelocityNED.y; |
osaka | 97:2b3242c2dd85 | 12 | vi.z = gps.VelocityNED.z; |
osaka | 97:2b3242c2dd85 | 13 | } |
osaka | 97:2b3242c2dd85 | 14 | if (gps.iTOW_POSLLH != itow_posllh) |
osaka | 97:2b3242c2dd85 | 15 | { |
osaka | 97:2b3242c2dd85 | 16 | itow_posllh = gps.iTOW_POSLLH; |
NaotoMorita | 99:98b892ca70ec | 17 | gps.Calculate(); |
osaka | 97:2b3242c2dd85 | 18 | change_refpos(); |
osaka | 97:2b3242c2dd85 | 19 | pi.x = gps.PositionNED.x; |
osaka | 97:2b3242c2dd85 | 20 | pi.y = gps.PositionNED.y; |
osaka | 97:2b3242c2dd85 | 21 | pi.z = gps.PositionNED.z; |
NaotoMorita | 100:7589b663d462 | 22 | gpsUpdateFlag = true; |
osaka | 97:2b3242c2dd85 | 23 | } |
osaka | 97:2b3242c2dd85 | 24 | } |
osaka | 97:2b3242c2dd85 | 25 | |
osaka | 97:2b3242c2dd85 | 26 | //20m以上の変位で基準位置更新 |
osaka | 97:2b3242c2dd85 | 27 | void change_refpos() |
osaka | 97:2b3242c2dd85 | 28 | { |
osaka | 97:2b3242c2dd85 | 29 | float x_dist = gps.PositionNED.x - pi.x; |
osaka | 97:2b3242c2dd85 | 30 | float y_dist = gps.PositionNED.y - pi.y; |
osaka | 97:2b3242c2dd85 | 31 | float z_dist = gps.PositionNED.z - pi.z; |
NaotoMorita | 98:bdaa6bbfb1d9 | 32 | float dist2 = x_dist*x_dist + y_dist*y_dist + z_dist*z_dist; |
NaotoMorita | 98:bdaa6bbfb1d9 | 33 | if (dist2 >= 400.0f) |
osaka | 97:2b3242c2dd85 | 34 | { |
osaka | 97:2b3242c2dd85 | 35 | gps.CalculateUnit(); |
osaka | 97:2b3242c2dd85 | 36 | eskf.setPihat(0.0f, 0.0f, 0.0f); |
NaotoMorita | 99:98b892ca70ec | 37 | //palt0 = palt; |
NaotoMorita | 99:98b892ca70ec | 38 | |
osaka | 97:2b3242c2dd85 | 39 | } |
osaka | 97:2b3242c2dd85 | 40 | else |
osaka | 97:2b3242c2dd85 | 41 | { |
osaka | 97:2b3242c2dd85 | 42 | //何もしない |
osaka | 97:2b3242c2dd85 | 43 | } |
osaka | 96:3645f8f9bdd3 | 44 | } |