dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Airspeed Selector: repurpose wind estimator into an airspeed (selection, validation) module. 

This new airspeed module does:
  -runns an airspeed validator for every airspeed sensor present, which checks measurement validity and estimates an airspeed scale
  -selects another airspeed sensor if for the current one a failure is detected
  -estimates airspeed with groundspeed-windspeed if no valid airspeed sensor is present
  -outputs airspeed_validated topic

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2019-08-07 13:06:00 +02:00 committed by Roman Bapst
parent 970e362e9a
commit cedf14e2ba
65 changed files with 767 additions and 461 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ROMFS/px4fmu_common/init.d-posix/1030_plane
View File

@ -37,7 +37,6 @@ then
param set RWTO_TKOFF 1
param set WEST_EN 1
fi
set MIXER_FILE etc/mixers-sitl/plane_sitl.main.mix

1
ROMFS/px4fmu_common/init.d-posix/1040_standard_vtol
View File

@ -38,7 +38,6 @@ then
param set VT_MOT_COUNT 4
param set VT_TYPE 2
param set WEST_EN 1
fi
set MAV_TYPE 22

1
ROMFS/px4fmu_common/init.d-posix/1041_tailsitter
View File

@ -38,7 +38,6 @@ then
param set VT_MOT_COUNT 0
param set VT_TYPE 0
param set WEST_EN 1
fi
set MAV_TYPE 20

1
ROMFS/px4fmu_common/init.d-posix/1042_tiltrotor
View File

@ -41,7 +41,6 @@ then
param set VT_ELEV_MC_LOCK 0
param set VT_TYPE 1
param set WEST_EN 1
fi
set MAV_TYPE 21

5
ROMFS/px4fmu_common/init.d-posix/rcS
View File

@ -164,7 +164,6 @@ then
param set TRIG_INTERFACE 3
param set WEST_EN 0
fi
# Adapt timeout parameters if simulation runs faster or slower than realtime.
@ -213,10 +212,6 @@ sensors start
commander start
navigator start
if param compare WEST_EN 1
then
wind_estimator start
fi
if ! param compare MNT_MODE_IN -1
then

3
ROMFS/px4fmu_common/init.d/rc.fw_apps
View File

@ -15,9 +15,8 @@ ekf2 start
#
fw_att_control start
fw_pos_control_l1 start
airspeed_selector start
#
# Start Land Detector.
#
land_detector start fixedwing

2
ROMFS/px4fmu_common/init.d/rc.vtol_apps
View File

@ -21,10 +21,10 @@ mc_att_control start
mc_pos_control start
fw_att_control start
fw_pos_control_l1 start
airspeed_selector start
#
# Start Land Detector
# Multicopter for now until we have something for VTOL
#
land_detector start vtol

8
ROMFS/px4fmu_common/init.d/rcS
View File

@ -435,14 +435,6 @@ else
#
navigator start
#
# Start the standalone wind estimator.
#
if param compare WEST_EN 1
then
wind_estimator start
fi
#
# Start a thermal calibration if required.
#

2
boards/aerotenna/ocpoc/ubuntu.cmake
View File

