From c6b58491bbd7390650723c65b4d4e9ec0922c8de Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 14 Oct 2013 22:18:44 +0200 Subject: Work queue in RGB driver without work_cancel() --- src/drivers/rgbled/rgbled.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index fedff769b..aeb7e2f78 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -248,6 +248,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: @@ -471,11 +476,6 @@ RGBLED::set_mode(rgbled_mode_t mode) 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); - } } } -- cgit v1.2.3 From fbe595a591734bffa95d28125b8e0bda117d7314 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 14 Oct 2013 23:10:12 +0200 Subject: Fixed some stupid compile errors --- src/drivers/rgbled/rgbled.cpp | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index aeb7e2f78..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)); @@ -414,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; @@ -428,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; @@ -436,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; @@ -444,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; @@ -452,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); @@ -471,7 +473,7 @@ 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); } -- cgit v1.2.3 From 39336fd58533c85ef6bad93b80dd51e62b6eba3a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 15 Oct 2013 10:46:28 +0200 Subject: Better defaults for RC SCALE --- ROMFS/px4fmu_common/init.d/1000_rc_fw.hil | 2 ++ ROMFS/px4fmu_common/init.d/100_mpx_easystar | 2 ++ ROMFS/px4fmu_common/init.d/101_hk_bixler | 2 ++ ROMFS/px4fmu_common/init.d/31_io_phantom | 2 ++ 4 files changed, 8 insertions(+) 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 -- cgit v1.2.3 From 8ed0796448e49ae34dbfdf31c056e402834c0b8f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 15 Oct 2013 22:17:53 +0200 Subject: L1 control diagram illustrating corner case --- Documentation/l1_control.odg | Bin 0 -> 11753 bytes 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 Documentation/l1_control.odg diff --git a/Documentation/l1_control.odg b/Documentation/l1_control.odg new file mode 100644 index 000000000..69910c677 Binary files /dev/null and b/Documentation/l1_control.odg differ -- cgit v1.2.3 From 4532eca4ef6f6170765062ccd2ca7f29814e0b3a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 15 Oct 2013 22:55:16 +0200 Subject: Covered corner case in L1 controller not adressed so far in existing concepts --- src/lib/ecl/l1/ecl_l1_pos_controller.cpp | 33 ++++++++++++++++++++++---------- 1 file changed, 23 insertions(+), 10 deletions(-) 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 */ -- cgit v1.2.3