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

Extend auxVel interface to support 3d velocity

Browse Source
This commit is contained in:
kamilritz
2020-01-08 14:36:03 +01:00
committed by Mathieu Bresciani
parent 07e8e88e88
commit 84dcb32bd8
4 changed files with 12 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/common.h
+2 -2
View File
@@ -167,8 +167,8 @@ struct dragSample {
}; };
struct auxVelSample { struct auxVelSample {
Vector2f velNE; ///< measured NE velocity relative to the local origin (m/sec) Vector3f vel; ///< measured NE velocity relative to the local origin (m/sec)
Vector2f velVarNE; ///< estimated error variance of the NE velocity (m/sec)**2 Vector3f velVar; ///< estimated error variance of the NE velocity (m/sec)**2
uint64_t time_us; ///< timestamp of the measurement (uSec) uint64_t time_us; ///< timestamp of the measurement (uSec)
}; };
EKF/control.cpp
+6 -4
View File
@@ -1398,14 +1398,16 @@ void Ekf::controlAuxVelFusion()
Vector2f aux_vel_innov_gate; Vector2f aux_vel_innov_gate;
Vector3f aux_vel_obs_var; Vector3f aux_vel_obs_var;
_aux_vel_innov(0) = _state.vel(0) - _auxvel_sample_delayed.velNE(0); _aux_vel_innov = _state.vel - _auxvel_sample_delayed.vel;
_aux_vel_innov(1) = _state.vel(1) - _auxvel_sample_delayed.velNE(1); aux_vel_obs_var = _auxvel_sample_delayed.velVar;
aux_vel_innov_gate(0) = _params.auxvel_gate; aux_vel_innov_gate(0) = _params.auxvel_gate;
aux_vel_obs_var(0) = _auxvel_sample_delayed.velVarNE(0);
aux_vel_obs_var(1) = _auxvel_sample_delayed.velVarNE(1);
fuseHorizontalVelocity(_aux_vel_innov, aux_vel_innov_gate, aux_vel_obs_var, fuseHorizontalVelocity(_aux_vel_innov, aux_vel_innov_gate, aux_vel_obs_var,
_aux_vel_innov_var, _aux_vel_test_ratio); _aux_vel_innov_var, _aux_vel_test_ratio);
// Can be enabled after bit for this is added to EKF_AID_MASK
// fuseVerticalVelocity(_aux_vel_innov, aux_vel_innov_gate, aux_vel_obs_var,
// _aux_vel_innov_var, _aux_vel_test_ratio);
} }
} }
EKF/estimator_interface.cpp
+3 -3
View File
@@ -447,7 +447,7 @@ void EstimatorInterface::setExtVisionData(uint64_t time_usec, ext_vision_message
} }
} }
void EstimatorInterface::setAuxVelData(uint64_t time_usec, float (&data)[2], float (&variance)[2]) void EstimatorInterface::setAuxVelData(uint64_t time_usec, const Vector3f &velocity, const Vector3f &variance)
{ {
if (!_initialised || _auxvel_buffer_fail) { if (!_initialised || _auxvel_buffer_fail) {
return; return;
@@ -473,8 +473,8 @@ void EstimatorInterface::setAuxVelData(uint64_t time_usec, float (&data)[2], flo
auxvel_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2; auxvel_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2;
_time_last_auxvel = time_usec; _time_last_auxvel = time_usec;
auxvel_sample_new.velNE = Vector2f(data); auxvel_sample_new.vel = velocity;
auxvel_sample_new.velVarNE = Vector2f(variance); auxvel_sample_new.velVar = variance;
_auxvel_buffer.push(auxvel_sample_new); _auxvel_buffer.push(auxvel_sample_new);
} }
EKF/estimator_interface.h
+1 -1
View File
@@ -203,7 +203,7 @@ public:
void setExtVisionData(uint64_t time_usec, ext_vision_message *evdata); void setExtVisionData(uint64_t time_usec, ext_vision_message *evdata);
// set auxiliary velocity data // set auxiliary velocity data
void setAuxVelData(uint64_t time_usec, float (&data)[2], float (&variance)[2]); void setAuxVelData(uint64_t time_usec, const Vector3f &vel, const Vector3f &variance);
// return a address to the parameters struct // return a address to the parameters struct
// in order to give access to the application // in order to give access to the application