fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 03:03:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Minor changes to get fixed wing control apps running for posix

Browse Source
This commit is contained in:
devbharat
2015-08-23 14:39:25 +02:00
committed by Lorenz Meier
parent 295e4be8c3
commit 20d71870ec
8 changed files with 81 additions and 67 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/ecl/attitude_fw/ecl_controller.cpp
+1 -1
View File
@@ -130,7 +130,7 @@ float ECL_Controller::get_desired_bodyrate()
float ECL_Controller::constrain_airspeed(float airspeed, float minspeed, float maxspeed) {
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);
} else if (airspeed < minspeed) {
src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
+12 -12
View File
@@ -62,10 +62,10 @@ float ECL_PitchController::control_attitude(const struct ECL_ControlData &ctl_da
{
/* 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))) {
if (!(PX4_ISFINITE(ctl_data.pitch_setpoint) &&
PX4_ISFINITE(ctl_data.roll) &&
PX4_ISFINITE(ctl_data.pitch) &&
PX4_ISFINITE(ctl_data.airspeed))) {
perf_count(_nonfinite_input_perf);
warnx("not controlling pitch");
return _rate_setpoint;
@@ -94,14 +94,14 @@ 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.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))) {
if (!(PX4_ISFINITE(ctl_data.roll) &&
PX4_ISFINITE(ctl_data.pitch) &&
PX4_ISFINITE(ctl_data.pitch_rate) &&
PX4_ISFINITE(ctl_data.yaw_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))) {
perf_count(_nonfinite_input_perf);
return math::constrain(_last_output, -1.0f, 1.0f);
}
src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
+8 -8
View File
@@ -59,7 +59,7 @@ ECL_RollController::~ECL_RollController()
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))) {
perf_count(_nonfinite_input_perf);
return _rate_setpoint;
}
@@ -83,13 +83,13 @@ 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.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))) {
if (!(PX4_ISFINITE(ctl_data.pitch) &&
PX4_ISFINITE(ctl_data.roll_rate) &&
PX4_ISFINITE(ctl_data.yaw_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))) {
perf_count(_nonfinite_input_perf);
return math::constrain(_last_output, -1.0f, 1.0f);
}
src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
+13 -13
View File
@@ -101,13 +101,13 @@ float ECL_YawController::control_bodyrate(const struct ECL_ControlData &ctl_data
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.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))) {
if (!(PX4_ISFINITE(ctl_data.roll) &&
PX4_ISFINITE(ctl_data.pitch) &&
PX4_ISFINITE(ctl_data.speed_body_u) &&
PX4_ISFINITE(ctl_data.speed_body_v) &&
PX4_ISFINITE(ctl_data.speed_body_w) &&
PX4_ISFINITE(ctl_data.roll_rate_setpoint) &&
PX4_ISFINITE(ctl_data.pitch_rate_setpoint))) {
perf_count(_nonfinite_input_perf);
return _rate_setpoint;
}
@@ -145,7 +145,7 @@ float ECL_YawController::control_attitude_impl_openloop(const struct ECL_Control
// counter++;
if (!isfinite(_rate_setpoint)) {
if (!PX4_ISFINITE(_rate_setpoint)) {
warnx("yaw rate sepoint not finite");
_rate_setpoint = 0.0f;
}
@@ -156,10 +156,10 @@ float ECL_YawController::control_attitude_impl_openloop(const struct ECL_Control
float ECL_YawController::control_bodyrate_impl(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.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))) {
if (!(PX4_ISFINITE(ctl_data.roll) && PX4_ISFINITE(ctl_data.pitch) && PX4_ISFINITE(ctl_data.pitch_rate) &&
PX4_ISFINITE(ctl_data.yaw_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))) {
perf_count(_nonfinite_input_perf);
return math::constrain(_last_output, -1.0f, 1.0f);
}
@@ -179,7 +179,7 @@ float ECL_YawController::control_bodyrate_impl(const struct ECL_ControlData &ctl
/* 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);
src/lib/external_lgpl/tecs/tecs.cpp
+4 -4
View File
@@ -98,7 +98,7 @@ void TECS::update_state(float baro_altitude, float airspeed, const math::Matrix<
// Only required if airspeed is being measured and controlled
float temp = 0;
if (isfinite(airspeed) && airspeed_sensor_enabled()) {
if (PX4_ISFINITE(airspeed) && airspeed_sensor_enabled()) {
// Get DCM
// Calculate speed rate of change
// XXX check
@@ -142,7 +142,7 @@ void TECS::_update_speed(float airspeed_demand, float indicated_airspeed,
// Get airspeed or default to halfway between min and max if
// airspeed is not being used and set speed rate to zero
if (!isfinite(indicated_airspeed) || !airspeed_sensor_enabled()) {
if (!PX4_ISFINITE(indicated_airspeed) || !airspeed_sensor_enabled()) {
// If no airspeed available use average of min and max
_EAS = 0.5f * (indicated_airspeed_min + indicated_airspeed_max);
@@ -228,12 +228,12 @@ void TECS::_update_speed_demand(void)
void TECS::_update_height_demand(float demand, float state)
{
// Handle initialization
if (isfinite(demand) && fabsf(_hgt_dem_in_old) < 0.1f) {
if (PX4_ISFINITE(demand) && fabsf(_hgt_dem_in_old) < 0.1f) {
_hgt_dem_in_old = demand;
}
// Apply 2 point moving average to demanded height
// This is required because height demand is updated in steps
if (isfinite(demand)) {
if (PX4_ISFINITE(demand)) {
_hgt_dem = 0.5f * (demand + _hgt_dem_in_old);
} else {
_hgt_dem = _hgt_dem_in_old;