Minor changes to get fixed wing control apps running for posix

makefiles/posix/config_posix_sitl.mk

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

src/lib/ecl/attitude_fw/ecl_controller.cpp

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

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

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

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

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

src/modules/fw_att_control/fw_att_control_main.cpp

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

src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp

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