solaESKF_EIGEN
Dependencies: mbed LPS25HB_I2C LSM9DS1 PIDcontroller LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM
autopilot.cpp@140:53dbdb207542, 2021-12-06 (annotated)
- Committer:
- cocorlow
- Date:
- Mon Dec 06 11:37:55 2021 +0000
- Revision:
- 140:53dbdb207542
- Parent:
- 139:b378528c05f2
- Child:
- 143:53808e4e684c
servo transferData imu
Who changed what in which revision?
User | Revision | Line number | New 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(); |
cocorlow | 140:53dbdb207542 | 8 | autopilot.update_val(rpy, -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(); |
cocorlow | 140:53dbdb207542 | 19 | autopilot.update_val(rpy, -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(); |
cocorlow | 140:53dbdb207542 | 30 | autopilot.update_val(rpy, -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(); |
cocorlow | 140:53dbdb207542 | 41 | autopilot.update_val(rpy, -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 | } |