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:
- 108:e582f8bd4729
- Parent:
- 107:78e6f7bee68e
- Child:
- 109:27ae949bc38e
--- a/run.cpp Mon Mar 07 09:16:40 2022 +0000
+++ b/run.cpp Tue Mar 08 04:06:35 2022 +0000
@@ -92,11 +92,19 @@
}
sensorUpdateID += 1;
}else{
- if(updateValues.dist_ir<1.0f){
- //Matrix vbOpt(2,1);
- //vbOpt(1,1) = updateValues.vx_opt;
- //vbOpt(2,1) = updateValues.vx_opt;
- //eskf.updateVb(vbOpt,Ropt);
+ float vnorm_opt2 = updateValues.vx_opt*updateValues.vx_opt+updateValues.vy_opt*updateValues.vy_opt;
+ if(updateValues.dist_ir<1.0f && vnorm_opt2 < 400.0f){
+ if(vnorm_opt2 < 0.1f){
+ Matrix vbOpt(2,1);
+ vbOpt(1,1) = updateValues.vx_opt;
+ vbOpt(2,1) = updateValues.vy_opt;
+ eskf.updateVb(vbOpt,Ropt*0.1f);
+ }else{
+ //Matrix vbOpt(2,1);
+ //vbOpt(1,1) = updateValues.vx_opt;
+ //vbOpt(2,1) = updateValues.vy_opt;
+ //eskf.updateVb(vbOpt,Ropt);
+ }
//pc.printf("opt\r\n");
}
sensorUpdateID=1;