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
Diff: gps.cpp
- Revision:
- 106:2d854e92cebb
- Parent:
- 100:7589b663d462
- Child:
- 139:b378528c05f2
--- a/gps.cpp Wed Nov 10 06:35:10 2021 +0000 +++ b/gps.cpp Fri Nov 12 09:03:41 2021 +0000 @@ -4,41 +4,23 @@ { gpsUpdateFlag = false; gps.Update(); - 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; - gps.Calculate(); - change_refpos(); - pi.x = gps.PositionNED.x; - pi.y = gps.PositionNED.y; - pi.z = gps.PositionNED.z; - gpsUpdateFlag = true; + if (gps.iTOW_STATUS != itow_status){ + itow_status = gps.iTOW_STATUS; + if(gps.gpsFix == 0x02 || gps.gpsFix == 0x03){ + vi.x = gps.VelocityNED.x; + vi.y = gps.VelocityNED.y; + vi.z = gps.VelocityNED.z; + if(gpsLlh0Fixed == false){ + gps.CalculateUnit(); + gpsLlh0Fixed = true; + }else{ + gps.Calculate(); + } + pi.x = gps.PositionNED.x; + pi.y = gps.PositionNED.y; + pi.z = gps.PositionNED.z; + gpsUpdateFlag = true; + } } } -//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 dist2 = x_dist*x_dist + y_dist*y_dist + z_dist*z_dist; - if (dist2 >= 400.0f) - { - gps.CalculateUnit(); - eskf.setPihat(0.0f, 0.0f, 0.0f); - //palt0 = palt; - - } - else - { - //何もしない - } -} \ No newline at end of file