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@143:53808e4e684c, 22 months ago (annotated)

Committer:
NaotoMorita
Date:
Fri Jun 24 05:44:34 2022 +0000
Revision:
143:53808e4e684c
Parent:
140:53dbdb207542 
complete

Who changed what in which revision?

UserRevisionLine numberNew contents of line
cocorlow 140:53dbdb207542 1 #include "global.hpp"
cocorlow 140:53dbdb207542 2
cocorlow 140:53dbdb207542 3 void level_flight()
cocorlow 140:53dbdb207542 4 {
cocorlow 140:53dbdb207542 5 Vector3f vdot = calc_vdot();
cocorlow 140:53dbdb207542 6 Vector3f pihat = eskf.getPihat();
cocorlow 140:53dbdb207542 7 Vector3f vihat = eskf.getVihat();
NaotoMorita 143:53808e4e684c 8 autopilot.update_val(euler, -palt, pihat, vihat, vdot);
cocorlow 140:53dbdb207542 9 autopilot.level();
cocorlow 140:53dbdb207542 10 autopilot.keep_alt();
cocorlow 140:53dbdb207542 11 autopilot.return_val(roll_obj, pitch_obj, dT_obj);
cocorlow 140:53dbdb207542 12 }
cocorlow 140:53dbdb207542 13
cocorlow 140:53dbdb207542 14 void point_guide()
cocorlow 140:53dbdb207542 15 {
cocorlow 140:53dbdb207542 16 Vector3f vdot = calc_vdot();
cocorlow 140:53dbdb207542 17 Vector3f pihat = eskf.getPihat();
cocorlow 140:53dbdb207542 18 Vector3f vihat = eskf.getVihat();
NaotoMorita 143:53808e4e684c 19 autopilot.update_val(euler, -palt, pihat, vihat, vdot);
cocorlow 140:53dbdb207542 20 autopilot.guide();
cocorlow 140:53dbdb207542 21 autopilot.keep_alt();
cocorlow 140:53dbdb207542 22 autopilot.return_val(roll_obj, pitch_obj, dT_obj);
cocorlow 140:53dbdb207542 23 }
cocorlow 140:53dbdb207542 24
cocorlow 140:53dbdb207542 25 void turning()
cocorlow 140:53dbdb207542 26 {
cocorlow 140:53dbdb207542 27 Vector3f vdot = calc_vdot();
cocorlow 140:53dbdb207542 28 Vector3f pihat = eskf.getPihat();
cocorlow 140:53dbdb207542 29 Vector3f vihat = eskf.getVihat();
NaotoMorita 143:53808e4e684c 30 autopilot.update_val(euler, -palt, pihat, vihat, vdot);
cocorlow 140:53dbdb207542 31 autopilot.turn();
cocorlow 140:53dbdb207542 32 autopilot.keep_alt();
cocorlow 140:53dbdb207542 33 autopilot.return_val(roll_obj, pitch_obj, dT_obj);
cocorlow 140:53dbdb207542 34 }
cocorlow 140:53dbdb207542 35
cocorlow 140:53dbdb207542 36 void climb()
cocorlow 140:53dbdb207542 37 {
cocorlow 140:53dbdb207542 38 Vector3f vdot = calc_vdot();
cocorlow 140:53dbdb207542 39 Vector3f pihat = eskf.getPihat();
cocorlow 140:53dbdb207542 40 Vector3f vihat = eskf.getVihat();
NaotoMorita 143:53808e4e684c 41 autopilot.update_val(euler, -palt, pihat, vihat, vdot);
cocorlow 140:53dbdb207542 42 autopilot.level();
cocorlow 140:53dbdb207542 43 autopilot.climb();
cocorlow 140:53dbdb207542 44 autopilot.return_val(roll_obj, pitch_obj, dT_obj);
cocorlow 140:53dbdb207542 45 }
cocorlow 140:53dbdb207542 46
cocorlow 140:53dbdb207542 47 Vector3f calc_vdot()
cocorlow 140:53dbdb207542 48 {
cocorlow 140:53dbdb207542 49 Vector3f m_vdot = eskf.calcDynAcc(acc);
cocorlow 140:53dbdb207542 50 Vector3f vdot;
cocorlow 140:53dbdb207542 51 vdot = m_vdot;
cocorlow 140:53dbdb207542 52 return vdot;
cocorlow 140:53dbdb207542 53 }