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
Diff: autopilot.cpp
- Revision:
- 131:45542fcc886f
- Parent:
- 130:3a0ab2affb0f
- Child:
- 137:240588882ae2
diff -r 3a0ab2affb0f -r 45542fcc886f autopilot.cpp --- a/autopilot.cpp Mon Nov 29 12:33:47 2021 +0000 +++ b/autopilot.cpp Mon Nov 29 13:13:34 2021 +0000 @@ -2,7 +2,9 @@ void level_flight() { - autopilot.update_val(rpy, -palt, pi, vi); + Matrix pihat = eskf.getPihat(); + Matrix vihat = eskf.getVihat(); + autopilot.update_val(rpy, -palt, pihat, vihat); autopilot.level(); autopilot.keep_alt(); autopilot.return_val(roll_obj, pitch_obj, dT_obj); @@ -10,7 +12,9 @@ void point_guide() { - autopilot.update_val(rpy, -palt, pi, vi); + Matrix pihat = eskf.getPihat(); + Matrix vihat = eskf.getVihat(); + autopilot.update_val(rpy, -palt, pihat, vihat); autopilot.guide(); autopilot.keep_alt(); autopilot.return_val(roll_obj, pitch_obj, dT_obj); @@ -18,7 +22,9 @@ void turning() { - autopilot.update_val(rpy, -palt, pi, vi); + Matrix pihat = eskf.getPihat(); + Matrix vihat = eskf.getVihat(); + autopilot.update_val(rpy, -palt, pihat, vihat); autopilot.turn(); autopilot.keep_alt(); autopilot.return_val(roll_obj, pitch_obj, dT_obj);