Merge branch 'fixedwing_l1' of github.com:PX4/Firmware into fixedwing_l1

ROMFS/px4fmu_common/init.d/rc.hil

@@ -5,10 +5,9 @@
 
 echo "[HIL] starting.."
 
-uorb start
-
 # Tell MAVLink that this link is "fast"
-mavlink start -b 230400 -d /dev/ttyS1
+sleep 2
+mavlink start -b 230400 -d /dev/ttyACM0
 
 # Create a fake HIL /dev/pwm_output interface
 hil mode_pwm
@@ -50,7 +49,8 @@ att_pos_estimator_ekf start
 # Load mixer and start controllers (depends on px4io)
 #
 mixer load /dev/pwm_output /etc/mixers/FMU_AET.mix
-fw_pos_control_l1 start
+#fw_pos_control_l1 start
 fw_att_control start
 
 echo "[HIL] setup done, running"
+

ROMFS/px4fmu_common/init.d/rc.usb

@@ -21,7 +21,6 @@ if mavlink stop
 then
 	echo "stopped other MAVLink instance"
 fi
-sleep 2
 mavlink start -b 230400 -d /dev/ttyACM0
 
 # Stop commander
@@ -37,13 +36,6 @@ then
 	echo "Commander started"
 fi
 
-# Stop px4io
-if px4io stop
-then
-	echo "PX4IO stopped"
-fi
-sleep 1
-
 # Start px4io if present
 if px4io start
 then

ROMFS/px4fmu_common/init.d/rcS

@@ -101,8 +101,14 @@ then
 		fi
 	fi
 
-	# Try to get an USB console
-	nshterm /dev/ttyACM0 &
+	if param compare SYS_AUTOSTART 1000
+	then
+		sh /etc/init.d/rc.hil
+		set MODE custom
+	else
+		# Try to get an USB console
+		nshterm /dev/ttyACM0 &
+	fi
 	
 	#
 	# Upgrade PX4IO firmware
@@ -202,7 +208,6 @@ then
 	then
 		# Telemetry port is on both FMU boards ttyS1
 		mavlink start -b 57600 -d /dev/ttyS1
-		usleep 5000
 	
 		# Start commander
 		commander start

src/drivers/px4io/px4io.cpp

