forked from Archive/PX4-Autopilot
Revert "attitude_fw delete unused and cleanup"
This reverts commit 25bd3ac5e6
.
This commit is contained in:
parent
1531269f92
commit
b0300b9723
|
@ -61,3 +61,4 @@ px4_add_module(
|
|||
DEPENDS
|
||||
platforms__common
|
||||
)
|
||||
# vim: set noet ft=cmake fenc=utf-8 ff=unix :
|
||||
|
|
|
@ -826,4 +826,4 @@ float _wrap_360(float bearing)
|
|||
|
||||
return bearing;
|
||||
}
|
||||
#endif //POSIX_SHARED
|
||||
#endif //POSIX_SHARED
|
|
@ -48,6 +48,25 @@
|
|||
|
||||
#include "ecl_controller.h"
|
||||
|
||||
#include <stdio.h>
|
||||
#include <mathlib/mathlib.h>
|
||||
|
||||
ECL_Controller::ECL_Controller(const char *name) :
|
||||
_last_run(0),
|
||||
_tc(0.1f),
|
||||
_k_p(0.0f),
|
||||
_k_i(0.0f),
|
||||
_k_ff(0.0f),
|
||||
_integrator_max(0.0f),
|
||||
_max_rate(0.0f),
|
||||
_last_output(0.0f),
|
||||
_integrator(0.0f),
|
||||
_rate_error(0.0f),
|
||||
_rate_setpoint(0.0f),
|
||||
_bodyrate_setpoint(0.0f)
|
||||
{
|
||||
}
|
||||
|
||||
void ECL_Controller::reset_integrator()
|
||||
{
|
||||
_integrator = 0.0f;
|
||||
|
@ -104,7 +123,7 @@ float ECL_Controller::constrain_airspeed(float airspeed, float minspeed, float m
|
|||
{
|
||||
float airspeed_result = airspeed;
|
||||
|
||||
if (!ISFINITE(airspeed)) {
|
||||
if (!PX4_ISFINITE(airspeed)) {
|
||||
/* airspeed is NaN, +- INF or not available, pick center of band */
|
||||
airspeed_result = 0.5f * (minspeed + maxspeed);
|
||||
|
||||
|
|
|
@ -48,8 +48,9 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include "ecl/ecl.h"
|
||||
#include "mathlib/mathlib.h"
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <systemlib/perf_counter.h>
|
||||
|
||||
struct ECL_ControlData {
|
||||
float roll;
|
||||
|
@ -76,7 +77,7 @@ struct ECL_ControlData {
|
|||
class __EXPORT ECL_Controller
|
||||
{
|
||||
public:
|
||||
ECL_Controller() = default;
|
||||
ECL_Controller(const char *name);
|
||||
~ECL_Controller() = default;
|
||||
|
||||
virtual float control_attitude(const struct ECL_ControlData &ctl_data) = 0;
|
||||
|
@ -90,7 +91,7 @@ public:
|
|||
void set_k_ff(float k_ff);
|
||||
void set_integrator_max(float max);
|
||||
void set_max_rate(float max_rate);
|
||||
void set_bodyrate_setpoint(float rate) { _bodyrate_setpoint = rate; }
|
||||
void set_bodyrate_setpoint(float rate) {_bodyrate_setpoint = rate;}
|
||||
|
||||
/* Getters */
|
||||
float get_rate_error();
|
||||
|
@ -100,22 +101,17 @@ public:
|
|||
void reset_integrator();
|
||||
|
||||
protected:
|
||||
uint64_t _last_run{0};
|
||||
|
||||
float _tc{0.5f};
|
||||
float _k_p{0.0f};
|
||||
float _k_i{0.0f};
|
||||
float _k_ff{0.0f};
|
||||
|
||||
float _integrator_max{0.0f};
|
||||
float _integrator{0.0f};
|
||||
|
||||
float _rate_setpoint{0.0f};
|
||||
float _bodyrate_setpoint{0.0f};
|
||||
|
||||
float _max_rate{0.0f};
|
||||
float _rate_error{0.0f};
|
||||
float _last_output{0.0f};
|
||||
|
||||
uint64_t _last_run;
|
||||
float _tc;
|
||||
float _k_p;
|
||||
float _k_i;
|
||||
float _k_ff;
|
||||
float _integrator_max;
|
||||
float _max_rate;
|
||||
float _last_output;
|
||||
float _integrator;
|
||||
float _rate_error;
|
||||
float _rate_setpoint;
|
||||
float _bodyrate_setpoint;
|
||||
float constrain_airspeed(float airspeed, float minspeed, float maxspeed);
|
||||
};
|
||||
|
|
|
@ -35,20 +35,34 @@
|
|||
* @file ecl_pitch_controller.cpp
|
||||
* Implementation of a simple orthogonal pitch PID controller.
|
||||
*
|
||||
* Authors and acknowledgments in header.
|
||||
* Authors and acknowledgements in header.
|
||||
*/
|
||||
|
||||
#include "ecl_pitch_controller.h"
|
||||
#include <math.h>
|
||||
#include <stdint.h>
|
||||
#include <float.h>
|
||||
#include <geo/geo.h>
|
||||
#include <ecl/ecl.h>
|
||||
#include <mathlib/mathlib.h>
|
||||
#include <systemlib/err.h>
|
||||
|
||||
ECL_PitchController::ECL_PitchController() :
|
||||
ECL_Controller("pitch"),
|
||||
_max_rate_neg(0.0f),
|
||||
_roll_ff(0.0f)
|
||||
{
|
||||
}
|
||||
|
||||
float ECL_PitchController::control_attitude(const struct ECL_ControlData &ctl_data)
|
||||
{
|
||||
/* Do not calculate control signal with bad inputs */
|
||||
if (!(ISFINITE(ctl_data.pitch_setpoint) &&
|
||||
ISFINITE(ctl_data.roll) &&
|
||||
ISFINITE(ctl_data.pitch) &&
|
||||
ISFINITE(ctl_data.airspeed))) {
|
||||
|
||||
ECL_WARN("not controlling pitch");
|
||||
/* Do not calculate control signal with bad inputs */
|
||||
if (!(PX4_ISFINITE(ctl_data.pitch_setpoint) &&
|
||||
PX4_ISFINITE(ctl_data.roll) &&
|
||||
PX4_ISFINITE(ctl_data.pitch) &&
|
||||
PX4_ISFINITE(ctl_data.airspeed))) {
|
||||
warnx("not controlling pitch");
|
||||
return _rate_setpoint;
|
||||
}
|
||||
|
||||
|
@ -59,13 +73,14 @@ float ECL_PitchController::control_attitude(const struct ECL_ControlData &ctl_da
|
|||
_rate_setpoint = pitch_error / _tc;
|
||||
|
||||
/* limit the rate */
|
||||
if (_max_rate >= 0.0f && _max_rate_neg >= 0.0f) {
|
||||
if (_max_rate > 0.01f && _max_rate_neg > 0.01f) {
|
||||
if (_rate_setpoint > 0.0f) {
|
||||
_rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint;
|
||||
|
||||
} else {
|
||||
_rate_setpoint = (_rate_setpoint < -_max_rate_neg) ? -_max_rate_neg : _rate_setpoint;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
return _rate_setpoint;
|
||||
|
@ -74,22 +89,21 @@ float ECL_PitchController::control_attitude(const struct ECL_ControlData &ctl_da
|
|||
float ECL_PitchController::control_bodyrate(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.body_y_rate) &&
|
||||
ISFINITE(ctl_data.body_z_rate) &&
|
||||
ISFINITE(ctl_data.yaw_rate_setpoint) &&
|
||||
ISFINITE(ctl_data.airspeed_min) &&
|
||||
ISFINITE(ctl_data.airspeed_max) &&
|
||||
ISFINITE(ctl_data.scaler))) {
|
||||
|
||||
if (!(PX4_ISFINITE(ctl_data.roll) &&
|
||||
PX4_ISFINITE(ctl_data.pitch) &&
|
||||
PX4_ISFINITE(ctl_data.body_y_rate) &&
|
||||
PX4_ISFINITE(ctl_data.body_z_rate) &&
|
||||
PX4_ISFINITE(ctl_data.yaw_rate_setpoint) &&
|
||||
PX4_ISFINITE(ctl_data.airspeed_min) &&
|
||||
PX4_ISFINITE(ctl_data.airspeed_max) &&
|
||||
PX4_ISFINITE(ctl_data.scaler))) {
|
||||
return math::constrain(_last_output, -1.0f, 1.0f);
|
||||
}
|
||||
|
||||
/* get the usual dt estimate */
|
||||
uint64_t dt_micros = ecl_elapsed_time(&_last_run);
|
||||
_last_run = ecl_absolute_time();
|
||||
float dt = dt_micros * 1e-6f;
|
||||
float dt = (float)dt_micros * 1e-6f;
|
||||
|
||||
/* lock integral for long intervals */
|
||||
bool lock_integrator = ctl_data.lock_integrator;
|
||||
|
|
|
@ -49,17 +49,21 @@
|
|||
#ifndef ECL_PITCH_CONTROLLER_H
|
||||
#define ECL_PITCH_CONTROLLER_H
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include "ecl_controller.h"
|
||||
|
||||
class __EXPORT ECL_PitchController final : public ECL_Controller
|
||||
class __EXPORT ECL_PitchController :
|
||||
public ECL_Controller
|
||||
{
|
||||
public:
|
||||
ECL_PitchController() = default;
|
||||
ECL_PitchController();
|
||||
~ECL_PitchController() = default;
|
||||
|
||||
float control_attitude(const struct ECL_ControlData &ctl_data) override;
|
||||
float control_euler_rate(const struct ECL_ControlData &ctl_data) override;
|
||||
float control_bodyrate(const struct ECL_ControlData &ctl_data) override;
|
||||
float control_attitude(const struct ECL_ControlData &ctl_data);
|
||||
float control_euler_rate(const struct ECL_ControlData &ctl_data);
|
||||
float control_bodyrate(const struct ECL_ControlData &ctl_data);
|
||||
|
||||
/* Additional Setters */
|
||||
void set_max_rate_pos(float max_rate_pos)
|
||||
|
@ -72,8 +76,14 @@ public:
|
|||
_max_rate_neg = max_rate_neg;
|
||||
}
|
||||
|
||||
void set_roll_ff(float roll_ff)
|
||||
{
|
||||
_roll_ff = roll_ff;
|
||||
}
|
||||
|
||||
protected:
|
||||
float _max_rate_neg{0.0f};
|
||||
float _max_rate_neg;
|
||||
float _roll_ff;
|
||||
};
|
||||
|
||||
#endif // ECL_PITCH_CONTROLLER_H
|
||||
|
|
|
@ -35,15 +35,27 @@
|
|||
* @file ecl_roll_controller.cpp
|
||||
* Implementation of a simple orthogonal roll PID controller.
|
||||
*
|
||||
* Authors and acknowledgments in header.
|
||||
* Authors and acknowledgements in header.
|
||||
*/
|
||||
|
||||
#include <ecl/ecl.h>
|
||||
#include "ecl_roll_controller.h"
|
||||
#include <stdint.h>
|
||||
#include <float.h>
|
||||
#include <geo/geo.h>
|
||||
#include <ecl/ecl.h>
|
||||
#include <mathlib/mathlib.h>
|
||||
#include <systemlib/err.h>
|
||||
|
||||
ECL_RollController::ECL_RollController() :
|
||||
ECL_Controller("roll")
|
||||
{
|
||||
}
|
||||
|
||||
float ECL_RollController::control_attitude(const struct ECL_ControlData &ctl_data)
|
||||
{
|
||||
/* Do not calculate control signal with bad inputs */
|
||||
if (!(ISFINITE(ctl_data.roll_setpoint) && ISFINITE(ctl_data.roll))) {
|
||||
if (!(PX4_ISFINITE(ctl_data.roll_setpoint) && PX4_ISFINITE(ctl_data.roll))) {
|
||||
return _rate_setpoint;
|
||||
}
|
||||
|
||||
|
@ -55,7 +67,7 @@ float ECL_RollController::control_attitude(const struct ECL_ControlData &ctl_dat
|
|||
|
||||
/* limit the rate */ //XXX: move to body angluar rates
|
||||
|
||||
if (_max_rate >= 0.0f) {
|
||||
if (_max_rate > 0.01f) {
|
||||
_rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint;
|
||||
_rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint;
|
||||
}
|
||||
|
@ -66,21 +78,20 @@ float ECL_RollController::control_attitude(const struct ECL_ControlData &ctl_dat
|
|||
float ECL_RollController::control_bodyrate(const struct ECL_ControlData &ctl_data)
|
||||
{
|
||||
/* Do not calculate control signal with bad inputs */
|
||||
if (!(ISFINITE(ctl_data.pitch) &&
|
||||
ISFINITE(ctl_data.body_x_rate) &&
|
||||
ISFINITE(ctl_data.body_z_rate) &&
|
||||
ISFINITE(ctl_data.yaw_rate_setpoint) &&
|
||||
ISFINITE(ctl_data.airspeed_min) &&
|
||||
ISFINITE(ctl_data.airspeed_max) &&
|
||||
ISFINITE(ctl_data.scaler))) {
|
||||
|
||||
if (!(PX4_ISFINITE(ctl_data.pitch) &&
|
||||
PX4_ISFINITE(ctl_data.body_x_rate) &&
|
||||
PX4_ISFINITE(ctl_data.body_z_rate) &&
|
||||
PX4_ISFINITE(ctl_data.yaw_rate_setpoint) &&
|
||||
PX4_ISFINITE(ctl_data.airspeed_min) &&
|
||||
PX4_ISFINITE(ctl_data.airspeed_max) &&
|
||||
PX4_ISFINITE(ctl_data.scaler))) {
|
||||
return math::constrain(_last_output, -1.0f, 1.0f);
|
||||
}
|
||||
|
||||
/* get the usual dt estimate */
|
||||
uint64_t dt_micros = ecl_elapsed_time(&_last_run);
|
||||
_last_run = ecl_absolute_time();
|
||||
float dt = dt_micros * 1e-6f;
|
||||
float dt = (float)dt_micros * 1e-6f;
|
||||
|
||||
/* lock integral for long intervals */
|
||||
bool lock_integrator = ctl_data.lock_integrator;
|
||||
|
@ -129,4 +140,6 @@ float ECL_RollController::control_euler_rate(const struct ECL_ControlData &ctl_d
|
|||
_bodyrate_setpoint = ctl_data.roll_rate_setpoint - sinf(ctl_data.pitch) * ctl_data.yaw_rate_setpoint;
|
||||
|
||||
return control_bodyrate(ctl_data);
|
||||
|
||||
}
|
||||
|
||||
|
|
|
@ -49,17 +49,21 @@
|
|||
#ifndef ECL_ROLL_CONTROLLER_H
|
||||
#define ECL_ROLL_CONTROLLER_H
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include "ecl_controller.h"
|
||||
|
||||
class __EXPORT ECL_RollController final : public ECL_Controller
|
||||
class __EXPORT ECL_RollController :
|
||||
public ECL_Controller
|
||||
{
|
||||
public:
|
||||
ECL_RollController() = default;
|
||||
ECL_RollController();
|
||||
~ECL_RollController() = default;
|
||||
|
||||
float control_attitude(const struct ECL_ControlData &ctl_data) override;
|
||||
float control_euler_rate(const struct ECL_ControlData &ctl_data) override;
|
||||
float control_bodyrate(const struct ECL_ControlData &ctl_data) override;
|
||||
float control_attitude(const struct ECL_ControlData &ctl_data);
|
||||
float control_euler_rate(const struct ECL_ControlData &ctl_data);
|
||||
float control_bodyrate(const struct ECL_ControlData &ctl_data);
|
||||
};
|
||||
|
||||
#endif // ECL_ROLL_CONTROLLER_H
|
||||
|
|
|
@ -35,26 +35,36 @@
|
|||
* @file ecl_wheel_controller.cpp
|
||||
* Implementation of a simple PID wheel controller for heading tracking.
|
||||
*
|
||||
* Authors and acknowledgments in header.
|
||||
* Authors and acknowledgements in header.
|
||||
*/
|
||||
|
||||
#include "ecl_wheel_controller.h"
|
||||
|
||||
#include <stdint.h>
|
||||
#include <float.h>
|
||||
#include <geo/geo.h>
|
||||
#include <ecl/ecl.h>
|
||||
#include <mathlib/mathlib.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <ecl/ecl.h>
|
||||
|
||||
ECL_WheelController::ECL_WheelController() :
|
||||
ECL_Controller("wheel")
|
||||
{
|
||||
}
|
||||
|
||||
float ECL_WheelController::control_bodyrate(const struct ECL_ControlData &ctl_data)
|
||||
{
|
||||
/* Do not calculate control signal with bad inputs */
|
||||
if (!(ISFINITE(ctl_data.body_z_rate) &&
|
||||
ISFINITE(ctl_data.groundspeed) &&
|
||||
ISFINITE(ctl_data.groundspeed_scaler))) {
|
||||
if (!(PX4_ISFINITE(ctl_data.body_z_rate) &&
|
||||
PX4_ISFINITE(ctl_data.groundspeed) &&
|
||||
PX4_ISFINITE(ctl_data.groundspeed_scaler))) {
|
||||
return math::constrain(_last_output, -1.0f, 1.0f);
|
||||
}
|
||||
|
||||
/* get the usual dt estimate */
|
||||
uint64_t dt_micros = ecl_elapsed_time(&_last_run);
|
||||
_last_run = ecl_absolute_time();
|
||||
float dt = dt_micros * 1e-6f;
|
||||
float dt = (float)dt_micros * 1e-6f;
|
||||
|
||||
/* lock integral for long intervals */
|
||||
bool lock_integrator = ctl_data.lock_integrator;
|
||||
|
@ -99,11 +109,12 @@ float ECL_WheelController::control_bodyrate(const struct ECL_ControlData &ctl_da
|
|||
return math::constrain(_last_output, -1.0f, 1.0f);
|
||||
}
|
||||
|
||||
|
||||
float ECL_WheelController::control_attitude(const struct ECL_ControlData &ctl_data)
|
||||
{
|
||||
/* Do not calculate control signal with bad inputs */
|
||||
if (!(ISFINITE(ctl_data.yaw_setpoint) &&
|
||||
ISFINITE(ctl_data.yaw))) {
|
||||
if (!(PX4_ISFINITE(ctl_data.yaw_setpoint) &&
|
||||
PX4_ISFINITE(ctl_data.yaw))) {
|
||||
return _rate_setpoint;
|
||||
}
|
||||
|
||||
|
@ -114,13 +125,14 @@ float ECL_WheelController::control_attitude(const struct ECL_ControlData &ctl_da
|
|||
_rate_setpoint = yaw_error / _tc;
|
||||
|
||||
/* limit the rate */
|
||||
if (_max_rate >= 0.0f) {
|
||||
if (_max_rate > 0.01f) {
|
||||
if (_rate_setpoint > 0.0f) {
|
||||
_rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint;
|
||||
|
||||
} else {
|
||||
_rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
return _rate_setpoint;
|
||||
|
|
|
@ -49,17 +49,23 @@
|
|||
#ifndef ECL_HEADING_CONTROLLER_H
|
||||
#define ECL_HEADING_CONTROLLER_H
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include "ecl_controller.h"
|
||||
|
||||
class __EXPORT ECL_WheelController final : public ECL_Controller
|
||||
class __EXPORT ECL_WheelController :
|
||||
public ECL_Controller
|
||||
{
|
||||
public:
|
||||
ECL_WheelController() = default;
|
||||
ECL_WheelController();
|
||||
~ECL_WheelController() = default;
|
||||
|
||||
float control_attitude(const struct ECL_ControlData &ctl_data) override;
|
||||
float control_bodyrate(const struct ECL_ControlData &ctl_data) override;
|
||||
float control_euler_rate(const struct ECL_ControlData &ctl_data) override { return 0; }
|
||||
float control_attitude(const struct ECL_ControlData &ctl_data);
|
||||
|
||||
float control_bodyrate(const struct ECL_ControlData &ctl_data);
|
||||
|
||||
float control_euler_rate(const struct ECL_ControlData &ctl_data) {return 0;}
|
||||
};
|
||||
|
||||
#endif // ECL_HEADING_CONTROLLER_H
|
||||
|
|
|
@ -35,18 +35,54 @@
|
|||
* @file ecl_yaw_controller.cpp
|
||||
* Implementation of a simple orthogonal coordinated turn yaw PID controller.
|
||||
*
|
||||
* Authors and acknowledgments in header.
|
||||
* Authors and acknowledgements in header.
|
||||
*/
|
||||
|
||||
#include "ecl_yaw_controller.h"
|
||||
#include <stdint.h>
|
||||
#include <float.h>
|
||||
#include <geo/geo.h>
|
||||
#include <ecl/ecl.h>
|
||||
#include <mathlib/mathlib.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <ecl/ecl.h>
|
||||
|
||||
ECL_YawController::ECL_YawController() :
|
||||
ECL_Controller("yaw"),
|
||||
_coordinated_min_speed(1.0f),
|
||||
_max_rate(0.0f), /* disable by default */
|
||||
_coordinated_method(0)
|
||||
{
|
||||
}
|
||||
|
||||
float ECL_YawController::control_attitude(const struct ECL_ControlData &ctl_data)
|
||||
{
|
||||
switch (_coordinated_method) {
|
||||
case COORD_METHOD_OPEN:
|
||||
return control_attitude_impl_openloop(ctl_data);
|
||||
|
||||
case COORD_METHOD_CLOSEACC:
|
||||
return control_attitude_impl_accclosedloop(ctl_data);
|
||||
|
||||
default:
|
||||
static hrt_abstime last_print = 0;
|
||||
|
||||
if (ecl_elapsed_time(&last_print) > 5e6) {
|
||||
warnx("invalid param setting FW_YCO_METHOD");
|
||||
last_print = ecl_absolute_time();
|
||||
}
|
||||
}
|
||||
|
||||
return _rate_setpoint;
|
||||
}
|
||||
|
||||
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) &&
|
||||
ISFINITE(ctl_data.pitch) &&
|
||||
ISFINITE(ctl_data.roll_rate_setpoint) &&
|
||||
ISFINITE(ctl_data.pitch_rate_setpoint))) {
|
||||
if (!(PX4_ISFINITE(ctl_data.roll) &&
|
||||
PX4_ISFINITE(ctl_data.pitch) &&
|
||||
PX4_ISFINITE(ctl_data.roll_rate_setpoint) &&
|
||||
PX4_ISFINITE(ctl_data.pitch_rate_setpoint))) {
|
||||
return _rate_setpoint;
|
||||
}
|
||||
|
||||
|
@ -84,12 +120,12 @@ float ECL_YawController::control_attitude(const struct ECL_ControlData &ctl_data
|
|||
|
||||
/* limit the rate */ //XXX: move to body angluar rates
|
||||
|
||||
if (_max_rate >= 0.0f) {
|
||||
if (_max_rate > 0.01f) {
|
||||
_rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint;
|
||||
_rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint;
|
||||
}
|
||||
|
||||
if (!ISFINITE(_rate_setpoint)) {
|
||||
if (!PX4_ISFINITE(_rate_setpoint)) {
|
||||
warnx("yaw rate sepoint not finite");
|
||||
_rate_setpoint = 0.0f;
|
||||
}
|
||||
|
@ -100,17 +136,17 @@ float ECL_YawController::control_attitude(const struct ECL_ControlData &ctl_data
|
|||
float ECL_YawController::control_bodyrate(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.body_y_rate) &&
|
||||
ISFINITE(ctl_data.body_z_rate) && ISFINITE(ctl_data.pitch_rate_setpoint) &&
|
||||
ISFINITE(ctl_data.airspeed_min) && ISFINITE(ctl_data.airspeed_max) &&
|
||||
ISFINITE(ctl_data.scaler))) {
|
||||
if (!(PX4_ISFINITE(ctl_data.roll) && PX4_ISFINITE(ctl_data.pitch) && PX4_ISFINITE(ctl_data.body_y_rate) &&
|
||||
PX4_ISFINITE(ctl_data.body_z_rate) && PX4_ISFINITE(ctl_data.pitch_rate_setpoint) &&
|
||||
PX4_ISFINITE(ctl_data.airspeed_min) && PX4_ISFINITE(ctl_data.airspeed_max) &&
|
||||
PX4_ISFINITE(ctl_data.scaler))) {
|
||||
return math::constrain(_last_output, -1.0f, 1.0f);
|
||||
}
|
||||
|
||||
/* get the usual dt estimate */
|
||||
uint64_t dt_micros = ecl_elapsed_time(&_last_run);
|
||||
_last_run = ecl_absolute_time();
|
||||
float dt = dt_micros * 1e-6f;
|
||||
float dt = (float)dt_micros * 1e-6f;
|
||||
|
||||
/* lock integral for long intervals */
|
||||
bool lock_integrator = ctl_data.lock_integrator;
|
||||
|
@ -122,7 +158,7 @@ float ECL_YawController::control_bodyrate(const struct ECL_ControlData &ctl_data
|
|||
/* input conditioning */
|
||||
float airspeed = ctl_data.airspeed;
|
||||
|
||||
if (!ISFINITE(airspeed)) {
|
||||
if (!PX4_ISFINITE(airspeed)) {
|
||||
/* airspeed is NaN, +- INF or not available, pick center of band */
|
||||
airspeed = 0.5f * (ctl_data.airspeed_min + ctl_data.airspeed_max);
|
||||
|
||||
|
@ -130,6 +166,12 @@ float ECL_YawController::control_bodyrate(const struct ECL_ControlData &ctl_data
|
|||
airspeed = ctl_data.airspeed_min;
|
||||
}
|
||||
|
||||
/* Close the acceleration loop if _coordinated_method wants this: change body_rate setpoint */
|
||||
if (_coordinated_method == COORD_METHOD_CLOSEACC) {
|
||||
// XXX lateral acceleration needs to go into integrator with a gain
|
||||
//_bodyrate_setpoint -= (ctl_data.acc_body_y / (airspeed * cosf(ctl_data.pitch)));
|
||||
}
|
||||
|
||||
/* Calculate body angular rate error */
|
||||
_rate_error = _bodyrate_setpoint - ctl_data.body_z_rate; // body angular rate error
|
||||
|
||||
|
@ -160,9 +202,16 @@ float ECL_YawController::control_bodyrate(const struct ECL_ControlData &ctl_data
|
|||
_last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained) * ctl_data.scaler *
|
||||
ctl_data.scaler; //scaler is proportional to 1/airspeed
|
||||
|
||||
|
||||
return math::constrain(_last_output, -1.0f, 1.0f);
|
||||
}
|
||||
|
||||
float ECL_YawController::control_attitude_impl_accclosedloop(const struct ECL_ControlData &ctl_data)
|
||||
{
|
||||
/* dont set a rate setpoint */
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
float ECL_YawController::control_euler_rate(const struct ECL_ControlData &ctl_data)
|
||||
{
|
||||
/* Transform setpoint to body angular rates (jacobian) */
|
||||
|
|
|
@ -48,17 +48,48 @@
|
|||
#ifndef ECL_YAW_CONTROLLER_H
|
||||
#define ECL_YAW_CONTROLLER_H
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include "ecl_controller.h"
|
||||
|
||||
class __EXPORT ECL_YawController final : public ECL_Controller
|
||||
class __EXPORT ECL_YawController :
|
||||
public ECL_Controller
|
||||
{
|
||||
public:
|
||||
ECL_YawController() = default;
|
||||
ECL_YawController();
|
||||
~ECL_YawController() = default;
|
||||
|
||||
float control_attitude(const struct ECL_ControlData &ctl_data) override;
|
||||
float control_euler_rate(const struct ECL_ControlData &ctl_data) override;
|
||||
float control_bodyrate(const struct ECL_ControlData &ctl_data) override;
|
||||
float control_attitude(const struct ECL_ControlData &ctl_data);
|
||||
float control_euler_rate(const struct ECL_ControlData &ctl_data);
|
||||
float control_bodyrate(const struct ECL_ControlData &ctl_data);
|
||||
|
||||
/* Additional setters */
|
||||
void set_coordinated_min_speed(float coordinated_min_speed)
|
||||
{
|
||||
_coordinated_min_speed = coordinated_min_speed;
|
||||
}
|
||||
|
||||
void set_coordinated_method(int32_t coordinated_method)
|
||||
{
|
||||
_coordinated_method = coordinated_method;
|
||||
}
|
||||
|
||||
enum {
|
||||
COORD_METHOD_OPEN = 0,
|
||||
COORD_METHOD_CLOSEACC = 1
|
||||
};
|
||||
|
||||
protected:
|
||||
float _coordinated_min_speed;
|
||||
float _max_rate;
|
||||
|
||||
int32_t _coordinated_method;
|
||||
|
||||
float control_attitude_impl_openloop(const struct ECL_ControlData &ctl_data);
|
||||
|
||||
float control_attitude_impl_accclosedloop(const struct ECL_ControlData &ctl_data);
|
||||
|
||||
};
|
||||
|
||||
#endif // ECL_YAW_CONTROLLER_H
|
||||
|
|
Loading…
Reference in New Issue