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

Hotfix: forbid integrator to accumulate NaN values if they ever would occur

This commit is contained in:
Lorenz Meier 2013-11-05 07:43:08 +01:00
parent 3042731d26
commit ed60dc50fc
1 changed files with 32 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -635,43 +635,46 @@ FixedwingAttitudeControl::task_main()
} }
} }
float roll_rad = _roll_ctrl.control(roll_sp, _att.roll, _att.rollspeed, if (isfinite(roll_sp) && isfinite(pitch_sp)) {
airspeed_scaling, lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed);
_actuators.control[0] = (isfinite(roll_rad)) ? roll_rad * actuator_scaling : 0.0f;
float pitch_rad = _pitch_ctrl.control(pitch_sp, _att.pitch, _att.pitchspeed, _att.roll, airspeed_scaling, float roll_rad = _roll_ctrl.control(roll_sp, _att.roll, _att.rollspeed,
lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed); airspeed_scaling, lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed);
_actuators.control[1] = (isfinite(pitch_rad)) ? pitch_rad * actuator_scaling : 0.0f; _actuators.control[0] = (isfinite(roll_rad)) ? roll_rad * actuator_scaling : 0.0f;
float yaw_rad = _yaw_ctrl.control(_att.roll, _att.yawspeed, _accel.y, airspeed_scaling, lock_integrator, float pitch_rad = _pitch_ctrl.control(pitch_sp, _att.pitch, _att.pitchspeed, _att.roll, airspeed_scaling,
_parameters.airspeed_min, _parameters.airspeed_max, airspeed); lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed);
_actuators.control[2] = (isfinite(yaw_rad)) ? yaw_rad * actuator_scaling : 0.0f; _actuators.control[1] = (isfinite(pitch_rad)) ? pitch_rad * actuator_scaling : 0.0f;
/* throttle passed through */ float yaw_rad = _yaw_ctrl.control(_att.roll, _att.yawspeed, _accel.y, airspeed_scaling, lock_integrator,
_actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f; _parameters.airspeed_min, _parameters.airspeed_max, airspeed);
_actuators.control[2] = (isfinite(yaw_rad)) ? yaw_rad * actuator_scaling : 0.0f;
// warnx("aspd: %s: %6.2f, aspd scaling: %6.2f, controls: %5.2f %5.2f %5.2f %5.2f", (_airspeed_valid) ? "valid" : "unknown", /* throttle passed through */
// airspeed, airspeed_scaling, _actuators.control[0], _actuators.control[1], _actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f;
// _actuators.control[2], _actuators.control[3]);
/* // warnx("aspd: %s: %6.2f, aspd scaling: %6.2f, controls: %5.2f %5.2f %5.2f %5.2f", (_airspeed_valid) ? "valid" : "unknown",
* Lazily publish the rate setpoint (for analysis, the actuators are published below) // airspeed, airspeed_scaling, _actuators.control[0], _actuators.control[1],
* only once available // _actuators.control[2], _actuators.control[3]);
*/
vehicle_rates_setpoint_s rates_sp;
rates_sp.roll = _roll_ctrl.get_desired_rate();
rates_sp.pitch = _pitch_ctrl.get_desired_rate();
rates_sp.yaw = 0.0f; // XXX not yet implemented
rates_sp.timestamp = hrt_absolute_time(); /*
* Lazily publish the rate setpoint (for analysis, the actuators are published below)
* only once available
*/
vehicle_rates_setpoint_s rates_sp;
rates_sp.roll = _roll_ctrl.get_desired_rate();
rates_sp.pitch = _pitch_ctrl.get_desired_rate();
rates_sp.yaw = 0.0f; // XXX not yet implemented
if (_rate_sp_pub > 0) { rates_sp.timestamp = hrt_absolute_time();
/* publish the attitude setpoint */
orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &rates_sp);
} else { if (_rate_sp_pub > 0) {
/* advertise and publish */ /* publish the attitude setpoint */
_rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &rates_sp);
} else {
/* advertise and publish */
_rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
}
} }
} else { } else {