@@ -487,25 +487,27 @@ PX4IO::detect()
 {
 	int ret;
 
-	ASSERT(_task == -1);
+	if (_task == -1) {
 
-	/* do regular cdev init */
-	ret = CDev::init();
-	if (ret != OK)
-		return ret;
+		/* do regular cdev init */
+		ret = CDev::init();
+		if (ret != OK)
+			return ret;
 
-	/* get some parameters */
-	unsigned protocol = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION);
-	if (protocol != PX4IO_PROTOCOL_VERSION) {
-		if (protocol == _io_reg_get_error) {
-			log("IO not installed");
-		} else {
-			log("IO version error");
-			mavlink_log_emergency(_mavlink_fd, "IO VERSION MISMATCH, PLEASE UPGRADE SOFTWARE!");
+		/* get some parameters */
+		unsigned protocol = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION);
+		if (protocol != PX4IO_PROTOCOL_VERSION) {
+			if (protocol == _io_reg_get_error) {
+				log("IO not installed");
+			} else {
+				log("IO version error");
+				mavlink_log_emergency(_mavlink_fd, "IO VERSION MISMATCH, PLEASE UPGRADE SOFTWARE!");
+			}
+			
+			return -1;
 		}
-		
-		return -1;
 	}
+
 	log("IO found");
 
 	return 0;

src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp

@@ -38,20 +38,87 @@
  */
 
 #include "ecl_pitch_controller.h"
+#include <math.h>
+#include <stdint.h>
+#include <float.h>
+#include <geo/geo.h>
+#include <ecl/ecl.h>
+#include <mathlib/mathlib.h>
 
 ECL_PitchController::ECL_PitchController() :
 	_last_run(0),
 	_last_output(0.0f),
 	_integrator(0.0f),
 	_rate_error(0.0f),
-	_desired_rate(0.0f)
+	_rate_setpoint(0.0f),
+	_max_deflection_rad(math::radians(45.0f))
 {
 }
 
 float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitch_rate, float roll, float scaler,
-				   bool lock_integrator, float airspeed_min, float airspeed_max, float aspeed)
+				   bool lock_integrator, float airspeed_min, float airspeed_max, float airspeed)
 {
-	return 0.0f;
+	/* get the usual dt estimate */
+	uint64_t dt_micros = ecl_elapsed_time(&_last_run);
+	_last_run = ecl_absolute_time();
+
+	float dt = dt_micros / 1000000;
+
+	/* lock integral for long intervals */
+	if (dt_micros > 500000)
+		lock_integrator = true;
+
+	float k_roll_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f);
+	float k_i_rate = _k_i * _tc;
+
+	/* input conditioning */
+	if (!isfinite(airspeed)) {
+		/* airspeed is NaN, +- INF or not available, pick center of band */
+		airspeed = 0.5f * (airspeed_min + airspeed_max);
+	} else if (airspeed < airspeed_min) {
+		airspeed = airspeed_min;
+	}
+
+	/* calculate the offset in the rate resulting from rolling  */
+	float turn_offset = fabsf((CONSTANTS_ONE_G / airspeed) *
+				tanf(roll) * sinf(roll)) * _roll_ff;
+
+	float pitch_error = pitch_setpoint - pitch;
+	/* rate setpoint from current error and time constant */
+	_rate_setpoint = pitch_error / _tc;
+
+	/* add turn offset */
+	_rate_setpoint += turn_offset;
+
+	_rate_error = _rate_setpoint - pitch_rate;
+
+	float ilimit_scaled = 0.0f;
+
+	if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) {
+
+		float id = _rate_error * k_i_rate * dt * scaler;
+
+		/*
+		 * anti-windup: do not allow integrator to increase into the
+		 * wrong direction if actuator is at limit
+		 */
+		if (_last_output < -_max_deflection_rad) {
+			/* only allow motion to center: increase value */
+			id = math::max(id, 0.0f);
+		} else if (_last_output > _max_deflection_rad) {
+			/* only allow motion to center: decrease value */
+			id = math::min(id, 0.0f);
+		}
+
+		_integrator += id;
+	}
+
+	/* integrator limit */
+	_integrator = math::constrain(_integrator, -ilimit_scaled, ilimit_scaled);
+	/* store non-limited output */
+	_last_output = ((_rate_error * _k_d * scaler) + _integrator + (_rate_setpoint * k_roll_ff)) * scaler;
+
+	return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad);
 }
 
 void ECL_PitchController::reset_integrator()

src/lib/ecl/attitude_fw/ecl_pitch_controller.h

@@ -49,7 +49,7 @@ public:
 	ECL_PitchController();
 
 	float control(float pitch_setpoint, float pitch, float pitch_rate, float roll, float scaler = 1.0f,
-		      bool lock_integrator = false, float airspeed_min = 0.0f, float airspeed_max = 0.0f, float aspeed = (0.0f / 0.0f));
+		      bool lock_integrator = false, float airspeed_min = 0.0f, float airspeed_max = 0.0f, float airspeed = (0.0f / 0.0f));
 
 	void reset_integrator();
 
@@ -83,7 +83,7 @@ public:
 	}
 
 	float get_desired_rate() {
-		return _desired_rate;
+		return _rate_setpoint;
 	}
 
 private:
@@ -100,7 +100,8 @@ private:
 	float _last_output;
 	float _integrator;
 	float _rate_error;
-	float _desired_rate;
+	float _rate_setpoint;
+	float _max_deflection_rad;
 };
 
 #endif // ECL_PITCH_CONTROLLER_H

src/lib/ecl/attitude_fw/ecl_roll_controller.cpp

@@ -39,6 +39,11 @@
 
 #include "../ecl.h"
 #include "ecl_roll_controller.h"
+#include <stdint.h>
+#include <float.h>
+#include <geo/geo.h>
+#include <ecl/ecl.h>
+#include <mathlib/mathlib.h>
 
 ECL_RollController::ECL_RollController() :
 	_last_run(0),
