Merge branch 'master' of github.com:PX4/Firmware

7 files changed, 45 insertions, 22 deletions

diff --git a/Documentation/l1_control.odg b/Documentation/l1_control.odg
new file mode 100644
index 000000000..69910c677
--- /dev/null
+++ b/Documentation/l1_control.odg
diff --git a/ROMFS/px4fmu_common/init.d/1000_rc_fw.hil b/ROMFS/px4fmu_common/init.d/1000_rc_fw.hil
index 6e29bd6f8..5e4028bbb 100644
--- a/ROMFS/px4fmu_common/init.d/1000_rc_fw.hil
+++ b/ROMFS/px4fmu_common/init.d/1000_rc_fw.hil
@@ -33,6 +33,8 @@ then
 	param set FW_T_SINK_MIN 4.0
 	param set FW_Y_ROLLFF 1.1
 	param set FW_L1_PERIOD 16
+	param set RC_SCALE_ROLL 1.0
+	param set RC_SCALE_PITCH 1.0
 
 	param set SYS_AUTOCONFIG 0
 	param save
diff --git a/ROMFS/px4fmu_common/init.d/100_mpx_easystar b/ROMFS/px4fmu_common/init.d/100_mpx_easystar
index 1aa1ef494..4f843e9aa 100644
--- a/ROMFS/px4fmu_common/init.d/100_mpx_easystar
+++ b/ROMFS/px4fmu_common/init.d/100_mpx_easystar
@@ -29,6 +29,8 @@ then
 	param set FW_T_SINK_MIN 4.0
 	param set FW_Y_ROLLFF 1.1
 	param set FW_L1_PERIOD 16
+	param set RC_SCALE_ROLL 1.0
+	param set RC_SCALE_PITCH 1.0
 	
 	param set SYS_AUTOCONFIG 0
 	param save
diff --git a/ROMFS/px4fmu_common/init.d/101_hk_bixler b/ROMFS/px4fmu_common/init.d/101_hk_bixler
index 4d8a24c4a..cef86c34d 100644
--- a/ROMFS/px4fmu_common/init.d/101_hk_bixler
+++ b/ROMFS/px4fmu_common/init.d/101_hk_bixler
@@ -29,6 +29,8 @@ then
 	param set FW_T_SINK_MIN 4.0
 	param set FW_Y_ROLLFF 1.1
 	param set FW_L1_PERIOD 16
+	param set RC_SCALE_ROLL 1.0
+	param set RC_SCALE_PITCH 1.0
 	
 	param set SYS_AUTOCONFIG 0
 	param save
diff --git a/ROMFS/px4fmu_common/init.d/31_io_phantom b/ROMFS/px4fmu_common/init.d/31_io_phantom
index 8fe94452f..652833745 100644
--- a/ROMFS/px4fmu_common/init.d/31_io_phantom
+++ b/ROMFS/px4fmu_common/init.d/31_io_phantom
@@ -29,6 +29,8 @@ then
 	param set FW_T_SINK_MIN 4.0
 	param set FW_Y_ROLLFF 1.1
 	param set FW_L1_PERIOD 17
+	param set RC_SCALE_ROLL 1.0
+	param set RC_SCALE_PITCH 1.0
 	
 	param set SYS_AUTOCONFIG 0
 	param save
diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp
index fedff769b..ea87b37d9 100644
--- a/src/drivers/rgbled/rgbled.cpp
+++ b/src/drivers/rgbled/rgbled.cpp
@@ -103,6 +103,7 @@ private:
 
 	bool			_running;
 	int			_led_interval;
+	bool			_should_run;
 	int			_counter;
 
 	void 			set_color(rgbled_color_t ledcolor);
@@ -136,6 +137,7 @@ RGBLED::RGBLED(int bus, int rgbled) :
 	_brightness(1.0f),
 	_running(false),
 	_led_interval(0),
