Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
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@98:bdaa6bbfb1d9, 2021-11-03 (annotated)
- Committer:
- NaotoMorita
- Date:
- Wed Nov 03 14:59:26 2021 +0000
- Revision:
- 98:bdaa6bbfb1d9
- Parent:
- 97:2b3242c2dd85
- Child:
- 99:98b892ca70ec
slightly modified
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 | { |
| osaka | 96:3645f8f9bdd3 | 5 | gps.Update(); |
| osaka | 97:2b3242c2dd85 | 6 | if (gps.iTOW_VELNED != itow_velned) |
| osaka | 97:2b3242c2dd85 | 7 | { |
| osaka | 97:2b3242c2dd85 | 8 | itow_velned = gps.iTOW_VELNED; |
| osaka | 97:2b3242c2dd85 | 9 | vi.x = gps.VelocityNED.x; |
| osaka | 97:2b3242c2dd85 | 10 | vi.y = gps.VelocityNED.y; |
| osaka | 97:2b3242c2dd85 | 11 | vi.z = gps.VelocityNED.z; |
| osaka | 97:2b3242c2dd85 | 12 | } |
| osaka | 97:2b3242c2dd85 | 13 | if (gps.iTOW_POSLLH != itow_posllh) |
| osaka | 97:2b3242c2dd85 | 14 | { |
| osaka | 97:2b3242c2dd85 | 15 | itow_posllh = gps.iTOW_POSLLH; |
| osaka | 97:2b3242c2dd85 | 16 | change_refpos(); |
| osaka | 97:2b3242c2dd85 | 17 | gps.Calculate(); |
| osaka | 97:2b3242c2dd85 | 18 | pi.x = gps.PositionNED.x; |
| osaka | 97:2b3242c2dd85 | 19 | pi.y = gps.PositionNED.y; |
| osaka | 97:2b3242c2dd85 | 20 | pi.z = gps.PositionNED.z; |
| osaka | 97:2b3242c2dd85 | 21 | } |
| osaka | 97:2b3242c2dd85 | 22 | } |
| osaka | 97:2b3242c2dd85 | 23 | |
| osaka | 97:2b3242c2dd85 | 24 | //20m以上の変位で基準位置更新 |
| osaka | 97:2b3242c2dd85 | 25 | void change_refpos() |
| osaka | 97:2b3242c2dd85 | 26 | { |
| osaka | 97:2b3242c2dd85 | 27 | float x_dist = gps.PositionNED.x - pi.x; |
| osaka | 97:2b3242c2dd85 | 28 | float y_dist = gps.PositionNED.y - pi.y; |
| osaka | 97:2b3242c2dd85 | 29 | float z_dist = gps.PositionNED.z - pi.z; |
| NaotoMorita | 98:bdaa6bbfb1d9 | 30 | float dist2 = x_dist*x_dist + y_dist*y_dist + z_dist*z_dist; |
| NaotoMorita | 98:bdaa6bbfb1d9 | 31 | if (dist2 >= 400.0f) |
| osaka | 97:2b3242c2dd85 | 32 | { |
| osaka | 97:2b3242c2dd85 | 33 | gps.CalculateUnit(); |
| osaka | 97:2b3242c2dd85 | 34 | eskf.setPihat(0.0f, 0.0f, 0.0f); |
| NaotoMorita | 98:bdaa6bbfb1d9 | 35 | palt0 = palt; |
| osaka | 97:2b3242c2dd85 | 36 | } |
| osaka | 97:2b3242c2dd85 | 37 | else |
| osaka | 97:2b3242c2dd85 | 38 | { |
| osaka | 97:2b3242c2dd85 | 39 | //何もしない |
| osaka | 97:2b3242c2dd85 | 40 | } |
| osaka | 96:3645f8f9bdd3 | 41 | } |