HAPSRG / Mbed 2 deprecated hexaTest_Eigen

solaESKF_EIGEN

Dependencies:   mbed LPS25HB_I2C LSM9DS1 PIDcontroller LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM

gps.cpp@107:46e039e12182, 2021-11-12 (annotated)

Committer:
osaka
Date:
Fri Nov 12 09:04:59 2021 +0000
Revision:
107:46e039e12182
Parent:
100:7589b663d462 
added autopilot

Who changed what in which revision?

UserRevisionLine numberNew 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 107:46e039e12182 35 autopilot.set_dest(destination.x - gps.PositionNED.x, destination.y - gps.PositionNED.y);
osaka 107:46e039e12182 36 autopilot.set_turn(turn_center.x - gps.PositionNED.x, turn_center.y - gps.PositionNED.y, turn_radius);
osaka 97:2b3242c2dd85 37 gps.CalculateUnit();
osaka 97:2b3242c2dd85 38 eskf.setPihat(0.0f, 0.0f, 0.0f);
NaotoMorita 99:98b892ca70ec 39 //palt0 = palt;
NaotoMorita 99:98b892ca70ec 40
osaka 97:2b3242c2dd85 41 }
osaka 97:2b3242c2dd85 42 else
osaka 97:2b3242c2dd85 43 {
osaka 97:2b3242c2dd85 44 //何もしない
osaka 97:2b3242c2dd85 45 }
osaka 96:3645f8f9bdd3 46 }