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
transferData.cpp
- Committer:
- NaotoMorita
- Date:
- 2021-11-15
- Revision:
- 111:0fae4fbe2a80
- Parent:
- 106:2d854e92cebb
- Child:
- 113:3e47d9881529
File content as of revision 111:0fae4fbe2a80:
#include "global.hpp" void sendData2PC() { sp.da = da; sp.de = de; sp.dT = dT; sp.rpy[0] = rpy.x*180.0f/M_PI; sp.rpy[1] = rpy.y*180.0f/M_PI; sp.rpy[2] = rpy.z*180.0f/M_PI; Matrix vihat = eskf.getVihat(); sp.vihat[0] = vihat(1,1); sp.vihat[1] = vihat(2,1); sp.vihat[2] = vihat(3,1); pc.Send(0000, &(sp)); } void sendTelemetry() { Matrix pihat = eskf.getPihat(); Matrix vihat = eskf.getVihat(); Matrix accBias = eskf.getAccBias(); Matrix gyroBias = eskf.getGyroBias(); Matrix gravity = eskf.getGravity(); Matrix magBias = eskf.getMagBias(); float magRadius = eskf.getMagRadius(); //twelite.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); //twelite.printf("%f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\r\n",1.0f/att_dt,rpy.x*180.0f/M_PI,rpy.y*180.0f/M_PI,rpy.z*180.0f/M_PI, pihat(1,1),pihat(2,1),pihat(3,1),vihat(1,1),vihat(2,1),vihat(3,1),accBias(1,1),accBias(2,1),accBias(3,1),gyroBias(1,1),gyroBias(2,1),gyroBias(3,1),gravity(1,1),gravity(2,1),gravity(3,1)); twelite.printf("%d %f %f %f %f %f %f %f %f %f %f %f\r\n",gps.gpsFix ,1.0f/att_dt,rpy.x*180.0f/M_PI,rpy.y*180.0f/M_PI,rpy.z*180.0f/M_PI,mag.x,mag.y,mag.z,magBias(1,1),magBias(2,1),magBias(3,1),magRadius); //twelite.printf("%f %f %f %f %f %f\r\n",accBias(1,1),accBias(2,1),accBias(3,1),gyroBias(1,1),gyroBias(2,1),gyroBias(3,1)); //twelite.printf("%f %f %f %f %f %f\r\n",pi.x,pi.y,pi.z,vi.x,vi.y,vi.z); //twelite.printf("%f %f %f %f %f %f %f \r\n", magGamma(7,1), magGamma(8,1), magGamma(9,1), mag.x, mag.y, mag.z,magres); } void writeSDcard() { Matrix pihat = eskf.getPihat(); Matrix vihat = eskf.getVihat(); Matrix accBias = eskf.getAccBias(); Matrix gyroBias = eskf.getGyroBias(); Matrix gravity = eskf.getGravity(); //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); sd.printf("%f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\r\n",_t.read(),1.0f/att_dt,da,de,dT,rc[0],rc[1],rc[2],rpy.x*180.0f/M_PI,rpy.y*180.0f/M_PI,rpy.z*180.0f/M_PI, pihat(1,1),pihat(2,1),pihat(3,1),vihat(1,1),vihat(2,1),vihat(3,1)); //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); }