yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

fw att control: cleanup, create base class for ECL

Browse Source 
Adding a new base class to remove a lot of boilerplate code, no
functionality changes
This commit is contained in:
Thomas Gubler
2015-01-03 18:24:26 +01:00
parent c1161d4e89
commit c6722fce0b
10 changed files with 403 additions and 330 deletions
Download Patch File Download Diff File Expand all files Collapse all files

128
src/lib/ecl/attitude_fw/ecl_controller.cpp Normal file
View File

@@ -0,0 +1,128 @@
/****************************************************************************
*
* Copyright (c) 2013 Estimation and Control Library (ECL). 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 ECL 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 ecl_controller.cpp
* Definition of base class for other controllers
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Thomas Gubler <thomasgubler@gmail.com>
*
* Acknowledgements:
*
* The control design is based on a design
* by Paul Riseborough and Andrew Tridgell, 2013,
* which in turn is based on initial work of
* Jonathan Challinger, 2012.
*/
#include "ecl_controller.h"
#include <stdio.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),
_perf_name()
{
/* Init performance counter */
snprintf(_perf_name, sizeof(_perf_name), "fw att control %s nonfinite input", name);
_nonfinite_input_perf = perf_alloc(PC_COUNT, _perf_name);
}
ECL_Controller::~ECL_Controller()
{
perf_free(_nonfinite_input_perf);
}
void ECL_Controller::reset_integrator()
{
_integrator = 0.0f;
}
void ECL_Controller::set_time_constant(float time_constant)
{
if (time_constant > 0.1f && time_constant < 3.0f) {
_tc = time_constant;
}
}
void ECL_Controller::set_k_p(float k_p)
{
_k_p = k_p;
}
void ECL_Controller::set_k_i(float k_i)
{
_k_i = k_i;
}
void ECL_Controller::set_k_ff(float k_ff)
{
_k_ff = k_ff;
}
void ECL_Controller::set_integrator_max(float max)
{
_integrator_max = max;
}
void ECL_Controller::set_max_rate(float max_rate)
{
_max_rate = max_rate;
}
float ECL_Controller::get_rate_error()
{
return _rate_error;
}
float ECL_Controller::get_desired_rate()
{
return _rate_setpoint;
}
float ECL_Controller::get_desired_bodyrate()
{
return _bodyrate_setpoint;
}

119
src/lib/ecl/attitude_fw/ecl_controller.h Normal file
View File

@@ -0,0 +1,119 @@
/****************************************************************************
*
* Copyright (c) 2013 Estimation and Control Library (ECL). 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 ECL 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 ecl_controller.h
* Definition of base class for other controllers
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Thomas Gubler <thomasgubler@gmail.com>
*
* Acknowledgements:
*
* The control design is based on a design
* by Paul Riseborough and Andrew Tridgell, 2013,
* which in turn is based on initial work of
* Jonathan Challinger, 2012.
*/
#pragma once
#include <stdbool.h>
#include <stdint.h>
#include <systemlib/perf_counter.h>
struct ECL_ControlData {
float roll;
float pitch;
float yaw;
float roll_rate;
float pitch_rate;
float yaw_rate;
float speed_body_u;
float speed_body_v;
float speed_body_w;
float roll_setpoint;
float pitch_setpoint;
float yaw_setpoint;
float roll_rate_setpoint;
float pitch_rate_setpoint;
float yaw_rate_setpoint;
float airspeed_min;
float airspeed_max;
float airspeed;
float scaler;
bool lock_integrator;
};
class __EXPORT ECL_Controller
{
public:
ECL_Controller(const char *name);
~ECL_Controller();
virtual float control_attitude(const struct ECL_ControlData &ctl_data) = 0;
virtual float control_bodyrate(const struct ECL_ControlData &ctl_data) = 0;
/* Setters */
void set_time_constant(float time_constant);
void set_k_p(float k_p);
void set_k_i(float k_i);
void set_k_ff(float k_ff);
void set_integrator_max(float max);
void set_max_rate(float max_rate);
/* Getters */
float get_rate_error();
float get_desired_rate();
float get_desired_bodyrate();
void reset_integrator();
protected:
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;
perf_counter_t _nonfinite_input_perf;
static const uint8_t _perf_name_max = 40;
char _perf_name[_perf_name_max];
};

