solaESKF_EIGEN
Dependencies: mbed LPS25HB_I2C LSM9DS1 PIDcontroller LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM
Diff: autopilot.cpp
- Revision:
- 137:240588882ae2
- Parent:
- 131:45542fcc886f
- Child:
- 139:b378528c05f2
--- 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