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

att ekf: add param to enable/disable J

Browse Source
This commit is contained in:
Thomas Gubler
2014-07-25 15:23:26 +02:00
parent 4b0230042f
commit 63fa17ef0d
3 changed files with 18 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+1 -1
View File
@@ -512,7 +512,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
/* Call the estimator */
AttitudeEKF(false, // approx_prediction
false, // use_inertia_matrix
(unsigned char)ekf_params.use_moment_inertia,
update_vect,
dt,
z_k,
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c
+15 -4
View File
@@ -79,7 +79,6 @@ PARAM_DEFINE_FLOAT(ATT_J11, 0.0018);
*/
PARAM_DEFINE_FLOAT(ATT_J22, 0.0018);
/*
* Moment of inertia matrix diagonal entry (3, 3)
*
@@ -88,6 +87,16 @@ PARAM_DEFINE_FLOAT(ATT_J22, 0.0018);
*/
PARAM_DEFINE_FLOAT(ATT_J33, 0.0037);
/*
* Moment of inertia enabled in estimator
*
* If set to != 0 the moment of inertia will be used in the estimator
*
* @group attitude_ekf
* @min 0
* @max 1
*/
PARAM_DEFINE_INT32(ATT_J_EN, 0);
int parameters_init(struct attitude_estimator_ekf_param_handles *h)
{
@@ -105,9 +114,10 @@ int parameters_init(struct attitude_estimator_ekf_param_handles *h)
h->acc_comp = param_find("ATT_ACC_COMP");
h->moment_inertia_J[0] = param_find("ATT_J11");
h->moment_inertia_J[1] = param_find("ATT_J22");
h->moment_inertia_J[2] = param_find("ATT_J33");
h->moment_inertia_J[0] = param_find("ATT_J11");
h->moment_inertia_J[1] = param_find("ATT_J22");
h->moment_inertia_J[2] = param_find("ATT_J33");
h->use_moment_inertia = param_find("ATT_J_EN");
return OK;
}
@@ -131,6 +141,7 @@ int parameters_update(const struct attitude_estimator_ekf_param_handles *h, stru
for (int i = 0; i < 3; i++) {
param_get(h->moment_inertia_J[i], &(p->moment_inertia_J[3 * i + i]));
}
param_get(h->use_moment_inertia, &(p->use_moment_inertia));
return OK;
}
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h
+2
View File
@@ -45,6 +45,7 @@ struct attitude_estimator_ekf_params {
float r[3];
float q[4];
float moment_inertia_J[9];
int32_t use_moment_inertia;
float roll_off;
float pitch_off;
float yaw_off;
@@ -56,6 +57,7 @@ struct attitude_estimator_ekf_param_handles {
param_t r0, r1, r2;
param_t q0, q1, q2, q3;
param_t moment_inertia_J[3]; /**< diagonal entries of the matrix */
param_t use_moment_inertia;
param_t mag_decl;
param_t acc_comp;
};