fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-27 05:50:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'fwattrobustify' into mtecs

Browse Source
This commit is contained in:
Thomas Gubler
2014-05-18 00:20:10 +02:00
parent 885efa2cfe 9a35bac2ad
commit 57d75caaf3
6 changed files with 126 additions and 59 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
+12 -1
View File
@@ -67,7 +67,11 @@ ECL_PitchController::ECL_PitchController() :
float ECL_PitchController::control_attitude(float pitch_setpoint, float roll, float pitch, float airspeed)
{
/* Do not calculate control signal with bad inputs */
if (!(isfinite(pitch_setpoint) && isfinite(roll) && isfinite(pitch) && isfinite(airspeed))) {
warnx("not controlling pitch");
return _rate_setpoint;
}
/* flying inverted (wings upside down) ? */
bool inverted = false;
@@ -123,6 +127,13 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch,
float yaw_rate_setpoint,
float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator)
{
/* 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))) {
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();
src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
+13 -2
View File
@@ -65,6 +65,10 @@ ECL_RollController::ECL_RollController() :
float ECL_RollController::control_attitude(float roll_setpoint, float roll)
{
/* Do not calculate control signal with bad inputs */
if (!(isfinite(roll_setpoint) && isfinite(roll))) {
return _rate_setpoint;
}
/* Calculate error */
float roll_error = roll_setpoint - roll;
@@ -86,6 +90,13 @@ float ECL_RollController::control_bodyrate(float pitch,
float yaw_rate_setpoint,
float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator)
{
/* 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))) {
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();
@@ -122,8 +133,8 @@ float ECL_RollController::control_bodyrate(float pitch,
float id = _rate_error * dt;
/*
* anti-windup: do not allow integrator to increase if actuator is at limit
*/
* anti-windup: do not allow integrator to increase if actuator is at limit
*/
if (_last_output < -1.0f) {
/* only allow motion to center: increase value */
id = math::max(id, 0.0f);
src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
+12
View File
@@ -66,6 +66,12 @@ 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)
{
/* 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))) {
return _rate_setpoint;
}
// static int counter = 0;
/* Calculate desired yaw rate from coordinated turn constraint / (no side forces) */
_rate_setpoint = 0.0f;
@@ -103,6 +109,12 @@ float ECL_YawController::control_bodyrate(float roll, float pitch,
float pitch_rate_setpoint,
float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator)
{
/* 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))) {
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();
src/modules/fw_att_control/fw_att_control_main.cpp
+35 -7
View File
@@ -592,6 +592,8 @@ FixedwingAttitudeControl::task_main()
while (!_task_should_exit) {
static int loop_counter = 0;
/* wait for up to 500ms for data */
int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
@@ -755,7 +757,9 @@ FixedwingAttitudeControl::task_main()
speed_body_v = _att.R[0][1] * _global_pos.vel_n + _att.R[1][1] * _global_pos.vel_e + _att.R[2][1] * _global_pos.vel_d;
speed_body_w = _att.R[0][2] * _global_pos.vel_n + _att.R[1][2] * _global_pos.vel_e + _att.R[2][2] * _global_pos.vel_d;
} else {
warnx("Did not get a valid R\n");
if (loop_counter % 10 == 0) {
warnx("Did not get a valid R\n");
}
}
/* Run attitude controllers */
@@ -773,7 +777,10 @@ FixedwingAttitudeControl::task_main()
_parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator);
_actuators.control[0] = (isfinite(roll_u)) ? roll_u + _parameters.trim_roll : _parameters.trim_roll;
if (!isfinite(roll_u)) {
warnx("roll_u %.4f", roll_u);
_roll_ctrl.reset_integrator();
if (loop_counter % 10 == 0) {
warnx("roll_u %.4f", roll_u);
}
}
float pitch_u = _pitch_ctrl.control_bodyrate(_att.roll, _att.pitch,
@@ -782,8 +789,21 @@ FixedwingAttitudeControl::task_main()
_parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator);
_actuators.control[1] = (isfinite(pitch_u)) ? pitch_u + _parameters.trim_pitch : _parameters.trim_pitch;
if (!isfinite(pitch_u)) {
warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f, airspeed %.4f, airspeed_scaling %.4f, roll_sp %.4f, pitch_sp %.4f, _roll_ctrl.get_desired_rate() %.4f, _pitch_ctrl.get_desired_rate() %.4f att_sp.roll_body %.4f",
(double)pitch_u, (double)_yaw_ctrl.get_desired_rate(), (double)airspeed, (double)airspeed_scaling, (double)roll_sp, (double)pitch_sp, (double)_roll_ctrl.get_desired_rate(), (double)_pitch_ctrl.get_desired_rate(), (double)_att_sp.roll_body);
_pitch_ctrl.reset_integrator();
if (loop_counter % 10 == 0) {
warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f,"
" airspeed %.4f, airspeed_scaling %.4f,"
" roll_sp %.4f, pitch_sp %.4f,"
" _roll_ctrl.get_desired_rate() %.4f,"
" _pitch_ctrl.get_desired_rate() %.4f"
" att_sp.roll_body %.4f",
(double)pitch_u, (double)_yaw_ctrl.get_desired_rate(),
(double)airspeed, (double)airspeed_scaling,
(double)roll_sp, (double)pitch_sp,
(double)_roll_ctrl.get_desired_rate(),
(double)_pitch_ctrl.get_desired_rate(),
(double)_att_sp.roll_body);
}
}
float yaw_u = _yaw_ctrl.control_bodyrate(_att.roll, _att.pitch,
@@ -792,16 +812,23 @@ FixedwingAttitudeControl::task_main()
_parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator);
_actuators.control[2] = (isfinite(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw;
if (!isfinite(yaw_u)) {
warnx("yaw_u %.4f", (double)yaw_u);
_yaw_ctrl.reset_integrator();
if (loop_counter % 10 == 0) {
warnx("yaw_u %.4f", (double)yaw_u);
}
}
/* throttle passed through */
_actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f;
if (!isfinite(throttle_sp)) {
warnx("throttle_sp %.4f", (double)throttle_sp);
if (loop_counter % 10 == 0) {
warnx("throttle_sp %.4f", (double)throttle_sp);
}
}
} else {
warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", (double)roll_sp, (double)pitch_sp);
if (loop_counter % 10 == 0) {
warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", (double)roll_sp, (double)pitch_sp);
}
}
/*
@@ -865,6 +892,7 @@ FixedwingAttitudeControl::task_main()
}
loop_counter++;
perf_end(_loop_perf);
}
src/modules/systemlib/mixer/mixer.h
+1 -1
View File
@@ -516,7 +516,7 @@ private:
float _roll_scale;
float _pitch_scale;
float _yaw_scale;
float _deadband;
float _idle_speed;
unsigned _rotor_count;
const Rotor *_rotors;
src/modules/systemlib/mixer/mixer_multirotor.cpp
+53 -48
View File
@@ -67,6 +67,11 @@
namespace
{
float constrain(float val, float min, float max)
{
return (val < min) ? min : ((val > max) ? max : val);
}
/*
* These tables automatically generated by multi_tables - do not edit.
*/
@@ -171,12 +176,12 @@ MultirotorMixer::MultirotorMixer(ControlCallback control_cb,
float roll_scale,
float pitch_scale,
float yaw_scale,
float deadband) :
float idle_speed) :
Mixer(control_cb, cb_handle),
_roll_scale(roll_scale),
_pitch_scale(pitch_scale),
_yaw_scale(yaw_scale),
_deadband(-1.0f + deadband), /* shift to output range here to avoid runtime calculation */
_idle_speed(-1.0f + idle_speed * 2.0f), /* shift to output range here to avoid runtime calculation */
_rotor_count(_config_rotor_count[geometry]),
_rotors(_config_index[geometry])
{
@@ -276,67 +281,67 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl
unsigned
MultirotorMixer::mix(float *outputs, unsigned space)
{
float roll = get_control(0, 0) * _roll_scale;
float roll = constrain(get_control(0, 0) * _roll_scale, -1.0f, 1.0f);
//lowsyslog("roll: %d, get_control0: %d, %d\n", (int)(roll), (int)(get_control(0, 0)), (int)(_roll_scale));
float pitch = get_control(0, 1) * _pitch_scale;
float yaw = get_control(0, 2) * _yaw_scale;
float thrust = get_control(0, 3);
float pitch = constrain(get_control(0, 1) * _pitch_scale, -1.0f, 1.0f);
float yaw = constrain(get_control(0, 2) * _yaw_scale, -1.0f, 1.0f);
float thrust = constrain(get_control(0, 3), 0.0f, 1.0f);
//lowsyslog("thrust: %d, get_control3: %d\n", (int)(thrust), (int)(get_control(0, 3)));
float max = 0.0f;
float fixup_scale;
float min_out = 0.0f;
float max_out = 0.0f;
float scale_yaw = 1.0f;
/* use an output factor to prevent too strong control signals at low throttle */
float min_thrust = 0.05f;
float max_thrust = 1.0f;
float startpoint_full_control = 0.40f;
float output_factor;
/* keep roll, pitch and yaw control to 0 below min thrust */
if (thrust <= min_thrust) {
output_factor = 0.0f;
/* linearly increase the output factor from 0 to 1 between min_thrust and startpoint_full_control */
} else if (thrust < startpoint_full_control && thrust > min_thrust) {
output_factor = (thrust / max_thrust) / (startpoint_full_control - min_thrust);
/* and then stay at full control */
} else {
output_factor = max_thrust;
}
roll *= output_factor;
pitch *= output_factor;
yaw *= output_factor;
/* perform initial mix pass yielding un-bounded outputs */
/* perform initial mix pass yielding unbounded outputs, ignore yaw */
for (unsigned i = 0; i < _rotor_count; i++) {
float tmp = roll * _rotors[i].roll_scale +
float out = roll * _rotors[i].roll_scale +
pitch * _rotors[i].pitch_scale +
yaw * _rotors[i].yaw_scale +
thrust;
if (tmp > max)
max = tmp;
/* limit yaw if it causes outputs clipping */
if (out >= 0.0f && out < -yaw * _rotors[i].yaw_scale) {
yaw = -out / _rotors[i].yaw_scale;
}
outputs[i] = tmp;
/* calculate min and max output values */
if (out < min_out) {
min_out = out;
}
if (out > max_out) {
max_out = out;
}
outputs[i] = out;
}
/* scale values into the -1.0 - 1.0 range */
if (max > 1.0f) {
fixup_scale = 2.0f / max;
/* scale down roll/pitch controls if some outputs are negative, don't add yaw, keep total thrust */
if (min_out < 0.0) {
float scale_in = thrust / (thrust - min_out);
/* mix again with adjusted controls */
for (unsigned i = 0; i < _rotor_count; i++) {
outputs[i] = scale_in * (roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) + thrust;
}
} else {
fixup_scale = 2.0f;
/* roll/pitch mixed without limiting, add yaw control */
for (unsigned i = 0; i < _rotor_count; i++) {
outputs[i] += yaw * _rotors[i].yaw_scale;
}
}
for (unsigned i = 0; i < _rotor_count; i++)
outputs[i] = -1.0f + (outputs[i] * fixup_scale);
/* scale down all outputs if some outputs are too large, reduce total thrust */
float scale_out;
if (max_out > 1.0f) {
scale_out = 1.0f / max_out;
/* ensure outputs are out of the deadband */
for (unsigned i = 0; i < _rotor_count; i++)
if (outputs[i] < _deadband)
outputs[i] = _deadband;
} else {
scale_out = 1.0f;
}
/* scale outputs to range _idle_speed..1 */
for (unsigned i = 0; i < _rotor_count; i++) {
outputs[i] = _idle_speed + (outputs[i] * (1.0f - _idle_speed) * scale_out);
}
return _rotor_count;
}