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
run.cpp
- Committer:
- haruki_kozuka
- Date:
- 2021-09-13
- Revision:
- 80:162ae3ce4874
- Parent:
- 77:2bf856e3eca4
File content as of revision 80:162ae3ce4874:
#include "global.hpp" void run() { //pc.serial.printf("\r\nrun Mode\r\n"); wait(0.5); Timer _t; _t.start(); magCalibrator.setExtremes(magbiasMin,magbiasMax); float sum2accnorm = 0; float sumaccnorm = 0; int n_init = 1000; if(hilFlag == false){ ekf.defineQhat(rpy_align); for(int i = 0; i < n_init; i++){ float tstart = _t.read(); if(hilFlag == true){ getHILval(); }else{ getIMUval(); } ekf.updateStaticAccMeasures(acc,accref); ekf.updateGyroBiasConstraints(gyro); ekf.fuseErr2Nominal(); ekf.resetBias(); //ekf.updateMagMeasures(mag); ekf.computeAngles(rpy, rpy_align); sumaccnorm += acc.Norm(); sum2accnorm += acc.Norm()*acc.Norm(); float tend = _t.read(); att_dt = (tend-tstart); } accref.z = sumaccnorm / float(n_init); } pc.Subscribe(0000, &(vp)); LoopTicker PIDtick; PIDtick.attach(calcServoOut,PID_dt); //Kozuka_change float prs_t_sub = 0.0f; float alt_sub = 0.0f; int prsCount = 0; BME280 prs_sensor(PB_9, PB_8); //for test Serial pc(USBTX, USBRX); //Kozuka_change_finish while(1) { float tstart = _t.read(); //姿勢角を更新 if(hilFlag == true){ getHILval(); }else{ getIMUval(); } ekf.updateNominal(gyro,acc,accref,att_dt); ekf.updateErrState(gyro,acc, att_dt); if(obsCount == 50){ if(ekf.determinDynStatus(acc,accref)){ ekf.updateMeasures(gyro,acc,accref); }else{ ekf.updateStaticMeasures(gyro,acc,accref); } obsCount = 0; }else{ if(ekf.determinDynStatus(acc,accref)){ ekf.updateAccMeasures(acc,accref); }else{ ekf.updateStaticAccMeasures(acc,accref); } obsCount += 1; } //Kozuka_change if(prsCount == 10){ //_t.start(); //float tstart = _t.read(); float temp = prs_sensor.getTemperature(); float press = prs_sensor.getPressure(); // pc.printf("pressure: %f \n", press); float alt = getAltitude(temp, press); //??no float-> error why? pc.printf("altitude: %f \n",alt); float prs_t= _t.read(); float v_speed = (alt-alt_sub)/(prs_t - prs_t_sub); alt_sub = alt; prs_t_sub = prs_t; //for test // pc.printf("v_speed: %f \n",v_speed); prsCount = 0; }else{ prsCount += 1; } //Kozuka_change_finish ekf.updateGyroBiasConstraints(gyro); //ekf.updateVelocityConstraints(); //ekf.updateMagMeasures(mag); ekf.fuseErr2Nominal(); ekf.resetBias(); ekf.computeAngles(rpy, rpy_align); ekf.computeVb(vb); PIDtick.loop(); float tend = _t.read(); att_dt = (tend-tstart); } } // Kozuka_change float getAltitude(float temp, float press) { float seaPress = 1013.25f ; float press1 = pow(seaPress/press, 1.0f / 5.257f ) - 1.0f ; float temp1 = temp + 273.15f ; float alt = press1 * temp1 / 0.0065f ; return alt ; } //Kozuka_change_finish /* if(serialParamSource){ while(1){ pc.serial.attach(NULL, Serial::RxIrq); pc.serial.printf("%d %d %d %d %d \r\n",checkParamSerial[0],checkParamSerial[1],checkParamSerial[2],checkParamSerial[3],checkParamSerial[4]); pc.serial.attach(&pc, &UsaPack::Receive, Serial::RxIrq); switch(vp.commandIndex){ case 1: NVIC_SystemReset(); break; case 10: ekf.setQqerr(float(vp.commandVal)); checkParamSerial[0] = 1; break; case 11: ekf.setQgbias(float(vp.commandVal)); checkParamSerial[1] = 1; break; case 12: Qabdyn=float(vp.commandVal); checkParamSerial[2] = 1; break; case 13: Rscdyn=float(vp.commandVal); checkParamSerial[3] = 1; break; case 14: checkParamSerial[4] = 1; break; default : break; } int cpsSum = 0; for(int i = 0;i<5;i++){ cpsSum += checkParamSerial[i]; } if(cpsSum == 5){ break; } wait(0.01); } } */