HAPSRG / Mbed 2 deprecated optWingforHAPS_Eigen

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

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-10
Revision:
104:20b8caa29185
Parent:
103:fec71c2051c5
Child:
106:2d854e92cebb

File content as of revision 104:20b8caa29185:

#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 magField = eskf.getMagField();
    Matrix magGamma = magCalibrator.getGamma();
    
    //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("%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,mag.x,mag.y,mag.z,magres);
    //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);
}