80
src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
View File

@@ -48,33 +48,24 @@
#include <systemlib/err.h>
ECL_PitchController::ECL_PitchController() :
_last_run(0),
_tc(0.1f),
_k_p(0.0f),
_k_i(0.0f),
_k_ff(0.0f),
_integrator_max(0.0f),
_max_rate_pos(0.0f),
ECL_Controller("pitch"),
_max_rate_neg(0.0f),
_roll_ff(0.0f),
_last_output(0.0f),
_integrator(0.0f),
_rate_error(0.0f),
_rate_setpoint(0.0f),
_bodyrate_setpoint(0.0f),
_nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control pitch nonfinite input"))
_roll_ff(0.0f)
{
}
ECL_PitchController::~ECL_PitchController()
{
perf_free(_nonfinite_input_perf);
}
float ECL_PitchController::control_attitude(float pitch_setpoint, float roll, float pitch, float airspeed)
float ECL_PitchController::control_attitude(const struct ECL_ControlData &ctl_data)
{
float roll = ctl_data.roll;
/* Do not calculate control signal with bad inputs */
if (!(isfinite(pitch_setpoint) && isfinite(roll) && isfinite(pitch) && isfinite(airspeed))) {
if (!(isfinite(ctl_data.pitch_setpoint) &&
isfinite(roll) &&
isfinite(ctl_data.pitch) &&
isfinite(ctl_data.airspeed))) {
perf_count(_nonfinite_input_perf);
warnx("not controlling pitch");
return _rate_setpoint;
@@ -102,13 +93,13 @@ float ECL_PitchController::control_attitude(float pitch_setpoint, float roll, fl
/* calculate the offset in the rate resulting from rolling */
//xxx needs explanation and conversion to body angular rates or should be removed
float turn_offset = fabsf((CONSTANTS_ONE_G / airspeed) *
float turn_offset = fabsf((CONSTANTS_ONE_G / ctl_data.airspeed) *
tanf(roll) * sinf(roll)) * _roll_ff;
if (inverted)
turn_offset = -turn_offset;
/* Calculate the error */
float pitch_error = pitch_setpoint - pitch;
float pitch_error = ctl_data.pitch_setpoint - ctl_data.pitch;
/* Apply P controller: rate setpoint from current error and time constant */
_rate_setpoint = pitch_error / _tc;
@@ -117,9 +108,9 @@ float ECL_PitchController::control_attitude(float pitch_setpoint, float roll, fl
_rate_setpoint += turn_offset;
/* limit the rate */ //XXX: move to body angluar rates
if (_max_rate_pos > 0.01f && _max_rate_neg > 0.01f) {
if (_max_rate > 0.01f && _max_rate_neg > 0.01f) {
if (_rate_setpoint > 0.0f) {
_rate_setpoint = (_rate_setpoint > _max_rate_pos) ? _max_rate_pos : _rate_setpoint;
_rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint;
} else {
_rate_setpoint = (_rate_setpoint < -_max_rate_neg) ? -_max_rate_neg : _rate_setpoint;
}
@@ -129,15 +120,17 @@ float ECL_PitchController::control_attitude(float pitch_setpoint, float roll, fl
return _rate_setpoint;
}
float ECL_PitchController::control_bodyrate(float roll, float pitch,
float pitch_rate, float yaw_rate,
float yaw_rate_setpoint,
float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator)
float ECL_PitchController::control_bodyrate(const struct ECL_ControlData &ctl_data)
{
/* Do not calculate control signal with bad inputs */
if (!(isfinite(roll) && isfinite(pitch) && isfinite(pitch_rate) && isfinite(yaw_rate) &&
isfinite(yaw_rate_setpoint) && isfinite(airspeed_min) &&
isfinite(airspeed_max) && isfinite(scaler))) {
if (!(isfinite(ctl_data.roll) &&
isfinite(ctl_data.pitch) &&
isfinite(ctl_data.pitch_rate) &&
isfinite(ctl_data.yaw_rate) &&
isfinite(ctl_data.yaw_rate_setpoint) &&
isfinite(ctl_data.airspeed_min) &&
isfinite(ctl_data.airspeed_max) &&
isfinite(ctl_data.scaler))) {
perf_count(_nonfinite_input_perf);
return math::constrain(_last_output, -1.0f, 1.0f);
}
@@ -148,28 +141,32 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch,
float dt = (float)dt_micros * 1e-6f;
/* lock integral for long intervals */
bool lock_integrator = ctl_data.lock_integrator;
if (dt_micros > 500000)
lock_integrator = true;
/* input conditioning */
float airspeed = ctl_data.airspeed;
if (!isfinite(airspeed)) {
/* airspeed is NaN, +- INF or not available, pick center of band */
airspeed = 0.5f * (airspeed_min + airspeed_max);
} else if (airspeed < airspeed_min) {
airspeed = airspeed_min;
airspeed = 0.5f * (ctl_data.airspeed_min + ctl_data.airspeed_max);
} else if (airspeed < ctl_data.airspeed_min) {
airspeed = ctl_data.airspeed_min;
}
/* Transform setpoint to body angular rates */
_bodyrate_setpoint = cosf(roll) * _rate_setpoint + cosf(pitch) * sinf(roll) * yaw_rate_setpoint; //jacobian
/* Transform setpoint to body angular rates (jacobian) */
_bodyrate_setpoint = cosf(ctl_data.roll) * _rate_setpoint +
cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.yaw_rate_setpoint;
/* Transform estimation to body angular rates */
float pitch_bodyrate = cosf(roll) * pitch_rate + cosf(pitch) * sinf(roll) * yaw_rate; //jacobian
/* Transform estimation to body angular rates (jacobian) */
float pitch_bodyrate = cosf(ctl_data.roll) * ctl_data.pitch_rate +
cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.yaw_rate;
_rate_error = _bodyrate_setpoint - pitch_bodyrate;
if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) {
if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * ctl_data.airspeed_min) {
float id = _rate_error * dt * scaler;
float id = _rate_error * dt * ctl_data.scaler;
/*
* anti-windup: do not allow integrator to increase if actuator is at limit
@@ -190,15 +187,10 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch,
float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max);
/* Apply PI rate controller and store non-limited output */
_last_output = _bodyrate_setpoint * _k_ff * scaler +
_rate_error * _k_p * scaler * scaler
_last_output = _bodyrate_setpoint * _k_ff * ctl_data.scaler +
_rate_error * _k_p * ctl_data.scaler * ctl_data.scaler
+ integrator_constrained; //scaler is proportional to 1/airspeed
// warnx("pitch: _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p);
// warnx("roll: _last_output %.4f", (double)_last_output);
return math::constrain(_last_output, -1.0f, 1.0f);
}
void ECL_PitchController::reset_integrator()
{
_integrator = 0.0f;
}

67
src/lib/ecl/attitude_fw/ecl_pitch_controller.h
View File

@@ -51,46 +51,23 @@
#include <stdbool.h>
#include <stdint.h>
#include <systemlib/perf_counter.h>
class __EXPORT ECL_PitchController //XXX: create controller superclass
#include "ecl_controller.h"
class __EXPORT ECL_PitchController :
public ECL_Controller
{
public:
ECL_PitchController();
~ECL_PitchController();
float control_attitude(float pitch_setpoint, float roll, float pitch, float airspeed);
float control_bodyrate(float roll, float pitch,
float pitch_rate, float yaw_rate,
float yaw_rate_setpoint,
float airspeed_min = 0.0f, float airspeed_max = 0.0f, float airspeed = (0.0f / 0.0f), float scaler = 1.0f, bool lock_integrator = false);
void reset_integrator();
void set_time_constant(float time_constant) {
_tc = time_constant;
}
void set_k_p(float k_p) {
_k_p = k_p;
}
void set_k_i(float k_i) {
_k_i = k_i;
}
void set_k_ff(float k_ff) {
_k_ff = k_ff;
}
void set_integrator_max(float max) {
_integrator_max = max;
}
float control_attitude(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) {
_max_rate_pos = max_rate_pos;
_max_rate = max_rate_pos;
}
void set_max_rate_neg(float max_rate_neg) {
@@ -101,35 +78,9 @@ public:
_roll_ff = roll_ff;
}
float get_rate_error() {
return _rate_error;
}
float get_desired_rate() {
return _rate_setpoint;
}
float get_desired_bodyrate() {
return _bodyrate_setpoint;
}
private:
uint64_t _last_run;
float _tc;
float _k_p;
float _k_i;
float _k_ff;
float _integrator_max;
float _max_rate_pos;
protected:
float _max_rate_neg;
float _roll_ff;
float _last_output;
float _integrator;
float _rate_error;
float _rate_setpoint;
float _bodyrate_setpoint;
perf_counter_t _nonfinite_input_perf;
};
#endif // ECL_PITCH_CONTROLLER_H

68
src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
View File

@@ -48,37 +48,24 @@
#include <systemlib/err.h>
ECL_RollController::ECL_RollController() :
_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),
_nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control roll nonfinite input"))
ECL_Controller("roll")
{
}
ECL_RollController::~ECL_RollController()
{
perf_free(_nonfinite_input_perf);
}
float ECL_RollController::control_attitude(float roll_setpoint, float roll)
float ECL_RollController::control_attitude(const struct ECL_ControlData &ctl_data)
{
/* Do not calculate control signal with bad inputs */
if (!(isfinite(roll_setpoint) && isfinite(roll))) {
if (!(isfinite(ctl_data.roll_setpoint) && isfinite(ctl_data.roll))) {
perf_count(_nonfinite_input_perf);
return _rate_setpoint;
}
/* Calculate error */
float roll_error = roll_setpoint - roll;
float roll_error = ctl_data.roll_setpoint - ctl_data.roll;
/* Apply P controller */
_rate_setpoint = roll_error / _tc;
@@ -92,15 +79,16 @@ float ECL_RollController::control_attitude(float roll_setpoint, float roll)
return _rate_setpoint;
}
float ECL_RollController::control_bodyrate(float pitch,
float roll_rate, float yaw_rate,
float yaw_rate_setpoint,
float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator)
float ECL_RollController::control_bodyrate(const struct ECL_ControlData &ctl_data)
{
/* Do not calculate control signal with bad inputs */
if (!(isfinite(pitch) && isfinite(roll_rate) && isfinite(yaw_rate) && isfinite(yaw_rate_setpoint) &&
isfinite(airspeed_min) && isfinite(airspeed_max) &&
isfinite(scaler))) {
if (!(isfinite(ctl_data.pitch) &&
isfinite(ctl_data.roll_rate) &&
isfinite(ctl_data.yaw_rate) &&
isfinite(ctl_data.yaw_rate_setpoint) &&
isfinite(ctl_data.airspeed_min) &&
isfinite(ctl_data.airspeed_max) &&
isfinite(ctl_data.scaler))) {
perf_count(_nonfinite_input_perf);
return math::constrain(_last_output, -1.0f, 1.0f);
}
@@ -111,31 +99,31 @@ float ECL_RollController::control_bodyrate(float pitch,
float dt = (float)dt_micros * 1e-6f;
/* lock integral for long intervals */
bool lock_integrator = ctl_data.lock_integrator;
if (dt_micros > 500000)
lock_integrator = true;
/* input conditioning */
// warnx("airspeed pre %.4f", (double)airspeed);
float airspeed = ctl_data.airspeed;
if (!isfinite(airspeed)) {
/* airspeed is NaN, +- INF or not available, pick center of band */
airspeed = 0.5f * (airspeed_min + airspeed_max);
} else if (airspeed < airspeed_min) {
airspeed = airspeed_min;
airspeed = 0.5f * (ctl_data.airspeed_min + ctl_data.airspeed_max);
} else if (airspeed < ctl_data.airspeed_min) {
airspeed = ctl_data.airspeed_min;
}
/* Transform setpoint to body angular rates (jacobian) */
_bodyrate_setpoint = _rate_setpoint - sinf(ctl_data.pitch) * ctl_data.yaw_rate_setpoint;
/* Transform setpoint to body angular rates */
_bodyrate_setpoint = _rate_setpoint - sinf(pitch) * yaw_rate_setpoint; //jacobian
/* Transform estimation to body angular rates */
float roll_bodyrate = roll_rate - sinf(pitch) * yaw_rate; //jacobian
/* Transform estimation to body angular rates (jacobian) */
float roll_bodyrate = ctl_data.roll_rate - sinf(ctl_data.pitch) * ctl_data.yaw_rate;
/* Calculate body angular rate error */
_rate_error = _bodyrate_setpoint - roll_bodyrate; //body angular rate error
if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) {
if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * ctl_data.airspeed_min) {
float id = _rate_error * dt * scaler;
float id = _rate_error * dt * ctl_data.scaler;
/*
* anti-windup: do not allow integrator to increase if actuator is at limit
@@ -157,15 +145,9 @@ float ECL_RollController::control_bodyrate(float pitch,
//warnx("roll: _integrator: %.4f, _integrator_max: %.4f", (double)_integrator, (double)_integrator_max);
/* Apply PI rate controller and store non-limited output */
_last_output = _bodyrate_setpoint * _k_ff * scaler +
_rate_error * _k_p * scaler * scaler
_last_output = _bodyrate_setpoint * _k_ff * ctl_data.scaler +
_rate_error * _k_p * ctl_data.scaler * ctl_data.scaler
+ integrator_constrained; //scaler is proportional to 1/airspeed
return math::constrain(_last_output, -1.0f, 1.0f);
}
void ECL_RollController::reset_integrator()
{
_integrator = 0.0f;
}

69
src/lib/ecl/attitude_fw/ecl_roll_controller.h
View File

@@ -51,76 +51,19 @@
#include <stdbool.h>
#include <stdint.h>
#include <systemlib/perf_counter.h>
class __EXPORT ECL_RollController //XXX: create controller superclass
#include "ecl_controller.h"
class __EXPORT ECL_RollController :
public ECL_Controller
{
public:
ECL_RollController();
~ECL_RollController();
float control_attitude(float roll_setpoint, float roll);
float control_bodyrate(float pitch,
float roll_rate, float yaw_rate,
float yaw_rate_setpoint,
float airspeed_min = 0.0f, float airspeed_max = 0.0f, float airspeed = (0.0f / 0.0f), float scaler = 1.0f, bool lock_integrator = false);
void reset_integrator();
void set_time_constant(float time_constant) {
if (time_constant > 0.1f && time_constant < 3.0f) {
_tc = time_constant;
}
}
void set_k_p(float k_p) {
_k_p = k_p;
}
void set_k_i(float k_i) {
_k_i = k_i;
}
void set_k_ff(float k_ff) {
_k_ff = k_ff;
}
void set_integrator_max(float max) {
_integrator_max = max;
}
void set_max_rate(float max_rate) {
_max_rate = max_rate;
}
float get_rate_error() {
return _rate_error;
}
float get_desired_rate() {
return _rate_setpoint;
}
float get_desired_bodyrate() {
return _bodyrate_setpoint;
}
private:
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;
perf_counter_t _nonfinite_input_perf;
float control_attitude(const struct ECL_ControlData &ctl_data);
float control_bodyrate(const struct ECL_ControlData &ctl_data);
};
#endif // ECL_ROLL_CONTROLLER_H

84
src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
View File

@@ -47,45 +47,43 @@
#include <systemlib/err.h>
ECL_YawController::ECL_YawController() :
_last_run(0),
_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),
_coordinated_min_speed(1.0f),
_nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control yaw nonfinite input"))
ECL_Controller("yaw"),
_coordinated_min_speed(1.0f)
{
}
ECL_YawController::~ECL_YawController()
{
perf_free(_nonfinite_input_perf);
}
float ECL_YawController::control_attitude(float roll, float pitch,
float speed_body_u, float speed_body_v, float speed_body_w,
float roll_rate_setpoint, float pitch_rate_setpoint)
float ECL_YawController::control_attitude(const struct ECL_ControlData &ctl_data)
{
/* Do not calculate control signal with bad inputs */
if (!(isfinite(roll) && isfinite(pitch) && isfinite(speed_body_u) && isfinite(speed_body_v) &&
isfinite(speed_body_w) && isfinite(roll_rate_setpoint) &&
isfinite(pitch_rate_setpoint))) {
if (!(isfinite(ctl_data.roll) &&
isfinite(ctl_data.pitch) &&
isfinite(ctl_data.speed_body_u) &&
isfinite(ctl_data.speed_body_v) &&
isfinite(ctl_data.speed_body_w) &&
isfinite(ctl_data.roll_rate_setpoint) &&
isfinite(ctl_data.pitch_rate_setpoint))) {
perf_count(_nonfinite_input_perf);
return _rate_setpoint;
}
// static int counter = 0;
/* Calculate desired yaw rate from coordinated turn constraint / (no side forces) */
_rate_setpoint = 0.0f;
if (sqrtf(speed_body_u * speed_body_u + speed_body_v * speed_body_v + speed_body_w * speed_body_w) > _coordinated_min_speed) {
float denumerator = (speed_body_u * cosf(roll) * cosf(pitch) + speed_body_w * sinf(pitch));
if (sqrtf(ctl_data.speed_body_u * ctl_data.speed_body_u + ctl_data.speed_body_v * ctl_data.speed_body_v +
ctl_data.speed_body_w * ctl_data.speed_body_w) > _coordinated_min_speed) {
float denumerator = (ctl_data.speed_body_u * cosf(ctl_data.roll) * cosf(ctl_data.pitch) +
ctl_data.speed_body_w * sinf(ctl_data.pitch));
if(fabsf(denumerator) > FLT_EPSILON) {
_rate_setpoint = (speed_body_w * roll_rate_setpoint + 9.81f * sinf(roll) * cosf(pitch) + speed_body_u * pitch_rate_setpoint * sinf(roll)) / denumerator;
_rate_setpoint = (ctl_data.speed_body_w * ctl_data.roll_rate_setpoint +
9.81f * sinf(ctl_data.roll) * cosf(ctl_data.pitch) +
ctl_data.speed_body_u * ctl_data.pitch_rate_setpoint * sinf(ctl_data.roll)) /
denumerator;
// warnx("yaw: speed_body_u %.f speed_body_w %1.f roll %.1f pitch %.1f denumerator %.1f _rate_setpoint %.1f", speed_body_u, speed_body_w, denumerator, _rate_setpoint);
}
@@ -111,46 +109,49 @@ float ECL_YawController::control_attitude(float roll, float pitch,
return _rate_setpoint;
}
float ECL_YawController::control_bodyrate(float roll, float pitch,
float pitch_rate, float yaw_rate,
float pitch_rate_setpoint,
float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator)
float ECL_YawController::control_bodyrate(const struct ECL_ControlData &ctl_data)
{
/* Do not calculate control signal with bad inputs */
if (!(isfinite(roll) && isfinite(pitch) && isfinite(pitch_rate) && isfinite(yaw_rate) &&
isfinite(pitch_rate_setpoint) && isfinite(airspeed_min) &&
isfinite(airspeed_max) && isfinite(scaler))) {
if (!(isfinite(ctl_data.roll) && isfinite(ctl_data.pitch) && isfinite(ctl_data.pitch_rate) &&
isfinite(ctl_data.yaw_rate) && isfinite(ctl_data.pitch_rate_setpoint) &&
isfinite(ctl_data.airspeed_min) && isfinite(ctl_data.airspeed_max) &&
isfinite(ctl_data.scaler))) {
perf_count(_nonfinite_input_perf);
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 = (float)dt_micros * 1e-6f;
/* lock integral for long intervals */
bool lock_integrator = ctl_data.lock_integrator;
if (dt_micros > 500000)
lock_integrator = true;
/* input conditioning */
float airspeed = ctl_data.airspeed;
if (!isfinite(airspeed)) {
/* airspeed is NaN, +- INF or not available, pick center of band */
airspeed = 0.5f * (airspeed_min + airspeed_max);
} else if (airspeed < airspeed_min) {
airspeed = airspeed_min;
airspeed = 0.5f * (ctl_data.airspeed_min + ctl_data.airspeed_max);
} else if (airspeed < ctl_data.airspeed_min) {
airspeed = ctl_data.airspeed_min;
}
/* Transform setpoint to body angular rates */
_bodyrate_setpoint = -sinf(roll) * pitch_rate_setpoint + cosf(roll)*cosf(pitch) * _rate_setpoint; //jacobian
/* Transform setpoint to body angular rates (jacobian) */
_bodyrate_setpoint = -sinf(ctl_data.roll) * ctl_data.pitch_rate_setpoint +
cosf(ctl_data.roll)*cosf(ctl_data.pitch) * _rate_setpoint;
/* Transform estimation to body angular rates */
float yaw_bodyrate = -sinf(roll) * pitch_rate + cosf(roll)*cosf(pitch) * yaw_rate; //jacobian
/* Transform estimation to body angular rates (jacobian) */
float yaw_bodyrate = -sinf(ctl_data.roll) * ctl_data.pitch_rate +
cosf(ctl_data.roll)*cosf(ctl_data.pitch) * ctl_data.yaw_rate;
/* Calculate body angular rate error */
_rate_error = _bodyrate_setpoint - yaw_bodyrate; //body angular rate error
if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) {
if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * ctl_data.airspeed_min) {
float id = _rate_error * dt;
@@ -173,14 +174,9 @@ float ECL_YawController::control_bodyrate(float roll, float pitch,
float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max);
/* Apply PI rate controller and store non-limited output */
_last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained) * scaler * scaler; //scaler is proportional to 1/airspeed
_last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained) * ctl_data.scaler * ctl_data.scaler; //scaler is proportional to 1/airspeed
//warnx("yaw:_last_output: %.4f, _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_last_output, (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p);
return math::constrain(_last_output, -1.0f, 1.0f);
}
void ECL_YawController::reset_integrator()
{
_integrator = 0.0f;
}

68
src/lib/ecl/attitude_fw/ecl_yaw_controller.h
View File

@@ -50,79 +50,27 @@
#include <stdbool.h>
#include <stdint.h>
#include <systemlib/perf_counter.h>
class __EXPORT ECL_YawController //XXX: create controller superclass
#include "ecl_controller.h"
class __EXPORT ECL_YawController :
public ECL_Controller
{
public:
ECL_YawController();
~ECL_YawController();
float control_attitude(float roll, float pitch,
float speed_body_u, float speed_body_v, float speed_body_w,
float roll_rate_setpoint, float pitch_rate_setpoint);
float control_bodyrate( float roll, float pitch,
float pitch_rate, float yaw_rate,
float pitch_rate_setpoint,
float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator);
void reset_integrator();
void set_k_p(float k_p) {
_k_p = k_p;
}
void set_k_i(float k_i) {
_k_i = k_i;
}
void set_k_ff(float k_ff) {
_k_ff = k_ff;
}
void set_integrator_max(float max) {
_integrator_max = max;
}
void set_max_rate(float max_rate) {
_max_rate = max_rate;
}
float control_attitude(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;
}
float get_rate_error() {
return _rate_error;
}
float get_desired_rate() {
return _rate_setpoint;
}
float get_desired_bodyrate() {
return _bodyrate_setpoint;
}
private:
uint64_t _last_run;
float _k_p;
float _k_i;
float _k_ff;
float _integrator_max;
float _max_rate;
float _roll_ff;
float _last_output;
float _integrator;
float _rate_error;
float _rate_setpoint;
float _bodyrate_setpoint;
protected:
float _coordinated_min_speed;
perf_counter_t _nonfinite_input_perf;
};
#endif // ECL_YAW_CONTROLLER_H

3
src/lib/ecl/module.mk
View File

@@ -35,7 +35,8 @@
# Estimation and Control Library
#
SRCS = attitude_fw/ecl_pitch_controller.cpp \
SRCS = attitude_fw/ecl_controller.cpp \
attitude_fw/ecl_pitch_controller.cpp \
attitude_fw/ecl_roll_controller.cpp \
attitude_fw/ecl_yaw_controller.cpp \
l1/ecl_l1_pos_controller.cpp

47
src/modules/fw_att_control/fw_att_control_main.cpp
View File

@@ -933,19 +933,38 @@ FixedwingAttitudeControl::task_main()
}
}
/* Prepare data for attitude controllers */
struct ECL_ControlData control_input = {};
control_input.roll = _att.roll;
control_input.pitch = _att.pitch;
control_input.yaw = _att.yaw;
control_input.roll_rate = _att.rollspeed;
control_input.pitch_rate = _att.pitchspeed;
control_input.yaw_rate = _att.yawspeed;
control_input.speed_body_u = speed_body_u;
control_input.speed_body_v = speed_body_v;
control_input.speed_body_w = speed_body_w;
control_input.roll_setpoint = roll_sp;
control_input.pitch_setpoint = pitch_sp;
control_input.airspeed_min = _parameters.airspeed_min;
control_input.airspeed_max = _parameters.airspeed_max;
control_input.airspeed = airspeed;
control_input.scaler = airspeed_scaling;
control_input.lock_integrator = lock_integrator;
/* Run attitude controllers */
if (isfinite(roll_sp) && isfinite(pitch_sp)) {
_roll_ctrl.control_attitude(roll_sp, _att.roll);
_pitch_ctrl.control_attitude(pitch_sp, _att.roll, _att.pitch, airspeed);
_yaw_ctrl.control_attitude(_att.roll, _att.pitch,
speed_body_u, speed_body_v, speed_body_w,
_roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate()); //runs last, because is depending on output of roll and pitch attitude
_roll_ctrl.control_attitude(control_input);
_pitch_ctrl.control_attitude(control_input);
_yaw_ctrl.control_attitude(control_input); //runs last, because is depending on output of roll and pitch attitude
/* Update input data for rate controllers */
control_input.roll_rate_setpoint = _roll_ctrl.get_desired_rate();
control_input.pitch_rate_setpoint = _pitch_ctrl.get_desired_rate();
control_input.yaw_rate_setpoint = _yaw_ctrl.get_desired_rate();
/* Run attitude RATE controllers which need the desired attitudes from above, add trim */
float roll_u = _roll_ctrl.control_bodyrate(_att.pitch,
_att.rollspeed, _att.yawspeed,
_yaw_ctrl.get_desired_rate(),
_parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator);
float roll_u = _roll_ctrl.control_bodyrate(control_input);
_actuators.control[0] = (isfinite(roll_u)) ? roll_u + _parameters.trim_roll : _parameters.trim_roll;
if (!isfinite(roll_u)) {
_roll_ctrl.reset_integrator();
@@ -956,10 +975,7 @@ FixedwingAttitudeControl::task_main()
}
}
float pitch_u = _pitch_ctrl.control_bodyrate(_att.roll, _att.pitch,
_att.pitchspeed, _att.yawspeed,
_yaw_ctrl.get_desired_rate(),
_parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator);
float pitch_u = _pitch_ctrl.control_bodyrate(control_input);
_actuators.control[1] = (isfinite(pitch_u)) ? pitch_u + _parameters.trim_pitch : _parameters.trim_pitch;
if (!isfinite(pitch_u)) {
_pitch_ctrl.reset_integrator();
@@ -980,10 +996,7 @@ FixedwingAttitudeControl::task_main()
}
}
float yaw_u = _yaw_ctrl.control_bodyrate(_att.roll, _att.pitch,
_att.pitchspeed, _att.yawspeed,
_pitch_ctrl.get_desired_rate(),
_parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator);
float yaw_u = _yaw_ctrl.control_bodyrate(control_input);
_actuators.control[2] = (isfinite(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw;
/* add in manual rudder control */