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

wind_estimator: cleanup

Browse Source 
Signed-off-by: Roman <bapstroman@gmail.com>
This commit is contained in:
Roman
2018-03-21 18:46:58 +01:00
committed by Roman Bapst
parent 33c1bcb983
commit 5b067df01e
1 changed files with 17 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/wind_estimator/wind_estimator_main.cpp
+17 -26
View File
@@ -72,18 +72,12 @@ public:
*/ */
void cycle(); void cycle();
/** @see ModuleBase::print_status() */
int print_status() override;
private: private:
static struct work_s _work; static struct work_s _work;
WindEstimator _wind_estimator; WindEstimator _wind_estimator;
orb_advert_t _wind_est_pub{nullptr}; /**< wind estimate topic */ orb_advert_t _wind_est_pub{nullptr}; /**< wind estimate topic */
hrt_abstime _time_last_airspeed_fused{0}; /* timestamp of when last time airspeed measurement was fused into wind estimator */
hrt_abstime _time_last_beta_fused{0}; /* timestamp of when last time sideslip measurement was fused into wind estimator */
int _vehicle_attitude_sub{-1}; int _vehicle_attitude_sub{-1};
int _vehicle_local_position_sub{-1}; int _vehicle_local_position_sub{-1};
int _airspeed_sub{-1}; int _airspeed_sub{-1};
@@ -158,43 +152,45 @@ WindEstimatorModule::cycle()
vehicle_local_position_s vehicle_local_position = {}; vehicle_local_position_s vehicle_local_position = {};
airspeed_s airspeed = {}; airspeed_s airspeed = {};
orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &vehicle_attitude);
orb_copy(ORB_ID(airspeed), _airspeed_sub, &airspeed);
orb_copy(ORB_ID(vehicle_local_position), _vehicle_local_position_sub, &vehicle_local_position);
hrt_abstime time_now_usec = hrt_absolute_time(); hrt_abstime time_now_usec = hrt_absolute_time();
// update wind and airspeed estimator // update wind and airspeed estimator
_wind_estimator.update(time_now_usec); _wind_estimator.update(time_now_usec);
// limit airspeed and sideslip fusion rate to 20Hz // validate required conditions for the filter to fuse measurements
bool fuse_airspeed = (time_now_usec - _time_last_airspeed_fused) > 50 * 1000; bool fuse = time_now_usec - vehicle_local_position.timestamp < 1000 * 1000 && vehicle_local_position.timestamp;
bool fuse_beta = (time_now_usec - _time_last_beta_fused) > 50 * 1000; fuse &= time_now_usec - vehicle_attitude.timestamp < 1000 * 1000;
fuse &= vehicle_local_position.v_xy_valid;
if (fuse_beta || fuse_airspeed) { // additionally, for airspeed fusion we need to have recent measurements
bool fuse_airspeed = fuse && time_now_usec - airspeed.timestamp < 1000 * 1000;
// we need to fuse, so update topics
orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &vehicle_attitude); if (fuse) {
orb_copy(ORB_ID(airspeed), _airspeed_sub, &airspeed);
orb_copy(ORB_ID(vehicle_local_position), _vehicle_local_position_sub, &vehicle_local_position);
matrix::Dcmf R_to_earth(matrix::Quatf(vehicle_attitude.q)); matrix::Dcmf R_to_earth(matrix::Quatf(vehicle_attitude.q));
matrix::Vector3f vI(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz); matrix::Vector3f vI(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz);
if (fuse_beta && vehicle_local_position.v_xy_valid) { // sideslip fusion
_wind_estimator.fuse_beta(time_now_usec, &vI._data[0][0], vehicle_attitude.q); _wind_estimator.fuse_beta(time_now_usec, &vI._data[0][0], vehicle_attitude.q);
_time_last_beta_fused = time_now_usec;
}
if (fuse_airspeed && vehicle_local_position.v_xy_valid) { if (fuse_airspeed) {
matrix::Vector3f vel_var(vehicle_local_position.evh, vehicle_local_position.evh, vehicle_local_position.evv); matrix::Vector3f vel_var(vehicle_local_position.evh, vehicle_local_position.evh, vehicle_local_position.evv);
vel_var = R_to_earth * vel_var; vel_var = R_to_earth * vel_var;
//airspeed fusion
_wind_estimator.fuse_airspeed(time_now_usec, airspeed.indicated_airspeed_m_s, &vI._data[0][0], _wind_estimator.fuse_airspeed(time_now_usec, airspeed.indicated_airspeed_m_s, &vI._data[0][0],
&vel_var._data[0][0]); &vel_var._data[0][0]);
_time_last_airspeed_fused = time_now_usec;
} }
} }
// if we fused either airspeed or sideslip we publish a wind_estimate message // if we fused either airspeed or sideslip we publish a wind_estimate message
if (_time_last_beta_fused == time_now_usec || _time_last_airspeed_fused == time_now_usec) { if (fuse) {
wind_estimate_s wind_est = {}; wind_estimate_s wind_est = {};
wind_est.timestamp = time_now_usec; wind_est.timestamp = time_now_usec;
@@ -296,11 +292,6 @@ measured_airspeed = scale_factor * real_airspeed.
return 0; return 0;
} }
int WindEstimatorModule::print_status()
{
return 0;
}
extern "C" __EXPORT int wind_estimator_main(int argc, char *argv[]); extern "C" __EXPORT int wind_estimator_main(int argc, char *argv[]);
int int