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

Minor changes to get fixed wing control apps running for posix

This commit is contained in:
devbharat 2015-08-23 12:20:26 +02:00 committed by Lorenz Meier
parent 295e4be8c3
commit 20d71870ec
8 changed files with 81 additions and 67 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
makefiles/posix/config_posix_sitl.mk
View File

@ -49,6 +49,10 @@ MODULES += modules/mc_att_control
MODULES += modules/mc_pos_control_multiplatform
MODULES += modules/mc_att_control_multiplatform
MODULES += modules/land_detector
MODULES += modules/fw_att_control
MODULES += modules/fw_pos_control_l1
#
# Library modules
@ -67,9 +71,13 @@ MODULES += modules/controllib
#
MODULES += lib/mathlib
MODULES += lib/mathlib/math/filter
MODULES += lib/ecl
MODULES += lib/external_lgpl
MODULES += lib/geo
MODULES += lib/geo_lookup
MODULES += lib/conversion
MODULES += lib/launchdetection
#
# POSIX port

2
src/lib/ecl/attitude_fw/ecl_controller.cpp
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) {

24
src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
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);
}

16
src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
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);
}

26
src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
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);

8
src/lib/external_lgpl/tecs/tecs.cpp
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;

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

@ -42,6 +42,9 @@
*/
#include <px4_config.h>
#include <px4_defines.h>
#include <px4_tasks.h>
#include <px4_posix.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
@ -421,7 +424,7 @@ FixedwingAttitudeControl::~FixedwingAttitudeControl()
/* if we have given up, kill it */
if (++i > 50) {
task_delete(_control_task);
px4_task_delete(_control_task);
break;
}
} while (_control_task != -1);
@ -800,7 +803,7 @@ FixedwingAttitudeControl::task_main()
float flaps_control = 0.0f;
/* map flaps by default to manual if valid */
if (isfinite(_manual.flaps)) {
if (PX4_ISFINITE(_manual.flaps)) {
flaps_control = _manual.flaps;
}
@ -810,7 +813,7 @@ FixedwingAttitudeControl::task_main()
float airspeed;
/* if airspeed is not updating, we assume the normal average speed */
if (bool nonfinite = !isfinite(_airspeed.true_airspeed_m_s) ||
if (bool nonfinite = !PX4_ISFINITE(_airspeed.true_airspeed_m_s) ||
hrt_elapsed_time(&_airspeed.timestamp) > 1e6) {
airspeed = _parameters.airspeed_trim;
if (nonfinite) {
@ -990,7 +993,7 @@ FixedwingAttitudeControl::task_main()
control_input.lock_integrator = lock_integrator;
/* Run attitude controllers */
if (isfinite(roll_sp) && isfinite(pitch_sp)) {
if (PX4_ISFINITE(roll_sp) && PX4_ISFINITE(pitch_sp)) {
_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
@ -1002,8 +1005,8 @@ FixedwingAttitudeControl::task_main()
/* Run attitude RATE controllers which need the desired attitudes from above, add trim */
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)) {
_actuators.control[0] = (PX4_ISFINITE(roll_u)) ? roll_u + _parameters.trim_roll : _parameters.trim_roll;
if (!PX4_ISFINITE(roll_u)) {
_roll_ctrl.reset_integrator();
perf_count(_nonfinite_output_perf);
@ -1013,8 +1016,8 @@ FixedwingAttitudeControl::task_main()
}
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)) {
_actuators.control[1] = (PX4_ISFINITE(pitch_u)) ? pitch_u + _parameters.trim_pitch : _parameters.trim_pitch;
if (!PX4_ISFINITE(pitch_u)) {
_pitch_ctrl.reset_integrator();
perf_count(_nonfinite_output_perf);
if (_debug && loop_counter % 10 == 0) {
@ -1034,11 +1037,11 @@ FixedwingAttitudeControl::task_main()
}
float yaw_u = _yaw_ctrl.control_bodyrate(control_input);
_actuators.control[2] = (isfinite(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw;
_actuators.control[2] = (PX4_ISFINITE(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw;
/* add in manual rudder control */
_actuators.control[2] += yaw_manual;
if (!isfinite(yaw_u)) {
if (!PX4_ISFINITE(yaw_u)) {
_yaw_ctrl.reset_integrator();
perf_count(_nonfinite_output_perf);
if (_debug && loop_counter % 10 == 0) {
@ -1048,11 +1051,11 @@ FixedwingAttitudeControl::task_main()
/* throttle passed through if it is finite and if no engine failure was
* detected */
_actuators.control[3] = (isfinite(throttle_sp) &&
_actuators.control[3] = (PX4_ISFINITE(throttle_sp) &&
!(_vehicle_status.engine_failure ||
_vehicle_status.engine_failure_cmd)) ?
throttle_sp : 0.0f;
if (!isfinite(throttle_sp)) {
if (!PX4_ISFINITE(throttle_sp)) {
if (_debug && loop_counter % 10 == 0) {
warnx("throttle_sp %.4f", (double)throttle_sp);
}
@ -1145,7 +1148,7 @@ FixedwingAttitudeControl::start()
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
1600,
(main_t)&FixedwingAttitudeControl::task_main_trampoline,
(px4_main_t)&FixedwingAttitudeControl::task_main_trampoline,
nullptr);
if (_control_task < 0) {
@ -1159,23 +1162,23 @@ FixedwingAttitudeControl::start()
int fw_att_control_main(int argc, char *argv[])
{
if (argc < 2) {
errx(1, "usage: fw_att_control {start|stop|status}");
warnx("usage: fw_att_control {start|stop|status}");
}
if (!strcmp(argv[1], "start")) {
if (att_control::g_control != nullptr)
errx(1, "already running");
warnx("already running");
att_control::g_control = new FixedwingAttitudeControl;
if (att_control::g_control == nullptr)
errx(1, "alloc failed");
warnx("alloc failed");
if (OK != att_control::g_control->start()) {
delete att_control::g_control;
att_control::g_control = nullptr;
err(1, "start failed");
warn("start failed");
}
/* check if the waiting is necessary at all */
@ -1194,7 +1197,7 @@ int fw_att_control_main(int argc, char *argv[])
if (!strcmp(argv[1], "stop")) {
if (att_control::g_control == nullptr)
errx(1, "not running");
warnx("not running");
delete att_control::g_control;
att_control::g_control = nullptr;
@ -1203,10 +1206,10 @@ int fw_att_control_main(int argc, char *argv[])
if (!strcmp(argv[1], "status")) {
if (att_control::g_control) {
errx(0, "running");
warnx("running");
} else {
errx(1, "not running");
warnx("not running");
}
}

21
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
View File

@ -52,6 +52,9 @@
*/
#include <px4_config.h>
#include <px4_defines.h>
#include <px4_tasks.h>
#include <px4_posix.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
@ -604,7 +607,7 @@ FixedwingPositionControl::~FixedwingPositionControl()
/* if we have given up, kill it */
if (++i > 50) {
task_delete(_control_task);
px4_task_delete(_control_task);
break;
}
} while (_control_task != -1);
@ -953,7 +956,7 @@ void FixedwingPositionControl::get_waypoint_heading_distance(float heading, floa
float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint_alt, const struct vehicle_global_position_s &global_pos)
{
if (!isfinite(global_pos.terrain_alt)) {
if (!PX4_ISFINITE(global_pos.terrain_alt)) {
return land_setpoint_alt;
}
@ -1958,7 +1961,7 @@ FixedwingPositionControl::start()
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
1600,
(main_t)&FixedwingPositionControl::task_main_trampoline,
(px4_main_t)&FixedwingPositionControl::task_main_trampoline,
nullptr);
if (_control_task < 0) {
@ -1972,16 +1975,16 @@ FixedwingPositionControl::start()
int fw_pos_control_l1_main(int argc, char *argv[])
{
if (argc < 2) {
errx(1, "usage: fw_pos_control_l1 {start|stop|status}");
warnx("usage: fw_pos_control_l1 {start|stop|status}");
}
if (!strcmp(argv[1], "start")) {
if (l1_control::g_control != nullptr)
errx(1, "already running");
warnx("already running");
if (OK != FixedwingPositionControl::start()) {
err(1, "start failed");
warn("start failed");
}
/* avoid memory fragmentation by not exiting start handler until the task has fully started */
@ -1997,7 +2000,7 @@ int fw_pos_control_l1_main(int argc, char *argv[])
if (!strcmp(argv[1], "stop")) {
if (l1_control::g_control == nullptr)
errx(1, "not running");
warnx("not running");
delete l1_control::g_control;
l1_control::g_control = nullptr;
@ -2006,10 +2009,10 @@ int fw_pos_control_l1_main(int argc, char *argv[])
if (!strcmp(argv[1], "status")) {
if (l1_control::g_control) {
errx(0, "running");
warnx("running");
} else {
errx(1, "not running");
warnx("not running");
}
}