HAPSRG / Mbed 2 deprecated optWingforHAPS_Eigen

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:
107:46e039e12182
Parent:
100:7589b663d462
--- a/gps.cpp	Wed Nov 10 06:35:10 2021 +0000
+++ b/gps.cpp	Fri Nov 12 09:04:59 2021 +0000
@@ -32,6 +32,8 @@
     float dist2 = x_dist*x_dist + y_dist*y_dist + z_dist*z_dist;
     if (dist2 >= 400.0f)
     {
+        autopilot.set_dest(destination.x  - gps.PositionNED.x, destination.y  - gps.PositionNED.y);
+        autopilot.set_turn(turn_center.x - gps.PositionNED.x, turn_center.y - gps.PositionNED.y, turn_radius);
         gps.CalculateUnit();
         eskf.setPihat(0.0f, 0.0f, 0.0f);
         //palt0 = palt;