@ -52,7 +52,7 @@ px4_add_board(
#simulator
vmount
vtol_att_control
wind_estimator
airspeed_selector
SYSTEMCMDS
esc_calib

2
boards/airmind/mindpx-v2/default.cmake
View File

@ -72,7 +72,7 @@ px4_add_board(
sih
vmount
vtol_att_control
wind_estimator
airspeed_selector
SYSTEMCMDS
bl_update

2
boards/atlflight/eagle/default.cmake
View File

@ -87,7 +87,7 @@ px4_add_board(
simulator
vmount
vtol_att_control
wind_estimator
airspeed_selector
SYSTEMCMDS
#bl_update

2
boards/atlflight/eagle/qurt-default.cmake
View File

@ -76,7 +76,7 @@ px4_add_board(
#sih
vmount
vtol_att_control
wind_estimator
airspeed_selector
SYSTEMCMDS
param

2
boards/atlflight/excelsior/default.cmake
View File

@ -87,7 +87,7 @@ px4_add_board(
simulator
vmount
vtol_att_control
wind_estimator
airspeed_selector
SYSTEMCMDS
#bl_update

2
boards/atlflight/excelsior/qurt-default.cmake
View File

@ -76,7 +76,7 @@ px4_add_board(
sih
vmount
vtol_att_control
wind_estimator
airspeed_selector
SYSTEMCMDS
param

2
boards/auav/x21/default.cmake
View File

@ -78,7 +78,7 @@ px4_add_board(
sih
vmount
vtol_att_control
wind_estimator
airspeed_selector
SYSTEMCMDS
bl_update

2
boards/av/x-v1/default.cmake
View File

@ -79,7 +79,7 @@ px4_add_board(
sih
vmount
vtol_att_control
wind_estimator
airspeed_selector
SYSTEMCMDS
#bl_update

2
boards/beaglebone/blue/cross.cmake
View File

@ -50,7 +50,7 @@ px4_add_board(
sih
vmount
vtol_att_control
wind_estimator
airspeed_selector
SYSTEMCMDS
esc_calib

2
boards/beaglebone/blue/native.cmake
View File

@ -48,7 +48,7 @@ px4_add_board(
sih
vmount
vtol_att_control
wind_estimator
airspeed_selector
SYSTEMCMDS
esc_calib

2
boards/bitcraze/crazyflie/default.cmake
View File

@ -38,7 +38,7 @@ px4_add_board(
sensors
sih
#vtol_att_control
wind_estimator
airspeed_selector
SYSTEMCMDS
bl_update

2
boards/emlid/navio2/cross.cmake
View File

@ -55,7 +55,7 @@ px4_add_board(
#simulator
vmount
vtol_att_control
wind_estimator
airspeed_selector
SYSTEMCMDS
dyn

2
boards/emlid/navio2/native.cmake
View File

@ -53,7 +53,7 @@ px4_add_board(
#simulator
vmount
vtol_att_control
wind_estimator
airspeed_selector
SYSTEMCMDS
dyn

2
boards/intel/aerofc-v1/default.cmake
View File

@ -55,7 +55,7 @@ px4_add_board(
sih
vmount
#vtol_att_control
#wind_estimator
#airspeed_selector
SYSTEMCMDS
bl_update

2
boards/intel/aerofc-v1/rtps.cmake
View File

@ -59,7 +59,7 @@ px4_add_board(
sih
vmount
#vtol_att_control
#wind_estimator
#airspeed_selector
SYSTEMCMDS
bl_update

2
boards/nxp/fmuk66-v3/default.cmake
View File

@ -74,7 +74,7 @@ px4_add_board(
sih
vmount
vtol_att_control
wind_estimator
airspeed_selector
SYSTEMCMDS
bl_update

2
boards/omnibus/f4sd/default.cmake
View File

@ -67,7 +67,7 @@ px4_add_board(
sih
#vmount
#vtol_att_control
#wind_estimator
#airspeed_selector
SYSTEMCMDS
#bl_update

2
boards/parrot/bebop/default.cmake
View File

@ -41,7 +41,7 @@ px4_add_board(
sensors
sih
#vtol_att_control
wind_estimator
airspeed_selector
SYSTEMCMDS
#config

2
boards/px4/fmu-v2/default.cmake
View File

@ -86,7 +86,7 @@ px4_add_board(
sensors
vmount
vtol_att_control
#wind_estimator
#airspeed_selector
SYSTEMCMDS
bl_update

2
boards/px4/fmu-v2/fixedwing.cmake
View File

@ -58,7 +58,7 @@ px4_add_board(
navigator
sensors
vmount
wind_estimator
airspeed_selector
SYSTEMCMDS
bl_update

2
boards/px4/fmu-v2/lpe.cmake
View File

@ -82,7 +82,7 @@ px4_add_board(
sensors
vmount
#vtol_att_control
#wind_estimator
#airspeed_selector
SYSTEMCMDS
bl_update

2
boards/px4/fmu-v2/test.cmake
View File

@ -82,7 +82,7 @@ px4_add_board(
sensors
vmount
vtol_att_control
#wind_estimator
#airspeed_selector
SYSTEMCMDS
#bl_update

2
boards/px4/fmu-v3/default.cmake
View File

@ -86,7 +86,7 @@ px4_add_board(
sih
vmount
vtol_att_control
wind_estimator
airspeed_selector
SYSTEMCMDS
bl_update

2
boards/px4/fmu-v3/rtps.cmake
View File

@ -86,7 +86,7 @@ px4_add_board(
sih
vmount
vtol_att_control
wind_estimator
airspeed_selector
SYSTEMCMDS
bl_update

2
boards/px4/fmu-v3/stackcheck.cmake
View File

@ -85,7 +85,7 @@ px4_add_board(
sih
vmount
vtol_att_control
wind_estimator
airspeed_selector
SYSTEMCMDS
bl_update

2
boards/px4/fmu-v4/default.cmake
View File

@ -70,7 +70,7 @@ px4_add_board(
sih
vmount
vtol_att_control
wind_estimator
airspeed_selector
SYSTEMCMDS
bl_update

2
boards/px4/fmu-v4/rtps.cmake
View File

@ -73,7 +73,7 @@ px4_add_board(
sih
vmount
vtol_att_control
wind_estimator
airspeed_selector
SYSTEMCMDS
bl_update

2
boards/px4/fmu-v4/stackcheck.cmake
View File

@ -70,7 +70,7 @@ px4_add_board(
sih
vmount
vtol_att_control
wind_estimator
airspeed_selector
SYSTEMCMDS
bl_update

2
boards/px4/fmu-v4pro/default.cmake
View File

@ -84,7 +84,7 @@ px4_add_board(
sih
vmount
vtol_att_control
wind_estimator
airspeed_selector
SYSTEMCMDS
bl_update

2
boards/px4/fmu-v4pro/rtps.cmake
View File

@ -84,7 +84,7 @@ px4_add_board(
sih
vmount
vtol_att_control
wind_estimator
airspeed_selector
SYSTEMCMDS
bl_update

2
boards/px4/fmu-v5/default.cmake
View File

@ -84,7 +84,7 @@ px4_add_board(
sih
vmount
vtol_att_control
wind_estimator
airspeed_selector
SYSTEMCMDS
bl_update

2
boards/px4/fmu-v5/fixedwing.cmake
View File

@ -58,7 +58,7 @@ px4_add_board(
navigator
sensors
vmount
wind_estimator
airspeed_selector
SYSTEMCMDS
bl_update

2
boards/px4/fmu-v5/multicopter.cmake
View File

@ -67,7 +67,7 @@ px4_add_board(
sensors
sih
vmount
wind_estimator
airspeed_selector
SYSTEMCMDS
bl_update

2
boards/px4/fmu-v5/rtps.cmake
View File

@ -83,7 +83,7 @@ px4_add_board(
sih
vmount
vtol_att_control
wind_estimator
airspeed_selector
SYSTEMCMDS
bl_update

2
boards/px4/fmu-v5/stackcheck.cmake
View File

@ -83,7 +83,7 @@ px4_add_board(
sih
vmount
vtol_att_control
wind_estimator
airspeed_selector
SYSTEMCMDS
bl_update

2
boards/px4/fmu-v5x/default.cmake
View File

@ -84,7 +84,7 @@ px4_add_board(
sih
vmount
vtol_att_control
wind_estimator
airspeed_selector
SYSTEMCMDS
bl_update

2
boards/px4/fmu-v5x/fixedwing.cmake
View File

@ -68,7 +68,7 @@ px4_add_board(
navigator
sensors
vmount
wind_estimator
airspeed_selector
SYSTEMCMDS
bl_update

2
boards/px4/fmu-v5x/multicopter.cmake
View File

@ -74,7 +74,7 @@ px4_add_board(
sensors
sih
vmount
wind_estimator
airspeed_selector
SYSTEMCMDS
bl_update

2
boards/px4/fmu-v5x/rtps.cmake
View File

@ -85,7 +85,7 @@ px4_add_board(
sih
vmount
vtol_att_control
wind_estimator
airspeed_selector
SYSTEMCMDS
bl_update

2
boards/px4/fmu-v5x/stackcheck.cmake
View File

@ -84,7 +84,7 @@ px4_add_board(
sih
vmount
vtol_att_control
wind_estimator
airspeed_selector
SYSTEMCMDS
bl_update

2
boards/px4/raspberrypi/cross.cmake
View File

@ -47,7 +47,7 @@ px4_add_board(
sih
vmount
vtol_att_control
wind_estimator
airspeed_selector
SYSTEMCMDS
dyn

2
boards/px4/raspberrypi/native.cmake
View File

@ -46,7 +46,7 @@ px4_add_board(
#simulator
vmount
vtol_att_control
wind_estimator
airspeed_selector
SYSTEMCMDS
dyn

2
boards/px4/sitl/default.cmake
View File

@ -45,7 +45,7 @@ px4_add_board(
simulator
vmount
vtol_att_control
wind_estimator
airspeed_selector
SYSTEMCMDS
#bl_update

2
boards/px4/sitl/rtps.cmake
View File

@ -46,7 +46,7 @@ px4_add_board(
simulator
vmount
vtol_att_control
wind_estimator
airspeed_selector
SYSTEMCMDS
#bl_update

2
boards/px4/sitl/test.cmake
View File

@ -45,7 +45,7 @@ px4_add_board(
simulator
vmount
vtol_att_control
wind_estimator
airspeed_selector
SYSTEMCMDS
#bl_update

1
msg/CMakeLists.txt
View File

@ -38,6 +38,7 @@ set(msg_files
actuator_outputs.msg
adc_report.msg
airspeed.msg
airspeed_validated.msg
battery_status.msg
camera_capture.msg
camera_trigger.msg

10
msg/airspeed_validated.msg Normal file
View File

@ -0,0 +1,10 @@
uint64 timestamp # time since system start (microseconds)
float32 indicated_airspeed_m_s # indicated airspeed in m/s (IAS), set to NAN if invalid
float32 equivalent_airspeed_m_s # equivalent airspeed in m/s (accounts for instrumentation errors) (EAS), set to NAN if invalid
float32 true_airspeed_m_s # true filtered airspeed in m/s (TAS), set to NAN if invalid
float32 true_ground_minus_wind_m_s # TAS calculated from groundspeed - windspeed, where windspeed is estimated based on a zero-sideslip assumption, set to NAN if invalid
float32 indicated_ground_minus_wind_m_s # IAS calculated from groundspeed - windspeed, where windspeed is estimated based on a zero-sideslip assumption, set to NAN if invalid
int8 selected_airspeed_index # 0-2: airspeed sensor index, -1: groundspeed-windspeed, -2: airspeed invalid

6
posix-configs/SITL/init/test/test_shutdown
View File

@ -29,7 +29,7 @@ mc_pos_control start
mc_att_control start
fw_pos_control_l1 start
fw_att_control start
wind_estimator start
airspeed_selector start
#mixer load /dev/pwm_output0 ROMFS/sitl/mixers/standard_vtol_sitl.main.mix
@ -56,7 +56,7 @@ mc_pos_control status
mc_att_control status
fw_pos_control_l1 status
fw_att_control status
wind_estimator status
airspeed_selector status
dataman status
uorb status
@ -72,7 +72,7 @@ navigator stop
commander stop
land_detector stop
ekf2 stop
wind_estimator stop
airspeed_selector stop
sensors stop
sleep 2

2
src/lib/AirspeedValidator/AirspeedValidator.hpp
View File

@ -99,7 +99,7 @@ public:
void set_wind_estimator_beta_gate(uint8_t gate_size) { _wind_estimator.set_beta_gate(gate_size); }
void set_wind_estimator_scale_estimation_on(bool scale_estimation_on) {_wind_estimator_scale_estimation_on = scale_estimation_on;}
void set_airspeed_scale(float airspeed_scale_manual) { _wind_estimator.enforce_airspeed_scale(airspeed_scale_manual);}
void set_airspeed_scale(float airspeed_scale_manual) { _wind_estimator.enforce_airspeed_scale(1.0f / airspeed_scale_manual);} // scale is inverted inside the wind estimator
// setters for failure detection tuning parameters
void set_tas_innov_threshold(float tas_innov_threshold) { _tas_innov_threshold = tas_innov_threshold; }

9
src/modules/wind_estimator/CMakeLists.txt → src/modules/airspeed_selector/CMakeLists.txt
View File

@ -31,13 +31,12 @@
#
############################################################################
px4_add_module(
MODULE modules__wind_estimator
MAIN wind_estimator
INCLUDES
${PX4_SOURCE_DIR}/src/lib/ecl
MODULE modules__airspeed_selector
MAIN airspeed_selector
SRCS
wind_estimator_main.cpp
airspeed_selector_main.cpp
DEPENDS
git_ecl
ecl_airdata
AirspeedValidator
)

601
src/modules/airspeed_selector/airspeed_selector_main.cpp Normal file
View File

@ -0,0 +1,601 @@
/****************************************************************************
*
* Copyright (c) 2018 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include <drivers/drv_hrt.h>
#include <ecl/airdata/WindEstimator.hpp>
#include <matrix/math.hpp>
#include <parameters/param.h>
#include <perf/perf_counter.h>
#include <px4_module.h>
#include <px4_module_params.h>
#include <px4_work_queue/ScheduledWorkItem.hpp>
#include <lib/airspeed/airspeed.h>
#include <AirspeedValidator.hpp>
#include <systemlib/mavlink_log.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/airspeed_validated.h>
#include <uORB/topics/estimator_status.h>
#include <uORB/topics/mavlink_log.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_acceleration.h>
#include <uORB/topics/vehicle_air_data.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vtol_vehicle_status.h>
#include <uORB/topics/wind_estimate.h>
using namespace time_literals;
static constexpr uint32_t SCHEDULE_INTERVAL{100_ms}; /**< The schedule interval in usec (10 Hz) */
using matrix::Dcmf;
using matrix::Quatf;
using matrix::Vector2f;
using matrix::Vector3f;
class AirspeedModule : public ModuleBase<AirspeedModule>, public ModuleParams,
public px4::ScheduledWorkItem
{
public:
AirspeedModule();
~AirspeedModule();
/** @see ModuleBase */
static int task_spawn(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 */
void Run() override;
int print_status() override;
private:
static constexpr int MAX_NUM_AIRSPEED_SENSORS = 3; /**< Support max 3 airspeed sensors */
orb_advert_t _airspeed_validated_pub {nullptr}; /**< airspeed validated topic*/
orb_advert_t _wind_est_pub[MAX_NUM_AIRSPEED_SENSORS + 1] {}; /**< wind estimate topic (for each airspeed validator + purely sideslip fusion) */
orb_advert_t _mavlink_log_pub {nullptr}; /**< mavlink log topic*/
uORB::Subscription _estimator_status_sub{ORB_ID(estimator_status)};
uORB::Subscription _param_sub{ORB_ID(parameter_update)};
uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};
uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)};
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
uORB::Subscription _vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)};
estimator_status_s _estimator_status {};
parameter_update_s _parameter_update {};
vehicle_acceleration_s _accel {};
vehicle_air_data_s _vehicle_air_data {};
vehicle_attitude_s _vehicle_attitude {};
vehicle_land_detected_s _vehicle_land_detected {};
vehicle_local_position_s _vehicle_local_position {};
vehicle_status_s _vehicle_status {};
vtol_vehicle_status_s _vtol_vehicle_status {};
WindEstimator _wind_estimator_sideslip; /**< wind estimator instance only fusing sideslip */
wind_estimate_s _wind_estimate_sideslip {}; /**< wind estimate message for wind estimator instance only fusing sideslip */
int _airspeed_sub[MAX_NUM_AIRSPEED_SENSORS] {}; /**< raw airspeed topics subscriptions. Max 3 airspeeds sensors. */
int _number_of_airspeed_sensors{0}; /**< number of airspeed sensors in use (detected during initialization)*/
AirspeedValidator *_airspeed_validator{nullptr}; /**< airspeedValidator instances (one for each sensor, assigned dynamically during startup) */
int _valid_airspeed_index{-1}; /**< index of currently chosen (valid) airspeed sensor */
int _prev_airspeed_index{-1}; /**< previously chosen airspeed sensor index */
bool _initialized{false}; /**< module initialized*/
bool _vehicle_local_position_valid{false}; /**< local position (from GPS) valid */
bool _in_takeoff_situation{true}; /**< in takeoff situation (defined as not yet stall speed reached) */
float _ground_minus_wind_TAS{0.0f}; /**< true airspeed from groundspeed minus windspeed */
float _ground_minus_wind_EAS{0.0f}; /**< true airspeed from groundspeed minus windspeed */
bool _scale_estimation_previously_on{false}; /**< scale_estimation was on in the last cycle */
perf_counter_t _perf_elapsed{};
perf_counter_t _perf_interval{};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::ARSP_W_P_NOISE>) _param_west_w_p_noise,
(ParamFloat<px4::params::ARSP_SC_P_NOISE>) _param_west_sc_p_noise,
(ParamFloat<px4::params::ARSP_TAS_NOISE>) _param_west_tas_noise,
(ParamFloat<px4::params::ARSP_BETA_NOISE>) _param_west_beta_noise,
(ParamInt<px4::params::ARSP_TAS_GATE>) _param_west_tas_gate,
(ParamInt<px4::params::ARSP_BETA_GATE>) _param_west_beta_gate,
(ParamInt<px4::params::ARSP_SCALE_EST>) _param_west_scale_estimation_on,
(ParamFloat<px4::params::ARSP_ARSP_SCALE>) _param_west_airspeed_scale,
(ParamFloat<px4::params::COM_TAS_FS_INNOV>) _tas_innov_threshold, /**< innovation check threshold */
(ParamFloat<px4::params::COM_TAS_FS_INTEG>) _tas_innov_integ_threshold, /**< innovation check integrator threshold */
(ParamInt<px4::params::COM_TAS_FS_T1>) _checks_fail_delay, /**< delay to declare airspeed invalid */
(ParamInt<px4::params::COM_TAS_FS_T2>) _checks_clear_delay, /**< delay to declare airspeed valid again */
(ParamFloat<px4::params::COM_ASPD_STALL>) _airspeed_stall /**< stall speed*/
)
int start();
void update_params(); /**< update parameters */
void poll_topics(); /**< poll all topics required beside airspeed (e.g. current temperature) */
void update_wind_estimator_sideslip(); /**< update the wind estimator instance only fusing sideslip */
void update_ground_minus_wind_airspeed(); /**< update airspeed estimate based on groundspeed minus windspeed */
void select_airspeed_and_publish(); /**< select airspeed sensor (or groundspeed-windspeed) */
void publish_wind_estimates(); /**< publish wind estimator states (from all wind estimators running) */
};
AirspeedModule::AirspeedModule():
ModuleParams(nullptr),
ScheduledWorkItem(px4::wq_configurations::lp_default)
{
// initialise parameters
update_params();
_perf_elapsed = perf_alloc_once(PC_ELAPSED, "airspeed_selector elapsed");
_perf_interval = perf_alloc_once(PC_INTERVAL, "airspeed_selector interval");
}
AirspeedModule::~AirspeedModule()
{
ScheduleClear();
for (int i = 0; i < MAX_NUM_AIRSPEED_SENSORS; i++) {
if (_wind_est_pub[i] != nullptr) {
orb_unadvertise(_wind_est_pub[i]);
}
}
orb_unadvertise(_airspeed_validated_pub);
perf_free(_perf_elapsed);
perf_free(_perf_interval);
if (_airspeed_validator != nullptr) {
delete[] _airspeed_validator;
}
}
int
AirspeedModule::task_spawn(int argc, char *argv[])
{
AirspeedModule *dev = new AirspeedModule();
// check if the trampoline is called for the first time
if (!dev) {
PX4_ERR("alloc failed");
return PX4_ERROR;
}
_object.store(dev);
if (dev) {
dev->ScheduleOnInterval(SCHEDULE_INTERVAL, 10000);
_task_id = task_id_is_work_queue;
return PX4_OK;
}
return PX4_ERROR;
}
void
AirspeedModule::Run()
{
perf_count(_perf_interval);
perf_begin(_perf_elapsed);
/* the first time we run through here, initialize N airspeedValidator
* instances (N = number of airspeed sensors detected)
*/
if (!_initialized) {
for (int i = 0; i < MAX_NUM_AIRSPEED_SENSORS; i++) {
if (orb_exists(ORB_ID(airspeed), i) != 0) {
continue;
}
_number_of_airspeed_sensors = i + 1;
}
_airspeed_validator = new AirspeedValidator[_number_of_airspeed_sensors];
if (_number_of_airspeed_sensors > 0) {
for (int i = 0; i < _number_of_airspeed_sensors; i++) {
_airspeed_sub[i] = orb_subscribe_multi(ORB_ID(airspeed), i);
_valid_airspeed_index = 0; // set index to first sensor
_prev_airspeed_index = 0; // set index to first sensor
}
}
_initialized = true;
}
parameter_update_s update;
if (_param_sub.update(&update)) {
update_params();
}
poll_topics();
update_wind_estimator_sideslip();
update_ground_minus_wind_airspeed();
if (_airspeed_validator != nullptr) {
bool armed = (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
bool fixed_wing = !_vtol_vehicle_status.vtol_in_rw_mode;
bool in_air = !_vehicle_land_detected.landed;
/* Prepare data for airspeed_validator */
struct airspeed_validator_update_data input_data = {};
input_data.timestamp = hrt_absolute_time();
input_data.lpos_vx = _vehicle_local_position.vx;
input_data.lpos_vy = _vehicle_local_position.vy;
input_data.lpos_vz = _vehicle_local_position.vz;
input_data.lpos_valid = _vehicle_local_position_valid;
input_data.lpos_evh = _vehicle_local_position.evh;
input_data.lpos_evv = _vehicle_local_position.evv;
input_data.att_q[0] = _vehicle_attitude.q[0];
input_data.att_q[1] = _vehicle_attitude.q[1];
input_data.att_q[2] = _vehicle_attitude.q[2];
input_data.att_q[3] = _vehicle_attitude.q[3];
input_data.air_pressure_pa = _vehicle_air_data.baro_pressure_pa;
input_data.accel_z = _accel.xyz[2];
input_data.vel_test_ratio = _estimator_status.vel_test_ratio;
input_data.mag_test_ratio = _estimator_status.mag_test_ratio;
/* iterate through all airspeed sensors, poll new data from them and update their validators */
for (int i = 0; i < _number_of_airspeed_sensors; i++) {
/* poll airspeed data */
airspeed_s airspeed_raw = {};
orb_copy(ORB_ID(airspeed), _airspeed_sub[i], &airspeed_raw); // poll raw airspeed topic of the i-th sensor
input_data.airspeed_indicated_raw = airspeed_raw.indicated_airspeed_m_s;
input_data.airspeed_true_raw = airspeed_raw.true_airspeed_m_s;
input_data.airspeed_timestamp = airspeed_raw.timestamp;
input_data.air_temperature_celsius = airspeed_raw.air_temperature_celsius;
/* update in_fixed_wing_flight for the current airspeed sensor validator */
/* takeoff situation is active from start till one of the sensors' IAS or groundspeed_EAS is above stall speed */
if (airspeed_raw.indicated_airspeed_m_s > _airspeed_stall.get() || _ground_minus_wind_EAS > _airspeed_stall.get()) {
_in_takeoff_situation = false;
}
/* reset takeoff_situation to true when not in air or not in fixed-wing mode */
if (!in_air || !fixed_wing) {
_in_takeoff_situation = true;
}
input_data.in_fixed_wing_flight = (armed && fixed_wing && in_air && !_in_takeoff_situation);
/* push input data into airspeed validator */
_airspeed_validator[i].update_airspeed_validator(input_data);
}
}
select_airspeed_and_publish();
perf_end(_perf_elapsed);
if (should_exit()) {
exit_and_cleanup();
}
}
void AirspeedModule::update_params()
{
updateParams();
/* update wind estimator (sideslip fusion only) parameters */
_wind_estimator_sideslip.set_wind_p_noise(_param_west_w_p_noise.get());
_wind_estimator_sideslip.set_tas_scale_p_noise(_param_west_sc_p_noise.get());
_wind_estimator_sideslip.set_tas_noise(_param_west_tas_noise.get());
_wind_estimator_sideslip.set_beta_noise(_param_west_beta_noise.get());
_wind_estimator_sideslip.set_tas_gate(_param_west_tas_gate.get());
_wind_estimator_sideslip.set_beta_gate(_param_west_beta_gate.get());
/* update airspeedValidator parameters */
for (int i = 0; i < _number_of_airspeed_sensors; i++) {
_airspeed_validator[i].set_wind_estimator_wind_p_noise(_param_west_w_p_noise.get());
_airspeed_validator[i].set_wind_estimator_tas_scale_p_noise(_param_west_sc_p_noise.get());
_airspeed_validator[i].set_wind_estimator_tas_noise(_param_west_tas_noise.get());
_airspeed_validator[i].set_wind_estimator_beta_noise(_param_west_beta_noise.get());
_airspeed_validator[i].set_wind_estimator_tas_gate(_param_west_tas_gate.get());
_airspeed_validator[i].set_wind_estimator_beta_gate(_param_west_beta_gate.get());
_airspeed_validator[i].set_wind_estimator_scale_estimation_on(_param_west_scale_estimation_on.get());
/* Only apply manual entered airspeed scale to first airspeed measurement */
_airspeed_validator[0].set_airspeed_scale(_param_west_airspeed_scale.get());
_airspeed_validator[i].set_tas_innov_threshold(_tas_innov_threshold.get());
_airspeed_validator[i].set_tas_innov_integ_threshold(_tas_innov_integ_threshold.get());
_airspeed_validator[i].set_checks_fail_delay(_checks_fail_delay.get());
_airspeed_validator[i].set_checks_clear_delay(_checks_clear_delay.get());
_airspeed_validator[i].set_airspeed_stall(_airspeed_stall.get());
}
/* when airspeed scale estimation is turned on and the airspeed is valid, then set the scale inside the wind estimator to -1 such that it starts to estimate it */
if (!_scale_estimation_previously_on && _param_west_scale_estimation_on.get()) {
if (_valid_airspeed_index >= 0) {
_airspeed_validator[0].set_airspeed_scale(
-1.0f); // set it to a negative value to start estimation inside wind estimator
} else {
mavlink_and_console_log_info(&_mavlink_log_pub, "Airspeed: can't estimate scale as no valid sensor.");
_param_west_scale_estimation_on.set(0); // reset this param to 0 as estimation was not turned on
_param_west_scale_estimation_on.commit_no_notification();
}
/* If one sensor is valid and we switched out of scale estimation, then publish message and change the value of param ARSP_ARSP_SCALE */
} else if (_scale_estimation_previously_on && !_param_west_scale_estimation_on.get()) {
if (_valid_airspeed_index >= 0) {
_param_west_airspeed_scale.set(_airspeed_validator[_valid_airspeed_index].get_EAS_scale());
_param_west_airspeed_scale.commit_no_notification();
_airspeed_validator[_valid_airspeed_index].set_airspeed_scale(_param_west_airspeed_scale.get());
mavlink_and_console_log_info(&_mavlink_log_pub, "Airspeed: estimated scale (ARSP_ARSP_SCALE): %0.2f",
(double)_airspeed_validator[_valid_airspeed_index].get_EAS_scale());
} else {
mavlink_and_console_log_info(&_mavlink_log_pub, "Airspeed: can't estimate scale as no valid sensor.");
}
}
_scale_estimation_previously_on = _param_west_scale_estimation_on.get();
}
void AirspeedModule::poll_topics()
{
_estimator_status_sub.update(&_estimator_status);
_vehicle_acceleration_sub.update(&_accel);
_vehicle_air_data_sub.update(&_vehicle_air_data);
_vehicle_attitude_sub.update(&_vehicle_attitude);
_vehicle_land_detected_sub.update(&_vehicle_land_detected);
_vehicle_status_sub.update(&_vehicle_status);
_vtol_vehicle_status_sub.update(&_vtol_vehicle_status);
_vehicle_local_position_sub.update(&_vehicle_local_position);
_vehicle_local_position_valid = (hrt_absolute_time() - _vehicle_local_position.timestamp < 1_s)
&& (_vehicle_local_position.timestamp > 0) && _vehicle_local_position.v_xy_valid;
}
void AirspeedModule::update_wind_estimator_sideslip()
{
bool att_valid = true; // TODO: check if attitude is valid
const hrt_abstime time_now_usec = hrt_absolute_time();
/* update wind and airspeed estimator */
_wind_estimator_sideslip.update(time_now_usec);
if (_vehicle_local_position_valid && att_valid) {
Vector3f vI(_vehicle_local_position.vx, _vehicle_local_position.vy, _vehicle_local_position.vz);
Quatf q(_vehicle_attitude.q);
/* sideslip fusion */
_wind_estimator_sideslip.fuse_beta(time_now_usec, vI, q);
}
/* fill message for publishing later */
_wind_estimate_sideslip.timestamp = time_now_usec;
float wind[2];
_wind_estimator_sideslip.get_wind(wind);
_wind_estimate_sideslip.windspeed_north = wind[0];
_wind_estimate_sideslip.windspeed_east = wind[1];
float wind_cov[2];
_wind_estimator_sideslip.get_wind_var(wind_cov);
_wind_estimate_sideslip.variance_north = wind_cov[0];
_wind_estimate_sideslip.variance_east = wind_cov[1];
_wind_estimate_sideslip.tas_innov = _wind_estimator_sideslip.get_tas_innov();
_wind_estimate_sideslip.tas_innov_var = _wind_estimator_sideslip.get_tas_innov_var();
_wind_estimate_sideslip.beta_innov = _wind_estimator_sideslip.get_beta_innov();
_wind_estimate_sideslip.beta_innov_var = _wind_estimator_sideslip.get_beta_innov_var();
_wind_estimate_sideslip.tas_scale = _wind_estimator_sideslip.get_tas_scale();
}
void AirspeedModule::update_ground_minus_wind_airspeed()
{
/* calculate airspeed estimate based on groundspeed-windspeed to use as fallback */
float TAS_north = _vehicle_local_position.vx - _wind_estimate_sideslip.windspeed_north;
float TAS_east = _vehicle_local_position.vy - _wind_estimate_sideslip.windspeed_east;
float TAS_down = _vehicle_local_position.vz; // no wind estimate in z
_ground_minus_wind_TAS = sqrtf(TAS_north * TAS_north + TAS_east * TAS_east + TAS_down * TAS_down);
_ground_minus_wind_EAS = calc_EAS_from_TAS(_ground_minus_wind_TAS, _vehicle_air_data.baro_pressure_pa,
_vehicle_air_data.baro_temp_celcius);
}
void AirspeedModule::select_airspeed_and_publish()
{
/* airspeed index:
/ 0: first airspeed sensor valid
/ 1: second airspeed sensor valid
/ -1: airspeed sensor(s) invalid, but groundspeed-windspeed estimate valid
/ -2: airspeed invalid (sensors and groundspeed-windspeed estimate invalid)
*/
bool find_new_valid_index = false;
/* find new valid index if airspeed currently invalid (but we have sensors) */
if ((_number_of_airspeed_sensors > 0 && _prev_airspeed_index < 0) ||
(_prev_airspeed_index >= 0 && !_airspeed_validator[_prev_airspeed_index].get_airspeed_valid()) ||
_prev_airspeed_index == -2) {
find_new_valid_index = true;
}
if (find_new_valid_index) {
_valid_airspeed_index = -1;
for (int i = 0; i < _number_of_airspeed_sensors; i++) {
if (_airspeed_validator[i].get_airspeed_valid()) {
_valid_airspeed_index = i;
break;
}
}
}
if (_valid_airspeed_index < 0 && !_vehicle_local_position_valid) {
_valid_airspeed_index = -2;
}
/* publish critical message (and log) in index has changed */
if (_valid_airspeed_index != _prev_airspeed_index) {
mavlink_log_critical(&_mavlink_log_pub, "Airspeed: switched from sensor %i to %i", _prev_airspeed_index,
_valid_airspeed_index);
}
_prev_airspeed_index = _valid_airspeed_index;
/* fill out airspeed_validated message for publishing it */
airspeed_validated_s airspeed_validated = {};
airspeed_validated.timestamp = hrt_absolute_time();
airspeed_validated.true_ground_minus_wind_m_s = NAN;
airspeed_validated.indicated_ground_minus_wind_m_s = NAN;
airspeed_validated.indicated_airspeed_m_s = NAN;
airspeed_validated.equivalent_airspeed_m_s = NAN;
airspeed_validated.true_airspeed_m_s = NAN;
airspeed_validated.selected_airspeed_index = _valid_airspeed_index;
switch (_valid_airspeed_index) {
case -2:
break;
case -1:
airspeed_validated.true_ground_minus_wind_m_s = _ground_minus_wind_TAS;
airspeed_validated.indicated_ground_minus_wind_m_s = _ground_minus_wind_EAS;
break;
default:
airspeed_validated.indicated_airspeed_m_s = _airspeed_validator[_valid_airspeed_index].get_IAS();
airspeed_validated.equivalent_airspeed_m_s = _airspeed_validator[_valid_airspeed_index].get_EAS();
airspeed_validated.true_airspeed_m_s = _airspeed_validator[_valid_airspeed_index].get_TAS();
airspeed_validated.true_ground_minus_wind_m_s = _ground_minus_wind_TAS;
airspeed_validated.indicated_ground_minus_wind_m_s = _ground_minus_wind_EAS;
break;
}
/* publish airspeed validated topic */
int instance;
orb_publish_auto(ORB_ID(airspeed_validated), &_airspeed_validated_pub, &airspeed_validated, &instance,
ORB_PRIO_DEFAULT);
/* publish sideslip-only-fusion wind topic */
orb_publish_auto(ORB_ID(wind_estimate), &_wind_est_pub[0], &_wind_estimate_sideslip, &instance, ORB_PRIO_LOW);
/* publish the wind estimator states from all airspeed validators */
for (int i = 0; i < _number_of_airspeed_sensors; i++) {
wind_estimate_s wind_est = _airspeed_validator[i].get_wind_estimator_states(hrt_absolute_time());
orb_publish_auto(ORB_ID(wind_estimate), &_wind_est_pub[i + 1], &wind_est, &instance, ORB_PRIO_LOW);
}
}
int AirspeedModule::custom_command(int argc, char *argv[])
{
if (!is_running()) {
int ret = AirspeedModule::task_spawn(argc, argv);
if (ret) {
return ret;
}
}
return print_usage("unknown command");
}
int AirspeedModule::print_usage(const char *reason)
{
if (reason) {
PX4_WARN("%s\n", reason);
}
PRINT_MODULE_DESCRIPTION(
R"DESCR_STR(
### Description
This module provides a single airspeed_validated topic, containing an indicated (IAS),
equivalend (EAS), true airspeed (TAS) and the information if the estimation currently
is invalid and if based sensor readings or on groundspeed minus windspeed.
Supporting the input of multiple "raw" airspeed inputs, this module automatically switches
to a valid sensor in case of failure detection. For failure detection as well as for
the estimation of a scale factor from IAS to EAS, it runs several wind estimators
and also publishes those.
)DESCR_STR");
PRINT_MODULE_USAGE_NAME("airspeed_estimator", "estimator");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
return 0;
}
int AirspeedModule::print_status()
{
perf_print_counter(_perf_elapsed);
perf_print_counter(_perf_interval);
int instance = 0;
uORB::SubscriptionData<airspeed_validated_s> est{ORB_ID(airspeed_validated), (uint8_t)instance};
est.update();
PX4_INFO("Number of airspeed sensors: %i", _number_of_airspeed_sensors);
print_message(est.get());
return 0;
}
extern "C" __EXPORT int airspeed_selector_main(int argc, char *argv[]);
int
airspeed_selector_main(int argc, char *argv[])
{
return AirspeedModule::main(argc, argv);
}

93
src/modules/airspeed_selector/airspeed_selector_params.c Normal file
View File

@ -0,0 +1,93 @@
/**
* Airspeed Selector: Wind estimator wind process noise
*
* Wind process noise of the internal wind estimator(s) of the airspeed selector.
*
* @min 0
* @max 1
* @unit m/s/s
* @group Airspeed Validator
*/
PARAM_DEFINE_FLOAT(ARSP_W_P_NOISE, 0.1f);
/**
* Airspeed Selector: Wind estimator true airspeed scale process noise
*
* Airspeed scale process noise of the internal wind estimator(s) of the airspeed selector.
*
* @min 0
* @max 0.1
* @unit 1/s
* @group Airspeed Validator
*/
PARAM_DEFINE_FLOAT(ARSP_SC_P_NOISE, 0.0001);
/**
* Airspeed Selector: Wind estimator true airspeed measurement noise
*
* True airspeed measurement noise of the internal wind estimator(s) of the airspeed selector.
*
* @min 0
* @max 4
* @unit m/s
* @group Airspeed Validator
*/
PARAM_DEFINE_FLOAT(ARSP_TAS_NOISE, 1.4);
/**
* Airspeed Selector: Wind estimator sideslip measurement noise
*
* Sideslip measurement noise of the internal wind estimator(s) of the airspeed selector.
*
* @min 0
* @max 1
* @unit rad
* @group Airspeed Validator
*/
PARAM_DEFINE_FLOAT(ARSP_BETA_NOISE, 0.3);
/**
* Airspeed Selector: Gate size for true airspeed fusion
*
* Sets the number of standard deviations used by the innovation consistency test.
*
* @min 1
* @max 5
* @unit SD
* @group Airspeed Validator
*/
PARAM_DEFINE_INT32(ARSP_TAS_GATE, 3);
/**
* Airspeed Selector: Gate size for true sideslip fusion
*
* Sets the number of standard deviations used by the innovation consistency test.
*
* @min 1
* @max 5
* @unit SD
* @group Airspeed Validator
*/
PARAM_DEFINE_INT32(ARSP_BETA_GATE, 1);
/**
* Automatic airspeed scale estimation on
*
* Turns the automatic airspeed scale (scale from IAS to CAS/EAS) on or off. It is recommended level (keeping altitude) while performing the estimation. Set to 1 to start estimation (best when already flying). Set to 0 to end scale estimation. The estimated scale is then saved in the ARSP_ARSP_SCALE parameter.
*
* @boolean
* @group Airspeed Validator
*/
PARAM_DEFINE_INT32(ARSP_SCALE_EST, 0);
/**
* Airspeed scale (scale from IAS to CAS/EAS)
*
* Scale can either be entered manually, or estimated in-flight by setting ARSP_SCALE_EST to 1.
*
* @min 0.5
* @max 1.5
* @group Airspeed Validator
*/
PARAM_DEFINE_FLOAT(ARSP_ARSP_SCALE, 1.0f);

5
src/modules/ekf2/ekf2_main.cpp
View File

@ -283,7 +283,7 @@ private:
uORB::Publication<sensor_bias_s> _sensor_bias_pub{ORB_ID(sensor_bias)};
uORB::Publication<vehicle_attitude_s> _att_pub{ORB_ID(vehicle_attitude)};
uORB::Publication<vehicle_odometry_s> _vehicle_odometry_pub{ORB_ID(vehicle_odometry)};
uORB::Publication<wind_estimate_s> _wind_pub{ORB_ID(wind_estimate)};
uORB::Publication<wind_estimate_s> _wind_pub{ORB_ID(wind_estimate), ORB_PRIO_DEFAULT};
uORB::PublicationData<vehicle_global_position_s> _vehicle_global_position_pub{ORB_ID(vehicle_global_position)};
uORB::PublicationData<vehicle_local_position_s> _vehicle_local_position_pub{ORB_ID(vehicle_local_position)};
@ -1764,12 +1764,13 @@ bool Ekf2::publish_wind_estimate(const hrt_abstime &timestamp)
_ekf.get_wind_velocity_var(wind_var);
// Publish wind estimate
wind_estimate_s wind_estimate;
wind_estimate_s wind_estimate{};
wind_estimate.timestamp = timestamp;
wind_estimate.windspeed_north = velNE_wind[0];
wind_estimate.windspeed_east = velNE_wind[1];
wind_estimate.variance_north = wind_var[0];
wind_estimate.variance_east = wind_var[1];
wind_estimate.tas_scale = 1.0f; //fix to 1 as scale not estimated in ekf
_wind_pub.publish(wind_estimate);

1
src/modules/logger/logger.cpp
View File

@ -511,6 +511,7 @@ void Logger::add_default_topics()
add_topic("actuator_controls_0", 100);
add_topic("actuator_controls_1", 100);
add_topic("airspeed", 200);
add_topic("airspeed_validated", 200);
add_topic("camera_capture");
add_topic("camera_trigger");
add_topic("camera_trigger_secondary");

6
src/modules/sensors/sensors.cpp
View File

@ -346,14 +346,14 @@ Sensors::diff_pres_poll(const vehicle_air_data_s &raw)
}
/* don't risk to feed negative airspeed into the system */
airspeed.indicated_airspeed_m_s = calc_indicated_airspeed_corrected((enum AIRSPEED_COMPENSATION_MODEL)
airspeed.indicated_airspeed_m_s = calc_IAS_corrected((enum AIRSPEED_COMPENSATION_MODEL)
_parameters.air_cmodel,
smodel, _parameters.air_tube_length, _parameters.air_tube_diameter_mm,
diff_pres.differential_pressure_filtered_pa, raw.baro_pressure_pa,
air_temperature_celsius);
airspeed.true_airspeed_m_s = calc_true_airspeed_from_indicated(airspeed.indicated_airspeed_m_s, raw.baro_pressure_pa,
air_temperature_celsius);
airspeed.true_airspeed_m_s = calc_TAS_from_EAS(airspeed.indicated_airspeed_m_s, raw.baro_pressure_pa,
air_temperature_celsius); // assume that EAS = IAS as we don't have an EAS-scale here
airspeed.air_temperature_celsius = air_temperature_celsius;

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

@ -1,311 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2018 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include <drivers/drv_hrt.h>
#include <ecl/airdata/WindEstimator.hpp>
#include <matrix/math.hpp>
#include <parameters/param.h>
#include <perf/perf_counter.h>
#include <px4_module.h>
#include <px4_module_params.h>
#include <px4_work_queue/ScheduledWorkItem.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/wind_estimate.h>
using namespace time_literals;
static constexpr uint32_t SCHEDULE_INTERVAL{100_ms}; /**< 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 px4::ScheduledWorkItem
{
public:
WindEstimatorModule();
~WindEstimatorModule();
/** @see ModuleBase */
static int task_spawn(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
void Run() override;
int print_status() override;
private:
WindEstimator _wind_estimator;
orb_advert_t _wind_est_pub{nullptr}; /**< wind estimate topic */
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
uORB::Subscription _airspeed_sub{ORB_ID(airspeed)};
uORB::Subscription _param_sub{ORB_ID(parameter_update)};
perf_counter_t _perf_elapsed{};
perf_counter_t _perf_interval{};
int _instance{-1};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::WEST_W_P_NOISE>) _param_west_w_p_noise,
(ParamFloat<px4::params::WEST_SC_P_NOISE>) _param_west_sc_p_noise,
(ParamFloat<px4::params::WEST_TAS_NOISE>) _param_west_tas_noise,
(ParamFloat<px4::params::WEST_BETA_NOISE>) _param_west_beta_noise,
(ParamInt<px4::params::WEST_TAS_GATE>) _param_west_tas_gate,
(ParamInt<px4::params::WEST_BETA_GATE>) _param_west_beta_gate
)
int start();
void update_params();
bool subscribe_topics();
};
WindEstimatorModule::WindEstimatorModule():
ModuleParams(nullptr),
ScheduledWorkItem(px4::wq_configurations::lp_default)
{
// 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()
{
ScheduleClear();
orb_unadvertise(_wind_est_pub);
perf_free(_perf_elapsed);
perf_free(_perf_interval);
}
int
WindEstimatorModule::task_spawn(int argc, char *argv[])
{
/* schedule a cycle to start things */
WindEstimatorModule *dev = new WindEstimatorModule();
// check if the trampoline is called for the first time
if (!dev) {
PX4_ERR("alloc failed");
return PX4_ERROR;
}
_object.store(dev);
if (dev) {
dev->ScheduleOnInterval(SCHEDULE_INTERVAL, 10000);
_task_id = task_id_is_work_queue;
return PX4_OK;
}
return PX4_ERROR;
}
void
WindEstimatorModule::Run()
{
perf_count(_perf_interval);
perf_begin(_perf_elapsed);
parameter_update_s param{};
if (_param_sub.update(&param)) {
update_params();
}
bool lpos_valid = false;
bool att_valid = false;
const hrt_abstime time_now_usec = hrt_absolute_time();
// validate required conditions for the filter to fuse measurements
vehicle_attitude_s att{};
if (_vehicle_attitude_sub.copy(&att)) {
att_valid = (time_now_usec - att.timestamp < 1_s) && (att.timestamp > 0);
}
vehicle_local_position_s lpos{};
if (_vehicle_local_position_sub.copy(&lpos)) {
lpos_valid = (time_now_usec - lpos.timestamp < 1_s) && (lpos.timestamp > 0) && lpos.v_xy_valid;
}
// update wind and airspeed estimator
_wind_estimator.update(time_now_usec);
if (lpos_valid && att_valid) {
Vector3f vI(lpos.vx, lpos.vy, lpos.vz);
Quatf q(att.q);
// sideslip fusion
_wind_estimator.fuse_beta(time_now_usec, vI, q);
// additionally, for airspeed fusion we need to have recent measurements
airspeed_s airspeed{};
if (_airspeed_sub.update(&airspeed)) {
if ((time_now_usec - airspeed.timestamp < 1_s) && (airspeed.timestamp > 0)) {
const Vector3f vel_var{Dcmf(q) *Vector3f{lpos.evh, lpos.evh, lpos.evv}};
// airspeed fusion
_wind_estimator.fuse_airspeed(time_now_usec, airspeed.true_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;
float wind[2];
_wind_estimator.get_wind(wind);
wind_est.windspeed_north = wind[0];
wind_est.windspeed_east = wind[1];
float wind_cov[2];
_wind_estimator.get_wind_var(wind_cov);
wind_est.variance_north = wind_cov[0];
wind_est.variance_east = wind_cov[1];
wind_est.tas_innov = _wind_estimator.get_tas_innov();
wind_est.tas_innov_var = _wind_estimator.get_tas_innov_var();
wind_est.beta_innov = _wind_estimator.get_beta_innov();
wind_est.beta_innov_var = _wind_estimator.get_beta_innov_var();
wind_est.tas_scale = _wind_estimator.get_tas_scale();
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();
}
}
void WindEstimatorModule::update_params()
{
updateParams();
// update wind & airspeed scale estimator parameters
_wind_estimator.set_wind_p_noise(_param_west_w_p_noise.get());
_wind_estimator.set_tas_scale_p_noise(_param_west_sc_p_noise.get());
_wind_estimator.set_tas_noise(_param_west_tas_noise.get());
_wind_estimator.set_beta_noise(_param_west_beta_noise.get());
_wind_estimator.set_tas_gate(_param_west_tas_gate.get());
_wind_estimator.set_beta_gate(_param_west_beta_gate.get());
}
int WindEstimatorModule::custom_command(int argc, char *argv[])
{
if (!is_running()) {
int ret = WindEstimatorModule::task_spawn(argc, argv);
if (ret) {
return ret;
}
}
return print_usage("unknown command");
}
int WindEstimatorModule::print_usage(const char *reason)
{
if (reason) {
PX4_WARN("%s\n", reason);
}
PRINT_MODULE_DESCRIPTION(
R"DESCR_STR(
### Description
This module runs a combined wind and airspeed scale factor estimator.
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 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");
PRINT_MODULE_USAGE_NAME("wind_estimator", "estimator");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
return 0;
}
int WindEstimatorModule::print_status()
{
perf_print_counter(_perf_elapsed);
perf_print_counter(_perf_interval);
if (_instance > -1) {
uORB::SubscriptionData<wind_estimate_s> est{ORB_ID(wind_estimate), (uint8_t)_instance};
est.update();
print_message(est.get());
} else {
PX4_INFO("Running, but never published");
}
return 0;
}
extern "C" __EXPORT int wind_estimator_main(int argc, char *argv[]);
int
wind_estimator_main(int argc, char *argv[])
{
return WindEstimatorModule::main(argc, argv);
}

71
src/modules/wind_estimator/wind_estimator_params.c
View File

@ -1,71 +0,0 @@
/**
* Enable Wind estimator
*
* @boolean
* @reboot_required true
* @group Wind Estimator
*/
PARAM_DEFINE_INT32(WEST_EN, 0);
/**
* Wind estimator wind process noise.
*
* @min 0
* @max 1
* @unit m/s/s
* @group Wind Estimator
*/
PARAM_DEFINE_FLOAT(WEST_W_P_NOISE, 0.1f);
/**
* Wind estimator true airspeed scale process noise.
*
* @min 0
* @max 0.1
* @group Wind Estimator
*/
PARAM_DEFINE_FLOAT(WEST_SC_P_NOISE, 0.0001);
/**
* Wind estimator true airspeed measurement noise.
*
* @min 0
* @max 4
* @unit m/s
* @group Wind Estimator
*/
PARAM_DEFINE_FLOAT(WEST_TAS_NOISE, 1.4);
/**
* Wind estimator sideslip measurement noise.
*
* @min 0
* @max 1
* @unit rad
* @group Wind Estimator
*/
PARAM_DEFINE_FLOAT(WEST_BETA_NOISE, 0.3);
/**
* Gate size for true airspeed fusion.
*
* Sets the number of standard deviations used by the innovation consistency test.
*
* @min 1
* @max 5
* @unit SD
* @group Wind Estimator
*/
PARAM_DEFINE_INT32(WEST_TAS_GATE, 3);
/**
* Gate size for true sideslip fusion.
*
* Sets the number of standard deviations used by the innovation consistency test.
*
* @min 1
* @max 5
* @unit SD
* @group Wind Estimator
*/
PARAM_DEFINE_INT32(WEST_BETA_GATE, 1);