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

Revert "Make LandingTargetEstimator a lib and run on work_queue"

Browse Source 
This reverts commit 37c2d242b159ccf55d6de999b268204fde2eed50.
This commit is contained in:
Julian Oes
2022-03-25 08:19:02 +13:00
committed by Alessandro Simovic
parent ad4d48454a
commit 4867638d9b
9 changed files with 179 additions and 107 deletions
Download Patch File Download Diff File Expand all files Collapse all files
boards/px4/sitl/default.px4board
+1
View File
@@ -22,6 +22,7 @@ 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,7 +49,6 @@ 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_library(LandingTargetEstimator
LandingTargetEstimator.cpp
KalmanFilter.cpp
)
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
)
target_compile_options(LandingTargetEstimator PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
target_link_libraries(LandingTargetEstimator
PUBLIC
px4_work_queue
)
src/lib/landing_target_estimator/LandingTargetEstimator.cpp
+1 -50
View File
@@ -50,9 +50,7 @@
namespace landing_target_estimator
{
LandingTargetEstimator::LandingTargetEstimator() :
ModuleParams(nullptr),
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers)
LandingTargetEstimator::LandingTargetEstimator()
{
_paramHandle.acc_unc = param_find("LTEST_ACC_UNC");
_paramHandle.meas_unc = param_find("LTEST_MEAS_UNC");
@@ -65,54 +63,7 @@ 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
+9 -20
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,16 +58,10 @@
#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;
@@ -75,24 +69,19 @@ using namespace time_literals;
namespace landing_target_estimator
{
class LandingTargetEstimator : public ModuleParams, public px4::ScheduledWorkItem
class LandingTargetEstimator
{
public:
LandingTargetEstimator();
~LandingTargetEstimator() override;
virtual ~LandingTargetEstimator() = default;
/*
* Get new measurements and update the state estimate
*/
bool Start();
void Stop();
void update();
protected:
void Run() override;
void update();
/*
* Update uORB topics.
@@ -160,11 +149,11 @@ private:
float rel_pos_z;
} _target_position_report;
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)};
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)};
vehicle_local_position_s _vehicleLocalPosition{};
vehicle_attitude_s _vehicleAttitude{};
src/lib/landing_target_estimator/landing_target_estimator_main.cpp
+153
View File
@@ -0,0 +1,153 @@
/****************************************************************************
*
* 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,
2000,
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
+3 -11
View File
@@ -32,16 +32,8 @@
############################################################################
px4_add_library(FlightTaskAutoPrecisionLanding
FlightTaskAutoPrecisionLanding.cpp
FlightTaskAutoPrecisionLanding.cpp
)
target_link_libraries(FlightTaskAutoPrecisionLanding
PUBLIC
FlightTaskAuto
LandingTargetEstimator
)
target_include_directories(FlightTaskAutoPrecisionLanding
PUBLIC
${CMAKE_CURRENT_SOURCE_DIR}
)
target_link_libraries(FlightTaskAutoPrecisionLanding PUBLIC FlightTaskAuto)
target_include_directories(FlightTaskAutoPrecisionLanding PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.cpp
-10
View File
@@ -45,16 +45,6 @@
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
+2 -5
View File
@@ -45,7 +45,6 @@
#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>
@@ -75,8 +74,8 @@ enum class PrecLandMode {
class FlightTaskAutoPrecisionLanding : public FlightTaskAuto
{
public:
FlightTaskAutoPrecisionLanding();
~FlightTaskAutoPrecisionLanding();
FlightTaskAutoPrecisionLanding() = default;
virtual ~FlightTaskAutoPrecisionLanding() = default;
bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
@@ -105,8 +104,6 @@ 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)};