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 LPS25HB_I2C LSM9DS1 PIDcontroller Autopilot_Eigen LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM
Revision 137:240588882ae2, committed 2021-12-01
- Comitter:
- osaka
- Date:
- Wed Dec 01 11:51:49 2021 +0000
- Parent:
- 136:c6216bb9b252
- Child:
- 138:4882d71c6a5f
- Commit message:
- climb
Changed in this revision
--- a/Autopilot.lib Wed Dec 01 09:24:24 2021 +0000 +++ b/Autopilot.lib Wed Dec 01 11:51:49 2021 +0000 @@ -1,1 +1,1 @@ -https://os.mbed.com/teams/HAPSRG/code/Autopilot/#75636841e43b +https://os.mbed.com/teams/HAPSRG/code/Autopilot/#7b6c3f8b85fb
--- a/autopilot.cpp Wed Dec 01 09:24:24 2021 +0000
+++ b/autopilot.cpp Wed Dec 01 11:51:49 2021 +0000
@@ -2,9 +2,10 @@
void level_flight()
{
+ Vector3 vdot = calc_vdot();
Matrix pihat = eskf.getPihat();
Matrix vihat = eskf.getVihat();
- autopilot.update_val(rpy, -palt, pihat, vihat);
+ autopilot.update_val(rpy, -palt, pihat, vihat, vdot);
autopilot.level();
autopilot.keep_alt();
autopilot.return_val(roll_obj, pitch_obj, dT_obj);
@@ -12,9 +13,10 @@
void point_guide()
{
+ Vector3 vdot = calc_vdot();
Matrix pihat = eskf.getPihat();
Matrix vihat = eskf.getVihat();
- autopilot.update_val(rpy, -palt, pihat, vihat);
+ autopilot.update_val(rpy, -palt, pihat, vihat, vdot);
autopilot.guide();
autopilot.keep_alt();
autopilot.return_val(roll_obj, pitch_obj, dT_obj);
@@ -22,10 +24,29 @@
void turning()
{
+ Vector3 vdot = calc_vdot();
Matrix pihat = eskf.getPihat();
Matrix vihat = eskf.getVihat();
- autopilot.update_val(rpy, -palt, pihat, vihat);
+ autopilot.update_val(rpy, -palt, pihat, vihat, vdot);
autopilot.turn();
autopilot.keep_alt();
autopilot.return_val(roll_obj, pitch_obj, dT_obj);
+}
+
+void climb()
+{
+ Vector3 vdot = calc_vdot();
+ Matrix pihat = eskf.getPihat();
+ Matrix vihat = eskf.getVihat();
+ autopilot.update_val(rpy, -palt, pihat, vihat, vdot);
+ autopilot.level();
+ autopilot.climb();
+ autopilot.return_val(roll_obj, pitch_obj, dT_obj);
+}
+
+Vector3 calc_vdot()
+{
+ Matrix m_vdot = eskf.calcDynAcc(MatrixMath::Vector2mat(acc));
+ Vector3 vdot(m_vdot(1, 1), m_vdot(2, 1), m_vdot(3, 1));
+ return vdot;
}
\ No newline at end of file
--- a/global.cpp Wed Dec 01 09:24:24 2021 +0000
+++ b/global.cpp Wed Dec 01 11:51:49 2021 +0000
@@ -86,7 +86,7 @@
telemetryPack tp;
// HIL
-bool hilFlag = false;
+bool hilFlag = true;
bool serialControlSource = false;
bool serialParamSource = false;
int checkParamSerial[5] = {0,0,0,0,0};
--- a/global.hpp Wed Dec 01 09:24:24 2021 +0000 +++ b/global.hpp Wed Dec 01 11:51:49 2021 +0000 @@ -201,6 +201,8 @@ extern void level_flight(); extern void point_guide(); extern void turning(); +extern void climb(); +extern Vector3 calc_vdot(); // servo.cpp extern void calcServoOut();
--- a/servo.cpp Wed Dec 01 09:24:24 2021 +0000
+++ b/servo.cpp Wed Dec 01 11:51:49 2021 +0000
@@ -27,7 +27,7 @@
{
//level_flight();
//point_guide();
- turning();
+ climb();
rollPID.setSetPoint(roll_obj);
pitchPID.setSetPoint(pitch_obj);
dT += dT_obj;
--- a/setup.cpp Wed Dec 01 09:24:24 2021 +0000
+++ b/setup.cpp Wed Dec 01 11:51:49 2021 +0000
@@ -54,6 +54,7 @@
autopilot.set_dest(0.0f, 50.0f);
autopilot.set_turn(0.0f, 50.0f, 50.0f);
autopilot.set_alt(30.0f, 10.0f);
+ autopilot.set_climb(3.0f*M_PI/180.0f, 0);
}
void calibrate()