diff options
author | Julian Oes <joes@student.ethz.ch> | 2012-11-11 11:55:27 -0800 |
---|---|---|
committer | Julian Oes <joes@student.ethz.ch> | 2012-11-11 11:55:27 -0800 |
commit | a8dfcaace27aa0abee4b3c44bffee9f94e391628 (patch) | |
tree | 95b36a23669be30709c91936c7f934172c71bd2b /apps | |
parent | 0baca3ee316ca70fd18bf2cd5128685220e57634 (diff) | |
download | px4-firmware-a8dfcaace27aa0abee4b3c44bffee9f94e391628.tar.gz px4-firmware-a8dfcaace27aa0abee4b3c44bffee9f94e391628.tar.bz2 px4-firmware-a8dfcaace27aa0abee4b3c44bffee9f94e391628.zip |
Several fixes, hex flies, failsafe not really tested yet
Diffstat (limited to 'apps')
-rw-r--r-- | apps/multirotor_att_control/multirotor_att_control_main.c | 42 | ||||
-rw-r--r-- | apps/px4io/mixer.c | 3 | ||||
-rw-r--r-- | apps/px4io/safety.c | 44 | ||||
-rw-r--r-- | apps/systemlib/mixer/mixer_multirotor.cpp | 2 |
4 files changed, 55 insertions, 36 deletions
diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index 262367370..de3a4440b 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -203,7 +203,7 @@ mc_thread_main(int argc, char *argv[]) if (state.offboard_control_signal_lost) { /* set failsafe thrust */ param_get(failsafe_throttle_handle, &failsafe_throttle); - att_sp.thrust = (failsafe_throttle < 0.6f) : failsafe_throttle ? 0.5f; + att_sp.thrust = failsafe_throttle; } else { /* no signal loss, normal throttle */ att_sp.thrust = offboard_sp.p4; @@ -252,28 +252,28 @@ mc_thread_main(int argc, char *argv[]) // XXX disable this for now until its better checked - // static bool rc_loss_first_time = true; - // /* if the RC signal is lost, try to stay level and go slowly back down to ground */ - // if(state.rc_signal_lost) { - // /* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */ - // param_get(failsafe_throttle_handle, &failsafe_throttle); - // att_sp.roll_body = 0.0f; - // att_sp.pitch_body = 0.0f; + static bool rc_loss_first_time = true; + /* if the RC signal is lost, try to stay level and go slowly back down to ground */ + if(state.rc_signal_lost) { + /* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */ + param_get(failsafe_throttle_handle, &failsafe_throttle); + att_sp.roll_body = 0.0f; + att_sp.pitch_body = 0.0f; - // /* keep current yaw, do not attempt to go to north orientation, - // * since if the pilot regains RC control, he will be lost regarding - // * the current orientation. - // */ + /* keep current yaw, do not attempt to go to north orientation, + * since if the pilot regains RC control, he will be lost regarding + * the current orientation. + */ - // if (rc_loss_first_time) - // att_sp.yaw_body = att.yaw; - // // XXX hard-limit it to prevent ballistic mishaps - this is just supposed to - // // slow a crash down, not actually keep the system in-air. - // att_sp.thrust = (failsafe_throttle < 0.5f) : failsafe_throttle ? 0.5f; - // rc_loss_first_time = false; - // } else { - // rc_loss_first_time = true; - // } + if (rc_loss_first_time) + att_sp.yaw_body = att.yaw; + // XXX hard-limit it to prevent ballistic mishaps - this is just supposed to + // slow a crash down, not actually keep the system in-air. + att_sp.thrust = failsafe_throttle; + rc_loss_first_time = false; + } else { + rc_loss_first_time = true; + } att_sp.timestamp = hrt_absolute_time(); } diff --git a/apps/px4io/mixer.c b/apps/px4io/mixer.c index 6cf499c39..572344d00 100644 --- a/apps/px4io/mixer.c +++ b/apps/px4io/mixer.c @@ -167,7 +167,8 @@ mixer_tick(void *arg) /* * Decide whether the servos should be armed right now. */ - should_arm = system_state.armed && system_state.arm_ok && (control_count > 0); + + should_arm = system_state.armed && system_state.arm_ok && (control_count > 0) && system_state.mixer_use_fmu; if (should_arm && !mixer_servos_armed) { /* need to arm, but not armed */ up_pwm_servo_arm(true); diff --git a/apps/px4io/safety.c b/apps/px4io/safety.c index 0ad636c0b..2a087705a 100644 --- a/apps/px4io/safety.c +++ b/apps/px4io/safety.c @@ -56,18 +56,20 @@ static struct hrt_call arming_call; * Count the number of times in a row that we see the arming button * held down. */ -static unsigned arm_counter; -#define ARM_COUNTER_THRESHOLD 10 +static unsigned counter; -static bool safety_led_state; +#define ARM_COUNTER_THRESHOLD 20 +#define DISARM_COUNTER_THRESHOLD 2 +static bool safety_led_state; +static bool safety_button_pressed; static void safety_check_button(void *arg); void safety_init(void) { - /* arrange for the button handler to be called at 10Hz */ - hrt_call_every(&arming_call, 1000, 100000, safety_check_button, NULL); + /* arrange for the button handler to be called at 20Hz */ + hrt_call_every(&arming_call, 1000, 50000, safety_check_button, NULL); } static void @@ -77,18 +79,34 @@ safety_check_button(void *arg) * Debounce the safety button, change state if it has been held for long enough. * */ + safety_button_pressed = BUTTON_SAFETY; + + if(safety_button_pressed) { + //printf("Pressed, Arm counter: %d, Disarm counter: %d\n", arm_counter, disarm_counter); + } - if (BUTTON_SAFETY) { - if (arm_counter < ARM_COUNTER_THRESHOLD) { - arm_counter++; - } else if (arm_counter == ARM_COUNTER_THRESHOLD) { - /* change our armed state and notify the FMU */ - system_state.armed = !system_state.armed; - arm_counter++; + /* Keep pressed for a while to arm */ + if (safety_button_pressed && !system_state.armed) { + if (counter < ARM_COUNTER_THRESHOLD) { + counter++; + } else if (counter == ARM_COUNTER_THRESHOLD) { + /* change to armed state and notify the FMU */ + system_state.armed = true; + counter++; + system_state.fmu_report_due = true; + } + /* Disarm quickly */ + } else if (safety_button_pressed && system_state.armed) { + if (counter < DISARM_COUNTER_THRESHOLD) { + counter++; + } else if (counter == DISARM_COUNTER_THRESHOLD) { + /* change to disarmed state and notify the FMU */ + system_state.armed = false; + counter++; system_state.fmu_report_due = true; } } else { - arm_counter = 0; + counter = 0; } /* when armed, toggle the LED; when safe, leave it on */ diff --git a/apps/systemlib/mixer/mixer_multirotor.cpp b/apps/systemlib/mixer/mixer_multirotor.cpp index 52d79e369..b5b25e532 100644 --- a/apps/systemlib/mixer/mixer_multirotor.cpp +++ b/apps/systemlib/mixer/mixer_multirotor.cpp @@ -164,7 +164,7 @@ MultirotorMixer::mix(float *outputs, unsigned space) /* use an output factor to prevent too strong control signals at low throttle */ float min_thrust = 0.05f; float max_thrust = 1.0f; - float startpoint_full_control = 0.20f; + float startpoint_full_control = 0.40f; float output_factor; /* keep roll, pitch and yaw control to 0 below min thrust */ |