fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-18 06:23:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

fw att: add param to select yawrate control method

Browse Source
This commit is contained in:
Thomas Gubler
2015-02-15 10:13:49 +01:00
committed by Lorenz Meier
parent 1b8a830a38
commit 53ffde30c2
4 changed files with 62 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
+34 -2
View File
@@ -45,10 +45,12 @@
#include <ecl/ecl.h>
#include <mathlib/mathlib.h>
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
ECL_YawController::ECL_YawController() :
ECL_Controller("yaw"),
_coordinated_min_speed(1.0f)
_coordinated_min_speed(1.0f),
_coordinated_method(0)
{
}
@@ -57,6 +59,36 @@ ECL_YawController::~ECL_YawController()
}
float ECL_YawController::control_attitude(const struct ECL_ControlData &ctl_data)
{
switch (_coordinated_method) {
case 0:
return control_attitude_impl_openloop(ctl_data);
default:
static hrt_abstime last_print = 0;
if (hrt_elapsed_time(&last_print) > 5e6) {
warnx("invalid param setting FW_YCO_METHOD");
last_print = hrt_absolute_time();
}
}
return _rate_setpoint;
}
float ECL_YawController::control_bodyrate(const struct ECL_ControlData &ctl_data)
{
switch (_coordinated_method) {
case 0:
return control_bodyrate_impl(ctl_data);
default:
static hrt_abstime last_print = 0;
if (hrt_elapsed_time(&last_print) > 5e6) {
warnx("invalid param setting FW_YCO_METHOD");
last_print = hrt_absolute_time();
}
}
return math::constrain(_last_output, -1.0f, 1.0f);
}
float ECL_YawController::control_attitude_impl_openloop(const struct ECL_ControlData &ctl_data)
{
/* Do not calculate control signal with bad inputs */
if (!(isfinite(ctl_data.roll) &&
@@ -109,7 +141,7 @@ float ECL_YawController::control_attitude(const struct ECL_ControlData &ctl_data
return _rate_setpoint;
}
float ECL_YawController::control_bodyrate(const struct ECL_ControlData &ctl_data)
float ECL_YawController::control_bodyrate_impl(const struct ECL_ControlData &ctl_data)
{
/* Do not calculate control signal with bad inputs */
if (!(isfinite(ctl_data.roll) && isfinite(ctl_data.pitch) && isfinite(ctl_data.pitch_rate) &&
src/lib/ecl/attitude_fw/ecl_yaw_controller.h
+10
View File
@@ -62,6 +62,7 @@ public:
~ECL_YawController();
float control_attitude(const struct ECL_ControlData &ctl_data);
float control_bodyrate(const struct ECL_ControlData &ctl_data);
/* Additional setters */
@@ -69,8 +70,17 @@ public:
_coordinated_min_speed = coordinated_min_speed;
}
void set_coordinated_method(int32_t coordinated_method) {
_coordinated_method = coordinated_method;
}
protected:
float _coordinated_min_speed;
int32_t _coordinated_method;;
float control_attitude_impl_openloop(const struct ECL_ControlData &ctl_data);
float control_bodyrate_impl(const struct ECL_ControlData &ctl_data);
};
#endif // ECL_YAW_CONTROLLER_H