fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-16 07:27:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Make LandingTargetEstimator a lib and run on work_queue

Browse Source 
Currently only precision landing is using the
LandingTargetEstimator and it is a waste to have it
running in the background all the time. With this change
the estimator is only started once the precision landing
flight task is activated, and stopped when the flight
task is deactivated.
This commit is contained in:
Alessandro Simovic
2022-02-23 14:20:33 +01:00
parent a0d8c481d9
commit ad4d48454a
9 changed files with 107 additions and 179 deletions
Download Patch File Download Diff File Expand all files Collapse all files
boards/px4/sitl/default.px4board
-1
View File
@@ -22,7 +22,6 @@ CONFIG_MODULES_FW_POS_CONTROL_L1=y
CONFIG_MODULES_GYRO_CALIBRATION=y
CONFIG_MODULES_GYRO_FFT=y
CONFIG_MODULES_LAND_DETECTOR=y
CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
CONFIG_MODULES_LOAD_MON=y
CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y
CONFIG_MODULES_LOGGER=y
src/lib/CMakeLists.txt
+1
View File
@@ -49,6 +49,7 @@ add_subdirectory(geo)
add_subdirectory(hysteresis)
add_subdirectory(l1)
add_subdirectory(landing_slope)
add_subdirectory(landing_target_estimator)
add_subdirectory(led)
add_subdirectory(matrix)
add_subdirectory(mathlib)
src/lib/landing_target_estimator/CMakeLists.txt
+10 -10
View File
@@ -31,14 +31,14 @@
#
############################################################################
px4_add_module(
MODULE modules__landing_target_estimator
MAIN landing_target_estimator
COMPILE_FLAGS
SRCS
landing_target_estimator_main.cpp
LandingTargetEstimator.cpp
KalmanFilter.cpp
DEPENDS
)
px4_add_library(LandingTargetEstimator
LandingTargetEstimator.cpp
KalmanFilter.cpp
)
target_compile_options(LandingTargetEstimator PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
target_link_libraries(LandingTargetEstimator
PUBLIC
px4_work_queue
)
src/lib/landing_target_estimator/LandingTargetEstimator.cpp
+50 -1
View File
@@ -50,7 +50,9 @@
namespace landing_target_estimator
{
LandingTargetEstimator::LandingTargetEstimator()
LandingTargetEstimator::LandingTargetEstimator() :
ModuleParams(nullptr),
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers)
{
_paramHandle.acc_unc = param_find("LTEST_ACC_UNC");
_paramHandle.meas_unc = param_find("LTEST_MEAS_UNC");
@@ -63,7 +65,54 @@ LandingTargetEstimator::LandingTargetEstimator()
_paramHandle.offset_x = param_find("LTEST_SENS_POS_X");
_paramHandle.offset_y = param_find("LTEST_SENS_POS_Y");
_paramHandle.offset_z = param_find("LTEST_SENS_POS_Z");
}
LandingTargetEstimator::~LandingTargetEstimator()
{
Stop();
}
bool LandingTargetEstimator::Start()
{
// force initialize parameters update
_check_params(true);
// messages needed for estimation
if (!_vehicleLocalPositionSub.registerCallback()) {
PX4_ERR("landing_target_estimator callback registration failed");
}
if (!_attitudeSub.registerCallback()) {
PX4_ERR("landing_target_estimator callback registration failed");
}
if (!_vehicle_acceleration_sub.registerCallback()) {
PX4_ERR("landing_target_estimator callback registration failed");
}
if (!_irlockReportSub.registerCallback()) {
PX4_ERR("landing_target_estimator callback registration failed");
}
return true;
}
void LandingTargetEstimator::Stop()
{
// clear all registered callbacks
_vehicleLocalPositionSub.unregisterCallback();
_attitudeSub.unregisterCallback();
_vehicle_acceleration_sub.unregisterCallback();
_irlockReportSub.unregisterCallback();
Deinit();
}
void LandingTargetEstimator::Run()
{
ScheduleDelayed(10_ms);
update();
}
void LandingTargetEstimator::update()
src/lib/landing_target_estimator/LandingTargetEstimator.h
+20 -9
View File
@@ -42,12 +42,12 @@
#pragma once
#include <px4_platform_common/workqueue.h>
#include <drivers/drv_hrt.h>
#include <parameters/param.h>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/vehicle_acceleration.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_local_position.h>
@@ -58,10 +58,16 @@
#include <uORB/topics/uwb_grid.h>
#include <uORB/topics/estimator_sensor_bias.h>
#include <uORB/topics/parameter_update.h>
#include <matrix/math.hpp>
#include <mathlib/mathlib.h>
#include <matrix/Matrix.hpp>
#include <lib/conversion/rotation.h>
#include <px4_platform_common/workqueue.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include "KalmanFilter.h"
using namespace time_literals;
@@ -69,19 +75,24 @@ using namespace time_literals;
namespace landing_target_estimator
{
class LandingTargetEstimator
class LandingTargetEstimator : public ModuleParams, public px4::ScheduledWorkItem
{
public:
LandingTargetEstimator();
virtual ~LandingTargetEstimator() = default;
~LandingTargetEstimator() override;
/*
* Get new measurements and update the state estimate
*/
void update();
bool Start();
void Stop();
protected:
void Run() override;
void update();
/*
* Update uORB topics.
@@ -149,11 +160,11 @@ private:
float rel_pos_z;
} _target_position_report;
uORB::Subscription _vehicleLocalPositionSub{ORB_ID(vehicle_local_position)};
uORB::Subscription _attitudeSub{ORB_ID(vehicle_attitude)};
uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};
uORB::Subscription _irlockReportSub{ORB_ID(irlock_report)};
uORB::Subscription _uwbDistanceSub{ORB_ID(uwb_distance)};
uORB::SubscriptionCallbackWorkItem _vehicleLocalPositionSub{this, ORB_ID(vehicle_local_position)};
uORB::SubscriptionCallbackWorkItem _attitudeSub{this, ORB_ID(vehicle_attitude)};
uORB::SubscriptionCallbackWorkItem _vehicle_acceleration_sub{this, ORB_ID(vehicle_acceleration)};
uORB::SubscriptionCallbackWorkItem _irlockReportSub{this, ORB_ID(irlock_report)};
uORB::SubscriptionCallbackWorkItem _uwbDistanceSub{this, ORB_ID(uwb_distance)};
vehicle_local_position_s _vehicleLocalPosition{};
vehicle_attitude_s _vehicleAttitude{};
src/lib/landing_target_estimator/landing_target_estimator_main.cpp
-153
View File
@@ -1,153 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2013-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.
*
****************************************************************************/
/**
* @file landing_target_estimator_main.cpp
* Landing target position estimator. Filter and publish the position of a landing target on the ground as observed by an onboard sensor.
*
* @author Nicolas de Palezieux (Sunflower Labs) <ndepal@gmail.com>
* @author Mohammed Kabir <kabir@uasys.io>
*
*/
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/tasks.h>
#include <px4_platform_common/posix.h>
#include <unistd.h>
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <errno.h>
#include <drivers/drv_hrt.h>
#include <systemlib/err.h>
#include "LandingTargetEstimator.h"
namespace landing_target_estimator
{
static bool thread_should_exit = false; /**< daemon exit flag */
static bool thread_running = false; /**< daemon status flag */
static int daemon_task; /**< Handle of daemon task / thread */
/* Run main loop at this rate in Hz. */
static constexpr uint32_t landing_target_estimator_UPDATE_RATE_HZ = 50;
/**
* Landing target position estimator app start / stop handling function
* This makes the module accessible from the nuttx shell
* @ingroup apps
*/
extern "C" __EXPORT int landing_target_estimator_main(int argc, char *argv[]);
/**
* Mainloop of daemon.
*/
int landing_target_estimator_thread_main(int argc, char *argv[]);
/**
* Main entry point for this module
**/
int landing_target_estimator_main(int argc, char *argv[])
{
if (argc < 2) {
goto exiterr;
}
if (argc >= 2 && !strcmp(argv[1], "start")) {
if (thread_running) {
PX4_INFO("already running");
/* this is not an error */
return 0;
}
thread_should_exit = false;
daemon_task = px4_task_spawn_cmd("landing_target_estimator",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
2100,
landing_target_estimator_thread_main,
(argv) ? (char *const *)&argv[2] : nullptr);
return 0;
}
if (!strcmp(argv[1], "stop")) {
thread_should_exit = true;
if (!thread_running) {
PX4_WARN("landing_target_estimator not running");
}
return 0;
}
if (!strcmp(argv[1], "status")) {
if (thread_running) {
PX4_INFO("running");
} else {
PX4_INFO("not started");
}
return 0;
}
exiterr:
PX4_WARN("usage: landing_target_estimator {start|stop|status}");
return 1;
}
int landing_target_estimator_thread_main(int argc, char *argv[])
{
PX4_DEBUG("starting");
thread_running = true;
LandingTargetEstimator est;
while (!thread_should_exit) {
est.update();
px4_usleep(1000000 / landing_target_estimator_UPDATE_RATE_HZ);
}
PX4_DEBUG("exiting");
thread_running = false;
return 0;
}
}
src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/CMakeLists.txt
+11 -3
View File
@@ -32,8 +32,16 @@
############################################################################
px4_add_library(FlightTaskAutoPrecisionLanding
FlightTaskAutoPrecisionLanding.cpp
FlightTaskAutoPrecisionLanding.cpp
)
target_link_libraries(FlightTaskAutoPrecisionLanding PUBLIC FlightTaskAuto)
target_include_directories(FlightTaskAutoPrecisionLanding PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
target_link_libraries(FlightTaskAutoPrecisionLanding
PUBLIC
FlightTaskAuto
LandingTargetEstimator
)
target_include_directories(FlightTaskAutoPrecisionLanding
PUBLIC
${CMAKE_CURRENT_SOURCE_DIR}
)
src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.cpp
+10
View File
@@ -45,6 +45,16 @@
static constexpr const char *LOST_TARGET_ERROR_MESSAGE = "Lost landing target while landing";
FlightTaskAutoPrecisionLanding::FlightTaskAutoPrecisionLanding()
{
_landing_target_estimator.Start();
}
FlightTaskAutoPrecisionLanding::~FlightTaskAutoPrecisionLanding()
{
_landing_target_estimator.Stop();
}
bool FlightTaskAutoPrecisionLanding::activate(const vehicle_local_position_setpoint_s &last_setpoint)
{
bool ret = FlightTask::activate(last_setpoint);
src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.hpp
+5 -2
View File
@@ -45,6 +45,7 @@
#include <matrix/math.hpp>
#include <mathlib/mathlib.h>
#include <landing_target_estimator/LandingTargetEstimator.h>
#include <lib/geo/geo.h>
#include <px4_platform_common/module_params.h>
#include <uORB/Subscription.hpp>
@@ -74,8 +75,8 @@ enum class PrecLandMode {
class FlightTaskAutoPrecisionLanding : public FlightTaskAuto
{
public:
FlightTaskAutoPrecisionLanding() = default;
virtual ~FlightTaskAutoPrecisionLanding() = default;
FlightTaskAutoPrecisionLanding();
~FlightTaskAutoPrecisionLanding();
bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
@@ -104,6 +105,8 @@ private:
bool check_state_conditions(PrecLandState state);
void slewrate(float &sp_x, float &sp_y);
landing_target_estimator::LandingTargetEstimator _landing_target_estimator;
landing_target_pose_s _target_pose{}; /**< precision landing target position */
uORB::Subscription _target_pose_sub{ORB_ID(landing_target_pose)};