@@ -46,13 +51,14 @@ ECL_RollController::ECL_RollController() :
 	_last_output(0.0f),
 	_integrator(0.0f),
 	_rate_error(0.0f),
-	_desired_rate(0.0f)
+	_rate_setpoint(0.0f),
+	_max_deflection_rad(math::radians(45.0f))
 {
 
 }
 
 float ECL_RollController::control(float roll_setpoint, float roll, float roll_rate,
-				  float scaler, bool lock_integrator, float airspeed_min, float airspeed_max, float aspeed)
+				  float scaler, bool lock_integrator, float airspeed_min, float airspeed_max, float airspeed)
 {
 	/* get the usual dt estimate */
 	uint64_t dt_micros = ecl_elapsed_time(&_last_run);
@@ -60,10 +66,56 @@ float ECL_RollController::control(float roll_setpoint, float roll, float roll_ra
 
 	float dt = (dt_micros > 500000) ? 0.0f : dt_micros / 1000000;
 
+	float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f);
+	float k_i_rate = _k_i * _tc;
 
+	/* input conditioning */
+	if (!isfinite(airspeed)) {
+		/* airspeed is NaN, +- INF or not available, pick center of band */
+		airspeed = 0.5f * (airspeed_min + airspeed_max);
+	} else if (airspeed < airspeed_min) {
+		airspeed = airspeed_min;
+	}
 
+	float roll_error = roll_setpoint - roll;
+	_rate_setpoint = roll_error / _tc;
 
-	return 0.0f;
+	/* limit the rate */
+	if (_max_rate > 0.01f) {
+		_rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint;
+		_rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint;
+	}
+
+	_rate_error = _rate_setpoint - roll_rate;
+
+
+	float ilimit_scaled = 0.0f;
+
+	if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) {
+
+		float id = _rate_error * k_i_rate * dt * scaler;
+
+		/*
+		 * anti-windup: do not allow integrator to increase into the
+		 * wrong direction if actuator is at limit
+		 */
+		if (_last_output < -_max_deflection_rad) {
+			/* only allow motion to center: increase value */
+			id = math::max(id, 0.0f);
+		} else if (_last_output > _max_deflection_rad) {
+			/* only allow motion to center: decrease value */
+			id = math::min(id, 0.0f);
+		}
+
+		_integrator += id;
+	}
+
+	/* integrator limit */
+	_integrator = math::constrain(_integrator, -ilimit_scaled, ilimit_scaled);
+	/* store non-limited output */
+	_last_output = ((_rate_error * _k_d * scaler) + _integrator + (_rate_setpoint * k_ff)) * scaler;
+
+	return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad);
 }
 
 void ECL_RollController::reset_integrator()

src/lib/ecl/attitude_fw/ecl_roll_controller.h

@@ -49,7 +49,7 @@ public:
 	ECL_RollController();
 
 	float control(float roll_setpoint, float roll, float roll_rate,
-		      float scaler = 1.0f, bool lock_integrator = false, float airspeed_min = 0.0f, float airspeed_max = 0.0f, float aspeed = (0.0f / 0.0f));
+		      float scaler = 1.0f, bool lock_integrator = false, float airspeed_min = 0.0f, float airspeed_max = 0.0f, float airspeed = (0.0f / 0.0f));
 
 	void reset_integrator();
 
@@ -79,7 +79,7 @@ public:
 	}
 
 	float get_desired_rate() {
-		return _desired_rate;
+		return _rate_setpoint;
 	}
 
 private:
@@ -93,7 +93,8 @@ private:
 	float _last_output;
 	float _integrator;
 	float _rate_error;
-	float _desired_rate;
+	float _rate_setpoint;
+	float _max_deflection_rad;
 };
 
 #endif // ECL_ROLL_CONTROLLER_H

src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp

@@ -38,6 +38,11 @@
  */
 
 #include "ecl_yaw_controller.h"
+#include <stdint.h>
+#include <float.h>
+#include <geo/geo.h>
+#include <ecl/ecl.h>
+#include <mathlib/mathlib.h>
 
 ECL_YawController::ECL_YawController() :
 	_last_run(0),
