dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Revert "attitude_fw delete unused and cleanup" 

This reverts commit 25bd3ac5e6.
This commit is contained in:
Paul Riseborough 2017-08-30 16:23:40 +02:00
parent 1531269f92
commit b0300b9723
12 changed files with 251 additions and 96 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
CMakeLists.txt
View File

@ -61,3 +61,4 @@ px4_add_module(
DEPENDS
platforms__common
)
# vim: set noet ft=cmake fenc=utf-8 ff=unix :

21
attitude_fw/ecl_controller.cpp
View File

@ -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);

38
attitude_fw/ecl_controller.h
View File

@ -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);
};

50
attitude_fw/ecl_pitch_controller.cpp
View File

@ -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;

22
attitude_fw/ecl_pitch_controller.h
View File

@ -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

37
attitude_fw/ecl_roll_controller.cpp
View File

@ -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);
}

14
attitude_fw/ecl_roll_controller.h
View File

@ -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

30
attitude_fw/ecl_wheel_controller.cpp
View File

@ -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;

16
attitude_fw/ecl_wheel_controller.h
View File

@ -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

75
attitude_fw/ecl_yaw_controller.cpp
View File

@ -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) */

41
attitude_fw/ecl_yaw_controller.h
View File

@ -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