HAPSRG / Mbed 2 deprecated hexaTest_Eigen

solaESKF_EIGEN

Dependencies:   mbed LPS25HB_I2C LSM9DS1 PIDcontroller LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM

run.cpp@66:e5afad70fdd8, 2021-06-22 (annotated)

Committer:
NaotoMorita
Date:
Tue Jun 22 02:19:14 2021 +0000
Revision:
66:e5afad70fdd8
Parent:
61:c05353850017
Child:
68:b9f6938fab9d
mbed ram start;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
cocorlow 56:888379912f81 1 #include "global.hpp"
cocorlow 56:888379912f81 2
cocorlow 56:888379912f81 3 void run()
cocorlow 56:888379912f81 4 {
cocorlow 56:888379912f81 5 getIMUval();
cocorlow 56:888379912f81 6 ekf.triad(acc/acc.Norm(), accref/accref.Norm(), mag/mag.Norm(), magref/magref.Norm());
NaotoMorita 66:e5afad70fdd8 7 float sum2accnorm = 0;
NaotoMorita 66:e5afad70fdd8 8 float sumaccnorm = 0;
cocorlow 56:888379912f81 9 for(int i = 0; i < 1000; i++){
cocorlow 56:888379912f81 10 getIMUval();
NaotoMorita 66:e5afad70fdd8 11 sumaccnorm += acc.Norm();
NaotoMorita 66:e5afad70fdd8 12 sum2accnorm += acc.Norm()*acc.Norm();
cocorlow 56:888379912f81 13 }
NaotoMorita 66:e5afad70fdd8 14 accref.z = -sumaccnorm / 1000.0f;
NaotoMorita 66:e5afad70fdd8 15 float sigma_accnorm = sqrt(sum2accnorm/1000.0f-accref.z*accref.z);
NaotoMorita 66:e5afad70fdd8 16 pc.Subscribe(0000, &(posValues));
cocorlow 56:888379912f81 17
cocorlow 56:888379912f81 18 LoopTicker PIDtick;
cocorlow 56:888379912f81 19 PIDtick.attach(calcServoOut,PID_dt);
cocorlow 56:888379912f81 20
cocorlow 56:888379912f81 21 Timer _t;
cocorlow 56:888379912f81 22 _t.start();
cocorlow 56:888379912f81 23
cocorlow 56:888379912f81 24 while(1)
cocorlow 56:888379912f81 25 {
cocorlow 56:888379912f81 26 float tstart = _t.read();
cocorlow 56:888379912f81 27 //姿勢角を更新
cocorlow 56:888379912f81 28 getIMUval();
NaotoMorita 66:e5afad70fdd8 29 //ekf.updateBetweenMeasures(gyro, att_dt);
NaotoMorita 66:e5afad70fdd8 30 //ekf.computeAngles(rpy, rpy_g, rpy_align);
cocorlow 56:888379912f81 31 PIDtick.loop();
cocorlow 56:888379912f81 32
cocorlow 56:888379912f81 33 float tend = _t.read();
cocorlow 56:888379912f81 34 att_dt = (tend-tstart);
cocorlow 56:888379912f81 35 }
cocorlow 56:888379912f81 36 }