HAPSRG / Mbed 2 deprecated optWingforHAPS_Eigen

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@137:240588882ae2, 2021-12-01 (annotated)

Committer:
osaka
Date:
Wed Dec 01 11:51:49 2021 +0000
Revision:
137:240588882ae2
Parent:
131:45542fcc886f
Child:
139:b378528c05f2
climb

Who changed what in which revision?

UserRevisionLine numberNew contents of line
osaka 109:eb255fc4462b 1 #include "global.hpp"
osaka 109:eb255fc4462b 2
osaka 109:eb255fc4462b 3 void level_flight()
osaka 109:eb255fc4462b 4 {
osaka 137:240588882ae2 5 Vector3 vdot = calc_vdot();
osaka 131:45542fcc886f 6 Matrix pihat = eskf.getPihat();
osaka 131:45542fcc886f 7 Matrix vihat = eskf.getVihat();
osaka 137:240588882ae2 8 autopilot.update_val(rpy, -palt, pihat, vihat, vdot);
osaka 109:eb255fc4462b 9 autopilot.level();
osaka 121:2523eef96b36 10 autopilot.keep_alt();
osaka 121:2523eef96b36 11 autopilot.return_val(roll_obj, pitch_obj, dT_obj);
osaka 109:eb255fc4462b 12 }
osaka 109:eb255fc4462b 13
osaka 109:eb255fc4462b 14 void point_guide()
osaka 109:eb255fc4462b 15 {
osaka 137:240588882ae2 16 Vector3 vdot = calc_vdot();
osaka 131:45542fcc886f 17 Matrix pihat = eskf.getPihat();
osaka 131:45542fcc886f 18 Matrix vihat = eskf.getVihat();
osaka 137:240588882ae2 19 autopilot.update_val(rpy, -palt, pihat, vihat, vdot);
osaka 109:eb255fc4462b 20 autopilot.guide();
osaka 121:2523eef96b36 21 autopilot.keep_alt();
osaka 121:2523eef96b36 22 autopilot.return_val(roll_obj, pitch_obj, dT_obj);
osaka 109:eb255fc4462b 23 }
osaka 109:eb255fc4462b 24
osaka 109:eb255fc4462b 25 void turning()
osaka 109:eb255fc4462b 26 {
osaka 137:240588882ae2 27 Vector3 vdot = calc_vdot();
osaka 131:45542fcc886f 28 Matrix pihat = eskf.getPihat();
osaka 131:45542fcc886f 29 Matrix vihat = eskf.getVihat();
osaka 137:240588882ae2 30 autopilot.update_val(rpy, -palt, pihat, vihat, vdot);
osaka 109:eb255fc4462b 31 autopilot.turn();
osaka 121:2523eef96b36 32 autopilot.keep_alt();
osaka 121:2523eef96b36 33 autopilot.return_val(roll_obj, pitch_obj, dT_obj);
osaka 137:240588882ae2 34 }
osaka 137:240588882ae2 35
osaka 137:240588882ae2 36 void climb()
osaka 137:240588882ae2 37 {
osaka 137:240588882ae2 38 Vector3 vdot = calc_vdot();
osaka 137:240588882ae2 39 Matrix pihat = eskf.getPihat();
osaka 137:240588882ae2 40 Matrix vihat = eskf.getVihat();
osaka 137:240588882ae2 41 autopilot.update_val(rpy, -palt, pihat, vihat, vdot);
osaka 137:240588882ae2 42 autopilot.level();
osaka 137:240588882ae2 43 autopilot.climb();
osaka 137:240588882ae2 44 autopilot.return_val(roll_obj, pitch_obj, dT_obj);
osaka 137:240588882ae2 45 }
osaka 137:240588882ae2 46
osaka 137:240588882ae2 47 Vector3 calc_vdot()
osaka 137:240588882ae2 48 {
osaka 137:240588882ae2 49 Matrix m_vdot = eskf.calcDynAcc(MatrixMath::Vector2mat(acc));
osaka 137:240588882ae2 50 Vector3 vdot(m_vdot(1, 1), m_vdot(2, 1), m_vdot(3, 1));
osaka 137:240588882ae2 51 return vdot;
osaka 109:eb255fc4462b 52 }