+	_should_run(false),
 	_counter(0)
 {
 	memset(&_work, 0, sizeof(_work));
@@ -248,6 +250,11 @@ RGBLED::led_trampoline(void *arg)
 void
 RGBLED::led()
 {
+	if (!_should_run) {
+		_running = false;
+		return;
+	}
+
 	switch (_mode) {
 	case RGBLED_MODE_BLINK_SLOW:
 	case RGBLED_MODE_BLINK_NORMAL:
@@ -409,10 +416,10 @@ RGBLED::set_mode(rgbled_mode_t mode)
 {
 	if (mode != _mode) {
 		_mode = mode;
-		bool should_run = false;
 
 		switch (mode) {
 		case RGBLED_MODE_OFF:
+			_should_run = false;
 			send_led_enable(false);
 			break;
 
@@ -423,7 +430,7 @@ RGBLED::set_mode(rgbled_mode_t mode)
 			break;
 
 		case RGBLED_MODE_BLINK_SLOW:
-			should_run = true;
+			_should_run = true;
 			_counter = 0;
 			_led_interval = 2000;
 			_brightness = 1.0f;
@@ -431,7 +438,7 @@ RGBLED::set_mode(rgbled_mode_t mode)
 			break;
 
 		case RGBLED_MODE_BLINK_NORMAL:
-			should_run = true;
+			_should_run = true;
 			_counter = 0;
 			_led_interval = 500;
 			_brightness = 1.0f;
@@ -439,7 +446,7 @@ RGBLED::set_mode(rgbled_mode_t mode)
 			break;
 
 		case RGBLED_MODE_BLINK_FAST:
-			should_run = true;
+			_should_run = true;
 			_counter = 0;
 			_led_interval = 100;
 			_brightness = 1.0f;
@@ -447,14 +454,14 @@ RGBLED::set_mode(rgbled_mode_t mode)
 			break;
 
 		case RGBLED_MODE_BREATHE:
-			should_run = true;
+			_should_run = true;
 			_counter = 0;
 			_led_interval = 25;
 			send_led_enable(true);
 			break;
 
 		case RGBLED_MODE_PATTERN:
-			should_run = true;
+			_should_run = true;
 			_counter = 0;
 			_brightness = 1.0f;
 			send_led_enable(true);
@@ -466,16 +473,11 @@ RGBLED::set_mode(rgbled_mode_t mode)
 		}
 
 		/* if it should run now, start the workq */
-		if (should_run && !_running) {
+		if (_should_run && !_running) {
 			_running = true;
 			work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, 1);
 		}
 
-		/* if it should stop, then cancel the workq */
-		if (!should_run && _running) {
-			_running = false;
-			work_cancel(LPWORK, &_work);
-		}
 	}
 }
 
diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp
index c5c0c7a3c..daf136d49 100644
--- a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp
+++ b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp
@@ -130,8 +130,12 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
 	float alongTrackDist = vector_A_to_airplane * vector_AB;
 
 	/* estimate airplane position WRT to B */
-	math::Vector2f vector_B_to_airplane_unit = get_local_planar_vector(vector_B, vector_curr_position).normalized();
-	float bearing_wp_b = atan2f(-vector_B_to_airplane_unit.getY() , -vector_B_to_airplane_unit.getX());
+	math::Vector2f vector_B_to_P_unit = get_local_planar_vector(vector_B, vector_curr_position).normalized();
+	
+	/* calculate angle of airplane position vector relative to line) */
+
+	// XXX this could probably also be based solely on the dot product
+	float AB_to_BP_bearing = atan2f(vector_B_to_P_unit % vector_AB, vector_B_to_P_unit * vector_AB);
 
 	/* extension from [2], fly directly to A */
 	if (distance_A_to_airplane > _L1_distance && alongTrackDist / math::max(distance_A_to_airplane , 1.0f) < -0.7071f) {
@@ -148,21 +152,30 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
 		/* bearing from current position to L1 point */
 		_nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX());
 
-// XXX this can be useful as last-resort guard, but is currently not needed
-#if 0
-	} else if (absf(bearing_wp_b) > math::radians(80.0f)) {
-		/* extension, fly back to waypoint */
+	/*
+	 * If the AB vector and the vector from B to airplane point in the same
+	 * direction, we have missed the waypoint. At +- 90 degrees we are just passing it.
+	 */
+	} else if (fabsf(AB_to_BP_bearing) < math::radians(100.0f)) {
+		/*
+		 * Extension, fly back to waypoint.
+		 * 
+		 * This corner case is possible if the system was following
+		 * the AB line from waypoint A to waypoint B, then is
+		 * switched to manual mode (or otherwise misses the waypoint)
+		 * and behind the waypoint continues to follow the AB line.
+		 */
 
 		/* calculate eta to fly to waypoint B */
 		
 		/* velocity across / orthogonal to line */
-		xtrack_vel = ground_speed_vector % (-vector_B_to_airplane_unit);
+		xtrack_vel = ground_speed_vector % (-vector_B_to_P_unit);
 		/* velocity along line */
-		ltrack_vel = ground_speed_vector * (-vector_B_to_airplane_unit);
+		ltrack_vel = ground_speed_vector * (-vector_B_to_P_unit);
 		eta = atan2f(xtrack_vel, ltrack_vel);
 		/* bearing from current position to L1 point */
-		_nav_bearing = bearing_wp_b;
-#endif
+		_nav_bearing = atan2f(-vector_B_to_P_unit.getY() , -vector_B_to_P_unit.getX());
+
 	} else {
 
 		/* calculate eta to fly along the line between A and B */