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@131:45542fcc886f, 2021-11-29 (annotated)
- Committer:
- osaka
- Date:
- Mon Nov 29 13:13:34 2021 +0000
- Revision:
- 131:45542fcc886f
- Parent:
- 130:3a0ab2affb0f
- Child:
- 137:240588882ae2
Autopilot modified
Who changed what in which revision?
User | Revision | Line number | New 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 | 131:45542fcc886f | 5 | Matrix pihat = eskf.getPihat(); |
osaka | 131:45542fcc886f | 6 | Matrix vihat = eskf.getVihat(); |
osaka | 131:45542fcc886f | 7 | autopilot.update_val(rpy, -palt, pihat, vihat); |
osaka | 109:eb255fc4462b | 8 | autopilot.level(); |
osaka | 121:2523eef96b36 | 9 | autopilot.keep_alt(); |
osaka | 121:2523eef96b36 | 10 | autopilot.return_val(roll_obj, pitch_obj, dT_obj); |
osaka | 109:eb255fc4462b | 11 | } |
osaka | 109:eb255fc4462b | 12 | |
osaka | 109:eb255fc4462b | 13 | void point_guide() |
osaka | 109:eb255fc4462b | 14 | { |
osaka | 131:45542fcc886f | 15 | Matrix pihat = eskf.getPihat(); |
osaka | 131:45542fcc886f | 16 | Matrix vihat = eskf.getVihat(); |
osaka | 131:45542fcc886f | 17 | autopilot.update_val(rpy, -palt, pihat, vihat); |
osaka | 109:eb255fc4462b | 18 | autopilot.guide(); |
osaka | 121:2523eef96b36 | 19 | autopilot.keep_alt(); |
osaka | 121:2523eef96b36 | 20 | autopilot.return_val(roll_obj, pitch_obj, dT_obj); |
osaka | 109:eb255fc4462b | 21 | } |
osaka | 109:eb255fc4462b | 22 | |
osaka | 109:eb255fc4462b | 23 | void turning() |
osaka | 109:eb255fc4462b | 24 | { |
osaka | 131:45542fcc886f | 25 | Matrix pihat = eskf.getPihat(); |
osaka | 131:45542fcc886f | 26 | Matrix vihat = eskf.getVihat(); |
osaka | 131:45542fcc886f | 27 | autopilot.update_val(rpy, -palt, pihat, vihat); |
osaka | 109:eb255fc4462b | 28 | autopilot.turn(); |
osaka | 121:2523eef96b36 | 29 | autopilot.keep_alt(); |
osaka | 121:2523eef96b36 | 30 | autopilot.return_val(roll_obj, pitch_obj, dT_obj); |
osaka | 109:eb255fc4462b | 31 | } |