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.
|
||||
#
|
||||
############################################################################
|
||||
add_subdirectory(fw_auto_trim)
|
||||
|
||||
px4_add_module(
|
||||
MODULE modules__fw_rate_control
|
||||
|
@ -38,6 +39,7 @@ px4_add_module(
|
|||
FixedwingRateControl.cpp
|
||||
FixedwingRateControl.hpp
|
||||
DEPENDS
|
||||
FwAutoTrim
|
||||
px4_work_queue
|
||||
RateControl
|
||||
SlewRate
|
||||
|
|
|
@ -432,6 +432,11 @@ void FixedwingRateControl::Run()
|
|||
_vehicle_torque_setpoint.timestamp_sample = angular_velocity.timestamp_sample;
|
||||
_vehicle_torque_setpoint_pub.publish(_vehicle_torque_setpoint);
|
||||
}
|
||||
|
||||
_auto_trim.update(_vehicle_torque_setpoint, dt);
|
||||
|
||||
} else {
|
||||
_auto_trim.reset();
|
||||
}
|
||||
|
||||
updateActuatorControlsStatus(dt);
|
||||
|
|
|
@ -69,6 +69,8 @@
|
|||
#include <uORB/topics/vehicle_thrust_setpoint.h>
|
||||
#include <uORB/topics/vehicle_torque_setpoint.h>
|
||||
|
||||
#include "fw_auto_trim/FwAutoTrim.hpp"
|
||||
|
||||
using matrix::Eulerf;
|
||||
using matrix::Quatf;
|
||||
|
||||
|
@ -209,6 +211,8 @@ private:
|
|||
|
||||
RateControl _rate_control; ///< class for rate control calculations
|
||||
|
||||
FwAutoTrim _auto_trim{this};
|
||||
|
||||
void updateActuatorControlsStatus(float dt);
|
||||
|
||||
/**
|
||||
|
|
|
@ -30,17 +30,9 @@
|
|||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
px4_add_module(
|
||||
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
|
||||
px4_add_library(FwAutoTrim
|
||||
FwAutoTrim.cpp
|
||||
)
|
||||
|
||||
# target_link_libraries(FwAutoTrim)
|
||||
target_include_directories(FwAutoTrim PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
|
@ -38,13 +38,11 @@
|
|||
using namespace time_literals;
|
||||
using matrix::Vector3f;
|
||||
|
||||
FwAutoTrim::FwAutoTrim(bool is_vtol) :
|
||||
ModuleParams(nullptr),
|
||||
WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers),
|
||||
_vehicle_torque_setpoint_sub(this, ORB_ID(vehicle_torque_setpoint), is_vtol ? 1 : 0)
|
||||
FwAutoTrim::FwAutoTrim(ModuleParams *parent) :
|
||||
ModuleParams(parent)
|
||||
{
|
||||
_auto_trim_status_pub.advertise();
|
||||
updateParams();
|
||||
reset();
|
||||
}
|
||||
|
||||
FwAutoTrim::~FwAutoTrim()
|
||||
|
@ -52,35 +50,18 @@ FwAutoTrim::~FwAutoTrim()
|
|||
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()
|
||||
{
|
||||
ModuleParams::updateParams();
|
||||
}
|
||||
|
||||
void FwAutoTrim::Run()
|
||||
void FwAutoTrim::reset()
|
||||
{
|
||||
if (should_exit()) {
|
||||
_vehicle_torque_setpoint_sub.unregisterCallback();
|
||||
exit_and_cleanup();
|
||||
return;
|
||||
}
|
||||
_state = state::idle;
|
||||
}
|
||||
|
||||
void FwAutoTrim::update(const vehicle_torque_setpoint_s &vehicle_torque_setpoint, const float dt)
|
||||
{
|
||||
if (_vehicle_land_detected_sub.updated()) {
|
||||
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);
|
||||
|
||||
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 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);
|
||||
}
|
||||
|
||||
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()
|
||||
{
|
||||
perf_print_counter(_cycle_perf);
|
||||
|
@ -279,27 +203,3 @@ int FwAutoTrim::print_status()
|
|||
|
||||
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_params.h>
|
||||
#include <px4_platform_common/posix.h>
|
||||
#include <px4_platform_common/px4_work_queue/WorkItem.hpp>
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/SubscriptionCallback.hpp>
|
||||
#include <uORB/topics/airspeed_validated.h>
|
||||
#include <uORB/topics/auto_trim_status.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
|
@ -55,41 +53,24 @@
|
|||
|
||||
using namespace time_literals;
|
||||
|
||||
class FwAutoTrim : public ModuleBase<FwAutoTrim>, public ModuleParams,
|
||||
public px4::WorkItem
|
||||
class FwAutoTrim : public ModuleParams
|
||||
{
|
||||
public:
|
||||
FwAutoTrim(bool is_vtol);
|
||||
~FwAutoTrim() override;
|
||||
|
||||
/** @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;
|
||||
FwAutoTrim(ModuleParams *parent);
|
||||
~FwAutoTrim();
|
||||
|
||||
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);
|
||||
|
||||
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 _airspeed_validated_sub{ORB_ID(airspeed_validated)};
|
||||
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
|
||||
|
@ -104,7 +85,6 @@ private:
|
|||
fail = auto_trim_status_s::STATE_FAIL,
|
||||
} _state{state::idle};
|
||||
|
||||
hrt_abstime _timestamp_last{0};
|
||||
hrt_abstime _state_start_time{0};
|
||||
|
||||
bool _armed{false};
|
Loading…
Reference in New Issue