Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

wind_estimator fix LPWORK/HPWORK confusion and cleanup #9122

Merged
merged 2 commits into from
Mar 21, 2018
Merged

wind_estimator fix LPWORK/HPWORK confusion and cleanup #9122

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
d2afff1
wind_estimator fix LPWORK/HPWORK confusion and cleanup
dagar Mar 20, 2018
df46960
wind_estimator trim module documentation to fit in limit
dagar Mar 21, 2018
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 2 additions & 0 deletions src/modules/wind_estimator/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
)
136 changes: 78 additions & 58 deletions src/modules/wind_estimator/wind_estimator_main.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand All @@ -60,20 +64,17 @@ class WindEstimatorModule : public ModuleBase<WindEstimatorModule>, public Modul
/** @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;

Expand All @@ -85,6 +86,9 @@ class WindEstimatorModule : public ModuleBase<WindEstimatorModule>, public Modul
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,
Expand Down Expand Up @@ -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()
Expand All @@ -123,6 +129,9 @@ WindEstimatorModule::~WindEstimatorModule()
orb_unsubscribe(_param_sub);

orb_unadvertise(_wind_est_pub);

perf_free(_perf_elapsed);
perf_free(_perf_interval);
}

int
Expand All @@ -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;
}

_task_id = task_id_is_work_queue;
} else {
_task_id = task_id_is_work_queue;
return PX4_OK;
}

return PX4_OK;
return PX4_ERROR;
}

void
Expand All @@ -159,62 +169,70 @@ 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);

if (param_updated) {
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

// update wind and airspeed estimator
_wind_estimator.update(time_now_usec);
vehicle_attitude_s att = {};

// 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 (orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &att) == PX4_OK) {
att_valid = (time_now_usec - att.timestamp < 1000 * 1000) && (att.timestamp > 0);
}

// additionally, for airspeed fusion we need to have recent measurements
bool fuse_airspeed = fuse && time_now_usec - airspeed.timestamp < 1000 * 1000;
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);

if (fuse) {
if (lpos_valid && att_valid) {

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;
Expand All @@ -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));
}
}

Expand All @@ -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);

Expand All @@ -287,9 +299,9 @@ int WindEstimatorModule::print_usage(const char *reason)
R"DESCR_STR(
### Description
This module runs a combined wind and airspeed scale factor estimator.
If provided the vehicle NED speed and the vehicle attitude it can estimate the horizontal wind components based on a zero
If provided the vehicle NED speed and attitude it can estimate the horizontal wind components based on a zero
sideslip assumption. This makes the estimator only suitable for fixed wing vehicles.
If additionally provided an airspeed measurment this module also estimates an airspeed scale factor based on the following model:
If provided an airspeed measurement this module also estimates an airspeed scale factor based on the following model:
measured_airspeed = scale_factor * real_airspeed.

)DESCR_STR");
Expand All @@ -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
Expand Down