diff --git a/flight/Libraries/alarms.c b/flight/Libraries/alarms.c
index bf9f10efeb..2ba464b671 100644
--- a/flight/Libraries/alarms.c
+++ b/flight/Libraries/alarms.c
@@ -282,6 +282,7 @@ static const char config_error_names[][15] = {
[SYSTEMALARMS_CONFIGERROR_DUPLICATEPORTCFG] = "CFG:DUP PORT",
[SYSTEMALARMS_CONFIGERROR_NAVFILTER] = "CFG:NAVFILTER",
[SYSTEMALARMS_CONFIGERROR_UNSAFETOARM] = "CFG:UNSAFE",
+ [SYSTEMALARMS_CONFIGERROR_LQG] = "CFG:LQG",
[SYSTEMALARMS_CONFIGERROR_UNDEFINED] = "CFG:UNDEF",
[SYSTEMALARMS_CONFIGERROR_NONE] = {0},
};
diff --git a/flight/Libraries/math/lqg.c b/flight/Libraries/math/lqg.c
new file mode 100644
index 0000000000..f6f43ad5d9
--- /dev/null
+++ b/flight/Libraries/math/lqg.c
@@ -0,0 +1,520 @@
+/**
+ ******************************************************************************
+ * @addtogroup Libraries Libraries
+ * @{
+ * @addtogroup FlightMath math support libraries
+ * @{
+ *
+ * @file lqg.c
+ * @author dRonin, http://dronin.org, Copyright (C) 2018
+ * @brief LQG Control algorithm
+ *
+ * @see The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, see
+ */
+
+#include "lqg.h"
+
+/*
+ References
+ =====================================================================================================
+
+ General idea
+
+ * Linear Gaussian Quadratic Control System (James Cotton (peabody124), Taulabs)
+ http://buildandcrash.blogspot.be/2016/10/linear-gaussian-quadratic-control-system.html
+
+ Covariance calculation for LQR, instead of DARE
+
+ * ECE5530: Multivariable Control Systems II (Dr. Gregory L. Plett)
+ http://mocha-java.uccs.edu/ECE5530/ECE5530-CH03.pdf
+ (Page 34)
+
+*/
+
+#define RTKF_SOLUTION_LIMIT 150
+#define LQR_SOLUTION_LIMIT 1000
+
+/* Bullshit to quickly make copypasta of MATLAB answers work. */
+#define P00 P[0][0]
+#define P10 P[1][0]
+#define P20 P[2][0]
+#define P01 P[0][1]
+#define P11 P[1][1]
+#define P21 P[2][1]
+#define P02 P[0][2]
+#define P12 P[1][2]
+#define P22 P[2][2]
+
+#define A00 A[0][0]
+#define A10 A[1][0]
+#define A20 A[2][0]
+#define A01 A[0][1]
+#define A11 A[1][1]
+#define A21 A[2][1]
+#define A02 A[0][2]
+#define A12 A[1][2]
+#define A22 A[2][2]
+
+#define Q00 Q[0][0]
+#define Q10 Q[1][0]
+#define Q20 Q[2][0]
+#define Q01 Q[0][1]
+#define Q11 Q[1][1]
+#define Q21 Q[2][1]
+#define Q02 Q[0][2]
+#define Q12 Q[1][2]
+#define Q22 Q[2][2]
+
+#define B0 B[0]
+#define B1 B[1]
+#define B2 B[2]
+
+#define X0 X[0]
+#define X1 X[1]
+#define X2 X[2]
+
+#define K0 K[0]
+#define K1 K[1]
+#define K2 K[2]
+
+/*
+ Kalman covariance cycle.
+
+ P = APA' + Q
+ K = PH' (R+HPH')^-1
+ P = (I-KH) P
+*/
+void rtkf_calculate_covariance_3x3(float A[3][3], float K[3], float P[3][3], float Q[3][3], float R)
+{
+ float nP[3][3];
+
+ nP[0][0] = Q00 + A00*(A00*P00 + A01*P10 + A02*P20) + A01*(A00*P01 + A01*P11 + A02*P21) + A02*(A00*P02 + A01*P12 + A02*P22);
+ nP[0][1] = A11*(A00*P01 + A01*P11 + A02*P21) + A12*(A00*P02 + A01*P12 + A02*P22);
+ nP[0][2] = A22*(A00*P02 + A01*P12 + A02*P22);
+
+ nP[1][0] = A00*(A11*P10 + A12*P20) + A01*(A11*P11 + A12*P21) + A02*(A11*P12 + A12*P22);
+ nP[1][1] = Q11 + A11*(A11*P11 + A12*P21) + A12*(A11*P12 + A12*P22);
+ nP[1][2] = A22*(A11*P12 + A12*P22);
+
+ nP[2][0] = A22*(A00*P20 + A01*P21 + A02*P22);
+ nP[2][1] = A22*(A11*P21 + A12*P22);
+ nP[2][2] = P22*A22*A22 + Q22;
+
+ memcpy(P, nP, sizeof(float)*9);
+
+ float S = P00 + R;
+
+ K0 = P00/S;
+ K1 = P10/S;
+ K2 = P20/S;
+
+ P10 = P10 - K1*P00;
+ P11 = P11 - K1*P01;
+ P12 = P12 - K1*P02;
+
+ P20 = P20 - K2*P00;
+ P21 = P21 - K2*P01;
+ P22 = P22 - K2*P02;
+
+ P00 = P00 - K0*P00;
+ P01 = P01 - K0*P01;
+ P02 = P02 - K0*P02;
+ }
+
+struct rtkf_state {
+
+ int solver_iterations;
+
+ float A[3][3];
+ float B[3];
+ float K[3];
+ float P[3][3];
+ float Q[3][3];
+ float R;
+ float X[3];
+
+ float biaslim;
+
+};
+
+/*
+ Repeat the Kalman covariance cycle for specified amount of iterations.
+
+ Tests show that 50 iterations should be plenty to become stable.
+*/
+ void rtkf_stabilize_covariance(rtkf_t rtkf, int iterations)
+ {
+ PIOS_Assert(rtkf);
+ for (int i = 0; i < iterations; i++) {
+ rtkf_calculate_covariance_3x3(rtkf->A, rtkf->K, rtkf->P, rtkf->Q, rtkf->R);
+ }
+ rtkf->solver_iterations += iterations;
+ }
+
+/*
+ Kalman prediction
+
+ X_k+1 = AX_k + Bu_k + K(y - HX)
+*/
+void rtkf_prediction_step(float A[3][3], float B[3], float K[3], float X[3], float signal, float input)
+{
+ float nX0, nX1, nX2;
+
+ nX0 = B0*input + A00*X0 + A01*X1 + A02*X2;
+ nX1 = B1*input + A11*X1 + A12*X2;
+ nX2 = B2*input + A22*X2;
+
+ X0 = nX0 - K0*(nX0 - signal);
+ X1 = nX1 - K1*(nX0 - signal);
+ float in2 = input*input;
+ X2 = (nX2 - K2*(nX0 - signal)) * bound_min_max(1.0f - in2*in2, 0, 1.0f);
+}
+
+void rtkf_predict_axis(rtkf_t rtkf, float signal, float input, float Xout[3])
+{
+ rtkf_prediction_step(rtkf->A, rtkf->B, rtkf->K, rtkf->X, signal, input);
+
+ rtkf->X2 = bound_sym(rtkf->X2, rtkf->biaslim);
+
+ Xout[0] = rtkf->X0;
+ Xout[1] = rtkf->X1;
+ Xout[2] = rtkf->X2;
+}
+
+/*
+ Rate-torque Kalman filter system.
+
+ A = [ 1 Beta*(tau-tau*e^(-Ts/tau)) -Ts*Beta+Beta*(tau-tau*e^(-Ts/tau)) ]
+ [ 0 e^(-Ts/tau) e^(-Ts/tau)-1 ]
+ [ 0 0 1 ]
+
+ B = [ Ts*Beta-Beta*(tau-tau*e^(-Ts/tau)) ]
+ [ 1-e^(-Ts/tau) ]
+ [ 0 ]
+*/
+void rtkf_initialize_matrices_int(float A[3][3], float B[3], float beta, float tau, float Ts)
+{
+ A00 = 1;
+ A01 = beta*(tau - tau*expf(-Ts/tau));
+ A02 = beta*(tau - tau*expf(-Ts/tau)) - Ts*beta;
+ A11 = expf(-Ts/tau);
+ A12 = expf(-Ts/tau) - 1;
+ A22 = 1;
+
+ B0 = Ts*beta - beta*(tau - tau*expf(-Ts/tau));
+ B1 = 1 - expf(-Ts/tau);
+ B2 = 0;
+}
+
+/*
+ Q matrix set by experimentation.
+
+ R = 1000 seems a workable value for raw gyro input.
+*/
+rtkf_t rtkf_create(float beta, float tau, float Ts, float R, float q1, float q2, float q3, float biaslim)
+{
+ struct rtkf_state *state = PIOS_malloc_no_dma(sizeof(*state));
+ PIOS_Assert(state);
+ memset(state, 0, sizeof(*state));
+
+ rtkf_initialize_matrices_int(state->A, state->B, expf(beta), tau, Ts);
+ state->Q00 = q1;
+ state->Q11 = q2;
+ state->Q22 = q3;
+ state->R = R;
+ state->biaslim = biaslim;
+
+ return state;
+}
+
+bool rtkf_is_solved(rtkf_t rtkf)
+{
+ return rtkf->solver_iterations >= RTKF_SOLUTION_LIMIT;
+}
+
+/*
+ LQR covariance cycle.
+
+ P = A'PA - A'PB (R+B'PB)^-1 B'PA + Q
+*/
+void lqr_calculate_covariance_2x2(float A[2][2], float B[2], float P[2][2], float Q[2][2])
+{
+ float nP[2][2];
+ const float R = 1;
+
+ nP[0][0] = (Q00*R + A00*A00*P00*R + B0*B0*P00*Q00 + B1*B1*P11*Q00 +
+ A10*A10*P11*R + A00*A00*B1*B1*P00*P11 - A00*A00*B1*B1*P01*P10 +
+ A10*A10*B0*B0*P00*P11 - A10*A10*B0*B0*P01*P10 + A00*A10*P01*R +
+ A00*A10*P10*R + B0*B1*P01*Q00 + B0*B1*P10*Q00 - 2*A00*A10*B0*B1*P00*P11 +
+ 2*A00*A10*B0*B1*P01*P10)
+ /
+ (R + B0*B0*P00 + B1*B1*P11 + B0*B1*P01 + B0*B1*P10);
+
+ nP[1][0] = (A00*A01*P00*R + A00*A11*P01*R + A01*A10*P10*R + A10*A11*P11*R +
+ A00*A01*B1*B1*P00*P11 - A00*A01*B1*B1*P01*P10 + A10*A11*B0*B0*P00*P11 -
+ A10*A11*B0*B0*P01*P10 - A00*A11*B0*B1*P00*P11 + A00*A11*B0*B1*P01*P10 -
+ A01*A10*B0*B1*P00*P11 + A01*A10*B0*B1*P01*P10)
+ /
+ (R + B0*B0*P00 + B1*B1*P11 + B0*B1*P01 + B0*B1*P10);
+
+ nP[0][1] = (A00*A01*P00*R + A01*A10*P01*R + A00*A11*P10*R + A10*A11*P11*R +
+ A00*A01*B1*B1*P00*P11 - A00*A01*B1*B1*P01*P10 + A10*A11*B0*B0*P00*P11 -
+ A10*A11*B0*B0*P01*P10 - A00*A11*B0*B1*P00*P11 + A00*A11*B0*B1*P01*P10 -
+ A01*A10*B0*B1*P00*P11 + A01*A10*B0*B1*P01*P10)
+ /
+ (R + B0*B0*P00 + B1*B1*P11 + B0*B1*P01 + B0*B1*P10);
+
+ nP[1][1] = (Q11*R + A01*A01*P00*R + B0*B0*P00*Q11 + A11*A11*P11*R + B1*B1*P11*Q11 +
+ A01*A01*B1*B1*P00*P11 - A01*A01*B1*B1*P01*P10 + A11*A11*B0*B0*P00*P11 -
+ A11*A11*B0*B0*P01*P10 + A01*A11*P01*R + A01*A11*P10*R + B0*B1*P01*Q11 +
+ B0*B1*P10*Q11 - 2*A01*A11*B0*B1*P00*P11 + 2*A01*A11*B0*B1*P01*P10)
+ /
+ (R + B0*B0*P00 + B1*B1*P11 + B0*B1*P01 + B0*B1*P10);
+
+ P00 = nP[0][0];
+ P01 = nP[0][1];
+ P10 = nP[1][0];
+ P11 = nP[1][1];
+}
+
+struct lqr_state {
+
+ int solver_iterations;
+
+ float A[2][2];
+ float B[2];
+ float K[2];
+ float P[2][2];
+ float Q[2][2];
+ float u;
+
+ float beta;
+ float tau;
+
+};
+
+/*
+ Calculate gains for LQR.
+
+ K = (R + B'PB)^-1 B'PA
+*/
+void lqr_calculate_gains_int(float A[2][2], float B[2], float P[2][2], float K[2])
+{
+ const float R = 1;
+
+ K[0] = (A00*(B0*P00 + B1*P10) + A10*(B0*P01 + B1*P11))/(R + B0*(B0*P00 + B1*P10) + B1*(B0*P01 + B1*P11));
+ K[1] = (A01*(B0*P00 + B1*P10) + A11*(B0*P01 + B1*P11))/(R + B0*(B0*P00 + B1*P10) + B1*(B0*P01 + B1*P11));
+}
+
+/*
+ Repeat the LQR covariance cycle for as much iterations needed.
+
+ Discrete algrebraic Riccati "solver" for dumb people.
+
+ Experimentation shows that it needs at least 700-800 iterations to become stable. Stabilizing
+ two rate-controller LQR systems (say for roll and pitch axis) with 1000 iterations works
+ within a fraction of a second on an STM32F405.
+
+ The end results pretty much match covariances calculated with dare() in MATLAB. Calculated
+ gains from this also match what dlqr() spits out.
+
+ Trying peabody124's old branch last year, his proper DARE solver using Eigen took the best of
+ five seconds. It was a tri-state LQR though.
+
+ Changing the Q state weight matrix in middle operation usually seems to restabilize within
+ a 100 cycles, so TxPID for tuning might qualify.
+*/
+void lqr_stabilize_covariance(lqr_t lqr, int iterations)
+{
+ PIOS_Assert(lqr);
+
+ for (int i = 0; i < iterations; i++) {
+ lqr_calculate_covariance_2x2(lqr->A, lqr->B, lqr->P, lqr->Q);
+ }
+
+ lqr_calculate_gains_int(lqr->A, lqr->B, lqr->P, lqr->K);
+
+ lqr->solver_iterations += iterations;
+}
+
+bool lqr_is_solved(lqr_t lqr)
+{
+ return lqr->solver_iterations >= LQR_SOLUTION_LIMIT;
+}
+
+/*
+ LQR rate controller.
+
+ A = [ 1 tau*Beta*(1-e^(-Ts/tau)) ]
+ [ 0 e^(-Ts/tau) ]
+
+ B = [ Ts*Beta-tau*Beta*(1-e^(-Ts/tau)) ]
+ [ 1-e^(-Ts/tau) ]
+*/
+void lqr_initialize_matrices_int(float A[2][2], float B[2], float beta, float tau, float Ts)
+{
+ A00 = 1;
+ A01 = -beta*tau*(expf(-Ts/tau) - 1);
+ A10 = 0;
+ A11 = expf(-Ts/tau);
+
+ B0 = Ts*beta + beta*tau*(expf(-Ts/tau) - 1);
+ B1 = 1 - expf(-Ts/tau);
+}
+
+/*
+ Q1 and Q2 in the state weighting matrix penalizes rate and torque.
+
+ Need to talk to peabody124 about the difference in magnitude. Q2 uses frame invariance idea.
+
+ Current workable values for 5" miniquads seems to be Q1 = 0.00001, Q2 = 0.00013333.
+*/
+lqr_t lqr_create(float beta, float tau, float Ts, float q1, float q2)
+{
+ struct lqr_state *state = PIOS_malloc_no_dma(sizeof(*state));
+ PIOS_Assert(state);
+ memset(state, 0, sizeof(*state));
+
+ lqr_initialize_matrices_int(state->A, state->B, expf(beta), tau, Ts);
+ state->Q00 = q1;
+ state->Q11 = q2*expf(beta);
+
+ state->beta = beta;
+ state->tau = tau;
+
+ return state;
+}
+
+void lqr_update(lqr_t lqr, float q1, float q2)
+{
+ PIOS_Assert(lqr);
+
+ lqr->Q00 = q1;
+ lqr->Q11 = q2*expf(lqr->beta);
+ lqr->solver_iterations = 0;
+}
+
+void lqr_get_gains(lqr_t lqr, float K[2])
+{
+ PIOS_Assert(lqr);
+ K[0] = lqr->K[0];
+ K[1] = lqr->K[1];
+}
+
+
+struct lqg_state {
+
+ int axis;
+
+ struct lqr_state *lqr;
+ struct rtkf_state *rtkf;
+
+};
+
+/*
+ Estimate the system state, i.e. presumed true rate and torque, then
+ feed it into the LQR.
+
+ xr = x - setpoint
+
+ u = -K*xr + bias
+
+ u gets clamped to -1..1 because that's the actuator range. Might also prevent the
+ RTKF to go overboard with predicting, when the LQR is trying to demand a lot from the
+ actuators.
+*/
+float lqg_controller(lqg_t lqg, float signal, float setpoint)
+{
+ lqr_t lqr = lqg->lqr;
+ rtkf_t rtkf = lqg->rtkf;
+
+ float x_est[3]; /* Rate, Torque, Bias */
+ rtkf_predict_axis(rtkf, signal, lqr->u, x_est);
+
+ float xr0 = x_est[0] - setpoint;
+
+ float u = x_est[2] - lqr->K0 * xr0 - lqr->K1 * x_est[1];
+ if (u < -1) u = -1;
+ else if (u > 1) u = 1;
+
+ lqr->u = u;
+
+ return u;
+}
+
+lqg_t lqg_create(rtkf_t rtkf, lqr_t lqr)
+{
+ PIOS_Assert(rtkf);
+ PIOS_Assert(lqr);
+
+ struct lqg_state *state = PIOS_malloc_no_dma(sizeof(*state));
+ PIOS_Assert(state);
+ memset(state, 0, sizeof(*state));
+
+ state->rtkf = rtkf;
+ state->lqr = lqr;
+
+ return state;
+}
+
+void lqg_get_rtkf_state(lqg_t lqg, float *rate, float *torque, float *bias)
+{
+ *rate = lqg->rtkf->X[0];
+ *torque = lqg->rtkf->X[1];
+ *bias = lqg->rtkf->X[2];
+}
+
+void lqg_set_x0(lqg_t lqg, float x0)
+{
+ lqg->rtkf->X[0] = x0;
+ lqg->rtkf->X[1] = 0;
+ lqg->rtkf->X[2] = 0;
+}
+
+bool lqg_is_solved(lqg_t lqg)
+{
+ return lqg != NULL && lqr_is_solved(lqg->lqr) && rtkf_is_solved(lqg->rtkf);
+}
+
+rtkf_t lqg_get_rtkf(lqg_t lqg)
+{
+ return lqg ? lqg->rtkf : NULL;
+}
+
+lqr_t lqg_get_lqr(lqg_t lqg)
+{
+ return lqg ? lqg->lqr : NULL;
+}
+
+void lqg_run_covariance(lqg_t lqg, int iter)
+{
+ rtkf_t rtkf = lqg_get_rtkf(lqg);
+ lqr_t lqr = lqg_get_lqr(lqg);
+ PIOS_Assert(rtkf);
+ PIOS_Assert(lqr);
+
+ if (!rtkf_is_solved(rtkf))
+ rtkf_stabilize_covariance(rtkf, iter);
+ if (!lqr_is_solved(lqr))
+ lqr_stabilize_covariance(lqr, iter);
+}
+
+/**
+ * @}
+ * @}
+ */
diff --git a/flight/Libraries/math/lqg.h b/flight/Libraries/math/lqg.h
new file mode 100644
index 0000000000..2dd7b7a5a7
--- /dev/null
+++ b/flight/Libraries/math/lqg.h
@@ -0,0 +1,69 @@
+/**
+ ******************************************************************************
+ * @addtogroup Libraries Libraries
+ * @{
+ * @addtogroup FlightMath math support libraries
+ * @{
+ *
+ * @file lqg.h
+ * @author dRonin, http://dronin.org, Copyright (C) 2018
+ * @brief LQG Control algorithm
+ *
+ * @see The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, see
+ */
+
+#ifndef LQG_H
+#define LQG_H
+
+#include
+#include
+
+typedef struct rtkf_state* rtkf_t;
+typedef struct lqr_state* lqr_t;
+
+typedef struct lqg_state* lqg_t;
+
+extern rtkf_t rtkf_create(float beta, float tau, float Ts, float R, float Q1, float Q2, float Q3, float biaslim);
+extern void rtkf_stabilize_covariance(rtkf_t rtkf, int iterations);
+extern bool rtkf_is_solved(rtkf_t rtkf);
+
+extern lqr_t lqr_create(float beta, float tau, float Ts, float q1, float q2);
+extern void lqr_stabilize_covariance(lqr_t lqr, int iterations);
+extern bool lqr_is_solved(lqr_t lqr);
+
+extern void lqr_update(lqr_t lqr, float q1, float q2);
+extern void lqr_get_gains(lqr_t lqg, float K[2]);
+
+extern lqg_t lqg_create(rtkf_t rtkf, lqr_t lqr);
+extern bool lqg_is_solved(lqg_t lqg);
+extern lqr_t lqg_get_lqr(lqg_t lqg);
+extern rtkf_t lqg_get_rtkf(lqg_t lqg);
+
+extern void lqg_run_covariance(lqg_t lqg, int iter);
+
+extern float lqg_controller(lqg_t lqg, float signal, float setpoint);
+
+extern void lqg_set_x0(lqg_t lqq, float x0);
+void lqg_get_rtkf_state(lqg_t lqg, float *rate, float *torque, float *bias);
+
+#endif // LQG_H
+
+/**
+ * @}
+ * @}
+ */
diff --git a/flight/Libraries/sanitycheck.c b/flight/Libraries/sanitycheck.c
index 613e341628..a5ebb71315 100644
--- a/flight/Libraries/sanitycheck.c
+++ b/flight/Libraries/sanitycheck.c
@@ -40,6 +40,7 @@
#include "stateestimation.h"
#include "systemalarms.h"
#include "systemsettings.h"
+#include "systemident.h"
#include "pios_sensors.h"
@@ -62,6 +63,21 @@ static int32_t check_stabilization_rates();
//! Check the system is safe for autonomous flight
static int32_t check_safe_autonomous();
+bool lqg_sysident_check()
+{
+ if (SystemIdentHandle()) {
+ SystemIdentData si;
+ SystemIdentGet(&si);
+
+ if (si.Beta[SYSTEMIDENT_BETA_ROLL] < 6 || si.Tau[SYSTEMIDENT_TAU_ROLL] < 0.001f ||
+ si.Beta[SYSTEMIDENT_BETA_PITCH] < 6 || si.Tau[SYSTEMIDENT_TAU_PITCH] < 0.001f ||
+ si.Beta[SYSTEMIDENT_BETA_YAW] < 6 || si.Tau[SYSTEMIDENT_TAU_YAW] < 0.001f) {
+ return false;
+ }
+ }
+ return true;
+}
+
/**
* Run a preflight check over the hardware configuration
* and currently active modules
@@ -123,6 +139,12 @@ int32_t configuration_check()
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_FAILSAFE:
// always ok
break;
+ case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_LQG:
+ case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_LQGLEVELING:
+ if (!lqg_sysident_check()) {
+ error_code = SYSTEMALARMS_CONFIGERROR_LQG;
+ }
+ break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED1:
ManualControlSettingsStabilization1ThrustGet(&thrust_mode);
error_code = (error_code == SYSTEMALARMS_CONFIGERROR_NONE) ? check_stabilization_settings(1, multirotor, thrust_mode) : error_code;
@@ -230,6 +252,13 @@ static int32_t check_stabilization_settings(int index, bool multirotor,
(!PIOS_Modules_IsEnabled(PIOS_MODULE_AUTOTUNE))) {
return SYSTEMALARMS_CONFIGERROR_AUTOTUNE;
}
+
+ /* Throw an error if LQG modes are configured without system identification data. */
+ if (modes[i] == MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_LQG ||
+ modes[i] == MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_ATTITUDELQG) {
+ if (!lqg_sysident_check())
+ return SYSTEMALARMS_CONFIGERROR_LQG;
+ }
}
// POI mode is only valid for YAW in the case it is enabled and camera stab is running
@@ -281,6 +310,12 @@ static int32_t check_safe_to_arm()
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD:
break;
+ case FLIGHTSTATUS_FLIGHTMODE_LQG:
+ case FLIGHTSTATUS_FLIGHTMODE_LQGLEVELING:
+ if (!lqg_sysident_check()) {
+ return SYSTEMALARMS_CONFIGERROR_LQG;
+ }
+ break;
case FLIGHTSTATUS_FLIGHTMODE_FAILSAFE:
/* for failsafe, we don't want to prevent
@@ -381,6 +416,7 @@ void set_config_error(SystemAlarmsConfigErrorOptions error_code)
case SYSTEMALARMS_CONFIGERROR_STABILIZATION:
case SYSTEMALARMS_CONFIGERROR_UNDEFINED:
case SYSTEMALARMS_CONFIGERROR_UNSAFETOARM:
+ case SYSTEMALARMS_CONFIGERROR_LQG:
severity = SYSTEMALARMS_ALARM_ERROR;
break;
}
diff --git a/flight/Modules/CharacterOSD/panel.c b/flight/Modules/CharacterOSD/panel.c
index 21b50eea07..010315c4a7 100644
--- a/flight/Modules/CharacterOSD/panel.c
+++ b/flight/Modules/CharacterOSD/panel.c
@@ -217,6 +217,12 @@ static void FLIGHTMODE_update(charosd_state_t state, uint8_t x, uint8_t y)
// There are many sub modes here that could be filled in.
mode = "TAB";
break;
+ case FLIGHTSTATUS_FLIGHTMODE_LQG:
+ mode = "LQGR";
+ break;
+ case FLIGHTSTATUS_FLIGHTMODE_LQGLEVELING:
+ mode = "LQGA";
+ break;
}
draw_rect(state, x, y, 6, 3, 0, 0);
diff --git a/flight/Modules/Logging/logging.c b/flight/Modules/Logging/logging.c
index 346c492cdb..1e2945c209 100644
--- a/flight/Modules/Logging/logging.c
+++ b/flight/Modules/Logging/logging.c
@@ -66,6 +66,8 @@
#include "systemident.h"
#include "velocityactual.h"
#include "waypointactive.h"
+#include "rtkfestimate.h"
+#include "lqgsolution.h"
#include "pios_bl_helper.h"
#include "pios_streamfs_priv.h"
@@ -645,6 +647,14 @@ static void register_default_profile()
if (GPSSatellitesHandle()) {
UAVObjConnectCallbackThrottled(GPSSatellitesHandle(), obj_updated_callback, NULL, EV_UPDATED | EV_UNPACKED, 500 * min_period);
}
+
+ // Log LQG data
+ if (RTKFEstimateHandle()) {
+ UAVObjConnectCallbackThrottled(RTKFEstimateHandle(), obj_updated_callback, NULL, EV_UPDATED | EV_UNPACKED, 2 * min_period);
+ }
+ if (LQGSolutionHandle()) {
+ UAVObjConnectCallbackThrottled(LQGSolutionHandle(), obj_updated_callback, NULL, EV_UPDATED | EV_UNPACKED, 100 * min_period);
+ }
}
diff --git a/flight/Modules/ManualControl/inc/transmitter_control.h b/flight/Modules/ManualControl/inc/transmitter_control.h
index a2c2188b5f..d01d7c9e3d 100644
--- a/flight/Modules/ManualControl/inc/transmitter_control.h
+++ b/flight/Modules/ManualControl/inc/transmitter_control.h
@@ -52,7 +52,9 @@
((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \
-((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \
+((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) &&\
+((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_LQG == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_LQG) &&\
+((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_ATTITUDELQG == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDELQG) \
)
#define assumptions3 ( \
@@ -61,7 +63,9 @@
((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \
-((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \
+((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) &&\
+((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_LQG == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_LQG) &&\
+((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_ATTITUDELQG == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDELQG) \
)
#define assumptions5 ( \
@@ -70,7 +74,9 @@
((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \
-((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \
+((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) &&\
+((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_LQG == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_LQG) &&\
+((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_ATTITUDELQG == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDELQG) \
)
#define assumptions_flightmode ( \
diff --git a/flight/Modules/ManualControl/transmitter_control.c b/flight/Modules/ManualControl/transmitter_control.c
index 1257269d73..d9f3c0360a 100644
--- a/flight/Modules/ManualControl/transmitter_control.c
+++ b/flight/Modules/ManualControl/transmitter_control.c
@@ -561,6 +561,10 @@ int32_t transmitter_control_select(bool reset_controller)
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
case FLIGHTSTATUS_FLIGHTMODE_FAILSAFE:
case FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE:
+ case FLIGHTSTATUS_FLIGHTMODE_LQG:
+ case FLIGHTSTATUS_FLIGHTMODE_LQGLEVELING:
+ update_stabilization_desired(&cmd, &settings);
+ break;
case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD:
update_stabilization_desired(&cmd, &settings);
break;
@@ -1007,6 +1011,7 @@ static inline float scale_stabilization(StabilizationSettingsData *stabSettings,
case SHAREDDEFS_STABILIZATIONMODE_RATE:
case SHAREDDEFS_STABILIZATIONMODE_WEAKLEVELING:
case SHAREDDEFS_STABILIZATIONMODE_AXISLOCK:
+ case SHAREDDEFS_STABILIZATIONMODE_LQG:
cmd = expoM(cmd, stabSettings->RateExpo[axis],
stabSettings->RateExponent[axis]*0.1f);
return cmd * stabSettings->ManualRate[axis];
@@ -1019,6 +1024,7 @@ static inline float scale_stabilization(StabilizationSettingsData *stabSettings,
return cmd;
case SHAREDDEFS_STABILIZATIONMODE_SYSTEMIDENT:
case SHAREDDEFS_STABILIZATIONMODE_ATTITUDE:
+ case SHAREDDEFS_STABILIZATIONMODE_ATTITUDELQG:
return cmd * stabSettings->MaxLevelAngle[axis];
case SHAREDDEFS_STABILIZATIONMODE_HORIZON:
cmd = expo3(cmd, stabSettings->HorizonExpo[axis]);
@@ -1076,6 +1082,15 @@ static void update_stabilization_desired(ManualControlCommandData * manual_contr
const uint8_t SYSTEMIDENT_SETTINGS[3] = { STABILIZATIONDESIRED_STABILIZATIONMODE_SYSTEMIDENT,
STABILIZATIONDESIRED_STABILIZATIONMODE_SYSTEMIDENT,
STABILIZATIONDESIRED_STABILIZATIONMODE_SYSTEMIDENTRATE };
+
+ const uint8_t LQG_SETTINGS[3] = { STABILIZATIONDESIRED_STABILIZATIONMODE_LQG,
+ STABILIZATIONDESIRED_STABILIZATIONMODE_LQG,
+ STABILIZATIONDESIRED_STABILIZATIONMODE_LQG };
+
+ const uint8_t ATTITUDELQG_SETTINGS[3] = { STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDELQG,
+ STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDELQG,
+ STABILIZATIONDESIRED_STABILIZATIONMODE_LQG };
+
const uint8_t * stab_modes = ATTITUDE_SETTINGS;
uint8_t reprojection = STABILIZATIONDESIRED_REPROJECTIONMODE_NONE;
@@ -1137,6 +1152,12 @@ static void update_stabilization_desired(ManualControlCommandData * manual_contr
STABILIZATIONDESIRED_THRUSTMODE_ALTITUDEWITHSTICKSCALING;
break;
#endif
+ case FLIGHTSTATUS_FLIGHTMODE_LQG:
+ stab_modes = LQG_SETTINGS;
+ break;
+ case FLIGHTSTATUS_FLIGHTMODE_LQGLEVELING:
+ stab_modes = ATTITUDELQG_SETTINGS;
+ break;
default:
{
// Major error, this should not occur because only enter this block when one of these is true
diff --git a/flight/Modules/OnScreenDisplay/onscreendisplay.c b/flight/Modules/OnScreenDisplay/onscreendisplay.c
index 6c408dbdea..7555949ef3 100644
--- a/flight/Modules/OnScreenDisplay/onscreendisplay.c
+++ b/flight/Modules/OnScreenDisplay/onscreendisplay.c
@@ -716,6 +716,12 @@ void draw_flight_mode(int x, int y, int xs, int ys, int va, int ha, int flags, i
break;
}
break;
+ case FLIGHTSTATUS_FLIGHTMODE_LQG:
+ write_string("LQG-R", x, y, xs, ys, va, ha, flags, font);
+ break;
+ case FLIGHTSTATUS_FLIGHTMODE_LQGLEVELING:
+ write_string("LQG-L", x, y, xs, ys, va, ha, flags, font);
+ break;
}
}
diff --git a/flight/Modules/Stabilization/stabilization.c b/flight/Modules/Stabilization/stabilization.c
index c781518ea8..c73d2b83f4 100644
--- a/flight/Modules/Stabilization/stabilization.c
+++ b/flight/Modules/Stabilization/stabilization.c
@@ -59,6 +59,9 @@
#include "systemsettings.h"
#include "manualcontrolcommand.h"
#include "vbarsettings.h"
+#include "lqgsettings.h"
+#include "rtkfestimate.h"
+#include "lqgsolution.h"
#include "altitudeholdsettings.h"
#include "altitudeholdstate.h"
@@ -71,6 +74,7 @@
#include "pid.h"
#include "misc_math.h"
#include "smoothcontrol.h"
+#include "lqg.h"
// Sensors subsystem which runs in this task
#include "sensors.h"
@@ -165,6 +169,9 @@ static float altitude_hold_maxdescentrate;
static struct pid pids[PID_MAX];
static smoothcontrol_state rc_smoothing;
+#if defined(STABILIZATION_LQG)
+static lqg_t lqg[MAX_AXES];
+#endif
#ifndef NO_CONTROL_DEADBANDS
struct pid_deadband *deadbands = NULL;
@@ -231,6 +238,15 @@ int32_t StabilizationInitialize()
return -1;
}
+#if defined(STABILIZATION_LQG)
+ if (LQGSettingsInitialize() == -1 ||
+ SystemIdentInitialize() == -1 ||
+ RTKFEstimateInitialize() == -1 ||
+ LQGSolutionInitialize() == -1) {
+ return -1;
+ }
+#endif
+
#if defined(RATEDESIRED_DIAGNOSTICS)
if (RateDesiredInitialize() == -1) {
return -1;
@@ -373,6 +389,7 @@ static void calculate_attitude_errors(uint8_t *axis_mode, float *raw_input,
case STABILIZATIONDESIRED_STABILIZATIONMODE_HORIZON:
case STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING:
case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE:
+ case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDELQG:
case STABILIZATIONDESIRED_STABILIZATIONMODE_SYSTEMIDENT:
break;
default:
@@ -406,6 +423,83 @@ static void calculate_attitude_errors(uint8_t *axis_mode, float *raw_input,
local_attitude_error[YAW] = circular_modulus_deg(local_attitude_error[YAW]);
}
+#if defined(STABILIZATION_LQG)
+static void initialize_lqg_controllers(float dT)
+{
+ if (SystemIdentHandle()) {
+ SystemIdentData sysIdent;
+ SystemIdentGet(&sysIdent);
+
+ LQGSettingsData lqgSettings;
+ LQGSettingsGet(&lqgSettings);
+
+ for (int i = 0; i < MAX_AXES; i++) {
+ if (lqg[i]) {
+ /* Update Q matrix. */
+ lqr_t lqr = lqg_get_lqr(lqg[i]);
+ lqr_update(lqr,
+ lqgSettings.LQR[i == YAW ? LQGSETTINGS_LQR_YAWQ1 : LQGSETTINGS_LQR_Q1],
+ lqgSettings.LQR[i == YAW ? LQGSETTINGS_LQR_YAWQ2 : LQGSETTINGS_LQR_Q2]
+ );
+ } else {
+ /* Initial setup. */
+ float beta = sysIdent.Beta[i];
+ float tau = (sysIdent.Tau[0] + sysIdent.Tau[1]) * 0.5f;
+
+ if (i == YAW)
+ {
+ beta += lqgSettings.LQR[LQGSETTINGS_LQR_YAWBETAOFFSET];
+ }
+
+ if (tau > 0.001f && beta > 6) {
+ rtkf_t rtkf = rtkf_create(beta, tau, dT,
+ lqgSettings.RTKF[i == YAW ? LQGSETTINGS_RTKF_YAWR : LQGSETTINGS_RTKF_R],
+ lqgSettings.RTKF[LQGSETTINGS_RTKF_Q1],
+ lqgSettings.RTKF[LQGSETTINGS_RTKF_Q2],
+ lqgSettings.RTKF[LQGSETTINGS_RTKF_Q3],
+ lqgSettings.RTKF[LQGSETTINGS_RTKF_BIASLIMIT]
+ );
+ lqr_t lqr = lqr_create(beta, tau, dT,
+ lqgSettings.LQR[i == YAW ? LQGSETTINGS_LQR_YAWQ1 : LQGSETTINGS_LQR_Q1],
+ lqgSettings.LQR[i == YAW ? LQGSETTINGS_LQR_YAWQ2 : LQGSETTINGS_LQR_Q2]
+ );
+ lqg[i] = lqg_create(rtkf, lqr);
+ }
+ }
+ }
+ }
+}
+
+static void dump_lqg_solution(lqg_t lqg, int axis)
+{
+ if (lqg_is_solved(lqg)) {
+ LQGSolutionData lqgsol;
+ LQGSolutionGet(&lqgsol);
+
+ lqr_t lqr = lqg_get_lqr(lqg);
+ float K[2];
+ lqr_get_gains(lqr, K);
+
+ switch(axis) {
+ case ROLL:
+ lqgsol.RollK[0] = K[0];
+ lqgsol.RollK[1] = K[1];
+ break;
+ case PITCH:
+ lqgsol.PitchK[0] = K[0];
+ lqgsol.PitchK[1] = K[1];
+ break;
+ case YAW:
+ lqgsol.YawK[0] = K[0];
+ lqgsol.YawK[1] = K[1];
+ break;
+ }
+
+ LQGSolutionSet(&lqgsol);
+ }
+}
+#endif
+
MODULE_HIPRI_INITCALL(StabilizationInitialize, StabilizationStart);
static float calculate_thrust(StabilizationDesiredThrustModeOptions mode,
@@ -610,6 +704,7 @@ static void stabilizationTask(void* parameters)
volatile bool settings_updated = true;
volatile bool flightstatus_updated = true;
+ volatile bool lqgsettings_updated = true;
float *actuatorDesiredAxis = &actuatorDesired.Roll;
float *rateDesiredAxis = &rateDesired.Roll;
@@ -631,6 +726,7 @@ static void stabilizationTask(void* parameters)
*/
PIOS_Assert(!PIOS_SENSORS_IsRegistered(PIOS_SENSOR_BARO));
#endif
+ LQGSettingsConnectCallbackCtx(UAVObjCbSetFlag, &lqgsettings_updated);
smoothcontrol_initialize(&rc_smoothing);
ManualControlCommandConnectCallbackCtx(UAVObjCbSetFlag, smoothcontrol_get_ringer(rc_smoothing));
@@ -744,6 +840,14 @@ static void stabilizationTask(void* parameters)
flightstatus_updated = false;
}
+ if (lqgsettings_updated) {
+#if defined(STABILIZATION_LQG)
+ /* Set up LQG controllers. */
+ initialize_lqg_controllers(dT_expected);
+#endif
+ lqgsettings_updated = false;
+ }
+
StabilizationDesiredGet(&stabDesired);
AttitudeActualGet(&attitudeActual);
GyrosGet(&gyrosData);
@@ -834,6 +938,18 @@ static void stabilizationTask(void* parameters)
uint16_t max_safe_rate = PIOS_SENSORS_GetMaxGyro() * 0.9f;
+#if defined(STABILIZATION_LQG)
+ bool lqg_in_use = false;
+
+ for (int i = 0; i < MAX_AXES; i++) {
+ /* Solve for LQG, if it's configured for an axis. */
+ if (lqg[i] && !lqg_is_solved(lqg[i])) {
+ lqg_run_covariance(lqg[i], 1);
+ dump_lqg_solution(lqg[i], i);
+ }
+ }
+#endif
+
//Run the selected stabilization algorithm on each axis:
for (uint8_t i = 0; i < MAX_AXES; i++) {
// Check whether this axis mode needs to be reinitialized
@@ -847,6 +963,27 @@ static void stabilizationTask(void* parameters)
PIOS_Assert(0); /* Shouldn't happen, per above */
break;
+ case STABILIZATIONDESIRED_STABILIZATIONMODE_LQG:
+#if defined(STABILIZATION_LQG)
+ if (lqg[i] && lqg_is_solved(lqg[i])) {
+ if (reinit) {
+ lqg_set_x0(lqg[i], gyro_filtered[i]);
+ }
+ lqg_in_use = true;
+
+ /* Store to rate desired variable for storing to UAVO */
+ rateDesiredAxis[i] = bound_sym(raw_input[i], settings.ManualRate[i]);
+
+ /* Compute the inner loop */
+ actuatorDesiredAxis[i] = lqg_controller(lqg[i], gyro_filtered[i], rateDesiredAxis[i]);
+ /* The LQG controller bounds data already, but whatever. */
+ actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i], 1.0f);
+ break;
+ }
+#endif
+ /* Fall through to rate, if LQR gains haven't been solved yet, or if
+ LQG wasn't initialized. */
+
case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE:
if(reinit) {
pids[PID_GROUP_RATE + i].iAccumulator = 0;
@@ -934,6 +1071,31 @@ static void stabilizationTask(void* parameters)
break;
+ case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDELQG:
+#if defined(STABILIZATION_LQG)
+ if (lqg[i] && lqg_is_solved(lqg[i])) {
+ if (reinit) {
+ pids[PID_GROUP_ATT + i].iAccumulator = 0;
+ lqg_set_x0(lqg[i], gyro_filtered[i]);
+ }
+ lqg_in_use = true;
+
+ /* Store to rate desired variable for storing to UAVO */
+ // Compute the outer loop
+ rateDesiredAxis[i] = pid_apply(&pids[PID_GROUP_ATT + i], local_attitude_error[i]);
+ rateDesiredAxis[i] = bound_sym(rateDesiredAxis[i], settings.MaximumRate[i]);
+
+ /* Compute the inner loop */
+ actuatorDesiredAxis[i] = lqg_controller(lqg[i], gyro_filtered[i], rateDesiredAxis[i]);
+ /* The LQG controller bounds data already, but whatever. */
+ actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i], 1.0f);
+ break;
+ }
+#endif
+ /* Fall through to attitude, if LQR gains haven't been solved yet, or if
+ LQG wasn't initialized. */
+
+
case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE:
if(reinit) {
pids[PID_GROUP_ATT + i].iAccumulator = 0;
@@ -1268,6 +1430,26 @@ static void stabilizationTask(void* parameters)
}
}
+#if defined(STABILIZATION_LQG)
+ /* If there's a LQG controller running, dump the RTKF state for logging. */
+ if (lqg_in_use && (lqg[0] || lqg[1] || lqg[2])) {
+ RTKFEstimateData est = { 0 };
+ if (lqg[0]) {
+ lqg_get_rtkf_state(lqg[0],
+ &est.Rate[RTKFESTIMATE_RATE_ROLL], &est.Torque[RTKFESTIMATE_TORQUE_ROLL], &est.Bias[RTKFESTIMATE_BIAS_ROLL]);
+ }
+ if (lqg[1]) {
+ lqg_get_rtkf_state(lqg[1],
+ &est.Rate[RTKFESTIMATE_RATE_PITCH], &est.Torque[RTKFESTIMATE_TORQUE_PITCH], &est.Bias[RTKFESTIMATE_BIAS_PITCH]);
+ }
+ if (lqg[2]) {
+ lqg_get_rtkf_state(lqg[2],
+ &est.Rate[RTKFESTIMATE_RATE_YAW], &est.Torque[RTKFESTIMATE_TORQUE_YAW], &est.Bias[RTKFESTIMATE_BIAS_YAW]);
+ }
+ RTKFEstimateSet(&est);
+ }
+#endif
+
// Run the smoothing over the throttle stick.
if (actuatorDesired.Thrust > THROTTLE_EPSILON || actuatorDesired.Thrust < -THROTTLE_EPSILON){
smoothcontrol_run_thrust(rc_smoothing, &actuatorDesired.Thrust);
diff --git a/flight/targets/aq32/fw/pios_config.h b/flight/targets/aq32/fw/pios_config.h
index 3a41a3c94f..30101b5f9f 100644
--- a/flight/targets/aq32/fw/pios_config.h
+++ b/flight/targets/aq32/fw/pios_config.h
@@ -32,6 +32,7 @@
#include
/* Major features */
+#define STABILIZATION_LQG
/* Enable/Disable PiOS Modules */
#define PIOS_INCLUDE_FLASH_JEDEC
diff --git a/flight/targets/brain/fw/pios_config.h b/flight/targets/brain/fw/pios_config.h
index 82d9ed4c92..f8b78cbe8d 100644
--- a/flight/targets/brain/fw/pios_config.h
+++ b/flight/targets/brain/fw/pios_config.h
@@ -32,6 +32,7 @@
#include
/* Major features */
+#define STABILIZATION_LQG
/* Enable/Disable PiOS Modules */
#define PIOS_INCLUDE_FLASH_JEDEC
diff --git a/flight/targets/brainre1/fw/Makefile b/flight/targets/brainre1/fw/Makefile
index 53bbf57c20..892e902b2d 100644
--- a/flight/targets/brainre1/fw/Makefile
+++ b/flight/targets/brainre1/fw/Makefile
@@ -63,7 +63,6 @@ OPTMODULES += Storm32Bgc
OPTMODULES += OnScreenDisplay
OPTMODULES += UAVOCrossfireTelemetry
-
# Paths
OPUAVOBJINC = $(OPUAVOBJ)/inc
PIOSINC = $(PIOS)/inc
diff --git a/flight/targets/brainre1/fw/pios_config.h b/flight/targets/brainre1/fw/pios_config.h
index 7d9ebd5e34..7d3190cd69 100644
--- a/flight/targets/brainre1/fw/pios_config.h
+++ b/flight/targets/brainre1/fw/pios_config.h
@@ -37,6 +37,7 @@
#include
/* Major features */
+#define STABILIZATION_LQG
/* Enable/Disable PiOS Modules */
#define PIOS_INCLUDE_FLASH_JEDEC
diff --git a/flight/targets/dtfc/fw/pios_config.h b/flight/targets/dtfc/fw/pios_config.h
index 773f591931..40416b790b 100644
--- a/flight/targets/dtfc/fw/pios_config.h
+++ b/flight/targets/dtfc/fw/pios_config.h
@@ -37,6 +37,7 @@
#include
/* Major features */
+#define STABILIZATION_LQG
/* Enable/Disable PiOS Modules */
#define PIOS_INCLUDE_DMA_CB_SUBSCRIBING_FUNCTION
diff --git a/flight/targets/lux/fw/pios_config.h b/flight/targets/lux/fw/pios_config.h
index fa04813b61..bcc30c7bda 100644
--- a/flight/targets/lux/fw/pios_config.h
+++ b/flight/targets/lux/fw/pios_config.h
@@ -33,6 +33,7 @@
#include
/* Major features */
+#define STABILIZATION_LQG
/* Enable/Disable PiOS Modules */
#define PIOS_INCLUDE_DMA_CB_SUBSCRIBING_FUNCTION
diff --git a/flight/targets/omnibusf3/fw/pios_config.h b/flight/targets/omnibusf3/fw/pios_config.h
index 9d56a9be90..3562b62b5e 100644
--- a/flight/targets/omnibusf3/fw/pios_config.h
+++ b/flight/targets/omnibusf3/fw/pios_config.h
@@ -33,6 +33,7 @@
#include
/* Major features */
+#define STABILIZATION_LQG
/* Enable/Disable PiOS Modules */
#define PIOS_INCLUDE_DMA_CB_SUBSCRIBING_FUNCTION
diff --git a/flight/targets/pikoblx/fw/pios_config.h b/flight/targets/pikoblx/fw/pios_config.h
index 517053155d..338c3620b6 100644
--- a/flight/targets/pikoblx/fw/pios_config.h
+++ b/flight/targets/pikoblx/fw/pios_config.h
@@ -32,6 +32,7 @@
#include
/* Major features */
+#define STABILIZATION_LQG
/* Enable/Disable PiOS Modules */
#define PIOS_INCLUDE_DMA_CB_SUBSCRIBING_FUNCTION
diff --git a/flight/targets/quanton/fw/pios_config.h b/flight/targets/quanton/fw/pios_config.h
index af5ea9370b..bb02caa76c 100644
--- a/flight/targets/quanton/fw/pios_config.h
+++ b/flight/targets/quanton/fw/pios_config.h
@@ -33,6 +33,7 @@
#include
/* Major features */
+#define STABILIZATION_LQG
/* Enable/Disable PiOS Modules */
#define PIOS_INCLUDE_FLASH_JEDEC
diff --git a/flight/targets/revolution/fw/pios_config.h b/flight/targets/revolution/fw/pios_config.h
index 80cac22cc4..426b4b87ba 100644
--- a/flight/targets/revolution/fw/pios_config.h
+++ b/flight/targets/revolution/fw/pios_config.h
@@ -40,6 +40,7 @@
#include
/* Major features */
+#define STABILIZATION_LQG
/* Enable/Disable PiOS Modules */
#define PIOS_INCLUDE_FLASH_JEDEC
diff --git a/flight/targets/seppuku/fw/pios_config.h b/flight/targets/seppuku/fw/pios_config.h
index a9fce22ede..d85f0f2ae7 100644
--- a/flight/targets/seppuku/fw/pios_config.h
+++ b/flight/targets/seppuku/fw/pios_config.h
@@ -37,6 +37,7 @@
#include
/* Major features */
+#define STABILIZATION_LQG
/* Enable/Disable PiOS Modules */
#define PIOS_INCLUDE_I2C
diff --git a/flight/targets/sparky2/fw/pios_config.h b/flight/targets/sparky2/fw/pios_config.h
index ae3388709a..ea63273ad6 100644
--- a/flight/targets/sparky2/fw/pios_config.h
+++ b/flight/targets/sparky2/fw/pios_config.h
@@ -34,6 +34,7 @@
/* Major features */
#define PIOS_INCLUDE_FLASH_JEDEC
+#define STABILIZATION_LQG
/* Enable/Disable PiOS Modules */
#define PIOS_INCLUDE_I2C
diff --git a/flight/targets/sprf3e/fw/pios_config.h b/flight/targets/sprf3e/fw/pios_config.h
index 919bb090a1..7aa00e0c1d 100644
--- a/flight/targets/sprf3e/fw/pios_config.h
+++ b/flight/targets/sprf3e/fw/pios_config.h
@@ -33,6 +33,7 @@
#include
/* Major features */
+#define STABILIZATION_LQG
/* Enable/Disable PiOS Modules */
#define PIOS_INCLUDE_DMA_CB_SUBSCRIBING_FUNCTION
diff --git a/ground/gcs/src/plugins/config/configautotunewidget.cpp b/ground/gcs/src/plugins/config/configautotunewidget.cpp
index 7c0bdf37f0..d8aa1b1a4e 100644
--- a/ground/gcs/src/plugins/config/configautotunewidget.cpp
+++ b/ground/gcs/src/plugins/config/configautotunewidget.cpp
@@ -441,6 +441,21 @@ void ConfigAutotuneWidget::openAutotuneDialog(bool autoOpened,
saveObjectToSD(stabilizationSettings);
+ SystemIdent *systemIdent = SystemIdent::GetInstance(getObjectManager());
+ if (systemIdent) {
+ SystemIdent::DataFields systemIdentData = systemIdent->getData();
+
+ for (int i = 0; i < 3; i++) {
+ systemIdentData.Tau[i] = av.tau[i];
+ systemIdentData.Beta[i] = av.beta[i];
+ }
+
+ systemIdent->setData(systemIdentData);
+ systemIdent->updated();
+
+ saveObjectToSD(systemIdent);
+ }
+
if (pg->shareBox->isChecked()) {
persistShareForm(pg);
diff --git a/ground/gcs/src/plugins/systemhealth/html/SystemConfiguration-Error-LQG.html b/ground/gcs/src/plugins/systemhealth/html/SystemConfiguration-Error-LQG.html
new file mode 100644
index 0000000000..9a0a0744b1
--- /dev/null
+++ b/ground/gcs/src/plugins/systemhealth/html/SystemConfiguration-Error-LQG.html
@@ -0,0 +1,11 @@
+
+
+
+
+
+
+
+
System Configuration: Error
+
At least one LQG-controller based flight mode has been configured, but no valid system identification has been data found. Please rerun the autotune wizard, or if necessary, or not done yet, the complete autotune procedure.