solaESKF_EIGEN
Dependencies: mbed LPS25HB_I2C LSM9DS1 PIDcontroller LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM
Diff: main.cpp
- Revision:
- 46:99ffb98e1257
- Parent:
- 44:f571273d3223
- Child:
- 47:392592d21d30
--- a/main.cpp Thu Apr 08 07:37:23 2021 +0000 +++ b/main.cpp Mon May 17 06:21:43 2021 +0000 @@ -10,6 +10,8 @@ #include "MatrixMath.h" #include "math.h" +#include "UsaPack.hpp" + #define MPU6050_PWR_MGMT_1 0x6B #define MPU_ADDRESS 0x68 #define pi 3.141562 @@ -102,6 +104,22 @@ int agoffset[6] = {0, 0, 0, -117, -563, 2 }; float magbias[4] = {-140.868240, 121.863251, -162.735092, 37.112610}; +// UsaPack +UsaPack cobs_sd(PC_12, PD_2, 115200); +const int cobs_address = 1234; +struct valuePack +{ + float dt; + int count; + float acc[3]; + float gyro[3]; + float mag[3]; + float rot[3]; + float rot_g[3]; +}; +valuePack vp; + + void writeSdcard() { //magcal.getExtremes(&magmin[0],&magmax[0]); @@ -116,6 +134,26 @@ //pc.printf("%f %f %f %f %f %f\r\n",dynacc_x,dynacc_y,dynacc_z,roll*180.0f/pi,pitch*180.0f/pi,yaw*180.0f/pi); //pc.printf("%f %f %f : %f %f %f\r\n",magref[0],magref[1],magref[2],mag_x,mag_y,mag_z); //pc.printf("%f %f %f %f %f : %f %f %f %f %f %f\r\n",f,magbias[0],magbias[1],magbias[2],magbias[3],magval[0],magval[1],magval[2],mdata.x,mdata.y,mdata.z); + + vp.dt = 1.0f/att_dt; + vp.count = obs_count; + vp.acc[0] = acc_x; + vp.acc[1] = acc_y; + vp.acc[2] = acc_z; + vp.gyro[0] = gyro_x; + vp.gyro[1] = gyro_y; + vp.gyro[2] = gyro_z; + vp.mag[0] = mag_x; + vp.mag[1] = mag_y; + vp.mag[2] = mag_z; + vp.rot[0] = roll*180.0f/pi; + vp.rot[1] = pitch*180.0f/pi; + vp.rot[2] = yaw*180.0f/pi; + vp.rot_g[0] = roll_g*180.0f/pi; + vp.rot_g[1] = pitch_g*180.0f/pi; + vp.rot_g[2] = yaw_g*180.0f/pi; + cobs_sd.Send(cobs_address, &vp); + } float mapfloat(float x, float in_min, float in_max, float out_min, float out_max) @@ -457,6 +495,7 @@ mrefs[2] /= iter_n; pc.printf("\r\naccreg : %f, %f, %f\r\n",arefs[0],arefs[1],arefs[2]); pc.printf("\r\nmagreg : %f, %f, %f\r\n",mrefs[0],mrefs[1],mrefs[2]); + while(1) {wait(1000);} }