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

wind_estimator fix LPWORK/HPWORK confusion and cleanup

Browse Source
This commit is contained in:
Daniel Agar 2018-03-20 19:38:40 -04:00 committed by Roman Bapst
parent 8400d42988
commit f1bb61769f
2 changed files with 79 additions and 57 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/modules/wind_estimator/CMakeLists.txt
View File

@ -33,6 +33,8 @@
px4_add_module(
MODULE modules__wind_estimator
MAIN wind_estimator
INCLUDES
${PX4_SOURCE_DIR}/src/lib/ecl
SRCS
wind_estimator_main.cpp
)

134
src/modules/wind_estimator/wind_estimator_main.cpp
View File

@ -31,24 +31,28 @@
*
****************************************************************************/
#include <drivers/drv_hrt.h>
#include <ecl/airdata/WindEstimator.hpp>
#include <matrix/matrix/math.hpp>
#include <px4_module.h>
#include <px4_module_params.h>
#include <px4_workqueue.h>
#include <uORB/topics/vehicle_local_position.h>
#include <drivers/drv_hrt.h>
#include <matrix/matrix/math.hpp>
#include <systemlib/param/param.h>
#include <systemlib/perf_counter.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/wind_estimate.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/parameter_update.h>
#include <systemlib/param/param.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/wind_estimate.h>
#define SCHEDULE_INTERVAL 100000 /**< The schedule interval in usec (10 Hz) */
using matrix::Dcmf;
using matrix::Quatf;
using matrix::Vector2f;
using matrix::Vector3f;
class WindEstimatorModule : public ModuleBase<WindEstimatorModule>, public ModuleParams
{
public:
@ -60,20 +64,17 @@ public:
/** @see ModuleBase */
static int task_spawn(int argc, char *argv[]);
/** @see ModuleBase */
static WindEstimatorModule *instantiate(int argc, char *argv[]);
/** @see ModuleBase */
static int custom_command(int argc, char *argv[]);
/** @see ModuleBase */
static int print_usage(const char *reason = nullptr);
/**
* run the main loop
*/
// run the main loop
void cycle();
int print_status() override;
private:
static struct work_s _work;
@ -85,6 +86,9 @@ private:
int _airspeed_sub{-1};
int _param_sub{-1};
perf_counter_t _perf_elapsed{};
perf_counter_t _perf_interval{};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::WEST_W_P_NOISE>) wind_p_noise,
(ParamFloat<px4::params::WEST_SC_P_NOISE>) tas_scale_p_noise,
@ -113,6 +117,8 @@ WindEstimatorModule::WindEstimatorModule():
// initialise parameters
update_params();
_perf_elapsed = perf_alloc_once(PC_ELAPSED, "wind_estimator elapsed");
_perf_interval = perf_alloc_once(PC_INTERVAL, "wind_estimator interval");
}
WindEstimatorModule::~WindEstimatorModule()
@ -123,6 +129,9 @@ WindEstimatorModule::~WindEstimatorModule()
orb_unsubscribe(_param_sub);
orb_unadvertise(_wind_est_pub);
perf_free(_perf_elapsed);
perf_free(_perf_interval);
}
int
@ -134,12 +143,13 @@ WindEstimatorModule::task_spawn(int argc, char *argv[])
// wait until task is up & running
if (wait_until_running() < 0) {
_task_id = -1;
return -1;
} else {
_task_id = task_id_is_work_queue;
return PX4_OK;
}
_task_id = task_id_is_work_queue;
return PX4_OK;
return PX4_ERROR;
}
void
@ -159,12 +169,17 @@ WindEstimatorModule::cycle_trampoline(void *arg)
_object = dev;
}
dev->cycle();
if (dev) {
dev->cycle();
}
}
void
WindEstimatorModule::cycle()
{
perf_count(_perf_interval);
perf_begin(_perf_elapsed);
bool param_updated;
orb_check(_param_sub, &param_updated);
@ -172,49 +187,52 @@ WindEstimatorModule::cycle()
update_params();
}
vehicle_attitude_s vehicle_attitude = {};
vehicle_local_position_s vehicle_local_position = {};
airspeed_s airspeed = {};
bool lpos_valid = false;
bool att_valid = false;
bool airspeed_valid = false;
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);
const hrt_abstime time_now_usec = hrt_absolute_time();
hrt_abstime time_now_usec = hrt_absolute_time();
// validate required conditions for the filter to fuse measurements
vehicle_attitude_s att = {};
if (orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &att) == PX4_OK) {
att_valid = (time_now_usec - att.timestamp < 1000 * 1000) && (att.timestamp > 0);
}
vehicle_local_position_s lpos = {};
if (orb_copy(ORB_ID(vehicle_local_position), _vehicle_local_position_sub, &lpos) == PX4_OK) {
lpos_valid = (time_now_usec - lpos.timestamp < 1000 * 1000) && (lpos.timestamp > 0) && lpos.v_xy_valid;
}
// update wind and airspeed estimator
_wind_estimator.update(time_now_usec);
// validate required conditions for the filter to fuse measurements
bool fuse = time_now_usec - vehicle_local_position.timestamp < 1000 * 1000 && vehicle_local_position.timestamp;
fuse &= time_now_usec - vehicle_attitude.timestamp < 1000 * 1000;
fuse &= vehicle_local_position.v_xy_valid;
if (lpos_valid && att_valid) {
// additionally, for airspeed fusion we need to have recent measurements
bool fuse_airspeed = fuse && time_now_usec - airspeed.timestamp < 1000 * 1000;
if (fuse) {
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);
Vector3f vI(lpos.vx, lpos.vy, lpos.vz);
Quatf q(att.q);
// sideslip fusion
_wind_estimator.fuse_beta(time_now_usec, &vI._data[0][0], vehicle_attitude.q);
_wind_estimator.fuse_beta(time_now_usec, vI, q);
if (fuse_airspeed) {
matrix::Vector3f vel_var(vehicle_local_position.evh, vehicle_local_position.evh, vehicle_local_position.evv);
vel_var = R_to_earth * vel_var;
// additionally, for airspeed fusion we need to have recent measurements
airspeed_s airspeed = {};
//airspeed fusion
_wind_estimator.fuse_airspeed(time_now_usec, airspeed.indicated_airspeed_m_s, &vI._data[0][0],
&vel_var._data[0][0]);
if (orb_copy(ORB_ID(airspeed), _airspeed_sub, &airspeed) == PX4_OK) {
airspeed_valid = (time_now_usec - airspeed.timestamp < 1000 * 1000) && (airspeed.timestamp > 0);
}
}
if (airspeed_valid) {
Vector3f vel_var{Dcmf(q) *Vector3f{lpos.evh, lpos.evh, lpos.evv}};
// if we fused either airspeed or sideslip we publish a wind_estimate message
if (fuse) {
// airspeed fusion
_wind_estimator.fuse_airspeed(time_now_usec, airspeed.indicated_airspeed_m_s, vI, Vector2f{vel_var(0), vel_var(1)});
}
// if we fused either airspeed or sideslip we publish a wind_estimate message
wind_estimate_s wind_est = {};
wind_est.timestamp = time_now_usec;
@ -236,12 +254,14 @@ WindEstimatorModule::cycle()
orb_publish_auto(ORB_ID(wind_estimate), &_wind_est_pub, &wind_est, &instance, ORB_PRIO_DEFAULT);
}
perf_end(_perf_elapsed);
if (should_exit()) {
exit_and_cleanup();
} else {
/* schedule next cycle */
work_queue(HPWORK, &_work, (worker_t)&WindEstimatorModule::cycle_trampoline, this, USEC2TICK(SCHEDULE_INTERVAL));
work_queue(LPWORK, &_work, (worker_t)&WindEstimatorModule::cycle_trampoline, this, USEC2TICK(SCHEDULE_INTERVAL));
}
}
@ -254,18 +274,10 @@ void WindEstimatorModule::update_params()
_wind_estimator.set_tas_scale_p_noise(tas_scale_p_noise.get());
_wind_estimator.set_tas_noise(tas_noise.get());
_wind_estimator.set_beta_noise(beta_noise.get());
}
WindEstimatorModule *WindEstimatorModule::instantiate(int argc, char *argv[])
{
// No arguments to parse. We also know that we should run as task
return new WindEstimatorModule();
}
int WindEstimatorModule::custom_command(int argc, char *argv[])
{
/* start the FMU if not running */
if (!is_running()) {
int ret = WindEstimatorModule::task_spawn(argc, argv);
@ -301,6 +313,14 @@ measured_airspeed = scale_factor * real_airspeed.
return 0;
}
int WindEstimatorModule::print_status()
{
perf_print_counter(_perf_elapsed);
perf_print_counter(_perf_interval);
return 0;
}
extern "C" __EXPORT int wind_estimator_main(int argc, char *argv[]);
int