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:
- osaka
- Date:
- 2021-12-01
- Revision:
- 137:240588882ae2
- Parent:
- 131:45542fcc886f
- Child:
- 139:b378528c05f2
File content as of revision 137:240588882ae2:
#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; }