solaESKF_EIGEN
Dependencies: mbed LPS25HB_I2C LSM9DS1 PIDcontroller LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM
Diff: run.cpp
- Revision:
- 76:7fd3ac1afe3e
- Parent:
- 75:a505b9896da1
- Child:
- 77:2bf856e3eca4
--- a/run.cpp Tue Jul 20 11:57:05 2021 +0000 +++ b/run.cpp Sat Aug 07 06:54:58 2021 +0000 @@ -22,7 +22,7 @@ ekf.updateStaticAccMeasures(acc,accref); //ekf.fuseErr2Qhat(); //ekf.resetBias(); - ekf.updateMagMeasures(mag); + //ekf.updateMagMeasures(mag); ekf.computeAngles(rpy, rpy_align); sumaccnorm += acc.Norm(); sum2accnorm += acc.Norm()*acc.Norm(); @@ -31,8 +31,52 @@ } accref.z = sumaccnorm / 1000.0f; float sigma_accnorm = sqrt(sum2accnorm/1000.0f-accref.z*accref.z); + float Rscdyn = 263.980f; + float Qabdyn = 124.810f; pc.serial.printf("%f \r\n",sigma_accnorm); pc.Subscribe(0000, &(vp)); + 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); + } + } LoopTicker PIDtick; PIDtick.attach(calcServoOut,PID_dt); @@ -51,16 +95,30 @@ ekf.updateQhat(gyro, att_dt); ekf.updateErrState(gyro, att_dt); - //ekf.updateAllMeasures(acc,gyro,mag,accref,tstart,att_dt); - if(ekf.determinDynStatus(acc,accref)){ + ekf.updateAccMeasures(acc,accref); + ekf.updateGyroBiasConstraints(gyro); + ekf.updateVelocityConstraints(); + /* + if(obsCount == 450){ + if(ekf.determinDynStatus(acc,accref)==false){ + ekf.setRsoftconst(1.0f,1.0f); + ekf.setQab(0.0f); + }else{ + ekf.setRsoftconst(263.0f,500.0f); + ekf.setQab(Qabdyn); + } ekf.updateAccMeasures(acc,accref); + obsCount = 0; }else{ - ekf.updateStaticAccMeasures(acc,accref); + obsCount += 1; + if(ekf.determinDynStatus(acc,accref)==false){ + ekf.updateStaticAccMeasures(acc,accref); + } } - //ekf.updateGyroBiasMeasures(gyro,mag,tstart,att_dt); + */ + //ekf.updateMagMeasures(mag); ekf.fuseErr2Qhat(); - ekf.resetBias(); - //ekf.updateMagMeasures(mag); + //ekf.resetBias(); ekf.computeAngles(rpy, rpy_align); PIDtick.loop();