solaESKF_EIGEN
Dependencies: mbed LPS25HB_I2C LSM9DS1 PIDcontroller LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM
Diff: transferData.cpp
- Revision:
- 144:b3a713b4f1c4
- Parent:
- 143:53808e4e684c
--- a/transferData.cpp Fri Jun 24 05:44:34 2022 +0000 +++ b/transferData.cpp Wed Jun 29 07:57:10 2022 +0000 @@ -2,9 +2,10 @@ void sendData2PC() { - sp.da = da; - sp.de = de; - sp.dT = dT; + + for(int i = 0;i<sizeof(scaledMotorOut)/sizeof(scaledMotorOut[0]) ;i++){ + sp.motor[i] = scaledMotorOut[i]; + } sp.euler[0] = euler(0)*180.0f/M_PI_F; sp.euler[1] = euler(1)*180.0f/M_PI_F; sp.euler[2] = euler(2)*180.0f/M_PI_F; @@ -62,7 +63,7 @@ } break; case 6 : - //gyroBias + //gravity for (int i = 0; i < 3; i++){ sp.state[i] = state(i+16); } @@ -131,18 +132,10 @@ tp.wind[0] = 0.0f; tp.wind[1] = 0.0f; tp.wind[2] = 0.0f; - tp.pitch_obj = pitch_obj; - tp.roll_obj = roll_obj; + tp.pitch_obj = 0.0f; + tp.roll_obj = 0.0f; tp.u_pitot = 0.0f; - - if (hinf_flag) - { - tp.hinf = 1.0f; - } - else - { - tp.hinf = 0.0f; - } + tp.hinf = 0.0f; wait(0.001f); @@ -189,8 +182,8 @@ lp.wind[0] = 0.0f; lp.wind[1] = 0.0f; lp.wind[2] = 0.0f; - lp.pitch_obj = pitch_obj; - lp.roll_obj = roll_obj; + lp.pitch_obj = 0.0f; + lp.roll_obj = 0.0f; lp.u_pitot = 0.0f; //sd.printf("%f %f %f %f %f %f\r\n",da,de,dT,rpy.x*180.0f/M_PI,rpy.y*180.0f/M_PI,rpy.z*180.0f/M_PI);