Eigen Revision
Dependencies: mbed LPS25HB_I2C LSM9DS1 PIDcontroller Autopilot_Eigen LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM
Diff: autopilot.cpp
- Revision:
- 140:53dbdb207542
- Parent:
- 139:b378528c05f2
- Child:
- 143:53808e4e684c
--- a/autopilot.cpp Mon Dec 06 08:26:16 2021 +0000 +++ b/autopilot.cpp Mon Dec 06 11:37:55 2021 +0000 @@ -1,52 +1,53 @@ -//#include "global.hpp" -// -//void level_flight() -//{ -// Vector3 vdot = calc_vdot(); -// Matrix pihat = eskf.getPihat(); -// Matrix vihat = eskf.getVihat(); -// autopilot.update_val(rpy, -palt, pihat, vihat, vdot); -// autopilot.level(); -// autopilot.keep_alt(); -// autopilot.return_val(roll_obj, pitch_obj, dT_obj); -//} -// -//void point_guide() -//{ -// Vector3 vdot = calc_vdot(); -// Matrix pihat = eskf.getPihat(); -// Matrix vihat = eskf.getVihat(); -// autopilot.update_val(rpy, -palt, pihat, vihat, vdot); -// autopilot.guide(); -// autopilot.keep_alt(); -// autopilot.return_val(roll_obj, pitch_obj, dT_obj); -//} -// -//void turning() -//{ -// Vector3 vdot = calc_vdot(); -// Matrix pihat = eskf.getPihat(); -// Matrix vihat = eskf.getVihat(); -// 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 +#include "global.hpp" + +void level_flight() +{ + Vector3f vdot = calc_vdot(); + Vector3f pihat = eskf.getPihat(); + Vector3f vihat = eskf.getVihat(); + autopilot.update_val(rpy, -palt, pihat, vihat, vdot); + autopilot.level(); + autopilot.keep_alt(); + autopilot.return_val(roll_obj, pitch_obj, dT_obj); +} + +void point_guide() +{ + Vector3f vdot = calc_vdot(); + Vector3f pihat = eskf.getPihat(); + Vector3f vihat = eskf.getVihat(); + autopilot.update_val(rpy, -palt, pihat, vihat, vdot); + autopilot.guide(); + autopilot.keep_alt(); + autopilot.return_val(roll_obj, pitch_obj, dT_obj); +} + +void turning() +{ + Vector3f vdot = calc_vdot(); + Vector3f pihat = eskf.getPihat(); + Vector3f vihat = eskf.getVihat(); + autopilot.update_val(rpy, -palt, pihat, vihat, vdot); + autopilot.turn(); + autopilot.keep_alt(); + autopilot.return_val(roll_obj, pitch_obj, dT_obj); +} + +void climb() +{ + Vector3f vdot = calc_vdot(); + Vector3f pihat = eskf.getPihat(); + Vector3f vihat = eskf.getVihat(); + autopilot.update_val(rpy, -palt, pihat, vihat, vdot); + autopilot.level(); + autopilot.climb(); + autopilot.return_val(roll_obj, pitch_obj, dT_obj); +} + +Vector3f calc_vdot() +{ + Vector3f m_vdot = eskf.calcDynAcc(acc); + Vector3f vdot; + vdot = m_vdot; + return vdot; +} \ No newline at end of file