From 948ff80b8062d438b843c6a9fbb47599881600a3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Oct 2015 13:46:32 +0200 Subject: [PATCH] IO firmware: Fix code style --- src/modules/px4iofirmware/adc.c | 9 +- src/modules/px4iofirmware/dsm.c | 50 ++-- src/modules/px4iofirmware/i2c.c | 50 ++-- src/modules/px4iofirmware/mixer.cpp | 99 ++++---- src/modules/px4iofirmware/protocol.h | 18 +- src/modules/px4iofirmware/px4io.c | 45 ++-- src/modules/px4iofirmware/registers.c | 331 +++++++++++++++----------- src/modules/px4iofirmware/safety.c | 4 +- src/modules/px4iofirmware/serial.c | 40 +++- 9 files changed, 395 insertions(+), 251 deletions(-) diff --git a/src/modules/px4iofirmware/adc.c b/src/modules/px4iofirmware/adc.c index d3db26c233..1688d53048 100644 --- a/src/modules/px4iofirmware/adc.c +++ b/src/modules/px4iofirmware/adc.c @@ -96,19 +96,22 @@ adc_init(void) rCR2 |= ADC_CR2_RSTCAL; up_udelay(1); - if (rCR2 & ADC_CR2_RSTCAL) + if (rCR2 & ADC_CR2_RSTCAL) { return -1; + } rCR2 |= ADC_CR2_CAL; up_udelay(100); - if (rCR2 & ADC_CR2_CAL) + if (rCR2 & ADC_CR2_CAL) { return -1; + } + #endif /* * Configure sampling time. - * + * * For electrical protection reasons, we want to be able to have * 10K in series with ADC inputs that leave the board. At 12MHz this * means we need 28.5 cycles of sampling time (per table 43 in the diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c index b0e96b1f03..fc6fbf82b8 100644 --- a/src/modules/px4iofirmware/dsm.c +++ b/src/modules/px4iofirmware/dsm.c @@ -90,8 +90,9 @@ static bool dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value) { - if (raw == 0xffff) + if (raw == 0xffff) { return false; + } *channel = (raw >> shift) & 0xf; @@ -132,18 +133,21 @@ dsm_guess_format(bool reset) unsigned channel, value; /* if the channel decodes, remember the assigned number */ - if (dsm_decode_channel(raw, 10, &channel, &value) && (channel < 31)) + if (dsm_decode_channel(raw, 10, &channel, &value) && (channel < 31)) { cs10 |= (1 << channel); + } - if (dsm_decode_channel(raw, 11, &channel, &value) && (channel < 31)) + if (dsm_decode_channel(raw, 11, &channel, &value) && (channel < 31)) { cs11 |= (1 << channel); + } /* XXX if we cared, we could look for the phase bit here to decide 1 vs. 2-dsm_frame format */ } /* wait until we have seen plenty of frames - 5 should normally be enough */ - if (samples++ < 5) + if (samples++ < 5) { return; + } /* * Iterate the set of sensible sniffed channel sets and see whether @@ -170,11 +174,13 @@ dsm_guess_format(bool reset) for (unsigned i = 0; i < (sizeof(masks) / sizeof(masks[0])); i++) { - if (cs10 == masks[i]) + if (cs10 == masks[i]) { votes10++; + } - if (cs11 == masks[i]) + if (cs11 == masks[i]) { votes11++; + } } if ((votes11 == 1) && (votes10 == 0)) { @@ -210,8 +216,9 @@ dsm_init(const char *device) POWER_SPEKTRUM(true); #endif - if (dsm_fd < 0) + if (dsm_fd < 0) { dsm_fd = open(device, O_RDONLY | O_NONBLOCK); + } if (dsm_fd >= 0) { @@ -251,13 +258,14 @@ void dsm_bind(uint16_t cmd, int pulses) { #if !defined(CONFIG_ARCH_BOARD_PX4IO_V1) && !defined(CONFIG_ARCH_BOARD_PX4IO_V2) - #warning DSM BIND NOT IMPLEMENTED ON UNKNOWN PLATFORM +#warning DSM BIND NOT IMPLEMENTED ON UNKNOWN PLATFORM #else const uint32_t usart1RxAsOutp = GPIO_OUTPUT | GPIO_CNF_OUTPP | GPIO_MODE_50MHz | GPIO_OUTPUT_SET | GPIO_PORTA | GPIO_PIN10; - if (dsm_fd < 0) + if (dsm_fd < 0) { return; + } switch (cmd) { @@ -297,6 +305,7 @@ dsm_bind(uint16_t cmd, int pulses) up_udelay(120); stm32_gpiowrite(usart1RxAsOutp, true); } + break; case dsm_bind_reinit_uart: @@ -306,6 +315,7 @@ dsm_bind(uint16_t cmd, int pulses) break; } + #endif } @@ -329,8 +339,9 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) * If we have lost signal for at least a second, reset the * format guessing heuristic. */ - if (((frame_time - dsm_last_frame_time) > 1000000) && (dsm_channel_shift != 0)) + if (((frame_time - dsm_last_frame_time) > 1000000) && (dsm_channel_shift != 0)) { dsm_guess_format(true); + } /* we have received something we think is a dsm_frame */ dsm_last_frame_time = frame_time; @@ -358,20 +369,24 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) uint16_t raw = (dp[0] << 8) | dp[1]; unsigned channel, value; - if (!dsm_decode_channel(raw, dsm_channel_shift, &channel, &value)) + if (!dsm_decode_channel(raw, dsm_channel_shift, &channel, &value)) { continue; + } /* ignore channels out of range */ - if (channel >= PX4IO_RC_INPUT_CHANNELS) + if (channel >= PX4IO_RC_INPUT_CHANNELS) { continue; + } /* update the decoded channel count */ - if (channel >= *num_values) + if (channel >= *num_values) { *num_values = channel + 1; + } /* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding. */ - if (dsm_channel_shift == 10) + if (dsm_channel_shift == 10) { value *= 2; + } /* * Spektrum scaling is special. There are these basic considerations @@ -421,8 +436,9 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) * lines, so if we get a channel count of 13, we'll return 12 (the last * data index that is stable). */ - if (*num_values == 13) + if (*num_values == 13) { *num_values = 12; + } if (dsm_channel_shift == 11) { /* Set the 11-bit data indicator */ @@ -482,6 +498,7 @@ dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **by /* if the read failed for any reason, just give up here */ if (ret < 1) { return false; + } else { *n_bytes = ret; *bytes = &dsm_frame[dsm_partial_frame_count]; @@ -497,8 +514,9 @@ dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **by /* * If we don't have a full dsm frame, return */ - if (dsm_partial_frame_count < DSM_FRAME_SIZE) + if (dsm_partial_frame_count < DSM_FRAME_SIZE) { return false; + } /* * Great, it looks like we might have a dsm frame. Go ahead and diff --git a/src/modules/px4iofirmware/i2c.c b/src/modules/px4iofirmware/i2c.c index 6d1d1fc2dd..74692a2675 100644 --- a/src/modules/px4iofirmware/i2c.c +++ b/src/modules/px4iofirmware/i2c.c @@ -120,7 +120,7 @@ interface_init(void) rCR1 = 0; /* set for DMA operation */ - rCR2 |= I2C_CR2_ITEVFEN |I2C_CR2_ITERREN | I2C_CR2_DMAEN; + rCR2 |= I2C_CR2_ITEVFEN | I2C_CR2_ITERREN | I2C_CR2_DMAEN; /* set the frequency value in CR2 */ rCR2 &= ~I2C_CR2_FREQ_MASK; @@ -128,8 +128,11 @@ interface_init(void) /* set divisor and risetime for fast mode */ uint16_t result = STM32_PCLK1_FREQUENCY / (400000 * 25); - if (result < 1) + + if (result < 1) { result = 1; + } + result = 3; rCCR &= ~I2C_CCR_CCR_MASK; rCCR |= I2C_CCR_DUTY | I2C_CCR_FS | result; @@ -163,7 +166,7 @@ i2c_reset(void) rCR1 = 0; /* set for DMA operation */ - rCR2 |= I2C_CR2_ITEVFEN |I2C_CR2_ITERREN | I2C_CR2_DMAEN; + rCR2 |= I2C_CR2_ITEVFEN | I2C_CR2_ITERREN | I2C_CR2_DMAEN; /* set the frequency value in CR2 */ rCR2 &= ~I2C_CR2_FREQ_MASK; @@ -171,8 +174,11 @@ i2c_reset(void) /* set divisor and risetime for fast mode */ uint16_t result = STM32_PCLK1_FREQUENCY / (400000 * 25); - if (result < 1) + + if (result < 1) { result = 1; + } + result = 3; rCCR &= ~I2C_CCR_CCR_MASK; rCCR |= I2C_CCR_DUTY | I2C_CCR_FS | result; @@ -203,13 +209,16 @@ i2c_interrupt(int irq, FAR void *context) case DIR_TX: i2c_tx_complete(); break; + case DIR_RX: i2c_rx_complete(); break; + default: /* not currently transferring - must be a new txn */ break; } + direction = DIR_NONE; } @@ -232,8 +241,9 @@ i2c_interrupt(int irq, FAR void *context) } /* clear any errors that might need it (this handles AF as well */ - if (sr1 & I2C_SR1_ERRORMASK) + if (sr1 & I2C_SR1_ERRORMASK) { rSR1 = 0; + } return 0; } @@ -248,13 +258,13 @@ i2c_rx_setup(void) */ rx_len = 0; stm32_dmasetup(rx_dma, (uintptr_t)&rDR, (uintptr_t)&rx_buf[0], sizeof(rx_buf), - DMA_CCR_CIRC | - DMA_CCR_MINC | - DMA_CCR_PSIZE_32BITS | - DMA_CCR_MSIZE_8BITS | - DMA_CCR_PRIMED); + DMA_CCR_CIRC | + DMA_CCR_MINC | + DMA_CCR_PSIZE_32BITS | + DMA_CCR_MSIZE_8BITS | + DMA_CCR_PRIMED); - stm32_dmastart(rx_dma, NULL, NULL, false); + stm32_dmastart(rx_dma, NULL, NULL, false); } static void @@ -269,8 +279,10 @@ i2c_rx_complete(void) /* work out how many registers are being written */ unsigned count = (rx_len - 2) / 2; + if (count > 0) { registers_set(selected_page, selected_offset, (const uint16_t *)&rx_buf[2], count); + } else { /* no registers written, must be an address cycle */ uint16_t *regs; @@ -278,9 +290,11 @@ i2c_rx_complete(void) /* work out which registers are being addressed */ int ret = registers_get(selected_page, selected_offset, ®s, ®_count); + if (ret == 0) { tx_buf = (uint8_t *)regs; tx_len = reg_count * 2; + } else { tx_buf = junk_buf; tx_len = sizeof(junk_buf); @@ -306,16 +320,16 @@ i2c_tx_setup(void) /* * Note that we configure DMA in circular mode; this means that a too-long * transfer will copy the buffer more than once, but that avoids us having - * to deal with bailing out of a transaction while the master is still + * to deal with bailing out of a transaction while the master is still * babbling at us. */ stm32_dmasetup(tx_dma, (uintptr_t)&rDR, (uintptr_t)&tx_buf[0], tx_len, - DMA_CCR_DIR | - DMA_CCR_CIRC | - DMA_CCR_MINC | - DMA_CCR_PSIZE_8BITS | - DMA_CCR_MSIZE_8BITS | - DMA_CCR_PRIMED); + DMA_CCR_DIR | + DMA_CCR_CIRC | + DMA_CCR_MINC | + DMA_CCR_PSIZE_8BITS | + DMA_CCR_MSIZE_8BITS | + DMA_CCR_PRIMED); stm32_dmastart(tx_dma, NULL, NULL, false); } diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index b608d5fc93..b15ec09610 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -97,12 +97,13 @@ mixer_tick(void) /* check that we are receiving fresh data from the FMU */ if ((system_state.fmu_data_received_time == 0) || - hrt_elapsed_time(&system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) { + hrt_elapsed_time(&system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) { /* too long without FMU input, time to go to failsafe */ if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) { isr_debug(1, "AP RX timeout"); } + r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK); r_status_alarms |= PX4IO_P_STATUS_ALARMS_FMU_LOST; @@ -122,7 +123,7 @@ mixer_tick(void) /* do not mix if RAW_PWM mode is on and FMU is good */ if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) && - (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) { + (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) { /* don't actually mix anything - we already have raw PWM values */ source = MIX_NONE; @@ -130,28 +131,29 @@ mixer_tick(void) } else { if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && - (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && - (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { + (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { /* mix from FMU controls */ source = MIX_FMU; } - if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && - (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && - (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) && - !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) && - !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && - /* do not enter manual override if we asked for termination failsafe and FMU is lost */ - !(r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE)) { + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) && + !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) && + !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && + /* do not enter manual override if we asked for termination failsafe and FMU is lost */ + !(r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE)) { - /* if allowed, mix from RC inputs directly */ + /* if allowed, mix from RC inputs directly */ source = MIX_OVERRIDE; - } else if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && - (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && - (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) && - !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) && - (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) { + + } else if ((r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) && + !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) { /* if allowed, mix from RC inputs directly up to available rc channels */ source = MIX_OVERRIDE_FMU_OK; @@ -166,31 +168,32 @@ mixer_tick(void) * */ 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) && !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) - ) - ); + /* 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) + && !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) + ) + ); should_arm_nothrottle = ( - /* 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 there is valid input via or mixer */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)); + /* 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 there is valid input via or mixer */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)); 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); + && (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) + && (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK); /* * Check if failsafe termination is set - if yes, * set the force failsafe flag once entering the first * failsafe condition. */ - if ( /* if we have requested flight termination style failsafe (noreturn) */ + if ( /* if we have requested flight termination style failsafe (noreturn) */ (r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) && /* and we ended up in a failsafe condition */ (source == MIX_FAILSAFE) && @@ -213,6 +216,7 @@ mixer_tick(void) */ if (source == MIX_FAILSAFE) { r_status_flags |= PX4IO_P_STATUS_FLAGS_FAILSAFE; + } else { r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FAILSAFE); } @@ -244,7 +248,8 @@ mixer_tick(void) in_mixer = false; /* the pwm limit call takes care of out of band errors */ - pwm_limit_calc(should_arm, should_arm_nothrottle, mixed, r_setup_pwm_reverse, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); + pwm_limit_calc(should_arm, should_arm_nothrottle, mixed, r_setup_pwm_reverse, r_page_servo_disarmed, + r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); /* clamp unused outputs to zero */ for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) { @@ -291,6 +296,7 @@ mixer_tick(void) if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) { sbus2_output(r_page_servos, PX4IO_SERVO_COUNT); + } else if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) { sbus1_output(r_page_servos, PX4IO_SERVO_COUNT); } @@ -324,10 +330,11 @@ mixer_callback(uintptr_t handle, switch (source) { case MIX_FMU: - if (control_index < PX4IO_CONTROL_CHANNELS && control_group < PX4IO_CONTROL_GROUPS ) { + if (control_index < PX4IO_CONTROL_CHANNELS && control_group < PX4IO_CONTROL_GROUPS) { control = REG_TO_FLOAT(r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)]); break; } + return -1; case MIX_OVERRIDE: @@ -335,17 +342,21 @@ mixer_callback(uintptr_t handle, control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]); break; } + return -1; case MIX_OVERRIDE_FMU_OK: + /* FMU is ok but we are in override mode, use direct rc control for the available rc channels. The remaining channels are still controlled by the fmu */ if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << CONTROL_PAGE_INDEX(control_group, control_index))) { control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]); break; + } else if (control_index < PX4IO_CONTROL_CHANNELS && control_group < PX4IO_CONTROL_GROUPS) { control = REG_TO_FLOAT(r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)]); break; } + return -1; case MIX_FAILSAFE: @@ -357,15 +368,15 @@ mixer_callback(uintptr_t handle, /* apply trim offsets for override channels */ if (source == MIX_OVERRIDE || source == MIX_OVERRIDE_FMU_OK) { if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE && - control_index == actuator_controls_s::INDEX_ROLL) { + control_index == actuator_controls_s::INDEX_ROLL) { control += REG_TO_FLOAT(r_setup_trim_roll); } else if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE && - control_index == actuator_controls_s::INDEX_PITCH) { + control_index == actuator_controls_s::INDEX_PITCH) { control += REG_TO_FLOAT(r_setup_trim_pitch); } else if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE && - control_index == actuator_controls_s::INDEX_YAW) { + control_index == actuator_controls_s::INDEX_YAW) { control += REG_TO_FLOAT(r_setup_trim_yaw); } } @@ -373,6 +384,7 @@ mixer_callback(uintptr_t handle, /* limit output */ if (control > 1.0f) { control = 1.0f; + } else if (control < -1.0f) { control = -1.0f; } @@ -380,7 +392,7 @@ mixer_callback(uintptr_t handle, /* motor spinup phase - lock throttle to zero */ if ((pwm_limit.state == PWM_LIMIT_STATE_RAMP) || (should_arm_nothrottle && !should_arm)) { if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE && - control_index == actuator_controls_s::INDEX_THROTTLE) { + control_index == actuator_controls_s::INDEX_THROTTLE) { /* limit the throttle output to zero during motor spinup, * as the motors cannot follow any demand yet */ @@ -391,7 +403,7 @@ mixer_callback(uintptr_t handle, /* only safety off, but not armed - set throttle as invalid */ if (should_arm_nothrottle && !should_arm) { if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE && - control_index == actuator_controls_s::INDEX_THROTTLE) { + control_index == actuator_controls_s::INDEX_THROTTLE) { /* mark the throttle as invalid */ control = NAN_VALUE; } @@ -414,7 +426,7 @@ mixer_handle_text(const void *buffer, size_t length) { /* do not allow a mixer change while safety off and FMU armed */ if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) && - (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { + (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { return 1; } @@ -444,7 +456,7 @@ mixer_handle_text(const void *buffer, size_t length) mixer_group.reset(); mixer_text_length = 0; - /* FALLTHROUGH */ + /* FALLTHROUGH */ case F2I_MIXER_ACTION_APPEND: isr_debug(2, "append %d", length); @@ -470,6 +482,7 @@ mixer_handle_text(const void *buffer, size_t length) /* only set mixer ok if no residual is left over */ if (resid == 0) { r_status_flags |= PX4IO_P_STATUS_FLAGS_MIXER_OK; + } else { /* not yet reached the end of the mixer, set as not ok */ r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK; @@ -497,13 +510,13 @@ mixer_handle_text(const void *buffer, size_t length) static void mixer_set_failsafe() { - /* + /* * Check if a custom failsafe value has been written, * or if the mixer is not ok and bail out. */ if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) || - !(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { + !(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { return; } diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 673da0e44d..1267b48853 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -46,7 +46,7 @@ * * The first two bytes of each write select a page and offset address * respectively. Subsequent reads and writes increment the offset within - * the page. + * the page. * * Some pages are read- or write-only. * @@ -56,7 +56,7 @@ * Writes to unimplemented registers are ignored. Reads from unimplemented * registers return undefined values. * - * As convention, values that would be floating point in other parts of + * As convention, values that would be floating point in other parts of * the PX4 system are expressed as signed integer values scaled by 10000, * e.g. control values range from -10000..10000. Use the REG_TO_SIGNED and * SIGNED_TO_REG macros to convert between register representation and @@ -66,7 +66,7 @@ * readable pages to be densely packed. Page numbers do not need to be * packed. * - * Definitions marked [1] are only valid on PX4IOv1 boards. Likewise, + * Definitions marked [1] are only valid on PX4IOv1 boards. Likewise, * [2] denotes definitions specific to the PX4IOv2 board. */ @@ -218,14 +218,14 @@ enum { /* DSM bind states */ dsm_bind_send_pulses, dsm_bind_reinit_uart }; - /* 8 */ +/* 8 */ #define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */ #define PX4IO_P_SETUP_REBOOT_BL 10 /* reboot IO into bootloader */ #define PX4IO_REBOOT_BL_MAGIC 14662 /* required argument for reboot (random) */ #define PX4IO_P_SETUP_CRC 11 /* get CRC of IO firmware */ - /* storage space of 12 occupied by CRC */ +/* storage space of 12 occupied by CRC */ #define PX4IO_P_SETUP_FORCE_SAFETY_OFF 12 /* force safety switch into 'armed' (PWM enabled) state - this is a non-data write and hence index 12 can safely be used. */ @@ -339,8 +339,7 @@ struct IOPacket { #define PKT_CODE(_p) ((_p).count_code & PKT_CODE_MASK) #define PKT_SIZE(_p) ((size_t)((uint8_t *)&((_p).regs[PKT_COUNT(_p)]) - ((uint8_t *)&(_p)))) -static const uint8_t crc8_tab[256] __attribute__((unused)) = -{ +static const uint8_t crc8_tab[256] __attribute__((unused)) = { 0x00, 0x07, 0x0E, 0x09, 0x1C, 0x1B, 0x12, 0x15, 0x38, 0x3F, 0x36, 0x31, 0x24, 0x23, 0x2A, 0x2D, 0x70, 0x77, 0x7E, 0x79, 0x6C, 0x6B, 0x62, 0x65, @@ -383,8 +382,9 @@ crc_packet(struct IOPacket *pkt) uint8_t *p = (uint8_t *)pkt; uint8_t c = 0; - while (p < end) - c = crc8_tab[c ^ *(p++)]; + while (p < end) { + c = crc8_tab[c ^ * (p++)]; + } return c; } diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index 94f1fc11ef..1ee50d5a7f 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -97,11 +97,12 @@ isr_debug(uint8_t level, const char *fmt, ...) if (level > r_page_setup[PX4IO_P_SETUP_SET_DEBUG]) { return; } + va_list ap; va_start(ap, fmt); vsnprintf(msg[msg_next_in], sizeof(msg[0]), fmt, ap); va_end(ap); - msg_next_in = (msg_next_in+1) % NUM_MSG; + msg_next_in = (msg_next_in + 1) % NUM_MSG; msg_counter++; } @@ -113,11 +114,14 @@ show_debug_messages(void) { if (msg_counter != last_msg_counter) { uint32_t n = msg_counter - last_msg_counter; - if (n > NUM_MSG) n = NUM_MSG; + + if (n > NUM_MSG) { n = NUM_MSG; } + last_msg_counter = msg_counter; + while (n--) { debug("%s", msg[msg_next_out]); - msg_next_out = (msg_next_out+1) % NUM_MSG; + msg_next_out = (msg_next_out + 1) % NUM_MSG; } } } @@ -135,7 +139,7 @@ ring_blink(void) #ifdef GPIO_LED4 if (/* IO armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) - /* and FMU is armed */ && (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { + /* and FMU is armed */ && (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { LED_RING(1); return; } @@ -151,7 +155,7 @@ ring_blink(void) if (brightness_counter < max_brightness) { - bool on = ((on_counter * 100) / brightness_counter+1) <= ((brightness * 100) / max_brightness+1); + bool on = ((on_counter * 100) / brightness_counter + 1) <= ((brightness * 100) / max_brightness + 1); // XXX once led is PWM driven, // remove the ! in the line below @@ -195,7 +199,7 @@ static uint64_t reboot_time; */ void schedule_reboot(uint32_t time_delta_usec) { - reboot_time = hrt_absolute_time() + time_delta_usec; + reboot_time = hrt_absolute_time() + time_delta_usec; } /** @@ -203,9 +207,9 @@ void schedule_reboot(uint32_t time_delta_usec) */ static void check_reboot(void) { - if (reboot_time != 0 && hrt_absolute_time() > reboot_time) { - up_systemreset(); - } + if (reboot_time != 0 && hrt_absolute_time() > reboot_time) { + up_systemreset(); + } } static void @@ -215,12 +219,14 @@ calculate_fw_crc(void) #define APP_LOAD_ADDRESS 0x08001000 // compute CRC of the current firmware uint32_t sum = 0; + for (unsigned p = 0; p < APP_SIZE_MAX; p += 4) { uint32_t bytes = *(uint32_t *)(p + APP_LOAD_ADDRESS); sum = crc32part((uint8_t *)&bytes, sizeof(bytes), sum); } + r_page_setup[PX4IO_P_SETUP_CRC] = sum & 0xFFFF; - r_page_setup[PX4IO_P_SETUP_CRC+1] = sum >> 16; + r_page_setup[PX4IO_P_SETUP_CRC + 1] = sum >> 16; } int @@ -308,7 +314,7 @@ user_start(int argc, char *argv[]) * allocations. We don't want him to be able to * get past that point. This needs to be clearly * documented in the dev guide. - * + * */ if (minfo.mxordblk < 600) { @@ -320,10 +326,12 @@ user_start(int argc, char *argv[]) if (phase) { LED_AMBER(true); LED_BLUE(false); + } else { LED_AMBER(false); LED_BLUE(true); } + up_udelay(250000); phase = !phase; @@ -338,7 +346,8 @@ user_start(int argc, char *argv[]) */ uint64_t last_debug_time = 0; - uint64_t last_heartbeat_time = 0; + uint64_t last_heartbeat_time = 0; + for (;;) { /* track the rate at which the loop is running */ @@ -354,14 +363,14 @@ user_start(int argc, char *argv[]) controls_tick(); perf_end(controls_perf); - if ((hrt_absolute_time() - last_heartbeat_time) > 250*1000) { - last_heartbeat_time = hrt_absolute_time(); - heartbeat_blink(); - } + if ((hrt_absolute_time() - last_heartbeat_time) > 250 * 1000) { + last_heartbeat_time = hrt_absolute_time(); + heartbeat_blink(); + } ring_blink(); - check_reboot(); + check_reboot(); /* check for debug activity (default: none) */ show_debug_messages(); @@ -371,7 +380,7 @@ user_start(int argc, char *argv[]) */ if (hrt_absolute_time() - last_debug_time > (1000 * 1000)) { - isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x m=%u", + isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x m=%u", (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG], (unsigned)r_status_flags, (unsigned)r_setup_arming, diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 8adc91cda9..46852003c8 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -112,15 +112,14 @@ uint16_t r_page_servos[PX4IO_SERVO_COUNT]; * * Raw RC input */ -uint16_t r_page_raw_rc_input[] = -{ +uint16_t r_page_raw_rc_input[] = { [PX4IO_P_RAW_RC_COUNT] = 0, [PX4IO_P_RAW_RC_FLAGS] = 0, [PX4IO_P_RAW_RC_NRSSI] = 0, [PX4IO_P_RAW_RC_DATA] = 0, [PX4IO_P_RAW_FRAME_COUNT] = 0, [PX4IO_P_RAW_LOST_FRAME_COUNT] = 0, - [PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + PX4IO_RC_INPUT_CHANNELS)] = 0 + [PX4IO_P_RAW_RC_BASE ...(PX4IO_P_RAW_RC_BASE + PX4IO_RC_INPUT_CHANNELS)] = 0 }; /** @@ -130,7 +129,7 @@ uint16_t r_page_raw_rc_input[] = */ uint16_t r_page_rc_input[] = { [PX4IO_P_RC_VALID] = 0, - [PX4IO_P_RC_BASE ... (PX4IO_P_RC_BASE + PX4IO_RC_MAPPED_CONTROL_CHANNELS)] = 0 + [PX4IO_P_RC_BASE ...(PX4IO_P_RC_BASE + PX4IO_RC_MAPPED_CONTROL_CHANNELS)] = 0 }; /** @@ -146,8 +145,7 @@ uint16_t r_page_scratch[32]; * * Setup registers */ -volatile uint16_t r_page_setup[] = -{ +volatile uint16_t r_page_setup[] = { #ifdef CONFIG_ARCH_BOARD_PX4IO_V2 /* default to RSSI ADC functionality */ [PX4IO_P_SETUP_FEATURES] = PX4IO_P_SETUP_FEATURES_ADC_RSSI, @@ -171,7 +169,7 @@ volatile uint16_t r_page_setup[] = #endif [PX4IO_P_SETUP_SET_DEBUG] = 0, [PX4IO_P_SETUP_REBOOT_BL] = 0, - [PX4IO_P_SETUP_CRC ... (PX4IO_P_SETUP_CRC+1)] = 0, + [PX4IO_P_SETUP_CRC ...(PX4IO_P_SETUP_CRC + 1)] = 0, [PX4IO_P_SETUP_RC_THR_FAILSAFE_US] = 0, [PX4IO_P_SETUP_PWM_REVERSE] = 0, [PX4IO_P_SETUP_TRIM_ROLL] = 0, @@ -181,23 +179,23 @@ volatile uint16_t r_page_setup[] = #ifdef CONFIG_ARCH_BOARD_PX4IO_V2 #define PX4IO_P_SETUP_FEATURES_VALID (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | \ - PX4IO_P_SETUP_FEATURES_SBUS2_OUT | \ - PX4IO_P_SETUP_FEATURES_ADC_RSSI | \ - PX4IO_P_SETUP_FEATURES_PWM_RSSI) + PX4IO_P_SETUP_FEATURES_SBUS2_OUT | \ + PX4IO_P_SETUP_FEATURES_ADC_RSSI | \ + PX4IO_P_SETUP_FEATURES_PWM_RSSI) #else #define PX4IO_P_SETUP_FEATURES_VALID 0 #endif #define PX4IO_P_SETUP_ARMING_VALID (PX4IO_P_SETUP_ARMING_FMU_ARMED | \ - PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | \ - PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | \ - PX4IO_P_SETUP_ARMING_IO_ARM_OK | \ - PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM | \ - PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE | \ - PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED | \ - PX4IO_P_SETUP_ARMING_LOCKDOWN | \ - PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE | \ - PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE | \ - PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE) + PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | \ + PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | \ + PX4IO_P_SETUP_ARMING_IO_ARM_OK | \ + PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM | \ + PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE | \ + PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED | \ + PX4IO_P_SETUP_ARMING_LOCKDOWN | \ + PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE | \ + PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE | \ + PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE) #define PX4IO_P_SETUP_RATES_VALID ((1 << PX4IO_SERVO_COUNT) - 1) #define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1) @@ -230,7 +228,7 @@ uint16_t r_page_rc_input_config[PX4IO_RC_INPUT_CHANNELS * PX4IO_P_RC_CONFIG_STR * PAGE 105 * * Failsafe servo PWM values - * + * * Disable pulses as default. */ uint16_t r_page_servo_failsafe[PX4IO_SERVO_COUNT] = { 0, 0, 0, 0, 0, 0, 0, 0 }; @@ -265,7 +263,7 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num switch (page) { - /* handle bulk controls input */ + /* handle bulk controls input */ case PX4IO_PAGE_CONTROLS: /* copy channel data */ @@ -282,10 +280,10 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num system_state.fmu_data_received_time = hrt_absolute_time(); r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK; r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RAW_PWM; - + break; - /* handle raw PWM input */ + /* handle raw PWM input */ case PX4IO_PAGE_DIRECT_PWM: /* copy channel data */ @@ -306,7 +304,7 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num break; - /* handle setup for servo failsafe values */ + /* handle setup for servo failsafe values */ case PX4IO_PAGE_FAILSAFE_PWM: /* copy channel data */ @@ -316,8 +314,10 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num /* ignore 0 */ } else if (*values < PWM_LOWEST_MIN) { r_page_servo_failsafe[offset] = PWM_LOWEST_MIN; + } else if (*values > PWM_HIGHEST_MAX) { r_page_servo_failsafe[offset] = PWM_HIGHEST_MAX; + } else { r_page_servo_failsafe[offset] = *values; } @@ -329,6 +329,7 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num num_values--; values++; } + break; case PX4IO_PAGE_CONTROL_MIN_PWM: @@ -340,8 +341,10 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num /* ignore 0 */ } else if (*values > PWM_HIGHEST_MIN) { r_page_servo_control_min[offset] = PWM_HIGHEST_MIN; + } else if (*values < PWM_LOWEST_MIN) { r_page_servo_control_min[offset] = PWM_LOWEST_MIN; + } else { r_page_servo_control_min[offset] = *values; } @@ -350,8 +353,9 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num num_values--; values++; } + break; - + case PX4IO_PAGE_CONTROL_MAX_PWM: /* copy channel data */ @@ -361,8 +365,10 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num /* ignore 0 */ } else if (*values > PWM_HIGHEST_MAX) { r_page_servo_control_max[offset] = PWM_HIGHEST_MAX; + } else if (*values < PWM_LOWEST_MAX) { r_page_servo_control_max[offset] = PWM_LOWEST_MAX; + } else { r_page_servo_control_max[offset] = *values; } @@ -371,10 +377,10 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num num_values--; values++; } + break; - case PX4IO_PAGE_DISARMED_PWM: - { + case PX4IO_PAGE_DISARMED_PWM: { /* flag for all outputs */ bool all_disarmed_off = true; @@ -384,12 +390,15 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num if (*values == 0) { /* 0 means disabling always PWM */ r_page_servo_disarmed[offset] = 0; + } else if (*values < PWM_LOWEST_MIN) { r_page_servo_disarmed[offset] = PWM_LOWEST_MIN; all_disarmed_off = false; + } else if (*values > PWM_HIGHEST_MAX) { r_page_servo_disarmed[offset] = PWM_HIGHEST_MAX; all_disarmed_off = false; + } else { r_page_servo_disarmed[offset] = *values; all_disarmed_off = false; @@ -403,6 +412,7 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num 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; @@ -410,7 +420,7 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num } break; - /* handle text going to the mixer parser */ + /* handle text going to the mixer parser */ case PX4IO_PAGE_MIXERLOAD: /* do not change the mixer if FMU is armed and IO's safety is off * this state defines an active system. This check is done in the @@ -419,19 +429,25 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num return mixer_handle_text(values, num_values * sizeof(*values)); default: + /* avoid offset wrap */ - if ((offset + num_values) > 255) + if ((offset + num_values) > 255) { num_values = 255 - offset; + } /* iterate individual registers, set each in turn */ while (num_values--) { - if (registers_set_one(page, offset, *values)) + if (registers_set_one(page, offset, *values)) { return -1; + } + offset++; values++; } + break; } + return 0; } @@ -448,19 +464,22 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) break; case PX4IO_P_STATUS_FLAGS: - /* + + /* * Allow FMU override of arming state (to allow in-air restores), * but only if the arming state is not in sync on the IO side. */ if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) { r_status_flags = value; } + break; default: /* just ignore writes to other registers in this page */ break; } + break; case PX4IO_PAGE_SETUP: @@ -472,36 +491,37 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) /* some of the options conflict - give S.BUS out precedence, then ADC RSSI, then PWM RSSI */ /* switch S.Bus output pin as needed */ - #ifdef ENABLE_SBUS_OUT +#ifdef ENABLE_SBUS_OUT ENABLE_SBUS_OUT(value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT)); /* disable the conflicting options with SBUS 1 */ if (value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT)) { value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI | - PX4IO_P_SETUP_FEATURES_ADC_RSSI | - PX4IO_P_SETUP_FEATURES_SBUS2_OUT); + PX4IO_P_SETUP_FEATURES_ADC_RSSI | + PX4IO_P_SETUP_FEATURES_SBUS2_OUT); } /* disable the conflicting options with SBUS 2 */ if (value & (PX4IO_P_SETUP_FEATURES_SBUS2_OUT)) { value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI | - PX4IO_P_SETUP_FEATURES_ADC_RSSI | - PX4IO_P_SETUP_FEATURES_SBUS1_OUT); + PX4IO_P_SETUP_FEATURES_ADC_RSSI | + PX4IO_P_SETUP_FEATURES_SBUS1_OUT); } - #endif + +#endif /* disable the conflicting options with ADC RSSI */ if (value & (PX4IO_P_SETUP_FEATURES_ADC_RSSI)) { value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI | - PX4IO_P_SETUP_FEATURES_SBUS1_OUT | - PX4IO_P_SETUP_FEATURES_SBUS2_OUT); + PX4IO_P_SETUP_FEATURES_SBUS1_OUT | + PX4IO_P_SETUP_FEATURES_SBUS2_OUT); } /* disable the conflicting options with PWM RSSI (without effect here, but for completeness) */ if (value & (PX4IO_P_SETUP_FEATURES_PWM_RSSI)) { value &= ~(PX4IO_P_SETUP_FEATURES_ADC_RSSI | - PX4IO_P_SETUP_FEATURES_SBUS1_OUT | - PX4IO_P_SETUP_FEATURES_SBUS2_OUT); + PX4IO_P_SETUP_FEATURES_SBUS1_OUT | + PX4IO_P_SETUP_FEATURES_SBUS2_OUT); } /* apply changes */ @@ -551,9 +571,11 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) if (value < 25) { value = 25; } + if (value > 400) { value = 400; } + pwm_configure_rates(r_setup_pwm_rates, value, r_setup_pwm_altrate); break; @@ -561,13 +583,16 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) if (value < 25) { value = 25; } + if (value > 400) { value = 400; } + pwm_configure_rates(r_setup_pwm_rates, r_setup_pwm_defaultrate, value); break; #ifdef CONFIG_ARCH_BOARD_PX4IO_V1 + case PX4IO_P_SETUP_RELAYS: value &= PX4IO_P_SETUP_RELAYS_VALID; r_setup_relays = value; @@ -598,10 +623,10 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) break; } - // we schedule a reboot rather than rebooting - // immediately to allow the IO board to ACK - // the reboot command - schedule_reboot(100000); + // we schedule a reboot rather than rebooting + // immediately to allow the IO board to ACK + // the reboot command + schedule_reboot(100000); break; case PX4IO_P_SETUP_DSM: @@ -612,18 +637,21 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) if (value == PX4IO_FORCE_SAFETY_MAGIC) { r_status_flags &= ~PX4IO_P_STATUS_FLAGS_SAFETY_OFF; } + break; case PX4IO_P_SETUP_FORCE_SAFETY_OFF: if (value == PX4IO_FORCE_SAFETY_MAGIC) { r_status_flags |= PX4IO_P_STATUS_FLAGS_SAFETY_OFF; } + break; case PX4IO_P_SETUP_RC_THR_FAILSAFE_US: if (value > 650 && value < 2350) { r_page_setup[PX4IO_P_SETUP_RC_THR_FAILSAFE_US] = value; } + break; case PX4IO_P_SETUP_PWM_REVERSE: @@ -639,93 +667,103 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) default: return -1; } + break; case PX4IO_PAGE_RC_CONFIG: { - /** - * do not allow a RC config change while safety is off - */ - if (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) { - break; - } - - unsigned channel = offset / PX4IO_P_RC_CONFIG_STRIDE; - unsigned index = offset - channel * PX4IO_P_RC_CONFIG_STRIDE; - uint16_t *conf = &r_page_rc_input_config[channel * PX4IO_P_RC_CONFIG_STRIDE]; - - if (channel >= PX4IO_RC_INPUT_CHANNELS) - return -1; - - /* disable the channel until we have a chance to sanity-check it */ - conf[PX4IO_P_RC_CONFIG_OPTIONS] &= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; - - switch (index) { - - case PX4IO_P_RC_CONFIG_MIN: - case PX4IO_P_RC_CONFIG_CENTER: - case PX4IO_P_RC_CONFIG_MAX: - case PX4IO_P_RC_CONFIG_DEADZONE: - case PX4IO_P_RC_CONFIG_ASSIGNMENT: - conf[index] = value; - break; - - case PX4IO_P_RC_CONFIG_OPTIONS: - value &= PX4IO_P_RC_CONFIG_OPTIONS_VALID; - r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK; - - /* clear any existing RC disabled flag */ - r_setup_arming &= ~(PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED); - - /* set all options except the enabled option */ - conf[index] = value & ~PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; - - /* should the channel be enabled? */ - /* this option is normally set last */ - if (value & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) { - uint8_t count = 0; - bool disabled = false; - - /* assert min..center..max ordering */ - if (conf[PX4IO_P_RC_CONFIG_MIN] < 500) { - count++; - } - if (conf[PX4IO_P_RC_CONFIG_MAX] > 2500) { - count++; - } - if (conf[PX4IO_P_RC_CONFIG_CENTER] < conf[PX4IO_P_RC_CONFIG_MIN]) { - count++; - } - if (conf[PX4IO_P_RC_CONFIG_CENTER] > conf[PX4IO_P_RC_CONFIG_MAX]) { - count++; - } - /* assert deadzone is sane */ - if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500) { - count++; - } - - if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] == UINT8_MAX) { - disabled = true; - } else if ((conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) && - (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] != PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH)) { - count++; - } - - /* sanity checks pass, enable channel */ - if (count) { - isr_debug(0, "ERROR: %d config error(s) for RC%d.\n", count, (channel + 1)); - r_status_flags &= ~PX4IO_P_STATUS_FLAGS_INIT_OK; - } else if (!disabled) { - conf[index] |= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; - } + /** + * do not allow a RC config change while safety is off + */ + if (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) { + break; } - break; - /* inner switch: case PX4IO_P_RC_CONFIG_OPTIONS */ + unsigned channel = offset / PX4IO_P_RC_CONFIG_STRIDE; + unsigned index = offset - channel * PX4IO_P_RC_CONFIG_STRIDE; + uint16_t *conf = &r_page_rc_input_config[channel * PX4IO_P_RC_CONFIG_STRIDE]; + + if (channel >= PX4IO_RC_INPUT_CHANNELS) { + return -1; + } + + /* disable the channel until we have a chance to sanity-check it */ + conf[PX4IO_P_RC_CONFIG_OPTIONS] &= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; + + switch (index) { + + case PX4IO_P_RC_CONFIG_MIN: + case PX4IO_P_RC_CONFIG_CENTER: + case PX4IO_P_RC_CONFIG_MAX: + case PX4IO_P_RC_CONFIG_DEADZONE: + case PX4IO_P_RC_CONFIG_ASSIGNMENT: + conf[index] = value; + break; + + case PX4IO_P_RC_CONFIG_OPTIONS: + value &= PX4IO_P_RC_CONFIG_OPTIONS_VALID; + r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK; + + /* clear any existing RC disabled flag */ + r_setup_arming &= ~(PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED); + + /* set all options except the enabled option */ + conf[index] = value & ~PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; + + /* should the channel be enabled? */ + /* this option is normally set last */ + if (value & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) { + uint8_t count = 0; + bool disabled = false; + + /* assert min..center..max ordering */ + if (conf[PX4IO_P_RC_CONFIG_MIN] < 500) { + count++; + } + + if (conf[PX4IO_P_RC_CONFIG_MAX] > 2500) { + count++; + } + + if (conf[PX4IO_P_RC_CONFIG_CENTER] < conf[PX4IO_P_RC_CONFIG_MIN]) { + count++; + } + + if (conf[PX4IO_P_RC_CONFIG_CENTER] > conf[PX4IO_P_RC_CONFIG_MAX]) { + count++; + } + + /* assert deadzone is sane */ + if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500) { + count++; + } + + if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] == UINT8_MAX) { + disabled = true; + + } else if ((conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) && + (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] != PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH)) { + count++; + } + + /* sanity checks pass, enable channel */ + if (count) { + isr_debug(0, "ERROR: %d config error(s) for RC%d.\n", count, (channel + 1)); + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_INIT_OK; + + } else if (!disabled) { + conf[index] |= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; + } + } + + break; + /* inner switch: case PX4IO_P_RC_CONFIG_OPTIONS */ + + } + + break; + /* case PX4IO_RC_PAGE_CONFIG */ } - break; - /* case PX4IO_RC_PAGE_CONFIG */ - } case PX4IO_PAGE_TEST: switch (offset) { @@ -733,11 +771,13 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) LED_AMBER(value & 1); break; } + break; default: return -1; } + return 0; } @@ -787,6 +827,7 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val * */ unsigned counts = adc_measure(ADC_VBATT); + if (counts != 0xffff) { unsigned mV = (166460 + (counts * 45934)) / 10000; unsigned corrected = (mV * r_page_setup[PX4IO_P_SETUP_VBATT_SCALE]) / 10000; @@ -806,6 +847,7 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val configuration for their sensor */ unsigned counts = adc_measure(ADC_IBATT); + if (counts != 0xffff) { r_page_status[PX4IO_P_STATUS_IBATT] = counts; } @@ -815,6 +857,7 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val /* PX4IO_P_STATUS_VSERVO */ { unsigned counts = adc_measure(ADC_VSERVO); + if (counts != 0xffff) { // use 3:1 scaling on 3.3V ADC input unsigned mV = counts * 9900 / 4096; @@ -826,6 +869,7 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val /* PX4IO_P_STATUS_VRSSI */ { unsigned counts = adc_measure(ADC_RSSI); + if (counts != 0xffff) { // use 1:1 scaling on 3.3V ADC input unsigned mV = counts * 3300 / 4096; @@ -858,8 +902,10 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val case PX4IO_PAGE_PWM_INFO: memset(r_page_scratch, 0, sizeof(r_page_scratch)); - for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) + + for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) { r_page_scratch[PX4IO_RATE_MAP_BASE + i] = up_pwm_servo_get_rate_group(i); + } SELECT_PAGE(r_page_scratch); break; @@ -872,15 +918,19 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val case PX4IO_PAGE_CONFIG: SELECT_PAGE(r_page_config); break; + case PX4IO_PAGE_ACTUATORS: SELECT_PAGE(r_page_actuators); break; + case PX4IO_PAGE_SERVOS: SELECT_PAGE(r_page_servos); break; + case PX4IO_PAGE_RAW_RC_INPUT: SELECT_PAGE(r_page_raw_rc_input); break; + case PX4IO_PAGE_RC_INPUT: SELECT_PAGE(r_page_rc_input); break; @@ -889,24 +939,31 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val case PX4IO_PAGE_SETUP: SELECT_PAGE(r_page_setup); break; + case PX4IO_PAGE_CONTROLS: SELECT_PAGE(r_page_controls); break; + case PX4IO_PAGE_RC_CONFIG: SELECT_PAGE(r_page_rc_input_config); break; + case PX4IO_PAGE_DIRECT_PWM: SELECT_PAGE(r_page_servos); break; + case PX4IO_PAGE_FAILSAFE_PWM: SELECT_PAGE(r_page_servo_failsafe); break; + case PX4IO_PAGE_CONTROL_MIN_PWM: SELECT_PAGE(r_page_servo_control_min); break; + case PX4IO_PAGE_CONTROL_MAX_PWM: SELECT_PAGE(r_page_servo_control_max); break; + case PX4IO_PAGE_DISARMED_PWM: SELECT_PAGE(r_page_servo_disarmed); break; @@ -918,12 +975,13 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val #undef SELECT_PAGE #undef COPY_PAGE -last_page = page; -last_offset = offset; + last_page = page; + last_offset = offset; /* if the offset is at or beyond the end of the page, we have no data */ - if (offset >= *num_values) + if (offset >= *num_values) { return -1; + } /* correct the data pointer and count for the offset */ *values += offset; @@ -943,8 +1001,10 @@ pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t altrate) /* get the channel mask for this rate group */ uint32_t mask = up_pwm_servo_get_rate_group(group); - if (mask == 0) + + if (mask == 0) { continue; + } /* all channels in the group must be either default or alt-rate */ uint32_t alt = map & mask; @@ -956,18 +1016,23 @@ pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t altrate) r_status_alarms |= PX4IO_P_STATUS_ALARMS_PWM_ERROR; return; } + } else { /* set it - errors here are unexpected */ if (alt != 0) { - if (up_pwm_servo_set_rate_group_update(group, r_setup_pwm_altrate) != OK) + if (up_pwm_servo_set_rate_group_update(group, r_setup_pwm_altrate) != OK) { r_status_alarms |= PX4IO_P_STATUS_ALARMS_PWM_ERROR; + } + } else { - if (up_pwm_servo_set_rate_group_update(group, r_setup_pwm_defaultrate) != OK) + if (up_pwm_servo_set_rate_group_update(group, r_setup_pwm_defaultrate) != OK) { r_status_alarms |= PX4IO_P_STATUS_ALARMS_PWM_ERROR; + } } } } } + r_setup_pwm_rates = map; r_setup_pwm_defaultrate = defaultrate; r_setup_pwm_altrate = altrate; diff --git a/src/modules/px4iofirmware/safety.c b/src/modules/px4iofirmware/safety.c index 06f8733a52..f4ab74a45a 100644 --- a/src/modules/px4iofirmware/safety.c +++ b/src/modules/px4iofirmware/safety.c @@ -110,7 +110,7 @@ safety_check_button(void *arg) * length in all cases of the if/else struct below. */ if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) && - (r_setup_arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK)) { + (r_setup_arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK)) { if (counter < ARM_COUNTER_THRESHOLD) { counter++; @@ -149,6 +149,7 @@ safety_check_button(void *arg) } else if (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) { pattern = LED_PATTERN_FMU_ARMED; + } else if (r_setup_arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK) { pattern = LED_PATTERN_FMU_OK_TO_ARM; @@ -176,6 +177,7 @@ failsafe_blink(void *arg) /* blink the failsafe LED if we don't have FMU input */ if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) { failsafe = !failsafe; + } else { failsafe = false; } diff --git a/src/modules/px4iofirmware/serial.c b/src/modules/px4iofirmware/serial.c index e9adc8cd69..300ec0b784 100644 --- a/src/modules/px4iofirmware/serial.c +++ b/src/modules/px4iofirmware/serial.c @@ -131,14 +131,19 @@ interface_init(void) rCR1 = USART_CR1_RE | USART_CR1_TE | USART_CR1_UE | USART_CR1_IDLEIE; #if 0 /* keep this for signal integrity testing */ + for (;;) { while (!(rSR & USART_SR_TXE)) ; + rDR = 0xfa; + while (!(rSR & USART_SR_TXE)) ; + rDR = 0xa0; } + #endif /* configure RX DMA and return to listening state */ @@ -153,6 +158,7 @@ rx_handle_packet(void) /* check packet CRC */ uint8_t crc = dma_packet.crc; dma_packet.crc = 0; + if (crc != crc_packet(&dma_packet)) { perf_count(pc_crcerr); @@ -170,11 +176,13 @@ rx_handle_packet(void) if (registers_set(dma_packet.page, dma_packet.offset, &dma_packet.regs[0], PKT_COUNT(dma_packet))) { perf_count(pc_regerr); dma_packet.count_code = PKT_CODE_ERROR; + } else { dma_packet.count_code = PKT_CODE_SUCCESS; } + return; - } + } if (PKT_CODE(dma_packet) == PKT_CODE_READ) { @@ -185,17 +193,22 @@ rx_handle_packet(void) if (registers_get(dma_packet.page, dma_packet.offset, ®isters, &count) < 0) { perf_count(pc_regerr); dma_packet.count_code = PKT_CODE_ERROR; + } else { /* constrain reply to requested size */ - if (count > PKT_MAX_REGS) + if (count > PKT_MAX_REGS) { count = PKT_MAX_REGS; - if (count > PKT_COUNT(dma_packet)) + } + + if (count > PKT_COUNT(dma_packet)) { count = PKT_COUNT(dma_packet); + } /* copy reply registers into DMA buffer */ memcpy((void *)&dma_packet.regs[0], registers, count * 2); dma_packet.count_code = count | PKT_CODE_SUCCESS; } + return; } @@ -250,16 +263,22 @@ serial_interrupt(int irq, void *context) (void)rDR; /* required to clear any of the interrupt status that brought us here */ if (sr & (USART_SR_ORE | /* overrun error - packet was too big for DMA or DMA was too slow */ - USART_SR_NE | /* noise error - we have lost a byte due to noise */ - USART_SR_FE)) { /* framing error - start/stop bit lost or line break */ + USART_SR_NE | /* noise error - we have lost a byte due to noise */ + USART_SR_FE)) { /* framing error - start/stop bit lost or line break */ perf_count(pc_errors); - if (sr & USART_SR_ORE) + + if (sr & USART_SR_ORE) { perf_count(pc_ore); - if (sr & USART_SR_NE) + } + + if (sr & USART_SR_NE) { perf_count(pc_ne); - if (sr & USART_SR_FE) + } + + if (sr & USART_SR_FE) { perf_count(pc_fe); + } /* send a line break - this will abort transmission/reception on the other end */ rCR1 |= USART_CR1_SBK; @@ -270,7 +289,7 @@ serial_interrupt(int irq, void *context) if (sr & USART_SR_IDLE) { - /* + /* * If we saw an error, don't bother looking at the packet - it should have * been aborted by the sender and will definitely be bad. Get the DMA reconfigured * ready for their retry. @@ -284,10 +303,11 @@ serial_interrupt(int irq, void *context) /* * The sender has stopped sending - this is probably the end of a packet. - * Check the received length against the length in the header to see if + * Check the received length against the length in the header to see if * we have something that looks like a packet. */ unsigned length = sizeof(dma_packet) - stm32_dmaresidual(rx_dma); + if ((length < 1) || (length < PKT_SIZE(dma_packet))) { /* it was too short - possibly truncated */