forked from Archive/PX4-Autopilot
migrate fw_auto_trim module to fw_rate_control
This commit is contained in:
parent
96517c05a0
commit
215081b6e2
|
@ -30,6 +30,7 @@
|
||||||
# POSSIBILITY OF SUCH DAMAGE.
|
# POSSIBILITY OF SUCH DAMAGE.
|
||||||
#
|
#
|
||||||
############################################################################
|
############################################################################
|
||||||
|
add_subdirectory(fw_auto_trim)
|
||||||
|
|
||||||
px4_add_module(
|
px4_add_module(
|
||||||
MODULE modules__fw_rate_control
|
MODULE modules__fw_rate_control
|
||||||
|
@ -38,6 +39,7 @@ px4_add_module(
|
||||||
FixedwingRateControl.cpp
|
FixedwingRateControl.cpp
|
||||||
FixedwingRateControl.hpp
|
FixedwingRateControl.hpp
|
||||||
DEPENDS
|
DEPENDS
|
||||||
|
FwAutoTrim
|
||||||
px4_work_queue
|
px4_work_queue
|
||||||
RateControl
|
RateControl
|
||||||
SlewRate
|
SlewRate
|
||||||
|
|
|
@ -432,6 +432,11 @@ void FixedwingRateControl::Run()
|
||||||
_vehicle_torque_setpoint.timestamp_sample = angular_velocity.timestamp_sample;
|
_vehicle_torque_setpoint.timestamp_sample = angular_velocity.timestamp_sample;
|
||||||
_vehicle_torque_setpoint_pub.publish(_vehicle_torque_setpoint);
|
_vehicle_torque_setpoint_pub.publish(_vehicle_torque_setpoint);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
_auto_trim.update(_vehicle_torque_setpoint, dt);
|
||||||
|
|
||||||
|
} else {
|
||||||
|
_auto_trim.reset();
|
||||||
}
|
}
|
||||||
|
|
||||||
updateActuatorControlsStatus(dt);
|
updateActuatorControlsStatus(dt);
|
||||||
|
|
|
@ -69,6 +69,8 @@
|
||||||
#include <uORB/topics/vehicle_thrust_setpoint.h>
|
#include <uORB/topics/vehicle_thrust_setpoint.h>
|
||||||
#include <uORB/topics/vehicle_torque_setpoint.h>
|
#include <uORB/topics/vehicle_torque_setpoint.h>
|
||||||
|
|
||||||
|
#include "fw_auto_trim/FwAutoTrim.hpp"
|
||||||
|
|
||||||
using matrix::Eulerf;
|
using matrix::Eulerf;
|
||||||
using matrix::Quatf;
|
using matrix::Quatf;
|
||||||
|
|
||||||
|
@ -209,6 +211,8 @@ private:
|
||||||
|
|
||||||
RateControl _rate_control; ///< class for rate control calculations
|
RateControl _rate_control; ///< class for rate control calculations
|
||||||
|
|
||||||
|
FwAutoTrim _auto_trim{this};
|
||||||
|
|
||||||
void updateActuatorControlsStatus(float dt);
|
void updateActuatorControlsStatus(float dt);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
|
|
@ -30,17 +30,9 @@
|
||||||
# POSSIBILITY OF SUCH DAMAGE.
|
# POSSIBILITY OF SUCH DAMAGE.
|
||||||
#
|
#
|
||||||
############################################################################
|
############################################################################
|
||||||
|
px4_add_library(FwAutoTrim
|
||||||
px4_add_module(
|
FwAutoTrim.cpp
|
||||||
MODULE modules__fw_auto_trim
|
|
||||||
MAIN fw_auto_trim
|
|
||||||
COMPILE_FLAGS
|
|
||||||
${MAX_CUSTOM_OPT_LEVEL}
|
|
||||||
SRCS
|
|
||||||
FwAutoTrim.cpp
|
|
||||||
FwAutoTrim.hpp
|
|
||||||
DEPENDS
|
|
||||||
hysteresis
|
|
||||||
mathlib
|
|
||||||
px4_work_queue
|
|
||||||
)
|
)
|
||||||
|
|
||||||
|
# target_link_libraries(FwAutoTrim)
|
||||||
|
target_include_directories(FwAutoTrim PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
|
@ -38,13 +38,11 @@
|
||||||
using namespace time_literals;
|
using namespace time_literals;
|
||||||
using matrix::Vector3f;
|
using matrix::Vector3f;
|
||||||
|
|
||||||
FwAutoTrim::FwAutoTrim(bool is_vtol) :
|
FwAutoTrim::FwAutoTrim(ModuleParams *parent) :
|
||||||
ModuleParams(nullptr),
|
ModuleParams(parent)
|
||||||
WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers),
|
|
||||||
_vehicle_torque_setpoint_sub(this, ORB_ID(vehicle_torque_setpoint), is_vtol ? 1 : 0)
|
|
||||||
{
|
{
|
||||||
|
_auto_trim_status_pub.advertise();
|
||||||
updateParams();
|
updateParams();
|
||||||
reset();
|
|
||||||
}
|
}
|
||||||
|
|
||||||
FwAutoTrim::~FwAutoTrim()
|
FwAutoTrim::~FwAutoTrim()
|
||||||
|
@ -52,35 +50,18 @@ FwAutoTrim::~FwAutoTrim()
|
||||||
perf_free(_cycle_perf);
|
perf_free(_cycle_perf);
|
||||||
}
|
}
|
||||||
|
|
||||||
bool FwAutoTrim::init()
|
|
||||||
{
|
|
||||||
if (!_vehicle_torque_setpoint_sub.registerCallback()) {
|
|
||||||
PX4_ERR("callback registration failed");
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
_auto_trim_status_pub.advertise();
|
|
||||||
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
|
|
||||||
void FwAutoTrim::reset()
|
|
||||||
{
|
|
||||||
}
|
|
||||||
|
|
||||||
void FwAutoTrim::updateParams()
|
void FwAutoTrim::updateParams()
|
||||||
{
|
{
|
||||||
ModuleParams::updateParams();
|
ModuleParams::updateParams();
|
||||||
}
|
}
|
||||||
|
|
||||||
void FwAutoTrim::Run()
|
void FwAutoTrim::reset()
|
||||||
{
|
{
|
||||||
if (should_exit()) {
|
_state = state::idle;
|
||||||
_vehicle_torque_setpoint_sub.unregisterCallback();
|
}
|
||||||
exit_and_cleanup();
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
void FwAutoTrim::update(const vehicle_torque_setpoint_s &vehicle_torque_setpoint, const float dt)
|
||||||
|
{
|
||||||
if (_vehicle_land_detected_sub.updated()) {
|
if (_vehicle_land_detected_sub.updated()) {
|
||||||
vehicle_land_detected_s vehicle_land_detected;
|
vehicle_land_detected_s vehicle_land_detected;
|
||||||
|
|
||||||
|
@ -89,24 +70,6 @@ void FwAutoTrim::Run()
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
vehicle_torque_setpoint_s vehicle_torque_setpoint;
|
|
||||||
|
|
||||||
if (!_vehicle_torque_setpoint_sub.update(&vehicle_torque_setpoint)
|
|
||||||
|| (vehicle_torque_setpoint.timestamp == _timestamp_last)) {
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
// check for parameter updates
|
|
||||||
if (_parameter_update_sub.updated()) {
|
|
||||||
// clear update
|
|
||||||
parameter_update_s pupdate;
|
|
||||||
_parameter_update_sub.copy(&pupdate);
|
|
||||||
|
|
||||||
// update parameters from storage
|
|
||||||
updateParams();
|
|
||||||
}
|
|
||||||
|
|
||||||
perf_begin(_cycle_perf);
|
perf_begin(_cycle_perf);
|
||||||
|
|
||||||
if (_vehicle_status_sub.updated()) {
|
if (_vehicle_status_sub.updated()) {
|
||||||
|
@ -136,9 +99,6 @@ void FwAutoTrim::Run()
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
const float dt = (vehicle_torque_setpoint.timestamp - _timestamp_last) * 1e-6f;
|
|
||||||
_timestamp_last = vehicle_torque_setpoint.timestamp;
|
|
||||||
|
|
||||||
const hrt_abstime now = hrt_absolute_time();
|
const hrt_abstime now = hrt_absolute_time();
|
||||||
const Vector3f torque{vehicle_torque_setpoint.xyz};
|
const Vector3f torque{vehicle_torque_setpoint.xyz};
|
||||||
|
|
||||||
|
@ -234,42 +194,6 @@ void FwAutoTrim::publishStatus(const hrt_abstime ×tamp_sample)
|
||||||
_auto_trim_status_pub.publish(status_msg);
|
_auto_trim_status_pub.publish(status_msg);
|
||||||
}
|
}
|
||||||
|
|
||||||
int FwAutoTrim::task_spawn(int argc, char *argv[])
|
|
||||||
{
|
|
||||||
bool is_vtol = false;
|
|
||||||
|
|
||||||
if (argc > 1) {
|
|
||||||
if (strcmp(argv[1], "vtol") == 0) {
|
|
||||||
is_vtol = true;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
FwAutoTrim *instance = new FwAutoTrim(is_vtol);
|
|
||||||
|
|
||||||
if (instance) {
|
|
||||||
_object.store(instance);
|
|
||||||
_task_id = task_id_is_work_queue;
|
|
||||||
|
|
||||||
if (instance->init()) {
|
|
||||||
return PX4_OK;
|
|
||||||
}
|
|
||||||
|
|
||||||
} else {
|
|
||||||
PX4_ERR("alloc failed");
|
|
||||||
}
|
|
||||||
|
|
||||||
delete instance;
|
|
||||||
_object.store(nullptr);
|
|
||||||
_task_id = -1;
|
|
||||||
|
|
||||||
return PX4_ERROR;
|
|
||||||
}
|
|
||||||
|
|
||||||
int FwAutoTrim::custom_command(int argc, char *argv[])
|
|
||||||
{
|
|
||||||
return print_usage("unknown command");
|
|
||||||
}
|
|
||||||
|
|
||||||
int FwAutoTrim::print_status()
|
int FwAutoTrim::print_status()
|
||||||
{
|
{
|
||||||
perf_print_counter(_cycle_perf);
|
perf_print_counter(_cycle_perf);
|
||||||
|
@ -279,27 +203,3 @@ int FwAutoTrim::print_status()
|
||||||
|
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
int FwAutoTrim::print_usage(const char *reason)
|
|
||||||
{
|
|
||||||
if (reason) {
|
|
||||||
PX4_WARN("%s\n", reason);
|
|
||||||
}
|
|
||||||
|
|
||||||
PRINT_MODULE_DESCRIPTION(
|
|
||||||
R"DESCR_STR(
|
|
||||||
### Description
|
|
||||||
|
|
||||||
)DESCR_STR");
|
|
||||||
|
|
||||||
PRINT_MODULE_USAGE_NAME("fw_auto_trim", "auto-trim");
|
|
||||||
PRINT_MODULE_USAGE_COMMAND("start");
|
|
||||||
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
|
|
||||||
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
extern "C" __EXPORT int fw_auto_trim_main(int argc, char *argv[])
|
|
||||||
{
|
|
||||||
return FwAutoTrim::main(argc, argv);
|
|
||||||
}
|
|
|
@ -41,10 +41,8 @@
|
||||||
#include <px4_platform_common/module.h>
|
#include <px4_platform_common/module.h>
|
||||||
#include <px4_platform_common/module_params.h>
|
#include <px4_platform_common/module_params.h>
|
||||||
#include <px4_platform_common/posix.h>
|
#include <px4_platform_common/posix.h>
|
||||||
#include <px4_platform_common/px4_work_queue/WorkItem.hpp>
|
|
||||||
#include <uORB/Publication.hpp>
|
#include <uORB/Publication.hpp>
|
||||||
#include <uORB/Subscription.hpp>
|
#include <uORB/Subscription.hpp>
|
||||||
#include <uORB/SubscriptionCallback.hpp>
|
|
||||||
#include <uORB/topics/airspeed_validated.h>
|
#include <uORB/topics/airspeed_validated.h>
|
||||||
#include <uORB/topics/auto_trim_status.h>
|
#include <uORB/topics/auto_trim_status.h>
|
||||||
#include <uORB/topics/parameter_update.h>
|
#include <uORB/topics/parameter_update.h>
|
||||||
|
@ -55,41 +53,24 @@
|
||||||
|
|
||||||
using namespace time_literals;
|
using namespace time_literals;
|
||||||
|
|
||||||
class FwAutoTrim : public ModuleBase<FwAutoTrim>, public ModuleParams,
|
class FwAutoTrim : public ModuleParams
|
||||||
public px4::WorkItem
|
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
FwAutoTrim(bool is_vtol);
|
FwAutoTrim(ModuleParams *parent);
|
||||||
~FwAutoTrim() override;
|
~FwAutoTrim();
|
||||||
|
|
||||||
/** @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);
|
|
||||||
|
|
||||||
bool init();
|
|
||||||
|
|
||||||
/** @see ModuleBase::print_status() */
|
|
||||||
int print_status() override;
|
|
||||||
|
|
||||||
private:
|
|
||||||
void Run() override;
|
|
||||||
void updateParams() override;
|
|
||||||
|
|
||||||
void reset();
|
void reset();
|
||||||
|
void update(const vehicle_torque_setpoint_s &vehicle_torque_setpoint, float dt);
|
||||||
|
int print_status();
|
||||||
|
|
||||||
|
protected:
|
||||||
|
void updateParams() override;
|
||||||
|
|
||||||
|
private:
|
||||||
void publishStatus(const hrt_abstime ×tamp_sample);
|
void publishStatus(const hrt_abstime ×tamp_sample);
|
||||||
|
|
||||||
uORB::Publication<auto_trim_status_s> _auto_trim_status_pub{ORB_ID(auto_trim_status)};
|
uORB::Publication<auto_trim_status_s> _auto_trim_status_pub{ORB_ID(auto_trim_status)};
|
||||||
|
|
||||||
uORB::SubscriptionCallbackWorkItem _vehicle_torque_setpoint_sub;
|
|
||||||
|
|
||||||
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
|
|
||||||
|
|
||||||
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
|
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
|
||||||
uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)};
|
uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)};
|
||||||
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
|
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
|
||||||
|
@ -104,7 +85,6 @@ private:
|
||||||
fail = auto_trim_status_s::STATE_FAIL,
|
fail = auto_trim_status_s::STATE_FAIL,
|
||||||
} _state{state::idle};
|
} _state{state::idle};
|
||||||
|
|
||||||
hrt_abstime _timestamp_last{0};
|
|
||||||
hrt_abstime _state_start_time{0};
|
hrt_abstime _state_start_time{0};
|
||||||
|
|
||||||
bool _armed{false};
|
bool _armed{false};
|
Loading…
Reference in New Issue