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

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

Browse Source
This commit is contained in:
Nicolas de Palezieux
2018-01-24 19:39:30 +01:00
committed by Lorenz Meier
parent c0615c9e70
commit b7dff95782
1 changed files with 2 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/landing_target_estimator/LandingTargetEstimator.cpp
+2 -4
View File
@@ -94,14 +94,13 @@ void LandingTargetEstimator::update()
PX4_WARN("Timeout"); PX4_WARN("Timeout");
_estimator_initialized = false; _estimator_initialized = false;
} else if (_vehicleLocalPosition_valid } else {
&& _vehicleLocalPosition.v_xy_valid) {
float dt = (hrt_absolute_time() - _last_predict) / SEC2USEC; float dt = (hrt_absolute_time() - _last_predict) / SEC2USEC;
// predict target position with the help of accel data // predict target position with the help of accel data
matrix::Vector3f a; matrix::Vector3f a;
if (_sensorBias_valid) { if (_vehicleAttitude_valid && _sensorBias_valid) {
matrix::Quaternion<float> q_att(&_vehicleAttitude.q[0]); matrix::Quaternion<float> q_att(&_vehicleAttitude.q[0]);
_R_att = matrix::Dcm<float>(q_att); _R_att = matrix::Dcm<float>(q_att);
a(0) = _sensorBias.accel_x; a(0) = _sensorBias.accel_x;
@@ -272,7 +271,6 @@ void LandingTargetEstimator::_check_params(const bool force)
void LandingTargetEstimator::_initialize_topics() void LandingTargetEstimator::_initialize_topics()
{ {
// subscribe to position, attitude, arming and velocity changes
_vehicleLocalPositionSub = orb_subscribe(ORB_ID(vehicle_local_position)); _vehicleLocalPositionSub = orb_subscribe(ORB_ID(vehicle_local_position));
_attitudeSub = orb_subscribe(ORB_ID(vehicle_attitude)); _attitudeSub = orb_subscribe(ORB_ID(vehicle_attitude));
_sensorBiasSub = orb_subscribe(ORB_ID(sensor_bias)); _sensorBiasSub = orb_subscribe(ORB_ID(sensor_bias));