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:
- 93:b0f7a1e91476
- Parent:
- 92:6a2319f7f6ed
- Child:
- 94:a90b4b920fa9
--- a/run.cpp Thu Jan 13 11:15:34 2022 +0000
+++ b/run.cpp Tue Feb 01 07:48:56 2022 +0000
@@ -104,16 +104,17 @@
tail.Subscribe(tail_address[pos_tail], &(updateValues));
//pre-flight check
+ int i = 0;
while(1){
float tstart = _t.read();
getIMUval();
eskf.updateNominal(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt);
eskf.updateErrState(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt);
- Vector3 pi(0.0f, 0.0f, 0.0f);
- Vector3 vi(0.0f, 0.0f, 0.0f);
- eskf.updateGPS(MatrixMath::Vector2mat(pi),0.0f,MatrixMath::Vector2mat(vi),Rgps);
- float heading = atan2f(-mag.y,mag.x);
- eskf.updateHeading(heading,Rheading);
+ //Vector3 pi(0.0f, 0.0f, 0.0f);
+ //Vector3 vi(0.0f, 0.0f, 0.0f);
+ //eskf.updateGPS(MatrixMath::Vector2mat(pi),0.0f,MatrixMath::Vector2mat(vi),Rgps);
+ //float heading = atan2f(-mag.y,mag.x);
+ //eskf.updateHeading(heading,Rheading);
Matrix Raccpreflight(3,3);
setDiag(Raccpreflight,5.0f);
eskf.updateAcc(MatrixMath::Vector2mat(acc),Raccpreflight);
@@ -157,16 +158,20 @@
preflightCheck = false;
}
- if(preflightCheck == true){
+ if(preflightCheck == true && i >100){
prefligt_finished = true;
break;
}
+ wait(0.01f);
float tend = _t.read();
att_dt = (tend-tstart);
pc.printf("pre-flight check\r\n");
send2center();
+ i++;
}
+
+ eskf.setPihat(updateValues.pi[0], updateValues.pi[1], palt);
LoopTicker PIDtick;
PIDtick.attach(calcServoOut,PID_dt);