@@ -54,6 +59,12 @@ ECL_YawController::ECL_YawController() :
 float ECL_YawController::control(float roll, float yaw_rate, float accel_y, float scaler, bool lock_integrator,
 				 float airspeed_min, float airspeed_max, float aspeed)
 {
+	/* get the usual dt estimate */
+	uint64_t dt_micros = ecl_elapsed_time(&_last_run);
+	_last_run = ecl_absolute_time();
+
+	float dt = (dt_micros > 500000) ? 0.0f : dt_micros / 1000000;
+
 	return 0.0f;
 }
 

src/modules/commander/commander.cpp

@@ -144,8 +144,8 @@ static int mavlink_fd;
 
 /* flags */
 static bool commander_initialized = false;
-static bool thread_should_exit = false;		/**< daemon exit flag */
-static bool thread_running = false;		/**< daemon status flag */
+static volatile bool thread_should_exit = false;		/**< daemon exit flag */
+static volatile bool thread_running = false;		/**< daemon status flag */
 static int daemon_task;				/**< Handle of daemon task / thread */
 
 static unsigned int leds_counter;
@@ -230,7 +230,7 @@ int commander_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "start")) {
 
 		if (thread_running) {
-			warnx("commander already running\n");
+			warnx("commander already running");
 			/* this is not an error */
 			exit(0);
 		}
@@ -242,21 +242,38 @@ int commander_main(int argc, char *argv[])
 					     3000,
 					     commander_thread_main,
 					     (argv) ? (const char **)&argv[2] : (const char **)NULL);
+
+		while (!thread_running) {
+			usleep(200);
+		}
+
 		exit(0);
 	}
 
 	if (!strcmp(argv[1], "stop")) {
+
+		if (!thread_running)
+			errx(0, "commander already stopped");
+
 		thread_should_exit = true;
+
+		while (thread_running) {
+			usleep(200000);
+			warnx(".");
+		}
+
+		warnx("terminated.");
+
 		exit(0);
 	}
 
 	if (!strcmp(argv[1], "status")) {
 		if (thread_running) {
-			warnx("\tcommander is running\n");
+			warnx("\tcommander is running");
 			print_status();
 
 		} else {
-			warnx("\tcommander not started\n");
+			warnx("\tcommander not started");
 		}
 
 		exit(0);
@@ -326,6 +343,9 @@ void print_status()
 	warnx("arming: %s", armed_str);
 }
 
+static orb_advert_t control_mode_pub;
+static orb_advert_t status_pub;
+
 void handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed)
 {
 	/* result of the command */
@@ -339,6 +359,10 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 			uint8_t base_mode = (uint8_t) cmd->param1;
 			uint8_t custom_main_mode = (uint8_t) cmd->param2;
 
+			/* set HIL state */
+			hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF;
+			hil_state_transition(new_hil_state, status_pub, status, control_mode_pub, control_mode, mavlink_fd);
+
 			// TODO remove debug code
 			//mavlink_log_critical(mavlink_fd, "[cmd] command setmode: %d %d", base_mode, custom_main_mode);
 			/* set arming state */
@@ -526,7 +550,6 @@ int commander_thread_main(int argc, char *argv[])
 	}
 
 	/* Main state machine */
-	orb_advert_t status_pub;
 	/* make sure we are in preflight state */
 	memset(&status, 0, sizeof(status));
 	status.condition_landed = true;	// initialize to safe value
@@ -538,7 +561,7 @@ int commander_thread_main(int argc, char *argv[])
 
 	/* flags for control apps */
 	struct vehicle_control_mode_s control_mode;
-	orb_advert_t control_mode_pub;
+	
 
 	/* Initialize all flags to false */
 	memset(&control_mode, 0, sizeof(control_mode));
@@ -595,16 +618,20 @@ int commander_thread_main(int argc, char *argv[])
 
 	mavlink_log_info(mavlink_fd, "[cmd] started");
 
