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

Merged IO feature branch

Browse Source
This commit is contained in:
Lorenz Meier
2012-12-13 11:12:34 +01:00
parent 26faab64e5 03076a72ca
commit 1fc0a6546e
13 changed files with 253 additions and 60 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/px4io/comms.c
+25 -8
View File
@@ -130,8 +130,10 @@ comms_main(void)
last_report_time = now;
/* populate the report */
for (unsigned i = 0; i < system_state.rc_channels; i++)
for (unsigned i = 0; i < system_state.rc_channels; i++) {
report.rc_channel[i] = system_state.rc_channel_data[i];
}
report.channel_count = system_state.rc_channels;
report.armed = system_state.armed;
@@ -168,26 +170,41 @@ comms_handle_command(const void *buffer, size_t length)
irqstate_t flags = irqsave();
/* fetch new PWM output values */
for (unsigned i = 0; i < PX4IO_OUTPUT_CHANNELS; i++)
for (unsigned i = 0; i < PX4IO_OUTPUT_CHANNELS; i++) {
system_state.fmu_channel_data[i] = cmd->servo_command[i];
}
/* if the IO is armed and FMU gets disarmed, IO must also disarm */
if(system_state.arm_ok && !cmd->arm_ok) {
/* if IO is armed and FMU gets disarmed, IO must also disarm */
if (system_state.arm_ok && !cmd->arm_ok) {
system_state.armed = false;
}
system_state.arm_ok = cmd->arm_ok;
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;
/* set PWM update rate if changed (after limiting) */
uint16_t new_servo_rate = cmd->servo_rate;
/* handle changes signalled by FMU */
// if (!system_state.arm_ok && system_state.armed)
// system_state.armed = false;
/* reject faster than 500 Hz updates */
if (new_servo_rate > 500) {
new_servo_rate = 500;
}
/* reject slower than 50 Hz updates */
if (new_servo_rate < 50) {
new_servo_rate = 50;
}
if (system_state.servo_rate != new_servo_rate) {
up_pwm_servo_set_rate(new_servo_rate);
system_state.servo_rate = new_servo_rate;
}
/* XXX do relay changes here */
for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++)
for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++) {
system_state.relays[i] = cmd->relay_state[i];
}
irqrestore(flags);
}
apps/px4io/controls.c
+4 -3
View File
@@ -110,12 +110,13 @@ controls_main(void)
if (!locked)
ppm_input();
/* force manual input override */
/* check for manual override status */
if (system_state.rc_channel_data[4] > RC_CHANNEL_HIGH_THRESH) {
system_state.mixer_use_fmu = false;
/* force manual input override */
system_state.mixer_manual_override = true;
} else {
/* override not engaged, use FMU */
system_state.mixer_use_fmu = true;
system_state.mixer_manual_override = false;
}
/*
apps/px4io/mixer.c
+6 -5
View File
@@ -85,7 +85,7 @@ mixer_tick(void)
/*
* Decide which set of inputs we're using.
*/
if (system_state.mixer_use_fmu && system_state.mixer_fmu_available) {
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];
@@ -96,21 +96,22 @@ mixer_tick(void)
/* 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;
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) {
} 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 */
/* XXX builtin failsafe would activate here */
// XXX builtin failsafe would activate here
control_count = 0;
}
apps/px4io/protocol.h
+6 -3
View File
@@ -52,9 +52,12 @@ struct px4io_command {
uint16_t f2i_magic;
#define F2I_MAGIC 0x636d
uint16_t servo_command[PX4IO_OUTPUT_CHANNELS];
bool relay_state[PX4IO_RELAY_CHANNELS];
bool arm_ok;
uint16_t servo_command[PX4IO_OUTPUT_CHANNELS]; /**< servo output channels */
uint16_t servo_rate; /**< PWM output rate in Hz */
bool relay_state[PX4IO_RELAY_CHANNELS]; /**< relay states as requested by FMU */
bool arm_ok; /**< FMU allows full arming */
bool vector_flight_mode_ok; /**< FMU aquired a valid position lock, ready for pos control */
bool manual_override_ok; /**< if true, IO performs a direct manual override */
};
/* config message from FMU to IO */
apps/px4io/px4io.c
+2
View File
@@ -61,6 +61,8 @@ int user_start(int argc, char *argv[])
{
/* reset all to zero */
memset(&system_state, 0, sizeof(system_state));
/* default to 50 Hz PWM outputs */
system_state.servo_rate = 50;
/* configure the high-resolution time/callout interface */
hrt_init();
apps/px4io/px4io.h
+12 -1
View File
@@ -71,6 +71,7 @@ struct sys_state_s
bool armed; /* IO armed */
bool arm_ok; /* FMU says OK to arm */
uint16_t servo_rate; /* output rate of servos in Hz */
/**
* Data from the remote control input(s)
@@ -92,7 +93,7 @@ struct sys_state_s
/**
* If true, we are using the FMU controls, else RC input if available.
*/
bool mixer_use_fmu;
bool mixer_manual_override;
/**
* If true, FMU input is available.
@@ -124,6 +125,16 @@ struct sys_state_s
* If true, the connection to FMU has been lost for more than a timeout interval
*/
bool fmu_lost;
/**
* If true, FMU is ready for autonomous position control. Only used for LED indication
*/
bool vector_flight_mode_ok;
/**
* If true, IO performs an on-board manual override with the RC channel values
*/
bool manual_override_ok;
};
extern struct sys_state_s system_state;
apps/px4io/safety.c
+9 -10
View File
@@ -63,15 +63,16 @@ static unsigned counter = 0;
/*
* Define the various LED flash sequences for each system state.
*/
#define LED_PATTERN_SAFE 0xffff // always on
#define LED_PATTERN_FMU_ARMED 0x4444 // slow blinking
#define LED_PATTERN_IO_ARMED 0x5555 // fast blinking
#define LED_PATTERN_IO_FMU_ARMED 0x5050 // long off then double blink
#define LED_PATTERN_SAFE 0xffff /**< always on */
#define LED_PATTERN_VECTOR_FLIGHT_MODE_OK 0x3000 /**< always on with short break */
#define LED_PATTERN_FMU_ARMED 0x4444 /**< slow blinking */
#define LED_PATTERN_IO_ARMED 0x5555 /**< fast blinking */
#define LED_PATTERN_IO_FMU_ARMED 0x5050 /**< long off then double blink */
static unsigned blink_counter = 0;
#define ARM_COUNTER_THRESHOLD 10
#define DISARM_COUNTER_THRESHOLD 2
#define DISARM_COUNTER_THRESHOLD 4
static bool safety_button_pressed;
@@ -101,10 +102,6 @@ safety_check_button(void *arg)
*/
safety_button_pressed = BUTTON_SAFETY;
if(safety_button_pressed) {
//printf("Pressed, Arm counter: %d, Disarm counter: %d\n", arm_counter, disarm_counter);
}
/* Keep pressed for a while to arm */
if (safety_button_pressed && !system_state.armed) {
if (counter < ARM_COUNTER_THRESHOLD) {
@@ -139,6 +136,8 @@ safety_check_button(void *arg)
}
} else if (system_state.arm_ok) {
pattern = LED_PATTERN_FMU_ARMED;
} else if (system_state.vector_flight_mode_ok) {
pattern = LED_PATTERN_VECTOR_FLIGHT_MODE_OK;
}
/* Turn the LED on if we have a 1 at the current bit position */
@@ -165,7 +164,7 @@ failsafe_blink(void *arg)
static bool failsafe = false;
/* blink the failsafe LED if we don't have FMU input */
if (!system_state.mixer_use_fmu) {
if (!system_state.mixer_fmu_available) {
failsafe = !failsafe;
} else {
failsafe = false;