solaESKF_EIGEN
Dependencies: mbed LPS25HB_I2C LSM9DS1 PIDcontroller LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM
Diff: gps.cpp
- Revision:
- 97:2b3242c2dd85
- Parent:
- 96:3645f8f9bdd3
- Child:
- 98:bdaa6bbfb1d9
--- a/gps.cpp Tue Nov 02 14:29:21 2021 +0000 +++ b/gps.cpp Wed Nov 03 09:28:38 2021 +0000 @@ -3,11 +3,38 @@ void getGPSval() { gps.Update(); - gps.Calculate(); - vi.x = gps.VelocityNED.x; - vi.y = gps.VelocityNED.y; - vi.z = gps.VelocityNED.z; - pi.x = gps.PositionNED.x; - pi.y = gps.PositionNED.y; - pi.z = gps.PositionNED.z; + if (gps.iTOW_VELNED != itow_velned) + { + itow_velned = gps.iTOW_VELNED; + vi.x = gps.VelocityNED.x; + vi.y = gps.VelocityNED.y; + vi.z = gps.VelocityNED.z; + } + if (gps.iTOW_POSLLH != itow_posllh) + { + itow_posllh = gps.iTOW_POSLLH; + change_refpos(); + gps.Calculate(); + pi.x = gps.PositionNED.x; + pi.y = gps.PositionNED.y; + pi.z = gps.PositionNED.z; + } +} + +//20m以上の変位で基準位置更新 +void change_refpos() +{ + float x_dist = gps.PositionNED.x - pi.x; + float y_dist = gps.PositionNED.y - pi.y; + float z_dist = gps.PositionNED.z - pi.z; + float dist = sqrt(x_dist*x_dist + y_dist*y_dist + z_dist*z_dist); + if (dist >= 20.0f) + { + gps.CalculateUnit(); + eskf.setPihat(0.0f, 0.0f, 0.0f); + } + else + { + //何もしない + } } \ No newline at end of file