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: run.cpp
- Revision:
- 105:aaaed895ffaf
- Parent:
- 104:f81befbc5ab7
- Child:
- 106:36458fb9b5b7
--- a/run.cpp Tue Mar 01 06:53:06 2022 +0000
+++ b/run.cpp Tue Mar 01 09:19:27 2022 +0000
@@ -33,6 +33,9 @@
Matrix Rheading(1,1);
Rheading(1,1) = 0.01f;
+ Matrix Ropt(2,2);
+ setDiag(Ropt,10.0f);
+
wait(0.5);
_t.start();
@@ -43,9 +46,6 @@
LoopTicker PIDtick;
PIDtick.attach(calcServoOut,PID_dt);
- float tgps = _t.read();
- float theading = _t.read();
-
while(1)
{
tstart = _t.read();
@@ -97,6 +97,13 @@
eskf.updateAcc(MatrixMath::Vector2mat(acc),Racc);
}
}
+ if(updateValues.dist_ir<1.0f){
+ Matrix vbOpt(2,1);
+ vbOpt(1,1) = opt_vx;
+ vbOpt(2,1) = opt_vy;
+ eskf.updateVb(vbOpt,Ropt);
+ }
+
}
PIDtick.loop();