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 MatrixMath LPS25HB_I2C LSM9DS1 Matrix2 PIDcontroller LoopTicker SBUS_without_mainfile UsaPack solaESKF_wind Vector3 CalibrateMagneto FastPWM
Diff: servo.cpp
- Revision:
- 69:0caaad87cf1d
- Parent:
- 68:11ffadc7adb1
- Child:
- 71:62eb45ecffe9
--- a/servo.cpp Tue Jun 22 12:00:40 2021 +0000
+++ b/servo.cpp Mon Jun 28 03:30:54 2021 +0000
@@ -70,28 +70,6 @@
servo.pulsewidth_us(servoOut[0]);
- //観測アップデート
- dynacc = ekf.calcDynAcc(LPacc, accref);
- th_mg = abs(acos(LPmag/LPmag.Norm() % LPacc/LPacc.Norm())-val_thmg);
-
- accnormerr = abs(LPacc.Norm()-accref.Norm());
- //静止時100個の平均 0.01877522 0.00514146 0.00477393
-
- //int ang_th = th_mg < 0.01877522;
- //int dyn_th = dynaccnorm < 0.00514146;
- //int norm_th = accnormerr< 0.00477393;
- int ang_th = th_mg < sigma_thmg/200.0;
- int dyn_th = dynacc.Norm() < sigma_accnorm/50.0;
- int norm_th = accnormerr< sigma_accnorm/50.0;
- if(dyn_th+ang_th+norm_th>0)
- {
- //if(dyn_th+ang_th+norm_th>-1){
- obs_count += 1;
- ekf.updateAcrossMeasures(LPmag/LPmag.Norm(), magref/magref.Norm(), ekf.Rmag);
- ekf.updateAcrossMeasures(LPacc/LPacc.Norm(), accref/accref.Norm(), ekf.Racc);
- }
-
-// if(loop_count >= 10)
if(loop_count >= 3)
{
writeSdcard();