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

Made PX4IO FMU timeout based on IOs HRT, updating mixers now on every FMU update and not at fixed rate, this is WIP and currently does not support mixing with RC-only

This commit is contained in:
Lorenz Meier 2012-12-15 23:28:03 +01:00
parent 42ffb4161d
commit f81d00594c
4 changed files with 15 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
apps/px4io/comms.c
View File

@ -183,7 +183,7 @@ comms_handle_command(const void *buffer, size_t length)
system_state.vector_flight_mode_ok = cmd->vector_flight_mode_ok;
system_state.manual_override_ok = cmd->manual_override_ok;
system_state.mixer_fmu_available = true;
system_state.fmu_data_received = true;
system_state.fmu_data_received_time = hrt_absolute_time();
/* set PWM update rate if changed (after limiting) */
uint16_t new_servo_rate = cmd->servo_rate;
@ -201,6 +201,9 @@ comms_handle_command(const void *buffer, size_t length)
system_state.servo_rate = new_servo_rate;
}
/* update servo values immediately */
mixer_tick();
/* XXX do relay changes here */
for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++) {
system_state.relays[i] = cmd->relay_state[i];

7
apps/px4io/controls.c
View File

@ -61,8 +61,8 @@
#include "px4io.h"
#define RC_FAILSAFE_TIMEOUT 2000000 /**< two seconds failsafe timeout */
#define RC_CHANNEL_HIGH_THRESH 1600
#define RC_CHANNEL_LOW_THRESH 1400
#define RC_CHANNEL_HIGH_THRESH 1700
#define RC_CHANNEL_LOW_THRESH 1300
static void ppm_input(void);
@ -133,8 +133,7 @@ controls_main(void)
system_state.fmu_report_due = true;
}
/* do PWM output updates */
mixer_tick();
/* PWM output updates are performed directly on each comms update */
}
}

15
apps/px4io/mixer.c
View File

@ -53,10 +53,9 @@
#include "px4io.h"
/*
* 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
/*
* Update a mixer based on the current control signals.
@ -91,25 +90,19 @@ mixer_tick(void)
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++;
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 */
if (fmu_input_drops >= FMU_INPUT_DROP_LIMIT) {
system_state.mixer_manual_override = true;
system_state.mixer_fmu_available = false;
}
} else {
fmu_input_drops = 0;
system_state.fmu_data_received = false;
}
} else if (system_state.rc_channels > 0 && system_state.manual_override_ok) {
/* we have control data from an R/C input */
control_count = system_state.rc_channels;
control_values = &system_state.rc_channel_data[0];
} else {
/* we have no control input */
/* we have no control input (no FMU, no RC) */
// XXX builtin failsafe would activate here
control_count = 0;

4
apps/px4io/px4io.h
View File

@ -106,9 +106,9 @@ struct sys_state_s
bool fmu_report_due;
/**
* If true, new control data from the FMU has been received.
* Last FMU receive time, in microseconds since system boot
*/
bool fmu_data_received;
uint64_t fmu_data_received_time;
/**
* Current serial interface mode, per the serial_rx_mode parameter