dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

landing target estimator: check validity of the correct data before computing acceleration

This commit is contained in:
Nicolas de Palezieux 2018-01-24 18:15:52 +01:00 committed by Lorenz Meier
parent c0615c9e70
commit b7dff95782
1 changed files with 2 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/modules/landing_target_estimator/LandingTargetEstimator.cpp
View File

@ -94,14 +94,13 @@ void LandingTargetEstimator::update()
PX4_WARN("Timeout");
_estimator_initialized = false;
} else if (_vehicleLocalPosition_valid
&& _vehicleLocalPosition.v_xy_valid) {
} else {
float dt = (hrt_absolute_time() - _last_predict) / SEC2USEC;
// predict target position with the help of accel data
matrix::Vector3f a;
if (_sensorBias_valid) {
if (_vehicleAttitude_valid && _sensorBias_valid) {
matrix::Quaternion<float> q_att(&_vehicleAttitude.q[0]);
_R_att = matrix::Dcm<float>(q_att);
a(0) = _sensorBias.accel_x;
@ -272,7 +271,6 @@ void LandingTargetEstimator::_check_params(const bool force)
void LandingTargetEstimator::_initialize_topics()
{
// subscribe to position, attitude, arming and velocity changes
_vehicleLocalPositionSub = orb_subscribe(ORB_ID(vehicle_local_position));
_attitudeSub = orb_subscribe(ORB_ID(vehicle_attitude));
_sensorBiasSub = orb_subscribe(ORB_ID(sensor_bias));