diff options
Diffstat (limited to 'apps/px4io')
| -rw-r--r-- | apps/px4io/comms.c | 58 | ||||
| -rw-r--r-- | apps/px4io/controls.c | 43 | ||||
| -rw-r--r-- | apps/px4io/mixer.cpp | 71 | ||||
| -rw-r--r-- | apps/px4io/protocol.h | 10 | ||||
| -rw-r--r-- | apps/px4io/px4io.c | 2 | ||||
| -rw-r--r-- | apps/px4io/px4io.h | 57 | ||||
| -rw-r--r-- | apps/px4io/safety.c | 42 | ||||
| -rw-r--r-- | apps/px4io/sbus.c | 56 |
8 files changed, 241 insertions, 98 deletions
diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c index 843d0978a..c208db3ad 100644 --- a/apps/px4io/comms.c +++ b/apps/px4io/comms.c @@ -52,6 +52,7 @@ #include <nuttx/clock.h> #include <drivers/drv_hrt.h> +#include <drivers/drv_pwm_output.h> #include <systemlib/hx_stream.h> #include <systemlib/perf_counter.h> @@ -137,8 +138,9 @@ 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; @@ -184,16 +186,54 @@ comms_handle_command(const void *buffer, size_t length) system_state.armed = false; system_state.arm_ok = cmd->arm_ok; - system_state.mixer_use_fmu = true; - system_state.fmu_data_received = true; + 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_time = hrt_absolute_time(); - /* handle changes signalled by FMU */ -// if (!system_state.arm_ok && system_state.armed) -// system_state.armed = false; + /* set PWM update rate if changed (after limiting) */ + uint16_t new_servo_rate = cmd->servo_rate; - /* XXX do relay changes here */ - for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++) - system_state.relays[i] = cmd->relay_state[i]; + /* 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; + } + + /* + * update servo values immediately. + * the updates are done in addition also + * in the mainloop, since this function will only + * update with a connected FMU. + */ + mixer_tick(); + + /* handle relay state changes here */ + for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++) { + if (system_state.relays[i] != cmd->relay_state[i]) { + switch (i) { + case 0: + POWER_ACC1(cmd->relay_state[i]); + break; + case 1: + POWER_ACC2(cmd->relay_state[i]); + break; + case 2: + POWER_RELAY1(cmd->relay_state[i]); + break; + case 3: + POWER_RELAY2(cmd->relay_state[i]); + break; + } + } + } irqrestore(flags); } diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c index 43e811ab0..564687b58 100644 --- a/apps/px4io/controls.c +++ b/apps/px4io/controls.c @@ -60,6 +60,10 @@ #define DEBUG #include "px4io.h" +#define RC_FAILSAFE_TIMEOUT 2000000 /**< two seconds failsafe timeout */ +#define RC_CHANNEL_HIGH_THRESH 1700 +#define RC_CHANNEL_LOW_THRESH 1300 + static void ppm_input(void); void @@ -88,11 +92,23 @@ controls_main(void) */ bool locked = false; + /* + * Store RC channel count to detect switch to RC loss sooner + * than just by timeout + */ + unsigned rc_channels = system_state.rc_channels; + + /* + * Track if any input got an update in this round + */ + bool rc_updated; + if (fds[0].revents & POLLIN) locked |= dsm_input(); if (fds[1].revents & POLLIN) - locked |= sbus_input(); + locked |= sbus_input(fds[1].fd, PX4IO_INPUT_CHANNELS, &system_state.rc_channel_data, + &system_state.rc_channels, &system_state.rc_channels_timestamp, &rc_updated); /* * If we don't have lock from one of the serial receivers, @@ -107,6 +123,15 @@ controls_main(void) if (!locked) ppm_input(); + /* check for manual override status */ + if (system_state.rc_channel_data[4] > RC_CHANNEL_HIGH_THRESH) { + /* force manual input override */ + system_state.mixer_manual_override = true; + } else { + /* override not engaged, use FMU */ + system_state.mixer_manual_override = false; + } + /* * If we haven't seen any new control data in 200ms, assume we * have lost input and tell FMU. @@ -115,14 +140,20 @@ controls_main(void) /* set the number of channels to zero - no inputs */ system_state.rc_channels = 0; - - /* trigger an immediate report to the FMU */ - system_state.fmu_report_due = true; + rc_updated = true; } - /* XXX do bypass mode, etc. here */ + /* + * If there was a RC update OR the RC signal status (lost / present) has + * just changed, request an update immediately. + */ + system_state.fmu_report_due |= rc_updated; - /* do PWM output updates */ + /* + * PWM output updates are performed in addition on each comm update. + * the updates here are required to ensure operation if FMU is not started + * or stopped responding. + */ mixer_tick(); } } diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp index d21b3a898..2acd60ce3 100644 --- a/apps/px4io/mixer.cpp +++ b/apps/px4io/mixer.cpp @@ -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" { @@ -57,10 +61,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; @@ -81,37 +84,47 @@ 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_CONTROL_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_CONTROL_CHANNELS; + control_values = &system_state.fmu_channel_data[0]; + + } else if (system_state.rc_channels > 0) { + /* when override is on or the fmu is not available, but RC is present */ + 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) */ - } 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]; + // XXX builtin failsafe would activate here + control_count = 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_CONTROL_CHANNELS; + control_values = &system_state.fmu_channel_data[0]; + /* we better shut everything off */ + } else { + control_count = 0; + } } /* @@ -145,7 +158,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) { @@ -229,4 +244,4 @@ mixer_handle_text(const void *buffer, size_t length) break; } -}
\ No newline at end of file +} diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h index afbf09cd3..c53b94eb0 100644 --- a/apps/px4io/protocol.h +++ b/apps/px4io/protocol.h @@ -54,9 +54,13 @@ struct px4io_command { uint16_t f2i_magic; #define F2I_MAGIC 0x636d - uint16_t output_control[PX4IO_CONTROL_CHANNELS]; - bool relay_state[PX4IO_RELAY_CHANNELS]; - bool arm_ok; + uint16_t servo_command[PX4IO_OUTPUT_CHANNELS]; /**< servo output channels */ + uint16_t servo_rate; + uint16_t output_control[PX4IO_CONTROL_CHANNELS]; /**< 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 */ }; /** diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c index 74362617d..bea9d59bc 100644 --- a/apps/px4io/px4io.c +++ b/apps/px4io/px4io.c @@ -66,6 +66,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(); diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h index 7257d6522..fef0a9ba4 100644 --- a/apps/px4io/px4io.h +++ b/apps/px4io/px4io.h @@ -70,49 +70,75 @@ 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) */ unsigned rc_channels; uint16_t rc_channel_data[PX4IO_INPUT_CHANNELS]; uint64_t rc_channels_timestamp; - /* + /** * Control signals from FMU. */ uint16_t fmu_channel_data[PX4IO_CONTROL_CHANNELS]; - /* + /** * Mixed servo outputs */ uint16_t servos[IO_SERVO_COUNT]; - /* + /** * Relay controls */ bool relays[PX4IO_RELAY_CHANNELS]; - /* - * If true, we are using the FMU controls. + /** + * 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. + */ + bool mixer_fmu_available; + + /** * If true, state that should be reported to FMU has been updated. */ 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 * in the config packet. */ uint8_t serial_rx_mode; + + /** + * If true, the RC signal has been lost for more than a timeout interval + */ + bool rc_lost; + + /** + * 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; @@ -140,8 +166,8 @@ extern volatile int timers[TIMER_NUM_TIMERS]; #define LED_SAFETY(_s) stm32_gpiowrite(GPIO_LED3, !(_s)) #define POWER_SERVO(_s) stm32_gpiowrite(GPIO_SERVO_PWR_EN, (_s)) -#define POWER_ACC1(_s) stm32_gpiowrite(GPIO_SERVO_ACC1_EN, (_s)) -#define POWER_ACC2(_s) stm32_gpiowrite(GPIO_SERVO_ACC2_EN, (_s)) +#define POWER_ACC1(_s) stm32_gpiowrite(GPIO_ACC1_PWR_EN, (_s)) +#define POWER_ACC2(_s) stm32_gpiowrite(GPIO_ACC2_PWR_EN, (_s)) #define POWER_RELAY1(_s) stm32_gpiowrite(GPIO_RELAY1_EN, (_s)) #define POWER_RELAY2(_s) stm32_gpiowrite(GPIO_RELAY2_EN, (_s)) @@ -172,7 +198,8 @@ extern void controls_main(void); extern int dsm_init(const char *device); extern bool dsm_input(void); extern int sbus_init(const char *device); -extern bool sbus_input(void); +extern bool sbus_input(int fd, unsigned max_channels, uint16_t *channel_data, unsigned *channel_count, + uint64_t *receive_time, bool *updated); /* * Assertion codes diff --git a/apps/px4io/safety.c b/apps/px4io/safety.c index 93596ca75..3e02b717d 100644 --- a/apps/px4io/safety.c +++ b/apps/px4io/safety.c @@ -58,20 +58,27 @@ static struct hrt_call failsafe_call; * Count the number of times in a row that we see the arming button * held down. */ -static unsigned counter; +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 0xFFFE /**< 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; +/* + * IMPORTANT: The arming state machine critically + * depends on using the same threshold + * for arming and disarming. Since disarming + * is quite deadly for the system, a similar + * length can be justified. + */ #define ARM_COUNTER_THRESHOLD 10 -#define DISARM_COUNTER_THRESHOLD 2 static bool safety_button_pressed; @@ -101,12 +108,16 @@ 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 */ + /* + * Keep pressed for a while to arm. + * + * Note that the counting sequence has to be same length + * for arming / disarming in order to end up as proper + * state machine, keep ARM_COUNTER_THRESHOLD the same + * length in all cases of the if/else struct below. + */ if (safety_button_pressed && !system_state.armed) { + if (counter < ARM_COUNTER_THRESHOLD) { counter++; @@ -120,10 +131,11 @@ safety_check_button(void *arg) /* Disarm quickly */ } else if (safety_button_pressed && system_state.armed) { - if (counter < DISARM_COUNTER_THRESHOLD) { + + if (counter < ARM_COUNTER_THRESHOLD) { counter++; - } else if (counter == DISARM_COUNTER_THRESHOLD) { + } else if (counter == ARM_COUNTER_THRESHOLD) { /* change to disarmed state and notify the FMU */ system_state.armed = false; counter++; @@ -147,6 +159,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 */ @@ -173,7 +187,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 { diff --git a/apps/px4io/sbus.c b/apps/px4io/sbus.c index 9679f657b..f7a74cd12 100644 --- a/apps/px4io/sbus.c +++ b/apps/px4io/sbus.c @@ -49,14 +49,11 @@ #define DEBUG #include "px4io.h" -#include "protocol.h" #include "debug.h" #define SBUS_FRAME_SIZE 25 #define SBUS_INPUT_CHANNELS 16 -static int sbus_fd = -1; - static hrt_abstime last_rx_time; static hrt_abstime last_frame_time; @@ -66,11 +63,14 @@ static unsigned partial_frame_count; unsigned sbus_frame_drops; -static void sbus_decode(hrt_abstime frame_time); +static int sbus_decode(hrt_abstime frame_time, unsigned max_channels, + uint16_t *channel_data, unsigned *channel_count, uint64_t *receive_time); int sbus_init(const char *device) { + static int sbus_fd = -1; + if (sbus_fd < 0) sbus_fd = open(device, O_RDONLY); @@ -97,7 +97,8 @@ sbus_init(const char *device) } bool -sbus_input(void) +sbus_input(int fd, unsigned max_channels, uint16_t *channel_data, unsigned *channel_count, + uint64_t *receive_time, bool *updated) { ssize_t ret; hrt_abstime now; @@ -130,7 +131,7 @@ sbus_input(void) * Fetch bytes, but no more than we would need to complete * the current frame. */ - ret = read(sbus_fd, &frame[partial_frame_count], SBUS_FRAME_SIZE - partial_frame_count); + ret = read(fd, &frame[partial_frame_count], SBUS_FRAME_SIZE - partial_frame_count); /* if the read failed for any reason, just give up here */ if (ret < 1) @@ -151,9 +152,9 @@ sbus_input(void) /* * Great, it looks like we might have a frame. Go ahead and - * decode it. + * decode it, report if the receiver got something. */ - sbus_decode(now); + *updated = (sbus_decode(now, max_channels, channel_data, channel_count, receive_time) == OK); partial_frame_count = 0; out: @@ -199,25 +200,32 @@ static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = { /* 15 */ { {20, 5, 0x07, 0}, {21, 0, 0xff, 3}, { 0, 0, 0x00, 0} } }; -static void -sbus_decode(hrt_abstime frame_time) +static int +sbus_decode(hrt_abstime frame_time, unsigned max_channels, uint16_t *channel_data, unsigned *channel_count, uint64_t *receive_time) { /* check frame boundary markers to avoid out-of-sync cases */ if ((frame[0] != 0x0f) || (frame[24] != 0x00)) { sbus_frame_drops++; - return; + return 1; } - /* if the failsafe bit is set, we consider the frame invalid */ - if (frame[23] & (1 << 4)) { - return; + /* if the failsafe or connection lost bit is set, we consider the frame invalid */ + if ((frame[23] & (1 << 2)) && /* signal lost */ + (frame[23] & (1 << 3))) { /* failsafe */ + + /* actively announce signal loss */ + *channel_count = 0; + + return 1; } + /* decode failsafe and RC status */ + /* we have received something we think is a frame */ last_frame_time = frame_time; - unsigned chancount = (PX4IO_INPUT_CHANNELS > SBUS_INPUT_CHANNELS) ? - SBUS_INPUT_CHANNELS : PX4IO_INPUT_CHANNELS; + unsigned chancount = (max_channels > SBUS_INPUT_CHANNELS) ? + SBUS_INPUT_CHANNELS : max_channels; /* use the decoder matrix to extract channel data */ for (unsigned channel = 0; channel < chancount; channel++) { @@ -240,17 +248,19 @@ sbus_decode(hrt_abstime frame_time) system_state.rc_channel_data[channel] = (value / 2) + 998; } - if (PX4IO_INPUT_CHANNELS >= 18) { - chancount = 18; - /* XXX decode the two switch channels */ + /* decode switch channels if data fields are wide enough */ + if (chancount > 17) { + /* channel 17 (index 16) */ + system_state.rc_channel_data[16] = (frame[23] & (1 << 0)) * 1000 + 998; + /* channel 18 (index 17) */ + system_state.rc_channel_data[17] = (frame[23] & (1 << 1)) * 1000 + 998; } /* note the number of channels decoded */ - system_state.rc_channels = chancount; + *channel_count = chancount; /* and note that we have received data from the R/C controller */ - system_state.rc_channels_timestamp = frame_time; + *receive_time = frame_time; - /* trigger an immediate report to the FMU */ - system_state.fmu_report_due = true; + return 0; } |