fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-15 18:33:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Controller and estimator updates

Browse Source
This commit is contained in:
daregger
2012-10-16 18:02:28 +02:00
parent b50bc7798a
commit 32e586d4b7
38 changed files with 327 additions and 268 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/attitude_estimator_ekf/Makefile
Executable → Regular
+1
View File
@@ -48,6 +48,7 @@ CSRCS = attitude_estimator_ekf_main.c \
codegen/rtGetNaN.c \
codegen/norm.c \
codegen/diag.c \
codegen/power.c \
codegen/cross.c
apps/attitude_estimator_ekf/attitudeKalmanfilter.m
Executable → Regular
View File
apps/attitude_estimator_ekf/attitudeKalmanfilter.prj
Executable → Regular
View File
apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c
Executable → Regular
View File
apps/attitude_estimator_ekf/attitude_estimator_ekf_params.h
Executable → Regular
View File
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c
Executable → Regular
+82 -85
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Sat Oct 13 17:51:09 2012
* C source code generated on: Tue Oct 16 15:27:58 2012
*
*/
@@ -16,6 +16,7 @@
#include "eye.h"
#include "mrdivide.h"
#include "diag.h"
#include "power.h"
/* Type Definitions */
@@ -74,11 +75,14 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
eulerAngles[3], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T
P_aposteriori[144])
{
real32_T a[12];
real32_T b_a[12];
int32_T i;
real32_T Q[144];
real32_T O[9];
real_T dv0[9];
real32_T a[9];
int32_T i;
real32_T b_a[9];
real32_T c_a[9];
real32_T d_a[9];
real32_T x_n_b[3];
real32_T b_x_aposteriori_k[3];
real32_T m_n_b[3];
@@ -91,69 +95,55 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
int32_T i1;
real32_T P_apriori[144];
real32_T f0;
static const real32_T fv0[144] = { 0.1F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F,
0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.1F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F,
0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.1F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F,
0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0E-10F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F,
0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0E-10F, 0.0F, 0.0F, 0.0F, 0.0F,
0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0E-10F, 0.0F, 0.0F, 0.0F,
0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0001F, 0.0001F, 0.0F, 0.1F, 0.0F, 0.0F,
0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0001F, 0.0001F, 0.0F, 0.0F, 0.1F, 0.0F,
0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.1F, 0.0F,
0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0001F, 0.0001F, 0.0F, 0.0F, 0.0F, 0.0F, 0.1F,
0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0001F, 0.0001F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F,
0.1F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F,
0.1F };
real32_T R[81];
real32_T b_P_apriori[108];
static const int8_T iv0[108] = { 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0,
0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
static const int8_T iv0[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 };
real32_T K_k[108];
static const int8_T iv1[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0,
0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0,
0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0,
1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0,
0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 };
real32_T fv1[81];
real32_T fv0[81];
real32_T c_P_apriori[36];
static const int8_T iv2[36] = { 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,
1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
static const int8_T iv2[36] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
real32_T fv2[36];
static const int8_T iv3[36] = { 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0,
real32_T fv1[36];
static const int8_T iv3[36] = { 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
real32_T S_k[36];
real32_T d_P_apriori[72];
static const int8_T iv4[72] = { 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,
1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
static const int8_T iv4[72] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
1, 0, 0, 0 };
real32_T b_K_k[72];
static const int8_T iv5[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0,
0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,
0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0 };
real32_T b_r[6];
static const int8_T iv6[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0,
0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0,
0, 0, 0, 1 };
static const int8_T iv7[72] = { 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,
1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
static const int8_T iv7[72] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 1 };
real32_T fv3[6];
real32_T fv2[6];
real32_T b_z[6];
/* Extended Attitude Kalmanfilter */
@@ -170,19 +160,26 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
/* coder.varsize('udpIndVect', [9,1], [1,0]) */
/* udpIndVect=find(updVect); */
/* process and measurement noise covariance matrix */
/* Q = diag(q.^2*dt); */
/* 'attitudeKalmanfilter:29' Q=[1e-1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; */
/* 'attitudeKalmanfilter:30' 0, 1e-1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; */
/* 'attitudeKalmanfilter:31' 0, 0, 1e-1, 0, 0, 0, 0, 0, 0, 0, 0, 0; */
/* 'attitudeKalmanfilter:32' 0, 0, 0, 1e-10, 0, 0, 1e-4, 1e-4, 0, 1e-4, 1e-4, 0; */
/* 'attitudeKalmanfilter:33' 0, 0, 0, 0, 1e-10, 0, 1e-4, 1e-4, 0, 1e-4, 1e-4, 0; */
/* 'attitudeKalmanfilter:34' 0, 0, 0, 0, 0, 1e-10, 0, 0, 0, 0, 0, 0; */
/* 'attitudeKalmanfilter:35' 0, 0, 0, 0, 0, 0, 1e-1, 0, 0, 0, 0, 0; */
/* 'attitudeKalmanfilter:36' 0, 0, 0, 0, 0, 0, 0, 1e-1, 0, 0, 0, 0; */
/* 'attitudeKalmanfilter:37' 0, 0, 0, 0, 0, 0, 0, 0, 1e-1, 0, 0, 0; */
/* 'attitudeKalmanfilter:38' 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-1, 0, 0; */
/* 'attitudeKalmanfilter:39' 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-1, 0; */
/* 'attitudeKalmanfilter:40' 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-1]; */
/* 'attitudeKalmanfilter:27' Q = diag(q.^2*dt); */
power(q, a);
for (i = 0; i < 12; i++) {
b_a[i] = a[i] * dt;
}
diag(b_a, Q);
/* Q=[1e-1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; */
/* 0, 1e-1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; */
/* 0, 0, 1e-1, 0, 0, 0, 0, 0, 0, 0, 0, 0; */
/* 0, 0, 0, 1e-10, 0, 0, 1e-4, 1e-4, 0, 1e-4, 1e-4, 0; */
/* 0, 0, 0, 0, 1e-10, 0, 1e-4, 1e-4, 0, 1e-4, 1e-4, 0; */
/* 0, 0, 0, 0, 0, 1e-10, 0, 0, 0, 0, 0, 0; */
/* 0, 0, 0, 0, 0, 0, 1e-1, 0, 0, 0, 0, 0; */
/* 0, 0, 0, 0, 0, 0, 0, 1e-1, 0, 0, 0, 0; */
/* 0, 0, 0, 0, 0, 0, 0, 0, 1e-1, 0, 0, 0; */
/* 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-1, 0, 0; */
/* 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-1, 0; */
/* 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-1]; */
/* observation matrix */
/* 'attitudeKalmanfilter:44' wx= x_aposteriori_k(1); */
/* 'attitudeKalmanfilter:45' wy= x_aposteriori_k(2); */
@@ -214,13 +211,13 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
/* 'attitudeKalmanfilter:67' zek =(eye(3)+O*dt)*[zex;zey;zez]; */
eye(dv0);
for (i = 0; i < 9; i++) {
a[i] = (real32_T)dv0[i] + O[i] * dt;
c_a[i] = (real32_T)dv0[i] + O[i] * dt;
}
/* 'attitudeKalmanfilter:68' muk =(eye(3)+O*dt)*[mux;muy;muz]; */
eye(dv0);
for (i = 0; i < 9; i++) {
b_a[i] = (real32_T)dv0[i] + O[i] * dt;
d_a[i] = (real32_T)dv0[i] + O[i] * dt;
}
/* 'attitudeKalmanfilter:70' EZ=[0,zez,-zey; */
@@ -250,12 +247,12 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
for (i = 0; i < 3; i++) {
m_n_b[i] = 0.0F;
for (i0 = 0; i0 < 3; i0++) {
m_n_b[i] += a[i + 3 * i0] * x_n_b[i0];
m_n_b[i] += c_a[i + 3 * i0] * x_n_b[i0];
}
z_n_b[i] = 0.0F;
for (i0 = 0; i0 < 3; i0++) {
z_n_b[i] += b_a[i + 3 * i0] * b_x_aposteriori_k[i0];
z_n_b[i] += d_a[i + 3 * i0] * b_x_aposteriori_k[i0];
}
x_apriori[i + 6] = m_n_b[i];
@@ -360,7 +357,7 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
f0 += A_lin[i + 12 * i1] * b_A_lin[i0 + 12 * i1];
}
P_apriori[i + 12 * i0] = f0 + fv0[i + 12 * i0];
P_apriori[i + 12 * i0] = f0 + Q[i + 12 * i0];
}
}
@@ -368,10 +365,10 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
/* 'attitudeKalmanfilter:102' if updateVect(1)==1&&updateVect(2)==1&&updateVect(3)==1 */
if ((updateVect[0] == 1) && (updateVect[1] == 1) && (updateVect[2] == 1)) {
/* 'attitudeKalmanfilter:103' R=diag(r); */
diag(r, R);
b_diag(r, R);
/* observation matrix */
/* 'attitudeKalmanfilter:106' H_k=[ E, Es, Z, Z; */
/* 'attitudeKalmanfilter:106' H_k=[ E, Z, Z, Z; */
/* 'attitudeKalmanfilter:107' Z, Z, E, Z; */
/* 'attitudeKalmanfilter:108' Z, Z, Z, E]; */
/* 'attitudeKalmanfilter:110' y_k=z(1:9)-H_k*x_apriori; */
@@ -403,11 +400,11 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
f0 += K_k[i + 9 * i1] * (real32_T)iv0[i1 + 12 * i0];
}
fv1[i + 9 * i0] = f0 + R[i + 9 * i0];
fv0[i + 9 * i0] = f0 + R[i + 9 * i0];
}
}
mrdivide(b_P_apriori, fv1, K_k);
mrdivide(b_P_apriori, fv0, K_k);
/* 'attitudeKalmanfilter:116' x_aposteriori=x_apriori+K_k*y_k; */
for (i = 0; i < 9; i++) {
@@ -416,13 +413,13 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
f0 += (real32_T)iv1[i + 9 * i0] * x_apriori[i0];
}
a[i] = z[i] - f0;
c_a[i] = z[i] - f0;
}
for (i = 0; i < 12; i++) {
f0 = 0.0F;
for (i0 = 0; i0 < 9; i0++) {
f0 += K_k[i + 12 * i0] * a[i0];
f0 += K_k[i + 12 * i0] * c_a[i0];
}
x_aposteriori[i] = x_apriori[i] + f0;
@@ -437,7 +434,7 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
f0 += K_k[i + 12 * i1] * (real32_T)iv1[i1 + 9 * i0];
}
b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0;
Q[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0;
}
}
@@ -445,8 +442,7 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
for (i0 = 0; i0 < 12; i0++) {
P_aposteriori[i + 12 * i0] = 0.0F;
for (i1 = 0; i1 < 12; i1++) {
P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1 + 12
* i0];
P_aposteriori[i + 12 * i0] += Q[i + 12 * i1] * P_apriori[i1 + 12 * i0];
}
}
}
@@ -455,10 +451,10 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
/* 'attitudeKalmanfilter:119' if updateVect(1)==1&&updateVect(2)==0&&updateVect(3)==0 */
if ((updateVect[0] == 1) && (updateVect[1] == 0) && (updateVect[2] == 0)) {
/* 'attitudeKalmanfilter:120' R=diag(r(1:3)); */
b_diag(*(real32_T (*)[3])&r[0], O);
c_diag(*(real32_T (*)[3])&r[0], O);
/* observation matrix */
/* 'attitudeKalmanfilter:123' H_k=[ E, Es, Z, Z]; */
/* 'attitudeKalmanfilter:123' H_k=[ E, Z, Z, Z]; */
/* 'attitudeKalmanfilter:125' y_k=z(1:3)-H_k(1:3,1:12)*x_apriori; */
/* 'attitudeKalmanfilter:127' S_k=H_k(1:3,1:12)*P_apriori*H_k(1:3,1:12)'+R(1:3,1:3); */
/* 'attitudeKalmanfilter:128' K_k=(P_apriori*H_k(1:3,1:12)'/(S_k)); */
@@ -474,9 +470,9 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
for (i = 0; i < 3; i++) {
for (i0 = 0; i0 < 12; i0++) {
fv2[i + 3 * i0] = 0.0F;
fv1[i + 3 * i0] = 0.0F;
for (i1 = 0; i1 < 12; i1++) {
fv2[i + 3 * i0] += (real32_T)iv3[i + 3 * i1] * P_apriori[i1 + 12 *
fv1[i + 3 * i0] += (real32_T)iv3[i + 3 * i1] * P_apriori[i1 + 12 *
i0];
}
}
@@ -486,14 +482,14 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
for (i0 = 0; i0 < 3; i0++) {
f0 = 0.0F;
for (i1 = 0; i1 < 12; i1++) {
f0 += fv2[i + 3 * i1] * (real32_T)iv2[i1 + 12 * i0];
f0 += fv1[i + 3 * i1] * (real32_T)iv2[i1 + 12 * i0];
}
a[i + 3 * i0] = f0 + O[i + 3 * i0];
c_a[i + 3 * i0] = f0 + O[i + 3 * i0];
}
}
b_mrdivide(c_P_apriori, a, S_k);
b_mrdivide(c_P_apriori, c_a, S_k);
/* 'attitudeKalmanfilter:131' x_aposteriori=x_apriori+K_k*y_k; */
for (i = 0; i < 3; i++) {
@@ -523,7 +519,7 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
f0 += S_k[i + 12 * i1] * (real32_T)iv3[i1 + 3 * i0];
}
b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0;
Q[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0;
}
}
@@ -531,8 +527,8 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
for (i0 = 0; i0 < 12; i0++) {
P_aposteriori[i + 12 * i0] = 0.0F;
for (i1 = 0; i1 < 12; i1++) {
P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1 +
12 * i0];
P_aposteriori[i + 12 * i0] += Q[i + 12 * i1] * P_apriori[i1 + 12 *
i0];
}
}
}
@@ -542,10 +538,10 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
if ((updateVect[0] == 1) && (updateVect[1] == 1) && (updateVect[2] == 0))
{
/* 'attitudeKalmanfilter:135' R=diag(r(1:6)); */
c_diag(*(real32_T (*)[6])&r[0], S_k);
d_diag(*(real32_T (*)[6])&r[0], S_k);
/* observation matrix */
/* 'attitudeKalmanfilter:138' H_k=[ E, Es, Z, Z; */
/* 'attitudeKalmanfilter:138' H_k=[ E, Z, Z, Z; */
/* 'attitudeKalmanfilter:139' Z, Z, E, Z]; */
/* 'attitudeKalmanfilter:141' y_k=z(1:6)-H_k(1:6,1:12)*x_apriori; */
/* 'attitudeKalmanfilter:143' S_k=H_k(1:6,1:12)*P_apriori*H_k(1:6,1:12)'+R(1:6,1:6); */
@@ -577,11 +573,11 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
f0 += b_K_k[i + 6 * i1] * (real32_T)iv4[i1 + 12 * i0];
}
fv2[i + 6 * i0] = f0 + S_k[i + 6 * i0];
fv1[i + 6 * i0] = f0 + S_k[i + 6 * i0];
}
}
c_mrdivide(d_P_apriori, fv2, b_K_k);
c_mrdivide(d_P_apriori, fv1, b_K_k);
/* 'attitudeKalmanfilter:147' x_aposteriori=x_apriori+K_k*y_k; */
for (i = 0; i < 6; i++) {
@@ -611,7 +607,7 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
f0 += b_K_k[i + 12 * i1] * (real32_T)iv5[i1 + 6 * i0];
}
b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0;
Q[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0;
}
}
@@ -619,8 +615,8 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
for (i0 = 0; i0 < 12; i0++) {
P_aposteriori[i + 12 * i0] = 0.0F;
for (i1 = 0; i1 < 12; i1++) {
P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1
+ 12 * i0];
P_aposteriori[i + 12 * i0] += Q[i + 12 * i1] * P_apriori[i1 + 12 *
i0];
}
}
}
@@ -631,7 +627,7 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
{
/* 'attitudeKalmanfilter:151' R=diag([r(1:3);r(7:9)]); */
/* observation matrix */
/* 'attitudeKalmanfilter:154' H_k=[ E, Es, Z, Z; */
/* 'attitudeKalmanfilter:154' H_k=[ E, Z, Z, Z; */
/* 'attitudeKalmanfilter:155' Z, Z, Z, E]; */
/* 'attitudeKalmanfilter:157' y_k=[z(1:3);z(7:9)]-H_k(1:6,1:12)*x_apriori; */
/* 'attitudeKalmanfilter:159' S_k=H_k(1:6,1:12)*P_apriori*H_k(1:6,1:12)'+R(1:6,1:6); */
@@ -684,12 +680,12 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
}
for (i = 0; i < 6; i++) {
fv3[i] = 0.0F;
fv2[i] = 0.0F;
for (i0 = 0; i0 < 12; i0++) {
fv3[i] += (real32_T)iv6[i + 6 * i0] * x_apriori[i0];
fv2[i] += (real32_T)iv6[i + 6 * i0] * x_apriori[i0];
}
b_z[i] = b_r[i] - fv3[i];
b_z[i] = b_r[i] - fv2[i];
}
for (i = 0; i < 12; i++) {
@@ -710,7 +706,7 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
f0 += b_K_k[i + 12 * i1] * (real32_T)iv6[i1 + 6 * i0];
}
b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0;
Q[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0;
}
}
@@ -718,8 +714,8 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
for (i0 = 0; i0 < 12; i0++) {
P_aposteriori[i + 12 * i0] = 0.0F;
for (i1 = 0; i1 < 12; i1++) {
P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] *
P_apriori[i1 + 12 * i0];
P_aposteriori[i + 12 * i0] += Q[i + 12 * i1] * P_apriori[i1 + 12
* i0];
}
}
}
@@ -785,7 +781,8 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
/* 'attitudeKalmanfilter:195' psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1)); */
/* 'attitudeKalmanfilter:196' eulerAngles=[phi;theta;psi]; */
eulerAngles[0] = rt_atan2f_snf(Rot_matrix[7], Rot_matrix[8]);
eulerAngles[1] = -(real32_T)asinf(Rot_matrix[6]);
eulerAngles[1] = -(real32_T)asin(Rot_matrix[6]);
eulerAngles[2] = rt_atan2f_snf(Rot_matrix[3], Rot_matrix[0]);
}
/* End of code generation (attitudeKalmanfilter.c) */
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h
Executable → Regular
+1 -1
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Sat Oct 13 16:28:18 2012
* C source code generated on: Tue Oct 16 15:27:58 2012
*
*/
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c
Executable → Regular
+1 -1
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter_initialize'
*
* C source code generated on: Sat Oct 13 16:28:18 2012
* C source code generated on: Tue Oct 16 15:27:58 2012
*
*/
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h
Executable → Regular
+1 -1
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter_initialize'
*
* C source code generated on: Sat Oct 13 16:28:18 2012
* C source code generated on: Tue Oct 16 15:27:58 2012
*
*/
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c
Executable → Regular
+1 -1
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter_terminate'
*
* C source code generated on: Sat Oct 13 16:28:18 2012
* C source code generated on: Tue Oct 16 15:27:58 2012
*
*/
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h
Executable → Regular
+1 -1
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter_terminate'
*
* C source code generated on: Sat Oct 13 16:28:18 2012
* C source code generated on: Tue Oct 16 15:27:58 2012
*
*/
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h
Executable → Regular
+1 -1
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Sat Oct 13 16:28:18 2012
* C source code generated on: Tue Oct 16 15:27:58 2012
*
*/
apps/attitude_estimator_ekf/codegen/cross.c
Executable → Regular
+1 -1
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'cross'
*
* C source code generated on: Sat Oct 13 16:28:18 2012
* C source code generated on: Tue Oct 16 15:27:58 2012
*
*/
apps/attitude_estimator_ekf/codegen/cross.h
Executable → Regular
+1 -1
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'cross'
*
* C source code generated on: Sat Oct 13 16:28:18 2012
* C source code generated on: Tue Oct 16 15:27:58 2012
*
*/
apps/attitude_estimator_ekf/codegen/diag.c
Executable → Regular
+19 -7
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'diag'
*
* C source code generated on: Sat Oct 13 16:28:18 2012
* C source code generated on: Tue Oct 16 15:27:58 2012
*
*/
@@ -27,7 +27,19 @@
/*
*
*/
void b_diag(const real32_T v[3], real32_T d[9])
void b_diag(const real32_T v[9], real32_T d[81])
{
int32_T j;
memset(&d[0], 0, 81U * sizeof(real32_T));
for (j = 0; j < 9; j++) {
d[j + 9 * j] = v[j];
}
}
/*
*
*/
void c_diag(const real32_T v[3], real32_T d[9])
{
int32_T j;
for (j = 0; j < 9; j++) {
@@ -42,7 +54,7 @@ void b_diag(const real32_T v[3], real32_T d[9])
/*
*
*/
void c_diag(const real32_T v[6], real32_T d[36])
void d_diag(const real32_T v[6], real32_T d[36])
{
int32_T j;
memset(&d[0], 0, 36U * sizeof(real32_T));
@@ -54,12 +66,12 @@ void c_diag(const real32_T v[6], real32_T d[36])
/*
*
*/
void diag(const real32_T v[9], real32_T d[81])
void diag(const real32_T v[12], real32_T d[144])
{
int32_T j;
memset(&d[0], 0, 81U * sizeof(real32_T));
for (j = 0; j < 9; j++) {
d[j + 9 * j] = v[j];
memset(&d[0], 0, 144U * sizeof(real32_T));
for (j = 0; j < 12; j++) {
d[j + 12 * j] = v[j];
}
}
apps/attitude_estimator_ekf/codegen/diag.h
Executable → Regular
+5 -4
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'diag'
*
* C source code generated on: Sat Oct 13 16:28:18 2012
* C source code generated on: Tue Oct 16 15:27:58 2012
*
*/
@@ -29,8 +29,9 @@
/* Variable Definitions */
/* Function Declarations */
extern void b_diag(const real32_T v[3], real32_T d[9]);
extern void c_diag(const real32_T v[6], real32_T d[36]);
extern void diag(const real32_T v[9], real32_T d[81]);
extern void b_diag(const real32_T v[9], real32_T d[81]);
extern void c_diag(const real32_T v[3], real32_T d[9]);
extern void d_diag(const real32_T v[6], real32_T d[36]);
extern void diag(const real32_T v[12], real32_T d[144]);
#endif
/* End of code generation (diag.h) */
apps/attitude_estimator_ekf/codegen/eye.c
Executable → Regular
+1 -1
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'eye'
*
* C source code generated on: Sat Oct 13 16:28:18 2012
* C source code generated on: Tue Oct 16 15:27:58 2012
*
*/
apps/attitude_estimator_ekf/codegen/eye.h
Executable → Regular
+1 -1
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'eye'
*
* C source code generated on: Sat Oct 13 16:28:18 2012
* C source code generated on: Tue Oct 16 15:27:58 2012
*
*/
apps/attitude_estimator_ekf/codegen/mrdivide.c
Executable → Regular
+1 -1
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'mrdivide'
*
* C source code generated on: Sat Oct 13 16:28:18 2012
* C source code generated on: Tue Oct 16 15:27:58 2012
*
*/
apps/attitude_estimator_ekf/codegen/mrdivide.h
Executable → Regular
+1 -1
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'mrdivide'
*
* C source code generated on: Sat Oct 13 16:28:18 2012
* C source code generated on: Tue Oct 16 15:27:58 2012
*
*/
apps/attitude_estimator_ekf/codegen/norm.c
Executable → Regular
+1 -1
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'norm'
*
* C source code generated on: Sat Oct 13 16:28:18 2012
* C source code generated on: Tue Oct 16 15:27:58 2012
*
*/
apps/attitude_estimator_ekf/codegen/norm.h
Executable → Regular
+1 -1
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'norm'
*
* C source code generated on: Sat Oct 13 16:28:18 2012
* C source code generated on: Tue Oct 16 15:27:58 2012
*
*/
apps/attitude_estimator_ekf/codegen/power.c
+84
View File
@@ -0,0 +1,84 @@
/*
* power.c
*
* Code generation for function 'power'
*
* C source code generated on: Tue Oct 16 15:27:58 2012
*
*/
/* Include files */
#include "rt_nonfinite.h"
#include "attitudeKalmanfilter.h"
#include "power.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
static real32_T rt_powf_snf(real32_T u0, real32_T u1);
/* Function Definitions */
static real32_T rt_powf_snf(real32_T u0, real32_T u1)
{
real32_T y;
real32_T f1;
real32_T f2;
if (rtIsNaNF(u0) || rtIsNaNF(u1)) {
y = ((real32_T)rtNaN);
} else {
f1 = (real32_T)fabs(u0);
f2 = (real32_T)fabs(u1);
if (rtIsInfF(u1)) {
if (f1 == 1.0F) {
y = ((real32_T)rtNaN);
} else if (f1 > 1.0F) {
if (u1 > 0.0F) {
y = ((real32_T)rtInf);
} else {
y = 0.0F;
}
} else if (u1 > 0.0F) {
y = 0.0F;
} else {
y = ((real32_T)rtInf);
}
} else if (f2 == 0.0F) {
y = 1.0F;
} else if (f2 == 1.0F) {
if (u1 > 0.0F) {
y = u0;
} else {
y = 1.0F / u0;
}
} else if (u1 == 2.0F) {
y = u0 * u0;
} else if ((u1 == 0.5F) && (u0 >= 0.0F)) {
y = (real32_T)sqrt(u0);
} else if ((u0 < 0.0F) && (u1 > (real32_T)floor(u1))) {
y = ((real32_T)rtNaN);
} else {
y = (real32_T)pow(u0, u1);
}
}
return y;
}
/*
*
*/
void power(const real32_T a[12], real32_T y[12])
{
int32_T k;
for (k = 0; k < 12; k++) {
y[k] = rt_powf_snf(a[k], 2.0F);
}
}
/* End of code generation (power.c) */
apps/attitude_estimator_ekf/codegen/power.h
+34
View File
@@ -0,0 +1,34 @@
/*
* power.h
*
* Code generation for function 'power'
*
* C source code generated on: Tue Oct 16 15:27:58 2012
*
*/
#ifndef __POWER_H__
#define __POWER_H__
/* Include files */
#include <math.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include "rt_defines.h"
#include "rt_nonfinite.h"
#include "rtwtypes.h"
#include "attitudeKalmanfilter_types.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
extern void power(const real32_T a[12], real32_T y[12]);
#endif
/* End of code generation (power.h) */
apps/attitude_estimator_ekf/codegen/rdivide.c
Executable → Regular
+1 -1
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'rdivide'
*
* C source code generated on: Sat Oct 13 16:28:18 2012
* C source code generated on: Tue Oct 16 15:27:58 2012
*
*/
apps/attitude_estimator_ekf/codegen/rdivide.h
Executable → Regular
+1 -1
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'rdivide'
*
* C source code generated on: Sat Oct 13 16:28:18 2012
* C source code generated on: Tue Oct 16 15:27:58 2012
*
*/
apps/attitude_estimator_ekf/codegen/rtGetInf.c
Executable → Regular
+1 -1
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Sat Oct 13 16:28:18 2012
* C source code generated on: Tue Oct 16 15:27:58 2012
*
*/
apps/attitude_estimator_ekf/codegen/rtGetInf.h
Executable → Regular
+1 -1
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Sat Oct 13 16:28:18 2012
* C source code generated on: Tue Oct 16 15:27:58 2012
*
*/
apps/attitude_estimator_ekf/codegen/rtGetNaN.c
Executable → Regular
+1 -1
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Sat Oct 13 16:28:18 2012
* C source code generated on: Tue Oct 16 15:27:58 2012
*
*/
apps/attitude_estimator_ekf/codegen/rtGetNaN.h
Executable → Regular
+1 -1
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Sat Oct 13 16:28:18 2012
* C source code generated on: Tue Oct 16 15:27:58 2012
*
*/
apps/attitude_estimator_ekf/codegen/rt_defines.h
Executable → Regular
+1 -1
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Sat Oct 13 16:28:18 2012
* C source code generated on: Tue Oct 16 15:27:58 2012
*
*/
apps/attitude_estimator_ekf/codegen/rt_nonfinite.c
Executable → Regular
+1 -1
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Sat Oct 13 16:28:18 2012
* C source code generated on: Tue Oct 16 15:27:58 2012
*
*/
apps/attitude_estimator_ekf/codegen/rt_nonfinite.h
Executable → Regular
+1 -1
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Sat Oct 13 16:28:18 2012
* C source code generated on: Tue Oct 16 15:27:58 2012
*
*/
apps/attitude_estimator_ekf/codegen/rtwtypes.h
Executable → Regular
+1 -1
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Sat Oct 13 16:28:18 2012
* C source code generated on: Tue Oct 16 15:27:58 2012
*
*/
apps/multirotor_att_control/multirotor_att_control_main.c
Executable → Regular
+7 -12
View File
@@ -172,7 +172,7 @@ mc_thread_main(int argc, char *argv[])
rates_sp.pitch = offboard_sp.p2;
rates_sp.yaw = offboard_sp.p3;
rates_sp.thrust = offboard_sp.p4;
//printf("thrust_rate=%8.4f\n",offboard_sp.p4);
// printf("thrust_rate=%8.4f\n",offboard_sp.p4);
rates_sp.timestamp = hrt_absolute_time();
orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
} else if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE) {
@@ -180,7 +180,7 @@ mc_thread_main(int argc, char *argv[])
att_sp.pitch_body = offboard_sp.p2;
att_sp.yaw_body = offboard_sp.p3;
att_sp.thrust = offboard_sp.p4;
//printf("thrust_att=%8.4f\n",offboard_sp.p4);
// printf("thrust_att=%8.4f\n",offboard_sp.p4);
att_sp.timestamp = hrt_absolute_time();
/* STEP 2: publish the result to the vehicle actuators */
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
@@ -193,6 +193,7 @@ mc_thread_main(int argc, char *argv[])
if (state.flag_control_rates_enabled && !state.flag_control_attitude_enabled) {
rates_sp.roll = manual.roll;
rates_sp.pitch = manual.pitch;
rates_sp.yaw = manual.yaw;
rates_sp.thrust = manual.throttle;
@@ -203,7 +204,7 @@ mc_thread_main(int argc, char *argv[])
if (state.flag_control_attitude_enabled) {
att_sp.roll_body = manual.roll;
att_sp.pitch_body = manual.pitch;
att_sp.yaw_body = manual.yaw; // XXX Hack, remove, switch to yaw rate controller
att_sp.yaw_body = manual.yaw; // XXX Hack, clean up
/* set yaw rate */
rates_sp.yaw = manual.yaw;
att_sp.thrust = manual.throttle;
@@ -226,21 +227,15 @@ mc_thread_main(int argc, char *argv[])
}
/** STEP 3: Identify the controller setup to run and set up the inputs correctly */
/* run attitude controller */
if (state.flag_control_attitude_enabled && !state.flag_control_rates_enabled) {
multirotor_control_attitude(&att_sp, &att, NULL, &actuators);
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
} else if (state.flag_control_attitude_enabled && state.flag_control_rates_enabled) {
if (state.flag_control_attitude_enabled) {
multirotor_control_attitude(&att_sp, &att, &rates_sp, NULL);
orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
}
}
if (state.flag_control_rates_enabled) {
float gyro[3] = {0.0f, 0.0f, 0.0f};
float gyro[3];
/* get current rate setpoint */
bool rates_sp_valid = false;
apps/multirotor_att_control/multirotor_attitude_control.c
Executable → Regular
+71 -136
View File
@@ -54,17 +54,13 @@
#include <systemlib/param/param.h>
#include <arch/board/up_hrt.h>
// PARAM_DEFINE_FLOAT(MC_YAWPOS_P, 0.3f);
// PARAM_DEFINE_FLOAT(MC_YAWPOS_I, 0.15f);
// PARAM_DEFINE_FLOAT(MC_YAWPOS_D, 0.0f);
// PARAM_DEFINE_FLOAT(MC_YAWPOS_AWU, 1.0f);
// PARAM_DEFINE_FLOAT(MC_YAWPOS_LIM, 3.0f);
PARAM_DEFINE_FLOAT(MC_YAWPOS_P, 0.3f);
PARAM_DEFINE_FLOAT(MC_YAWPOS_I, 0.15f);
PARAM_DEFINE_FLOAT(MC_YAWPOS_D, 0.0f);
PARAM_DEFINE_FLOAT(MC_YAWPOS_AWU, 1.0f);
PARAM_DEFINE_FLOAT(MC_YAWPOS_LIM, 3.0f);
PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.08f); /* same on Flamewheel */
PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.02f);
PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f);
PARAM_DEFINE_FLOAT(MC_YAWRATE_AWU, 0.02f);
PARAM_DEFINE_FLOAT(MC_YAWRATE_LIM, 0.1f);
PARAM_DEFINE_FLOAT(MC_ATT_P, 0.2f); /* 0.15 F405 Flamewheel */
PARAM_DEFINE_FLOAT(MC_ATT_I, 0.0f);
@@ -76,17 +72,17 @@ PARAM_DEFINE_FLOAT(MC_ATT_XOFF, 0.0f);
PARAM_DEFINE_FLOAT(MC_ATT_YOFF, 0.0f);
struct mc_att_control_params {
// float yaw_p;
// float yaw_i;
// float yaw_d;
// float yaw_awu;
// float yaw_lim;
float yaw_p;
float yaw_i;
float yaw_d;
float yaw_awu;
float yaw_lim;
float yawrate_p;
float yawrate_i;
float yawrate_d;
float yawrate_awu;
float yawrate_lim;
// float yawrate_p;
// float yawrate_i;
// float yawrate_d;
// float yawrate_awu;
// float yawrate_lim;
float att_p;
float att_i;
@@ -99,17 +95,17 @@ struct mc_att_control_params {
};
struct mc_att_control_param_handles {
// param_t yaw_p;
// param_t yaw_i;
// param_t yaw_d;
// param_t yaw_awu;
// param_t yaw_lim;
param_t yaw_p;
param_t yaw_i;
param_t yaw_d;
param_t yaw_awu;
param_t yaw_lim;
param_t yawrate_p;
param_t yawrate_i;
param_t yawrate_d;
param_t yawrate_awu;
param_t yawrate_lim;
// param_t yawrate_p;
// param_t yawrate_i;
// param_t yawrate_d;
// param_t yawrate_awu;
// param_t yawrate_lim;
param_t att_p;
param_t att_i;
@@ -137,17 +133,17 @@ static int parameters_update(const struct mc_att_control_param_handles *h, struc
static int parameters_init(struct mc_att_control_param_handles *h)
{
/* PID parameters */
// h->yaw_p = param_find("MC_YAWPOS_P");
// h->yaw_i = param_find("MC_YAWPOS_I");
// h->yaw_d = param_find("MC_YAWPOS_D");
// h->yaw_awu = param_find("MC_YAWPOS_AWU");
// h->yaw_lim = param_find("MC_YAWPOS_LIM");
h->yaw_p = param_find("MC_YAWPOS_P");
h->yaw_i = param_find("MC_YAWPOS_I");
h->yaw_d = param_find("MC_YAWPOS_D");
h->yaw_awu = param_find("MC_YAWPOS_AWU");
h->yaw_lim = param_find("MC_YAWPOS_LIM");
h->yawrate_p = param_find("MC_YAWRATE_P");
h->yawrate_i = param_find("MC_YAWRATE_I");
h->yawrate_d = param_find("MC_YAWRATE_D");
h->yawrate_awu = param_find("MC_YAWRATE_AWU");
h->yawrate_lim = param_find("MC_YAWRATE_LIM");
// h->yawrate_p = param_find("MC_YAWRATE_P");
// h->yawrate_i = param_find("MC_YAWRATE_I");
// h->yawrate_d = param_find("MC_YAWRATE_D");
// h->yawrate_awu = param_find("MC_YAWRATE_AWU");
// h->yawrate_lim = param_find("MC_YAWRATE_LIM");
h->att_p = param_find("MC_ATT_P");
h->att_i = param_find("MC_ATT_I");
@@ -163,17 +159,17 @@ static int parameters_init(struct mc_att_control_param_handles *h)
static int parameters_update(const struct mc_att_control_param_handles *h, struct mc_att_control_params *p)
{
// param_get(h->yaw_p, &(p->yaw_p));
// param_get(h->yaw_i, &(p->yaw_i));
// param_get(h->yaw_d, &(p->yaw_d));
// param_get(h->yaw_awu, &(p->yaw_awu));
// param_get(h->yaw_lim, &(p->yaw_lim));
param_get(h->yaw_p, &(p->yaw_p));
param_get(h->yaw_i, &(p->yaw_i));
param_get(h->yaw_d, &(p->yaw_d));
param_get(h->yaw_awu, &(p->yaw_awu));
param_get(h->yaw_lim, &(p->yaw_lim));
param_get(h->yawrate_p, &(p->yawrate_p));
param_get(h->yawrate_i, &(p->yawrate_i));
param_get(h->yawrate_d, &(p->yawrate_d));
param_get(h->yawrate_awu, &(p->yawrate_awu));
param_get(h->yawrate_lim, &(p->yawrate_lim));
// param_get(h->yawrate_p, &(p->yawrate_p));
// param_get(h->yawrate_i, &(p->yawrate_i));
// param_get(h->yawrate_d, &(p->yawrate_d));
// param_get(h->yawrate_awu, &(p->yawrate_awu));
// param_get(h->yawrate_lim, &(p->yawrate_lim));
param_get(h->att_p, &(p->att_p));
param_get(h->att_i, &(p->att_i));
@@ -188,16 +184,23 @@ static int parameters_update(const struct mc_att_control_param_handles *h, struc
}
void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, struct actuator_controls_s *actuators)
const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, struct actuator_controls_s *actuators)
{
static uint64_t last_run = 0;
const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
static uint64_t last_input = 0;
float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
float dT_input = (hrt_absolute_time() - last_input) / 1000000.0f;
last_run = hrt_absolute_time();
if (last_input != att_sp->timestamp) {
last_input = att_sp->timestamp;
}
static int sensor_delay;
sensor_delay = hrt_absolute_time() - att->timestamp;
static int motor_skip_counter = 0;
// static PID_t yaw_pos_controller;
static PID_t yaw_speed_controller;
// static PID_t yaw_speed_controller;
static PID_t pitch_controller;
static PID_t roll_controller;
@@ -213,112 +216,44 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
// pid_init(&yaw_pos_controller, p.yaw_p, p.yaw_i, p.yaw_d, p.yaw_awu,
// PID_MODE_DERIVATIV_SET, 154);
pid_init(&yaw_speed_controller, p.yawrate_p, p.yawrate_i, p.yawrate_d, p.yawrate_awu,
PID_MODE_DERIVATIV_SET);
// pid_init(&yaw_speed_controller, p.yawrate_p, p.yawrate_i, p.yawrate_d, p.yawrate_awu,
// PID_MODE_DERIVATIV_SET);
pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, p.att_awu,
PID_MODE_DERIVATIV_SET);
PID_MODE_DERIVATIV_SET);
pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, p.att_awu,
PID_MODE_DERIVATIV_SET);
PID_MODE_DERIVATIV_SET);
initialized = true;
}
/* load new parameters with lower rate */
if (motor_skip_counter % 50 == 0) {
if (motor_skip_counter % 1000 == 0) {
/* update parameters from storage */
parameters_update(&h, &p);
/* apply parameters */
// pid_set_parameters(&yaw_pos_controller, p.yaw_p, p.yaw_i, p.yaw_d, p.yaw_awu);
pid_set_parameters(&yaw_speed_controller, p.yawrate_p, p.yawrate_i, p.yawrate_d, p.yawrate_awu);
// pid_set_parameters(&yaw_speed_controller, p.yawrate_p, p.yawrate_i, p.yawrate_d, p.yawrate_awu);
pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, p.att_awu);
pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, p.att_awu);
printf("delays: %d us sens->ctrl, rate: %d Hz, input: %d Hz\n", sensor_delay, (int)(1.0f/deltaT), (int)(1.0f/dT_input));
}
/* calculate current control outputs */
/* control pitch (forward) output */
float pitch_control = pid_calculate(&pitch_controller, att_sp->pitch_body + p.att_xoff,
att->pitch, att->pitchspeed, deltaT)*1/10.0f;
rates_sp->pitch = pid_calculate(&pitch_controller, att_sp->pitch_body + p.att_xoff,
att->pitch, att->pitchspeed, deltaT)*1/10.0f;
/* control roll (left/right) output */
float roll_control = pid_calculate(&roll_controller, att_sp->roll_body + p.att_yoff,
att->roll, att->rollspeed, deltaT)*1/10.0f;
rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body + p.att_yoff,
att->roll, att->rollspeed, deltaT)*1/10.0f;
/* control yaw rate */
float yaw_rate_control = pid_calculate(&yaw_speed_controller, att_sp->yaw_body, att->yawspeed, 0.0f, deltaT)*1/10.0f;
/*
* compensate the vertical loss of thrust
* when thrust plane has an angle.
* start with a factor of 1.0 (no change)
*/
float zcompensation = 1.0f;
if (fabsf(att->roll) > 0.3f) {
zcompensation *= 1.04675160154f;
} else {
zcompensation *= 1.0f / cosf(att->roll);
}
if (fabsf(att->pitch) > 0.3f) {
zcompensation *= 1.04675160154f;
} else {
zcompensation *= 1.0f / cosf(att->pitch);
}
float motor_thrust = 0.0f;
motor_thrust = att_sp->thrust;
/* compensate thrust vector for roll / pitch contributions */
motor_thrust *= zcompensation;
/* limit yaw rate output */
if (yaw_rate_control > p.yawrate_lim) {
yaw_rate_control = p.yawrate_lim;
yaw_speed_controller.saturated = 1;
}
if (yaw_rate_control < -p.yawrate_lim) {
yaw_rate_control = -p.yawrate_lim;
yaw_speed_controller.saturated = 1;
}
if (pitch_control > p.att_lim) {
pitch_control = p.att_lim;
pitch_controller.saturated = 1;
}
if (pitch_control < -p.att_lim) {
pitch_control = -p.att_lim;
pitch_controller.saturated = 1;
}
//float yaw_rate_control = pid_calculate(&yaw_speed_controller, att_sp->yaw_body, att->yawspeed, 0.0f, deltaT)*1/10.0f;
rates_sp->yaw= deltaT*p.yaw_p*atan2f(sinf(att->yaw-att_sp->yaw_body),cos(att->yaw-att_sp->yaw_body));
if (roll_control > p.att_lim) {
roll_control = p.att_lim;
roll_controller.saturated = 1;
}
if (roll_control < -p.att_lim) {
roll_control = -p.att_lim;
roll_controller.saturated = 1;
}
rates_sp->thrust = att_sp->thrust;
if (actuators) {
actuators->control[0] = roll_control;
actuators->control[1] = pitch_control;
actuators->control[2] = yaw_rate_control;
actuators->control[3] = motor_thrust;
}
// XXX change yaw rate to yaw pos controller
if (rates_sp) {
rates_sp->roll = roll_control;
rates_sp->pitch = pitch_control;
rates_sp->yaw = yaw_rate_control;
rates_sp->thrust = motor_thrust;
}
motor_skip_counter++;
}
apps/multirotor_att_control/multirotor_attitude_control.h
Executable → Regular
View File
apps/multirotor_att_control/multirotor_rate_control.h
Executable → Regular
View File