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

distance_sensor: adopt message in both range drivers and on ekf_att_pos_estimator

Browse Source
This commit is contained in:
TSC21
2015-05-23 17:49:52 +01:00
parent 93f8d7c4e8
commit 43668cc8c8
9 changed files with 97 additions and 128 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
+2 -2
View File
@@ -58,13 +58,13 @@
#include <uORB/topics/home_position.h>
#include <uORB/topics/wind_estimate.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/distance_sensor.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_gyro.h>
#include <drivers/drv_accel.h>
#include <drivers/drv_mag.h>
#include <drivers/drv_baro.h>
#include <drivers/drv_range_finder.h>
#include <geo/geo.h>
#include <systemlib/perf_counter.h>
@@ -162,7 +162,7 @@ private:
struct vehicle_local_position_s _local_pos; /**< local vehicle position */
struct vehicle_gps_position_s _gps; /**< GPS position */
struct wind_estimate_s _wind; /**< wind estimate */
struct range_finder_report _distance; /**< distance estimate */
struct distance_sensor_s _distance; /**< distance estimate */
struct vehicle_land_detected_s _landDetector;
struct actuator_armed_s _armed;
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+6 -6
View File
@@ -501,7 +501,7 @@ void AttitudePositionEstimatorEKF::task_main()
/*
* do subscriptions
*/
_distance_sub = orb_subscribe(ORB_ID(sensor_range_finder));
_distance_sub = orb_subscribe(ORB_ID(distance_sensor));
_baro_sub = orb_subscribe_multi(ORB_ID(sensor_baro), 0);
_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
@@ -1471,11 +1471,11 @@ void AttitudePositionEstimatorEKF::pollData()
orb_check(_distance_sub, &_newRangeData);
if (_newRangeData) {
orb_copy(ORB_ID(sensor_range_finder), _distance_sub, &_distance);
if (_distance.valid) {
_ekf->rngMea = _distance.distance;
_distance_last_valid = _distance.timestamp;
orb_copy(ORB_ID(distance_sensor), _distance_sub, &_distance);
if ((_distance.current_distance > _distance.min_distance)
&& (_distance.current_distance < _distance.max_distance)) {
_ekf->rngMea = _distance.current_distance;
_distance_last_valid = _distance.time_boot_ms;
} else {
_newRangeData = false;