USNA WSE ES456 / Mbed 2 deprecated Emaxx_Navigation_madpulse

Emaxx Navigation code ported for MBED

Dependencies:   BNO055_fusion Emaxx_Navigation_Dynamic_HIL MODSERIAL ServoIn ServoOut Vehicle_Model mbed

Fork of Emaxx_Navigation_Dynamic_HIL by Emaxx Navigation Group

Diff: main.cpp

Revision:
3:9b6db94aa2b3
Parent:
2:0c9c3c1f3b18
Child:
4:e27ca0c82814
--- a/main.cpp	Thu Aug 25 15:47:50 2016 +0000
+++ b/main.cpp	Wed Sep 14 18:19:50 2016 +0000
@@ -250,7 +250,7 @@
                         psi_err = wrapToPi(des_psi-imu.euler.yaw);
                         spd_err = des_spd - ekf.getSpd();
                         spd_err_i += spd_err;
-                        str = -Kp_psi*psi_err;
+                        str = -Kp_psi*psi_err/ekf.getSpd();
 
                         thr =  Kp_spd*spd_err + Ki_spd*spd_err_i/LOOP_RATE;