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
autopilot.cpp
- Committer:
- cocorlow
- Date:
- 2021-12-06
- Revision:
- 140:53dbdb207542
- Parent:
- 139:b378528c05f2
- Child:
- 143:53808e4e684c
File content as of revision 140:53dbdb207542:
#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; }