fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-24 15:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

merged

Browse Source
This commit is contained in:
Lorenz Meier
2012-12-30 10:03:05 +01:00
parent b8250de1e6 62a95bf8e6
commit 142556b442
44 changed files with 2125 additions and 1308 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/px4io/mixer.cpp
+41 -28
View File
@@ -48,7 +48,11 @@
#include <unistd.h>
#include <fcntl.h>
#include <debug.h>
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_hrt.h>
#include <systemlib/mixer/mixer.h>
extern "C" {
@@ -56,10 +60,9 @@ extern "C" {
}
/*
* Count of periodic calls in which we have no FMU input.
* Maximum interval in us before FMU signal is considered lost
*/
static unsigned fmu_input_drops;
#define FMU_INPUT_DROP_LIMIT 20
#define FMU_INPUT_DROP_LIMIT_US 200000
/* current servo arm/disarm state */
bool mixer_servos_armed = false;
@@ -80,37 +83,45 @@ mixer_tick(void)
{
bool should_arm;
/* check that we are receiving fresh data from the FMU */
if ((hrt_absolute_time() - system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) {
/* too many frames without FMU input, time to go to failsafe */
system_state.mixer_manual_override = true;
system_state.mixer_fmu_available = false;
lib_lowprintf("\nRX timeout\n");
}
/*
* Decide which set of inputs we're using.
*/
if (system_state.mixer_use_fmu) {
/* we have recent control data from the FMU */
control_count = PX4IO_OUTPUT_CHANNELS;
control_values = &system_state.fmu_channel_data[0];
/* check that we are receiving fresh data from the FMU */
if (!system_state.fmu_data_received) {
fmu_input_drops++;
/* too many frames without FMU input, time to go to failsafe */
if (fmu_input_drops >= FMU_INPUT_DROP_LIMIT) {
system_state.mixer_use_fmu = false;
}
/* this is for planes, where manual override makes sense */
if(system_state.manual_override_ok) {
/* if everything is ok */
if (!system_state.mixer_manual_override && system_state.mixer_fmu_available) {
/* we have recent control data from the FMU */
control_count = PX4IO_OUTPUT_CHANNELS;
control_values = &system_state.fmu_channel_data[0];
/* when override is on or the fmu is not available */
} else if (system_state.rc_channels > 0) {
control_count = system_state.rc_channels;
control_values = &system_state.rc_channel_data[0];
} else {
fmu_input_drops = 0;
system_state.fmu_data_received = false;
/* we have no control input (no FMU, no RC) */
// XXX builtin failsafe would activate here
control_count = 0;
}
} else if (system_state.rc_channels > 0) {
/* we have control data from an R/C input */
control_count = system_state.rc_channels;
control_values = &system_state.rc_channel_data[0];
/* this is for multicopters, etc. where manual override does not make sense */
} else {
/* we have no control input */
/* XXX builtin failsafe would activate here */
control_count = 0;
/* if the fmu is available whe are good */
if(system_state.mixer_fmu_available) {
control_count = PX4IO_OUTPUT_CHANNELS;
control_values = &system_state.fmu_channel_data[0];
/* we better shut everything off */
} else {
control_count = 0;
}
}
/*
@@ -144,7 +155,9 @@ mixer_tick(void)
/*
* Decide whether the servos should be armed right now.
* A sufficient reason is armed state and either FMU or RC control inputs
*/
should_arm = system_state.armed && system_state.arm_ok && (control_count > 0);
if (should_arm && !mixer_servos_armed) {
@@ -216,4 +229,4 @@ mixer_handle_text(const void *buffer, size_t length)
break;
}
}
}