+	int ret;
+
 	pthread_attr_t commander_low_prio_attr;
 	pthread_attr_init(&commander_low_prio_attr);
 	pthread_attr_setstacksize(&commander_low_prio_attr, 2992);
 
 	struct sched_param param;
 	(void)pthread_attr_getschedparam(&commander_low_prio_attr, &param);
+
 	/* low priority */
 	param.sched_priority = SCHED_PRIORITY_DEFAULT - 50;
 	(void)pthread_attr_setschedparam(&commander_low_prio_attr, &param);
 	pthread_create(&commander_low_prio_thread, &commander_low_prio_attr, commander_low_prio_loop, NULL);
+	pthread_attr_destroy(&commander_low_prio_attr);
 
 	/* Start monitoring loop */
 	unsigned counter = 0;
@@ -1200,7 +1227,12 @@ int commander_thread_main(int argc, char *argv[])
 	}
 
 	/* wait for threads to complete */
-	pthread_join(commander_low_prio_thread, NULL);
+	ret = pthread_join(commander_low_prio_thread, NULL);
+	if (ret) {
+		warn("join failed", ret);
+	}
+
+	rgbled_set_mode(RGBLED_MODE_OFF);
 
 	/* close fds */
 	led_deinit();
@@ -1218,9 +1250,6 @@ int commander_thread_main(int argc, char *argv[])
 	close(param_changed_sub);
 	close(battery_sub);
 
-	warnx("exiting");
-	fflush(stdout);
-
 	thread_running = false;
 
 	return 0;
@@ -1628,7 +1657,7 @@ void *commander_low_prio_loop(void *arg)
 	while (!thread_should_exit) {
 
 		/* wait for up to 100ms for data */
-		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 1000);
+		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 200);
 
 		/* timed out - periodic check for _task_should_exit, etc. */
 		if (pret == 0)
@@ -1785,5 +1814,5 @@ void *commander_low_prio_loop(void *arg)
 
 	close(cmd_sub);
 
-	return 0;
+	return NULL;
 }

src/modules/commander/state_machine_helper.cpp

@@ -504,6 +504,8 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
 		current_control_mode->timestamp = hrt_absolute_time();
 		orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, current_control_mode);
 
+		// XXX also set lockdown here
+
 		ret = OK;
 
 	} else {

src/modules/mavlink/mavlink.c

@@ -743,7 +743,7 @@ int mavlink_thread_main(int argc, char *argv[])
 
 	thread_running = false;
 
-	exit(0);
+	return 0;
 }
 
 static void
@@ -767,7 +767,7 @@ int mavlink_main(int argc, char *argv[])
 
 		/* this is not an error */
 		if (thread_running)
-			errx(0, "mavlink already running\n");
+			errx(0, "mavlink already running");
 
 		thread_should_exit = false;
 		mavlink_task = task_spawn_cmd("mavlink",
@@ -776,15 +776,25 @@ int mavlink_main(int argc, char *argv[])
 					  2048,
 					  mavlink_thread_main,
 					  (const char **)argv);
+
+		while (!thread_running) {
+			usleep(200);
+		}
+
 		exit(0);
 	}
 
 	if (!strcmp(argv[1], "stop")) {
+
+		/* this is not an error */
+		if (!thread_running)
+			errx(0, "mavlink already stopped");
+
 		thread_should_exit = true;
 
 		while (thread_running) {
 			usleep(200000);
-			printf(".");
+			warnx(".");
 		}
 
 		warnx("terminated.");

src/modules/mavlink/mavlink_receiver.cpp

@@ -755,5 +755,7 @@ receive_start(int uart)
 
 	pthread_t thread;
 	pthread_create(&thread, &receiveloop_attr, receive_thread, &uart);
+
+	pthread_attr_destroy(&receiveloop_attr);
 	return thread;
 }

src/modules/mavlink/orb_listener.c

@@ -829,5 +829,7 @@ uorb_receive_start(void)
 
 	pthread_t thread;
 	pthread_create(&thread, &uorb_attr, uorb_receive_thread, NULL);
+
+	pthread_attr_destroy(&uorb_attr);
 	return thread;
 }