diff options
Diffstat (limited to 'src/modules/px4iofirmware')
-rw-r--r-- | src/modules/px4iofirmware/i2c.c | 6 | ||||
-rw-r--r-- | src/modules/px4iofirmware/mixer.cpp | 139 | ||||
-rw-r--r-- | src/modules/px4iofirmware/module.mk | 1 | ||||
-rw-r--r-- | src/modules/px4iofirmware/protocol.h | 4 | ||||
-rw-r--r-- | src/modules/px4iofirmware/px4io.c | 16 | ||||
-rw-r--r-- | src/modules/px4iofirmware/px4io.h | 9 | ||||
-rw-r--r-- | src/modules/px4iofirmware/registers.c | 111 | ||||
-rw-r--r-- | src/modules/px4iofirmware/serial.c | 20 |
8 files changed, 114 insertions, 192 deletions
diff --git a/src/modules/px4iofirmware/i2c.c b/src/modules/px4iofirmware/i2c.c index 10aeb5c9f..79b6546b3 100644 --- a/src/modules/px4iofirmware/i2c.c +++ b/src/modules/px4iofirmware/i2c.c @@ -149,12 +149,6 @@ interface_init(void) #endif } -void -interface_tick() -{ -} - - /* reset the I2C bus used to recover from lockups diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 30aff7d20..05897b4ce 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -47,6 +47,7 @@ #include <drivers/drv_pwm_output.h> #include <drivers/drv_hrt.h> +#include <systemlib/pwm_limit/pwm_limit.h> #include <systemlib/mixer/mixer.h> extern "C" { @@ -59,12 +60,6 @@ extern "C" { */ #define FMU_INPUT_DROP_LIMIT_US 200000 -/* - * Time that the ESCs need to initialize - */ - #define ESC_INIT_TIME_US 1000000 - #define ESC_RAMP_TIME_US 2000000 - /* XXX need to move the RC_CHANNEL_FUNCTION out of rc_channels.h and into systemlib */ #define ROLL 0 #define PITCH 1 @@ -76,15 +71,6 @@ extern "C" { static bool mixer_servos_armed = false; static bool should_arm = false; static bool should_always_enable_pwm = false; -static uint64_t esc_init_time; - -enum esc_state_e { - ESC_OFF, - ESC_INIT, - ESC_RAMP, - ESC_ON -}; -static esc_state_e esc_state; /* selected control values and count for mixing */ enum mixer_source { @@ -166,6 +152,30 @@ mixer_tick(void) } /* + * Decide whether the servos should be armed right now. + * + * We must be armed, and we must have a PWM source; either raw from + * FMU or from the mixer. + * + * XXX correct behaviour for failsafe may require an additional case + * here. + */ + should_arm = ( + /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) + /* and IO is armed */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) + /* and FMU is armed */ && ( + ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) + /* and there is valid input via or mixer */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) ) + /* or direct PWM is set */ || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) + /* or failsafe was set manually */ || (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) + ) + ); + + should_always_enable_pwm = (r_setup_arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) + && (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) + && (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK); + + /* * Run the mixers. */ if (source == MIX_FAILSAFE) { @@ -184,107 +194,15 @@ mixer_tick(void) float outputs[PX4IO_SERVO_COUNT]; unsigned mixed; - uint16_t ramp_promille; - - /* update esc init state, but only if we are truely armed and not just PWM enabled */ - if (mixer_servos_armed && should_arm) { - - switch (esc_state) { - - /* after arming, some ESCs need an initalization period, count the time from here */ - case ESC_OFF: - esc_init_time = hrt_absolute_time(); - esc_state = ESC_INIT; - break; - - /* after waiting long enough for the ESC initialization, we can start with the ramp to start the ESCs */ - case ESC_INIT: - if (hrt_elapsed_time(&esc_init_time) > ESC_INIT_TIME_US) { - esc_state = ESC_RAMP; - } - break; - - /* then ramp until the min speed is reached */ - case ESC_RAMP: - if (hrt_elapsed_time(&esc_init_time) > (ESC_INIT_TIME_US + ESC_RAMP_TIME_US)) { - esc_state = ESC_ON; - } - break; - - case ESC_ON: - default: - - break; - } - } else { - esc_state = ESC_OFF; - } - - /* do the calculations during the ramp for all at once */ - if (esc_state == ESC_RAMP) { - ramp_promille = (1000*(hrt_elapsed_time(&esc_init_time)-ESC_INIT_TIME_US))/ESC_RAMP_TIME_US; - } - - /* mix */ mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT); - /* scale to PWM and update the servo outputs as required */ - for (unsigned i = 0; i < mixed; i++) { - - /* save actuator values for FMU readback */ - r_page_actuators[i] = FLOAT_TO_REG(outputs[i]); - - switch (esc_state) { - case ESC_INIT: - r_page_servos[i] = (outputs[i] * 600 + 1500); - break; - - case ESC_RAMP: - r_page_servos[i] = (outputs[i] - * (ramp_promille*r_page_servo_control_max[i] + (1000-ramp_promille)*2100 - ramp_promille*r_page_servo_control_min[i] - (1000-ramp_promille)*900)/2/1000 - + (ramp_promille*r_page_servo_control_max[i] + (1000-ramp_promille)*2100 + ramp_promille*r_page_servo_control_min[i] + (1000-ramp_promille)*900)/2/1000); - break; - - case ESC_ON: - r_page_servos[i] = (outputs[i] - * (r_page_servo_control_max[i] - r_page_servo_control_min[i])/2 - + (r_page_servo_control_max[i] + r_page_servo_control_min[i])/2); - break; + pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); - case ESC_OFF: - default: - break; - } - } for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) r_page_servos[i] = 0; } - /* - * Decide whether the servos should be armed right now. - * - * We must be armed, and we must have a PWM source; either raw from - * FMU or from the mixer. - * - * XXX correct behaviour for failsafe may require an additional case - * here. - */ - should_arm = ( - /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) - /* and IO is armed */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) - /* and FMU is armed */ && ( - ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) - /* and there is valid input via or mixer */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) ) - /* or direct PWM is set */ || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) - /* or failsafe was set manually */ || (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) - ) - ); - - should_always_enable_pwm = (r_setup_arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) - && (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) - && (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK); - if ((should_arm || should_always_enable_pwm) && !mixer_servos_armed) { /* need to arm, but not armed */ up_pwm_servo_arm(true); @@ -298,7 +216,6 @@ mixer_tick(void) mixer_servos_armed = false; r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED); isr_debug(5, "> PWM disabled"); - } if (mixer_servos_armed && should_arm) { @@ -307,9 +224,9 @@ mixer_tick(void) up_pwm_servo_set(i, r_page_servos[i]); } else if (mixer_servos_armed && should_always_enable_pwm) { - /* set the idle servo outputs. */ + /* set the disarmed servo outputs. */ for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) - up_pwm_servo_set(i, r_page_servo_idle[i]); + up_pwm_servo_set(i, r_page_servo_disarmed[i]); } } diff --git a/src/modules/px4iofirmware/module.mk b/src/modules/px4iofirmware/module.mk index 59f470a94..01869569f 100644 --- a/src/modules/px4iofirmware/module.mk +++ b/src/modules/px4iofirmware/module.mk @@ -14,6 +14,7 @@ SRCS = adc.c \ ../systemlib/mixer/mixer_group.cpp \ ../systemlib/mixer/mixer_multirotor.cpp \ ../systemlib/mixer/mixer_simple.cpp \ + ../systemlib/pwm_limit/pwm_limit.c ifeq ($(BOARD),px4io-v1) SRCS += i2c.c diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 0e2cd1689..5e5396782 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -220,8 +220,8 @@ enum { /* DSM bind states */ /* PWM maximum values for certain ESCs */ #define PX4IO_PAGE_CONTROL_MAX_PWM 107 /* 0..CONFIG_ACTUATOR_COUNT-1 */ -/* PWM idle values that are active, even when SAFETY_SAFE */ -#define PX4IO_PAGE_IDLE_PWM 108 /* 0..CONFIG_ACTUATOR_COUNT-1 */ +/* PWM disarmed values that are active, even when SAFETY_SAFE */ +#define PX4IO_PAGE_DISARMED_PWM 108 /* 0..CONFIG_ACTUATOR_COUNT-1 */ /** * As-needed mixer data upload. diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index e70b3fe88..ff9eecd74 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -50,6 +50,7 @@ #include <drivers/drv_hrt.h> #include <systemlib/perf_counter.h> +#include <systemlib/pwm_limit/pwm_limit.h> #include <stm32_uart.h> @@ -64,6 +65,8 @@ struct sys_state_s system_state; static struct hrt_call serial_dma_call; +pwm_limit_t pwm_limit; + /* * a set of debug buffers to allow us to send debug information from ISRs */ @@ -159,9 +162,6 @@ user_start(int argc, char *argv[]) /* start the FMU interface */ interface_init(); - /* add a performance counter for the interface */ - perf_counter_t interface_perf = perf_alloc(PC_ELAPSED, "interface"); - /* add a performance counter for mixing */ perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix"); @@ -174,6 +174,9 @@ user_start(int argc, char *argv[]) struct mallinfo minfo = mallinfo(); lowsyslog("MEM: free %u, largest %u\n", minfo.mxordblk, minfo.fordblks); + /* initialize PWM limit lib */ + pwm_limit_init(&pwm_limit); + #if 0 /* not enough memory, lock down */ if (minfo.mxordblk < 500) { @@ -203,11 +206,6 @@ user_start(int argc, char *argv[]) /* track the rate at which the loop is running */ perf_count(loop_perf); - /* kick the interface */ - perf_begin(interface_perf); - interface_tick(); - perf_end(interface_perf); - /* kick the mixer */ perf_begin(mixer_perf); mixer_tick(); @@ -218,6 +216,7 @@ user_start(int argc, char *argv[]) controls_tick(); perf_end(controls_perf); +#if 0 /* check for debug activity */ show_debug_messages(); @@ -234,6 +233,7 @@ user_start(int argc, char *argv[]) (unsigned)minfo.mxordblk); last_debug_time = hrt_absolute_time(); } +#endif } } diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 66c4ca906..4fea0288c 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -46,6 +46,8 @@ #include "protocol.h" +#include <systemlib/pwm_limit/pwm_limit.h> + /* * Constants and limits. */ @@ -80,7 +82,7 @@ extern uint16_t r_page_rc_input_config[]; /* PX4IO_PAGE_RC_INPUT_CONFIG */ extern uint16_t r_page_servo_failsafe[]; /* PX4IO_PAGE_FAILSAFE_PWM */ extern uint16_t r_page_servo_control_min[]; /* PX4IO_PAGE_CONTROL_MIN_PWM */ extern uint16_t r_page_servo_control_max[]; /* PX4IO_PAGE_CONTROL_MAX_PWM */ -extern uint16_t r_page_servo_idle[]; /* PX4IO_PAGE_IDLE_PWM */ +extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */ /* * Register aliases. @@ -123,6 +125,11 @@ struct sys_state_s { extern struct sys_state_s system_state; /* + * PWM limit structure + */ +extern pwm_limit_t pwm_limit; + +/* * GPIO handling. */ #define LED_BLUE(_s) stm32_gpiowrite(GPIO_LED1, !(_s)) diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 9d9ef7c6d..40597adf1 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -199,7 +199,7 @@ uint16_t r_page_rc_input_config[PX4IO_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRI * * Disable pulses as default. */ -uint16_t r_page_servo_failsafe[PX4IO_SERVO_COUNT] = { 0 }; +uint16_t r_page_servo_failsafe[PX4IO_SERVO_COUNT] = { 0, 0, 0, 0, 0, 0, 0, 0 }; /** * PAGE 106 @@ -207,7 +207,7 @@ uint16_t r_page_servo_failsafe[PX4IO_SERVO_COUNT] = { 0 }; * minimum PWM values when armed * */ -uint16_t r_page_servo_control_min[PX4IO_SERVO_COUNT] = { 900, 900, 900, 900, 900, 900, 900, 900 }; +uint16_t r_page_servo_control_min[PX4IO_SERVO_COUNT] = { PWM_MIN, PWM_MIN, PWM_MIN, PWM_MIN, PWM_MIN, PWM_MIN, PWM_MIN, PWM_MIN }; /** * PAGE 107 @@ -215,15 +215,15 @@ uint16_t r_page_servo_control_min[PX4IO_SERVO_COUNT] = { 900, 900, 900, 900, 90 * maximum PWM values when armed * */ -uint16_t r_page_servo_control_max[PX4IO_SERVO_COUNT] = { 2100, 2100, 2100, 2100, 2100, 2100, 2100, 2100 }; +uint16_t r_page_servo_control_max[PX4IO_SERVO_COUNT] = { PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX }; /** * PAGE 108 * - * idle PWM values for difficult ESCs + * disarmed PWM values for difficult ESCs * */ -uint16_t r_page_servo_idle[PX4IO_SERVO_COUNT] = { 900, 900, 900, 900, 900, 900, 900, 900 }; +uint16_t r_page_servo_disarmed[PX4IO_SERVO_COUNT] = { 0, 0, 0, 0, 0, 0, 0, 0 }; int registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) @@ -276,8 +276,15 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num /* copy channel data */ while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) { - /* XXX range-check value? */ - r_page_servo_failsafe[offset] = *values; + if (*values == 0) { + /* ignore 0 */ + } else if (*values < PWM_MIN) { + r_page_servo_failsafe[offset] = PWM_MIN; + } else if (*values > PWM_MAX) { + r_page_servo_failsafe[offset] = PWM_MAX; + } else { + r_page_servo_failsafe[offset] = *values; + } /* flag the failsafe values as custom */ r_setup_arming |= PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM; @@ -293,16 +300,15 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num /* copy channel data */ while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) { - if (*values == 0) - /* set to default */ - r_page_servo_control_min[offset] = 900; - - else if (*values > 1200) - r_page_servo_control_min[offset] = 1200; - else if (*values < 900) - r_page_servo_control_min[offset] = 900; - else + if (*values == 0) { + /* ignore 0 */ + } else if (*values > PWM_HIGHEST_MIN) { + r_page_servo_control_min[offset] = PWM_HIGHEST_MIN; + } else if (*values < PWM_MIN) { + r_page_servo_control_min[offset] = PWM_MIN; + } else { r_page_servo_control_min[offset] = *values; + } offset++; num_values--; @@ -315,16 +321,15 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num /* copy channel data */ while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) { - if (*values == 0) - /* set to default */ - r_page_servo_control_max[offset] = 2100; - - else if (*values > 2100) - r_page_servo_control_max[offset] = 2100; - else if (*values < 1800) - r_page_servo_control_max[offset] = 1800; - else + if (*values == 0) { + /* ignore 0 */ + } else if (*values > PWM_MAX) { + r_page_servo_control_max[offset] = PWM_MAX; + } else if (*values < PWM_LOWEST_MAX) { + r_page_servo_control_max[offset] = PWM_LOWEST_MAX; + } else { r_page_servo_control_max[offset] = *values; + } offset++; num_values--; @@ -332,28 +337,40 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num } break; - case PX4IO_PAGE_IDLE_PWM: - - /* copy channel data */ - while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) { - - if (*values == 0) - /* set to default */ - r_page_servo_idle[offset] = 0; - - else if (*values < 900) - r_page_servo_idle[offset] = 900; - else if (*values > 2100) - r_page_servo_idle[offset] = 2100; - else - r_page_servo_idle[offset] = *values; + case PX4IO_PAGE_DISARMED_PWM: + { + /* flag for all outputs */ + bool all_disarmed_off = true; + + /* copy channel data */ + while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) { + + if (*values == 0) { + /* 0 means disabling always PWM */ + r_page_servo_disarmed[offset] = 0; + } else if (*values < PWM_MIN) { + r_page_servo_disarmed[offset] = PWM_MIN; + all_disarmed_off = false; + } else if (*values > PWM_MAX) { + r_page_servo_disarmed[offset] = PWM_MAX; + all_disarmed_off = false; + } else { + r_page_servo_disarmed[offset] = *values; + all_disarmed_off = false; + } - /* flag the failsafe values as custom */ - r_setup_arming |= PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE; + offset++; + num_values--; + values++; + } - offset++; - num_values--; - values++; + if (all_disarmed_off) { + /* disable PWM output if disarmed */ + r_setup_arming &= ~(PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE); + } else { + /* enable PWM output always */ + r_setup_arming |= PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE; + } } break; @@ -767,8 +784,8 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val case PX4IO_PAGE_CONTROL_MAX_PWM: SELECT_PAGE(r_page_servo_control_max); break; - case PX4IO_PAGE_IDLE_PWM: - SELECT_PAGE(r_page_servo_idle); + case PX4IO_PAGE_DISARMED_PWM: + SELECT_PAGE(r_page_servo_disarmed); break; default: diff --git a/src/modules/px4iofirmware/serial.c b/src/modules/px4iofirmware/serial.c index 94d7407df..e9adc8cd6 100644 --- a/src/modules/px4iofirmware/serial.c +++ b/src/modules/px4iofirmware/serial.c @@ -74,9 +74,6 @@ static DMA_HANDLE rx_dma; static int serial_interrupt(int irq, void *context); static void dma_reset(void); -/* if we spend this many ticks idle, reset the DMA */ -static unsigned idle_ticks; - static struct IOPacket dma_packet; /* serial register accessors */ @@ -150,16 +147,6 @@ interface_init(void) debug("serial init"); } -void -interface_tick() -{ - /* XXX look for stuck/damaged DMA and reset? */ - if (idle_ticks++ > 100) { - dma_reset(); - idle_ticks = 0; - } -} - static void rx_handle_packet(void) { @@ -230,9 +217,6 @@ rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) /* disable UART DMA */ rCR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR); - /* reset the idle counter */ - idle_ticks = 0; - /* handle the received packet */ rx_handle_packet(); @@ -308,6 +292,7 @@ serial_interrupt(int irq, void *context) /* it was too short - possibly truncated */ perf_count(pc_badidle); + dma_reset(); return 0; } @@ -343,7 +328,8 @@ dma_reset(void) sizeof(dma_packet), DMA_CCR_MINC | DMA_CCR_PSIZE_8BITS | - DMA_CCR_MSIZE_8BITS); + DMA_CCR_MSIZE_8BITS | + DMA_CCR_PRIVERYHI); /* start receive DMA ready for the next packet */ stm32_dmastart(rx_dma, rx_dma_callback, NULL, false); |