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

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

This commit is contained in:
Lorenz Meier 2013-10-17 13:38:30 +02:00
parent 3ca94e7943 4532eca4ef
commit 53c85a24ea
7 changed files with 45 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

BIN
Documentation/l1_control.odg Normal file
View File

Binary file not shown.

2
ROMFS/px4fmu_common/init.d/1000_rc_fw.hil
View File

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

2
ROMFS/px4fmu_common/init.d/100_mpx_easystar
View File

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

2
ROMFS/px4fmu_common/init.d/101_hk_bixler
View File

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

2
ROMFS/px4fmu_common/init.d/31_io_phantom
View File

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

26
src/drivers/rgbled/rgbled.cpp
View File

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

33
src/lib/ecl/l1/ecl_l1_pos_controller.cpp
View File

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