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

IO firmware: Fix code style

Browse Source
This commit is contained in:
Lorenz Meier
2015-10-19 13:46:32 +02:00
parent 59ee7b23ee
commit 948ff80b80
9 changed files with 395 additions and 251 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/px4iofirmware/adc.c
+6 -3
View File
@@ -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
src/modules/px4iofirmware/dsm.c
+34 -16
View File
@@ -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
src/modules/px4iofirmware/i2c.c
+32 -18
View File
@@ -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, &regs, &reg_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);
}
src/modules/px4iofirmware/mixer.cpp
+56 -43
View File
@@ -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;
}
src/modules/px4iofirmware/protocol.h
+9 -9
View File
@@ -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;
}
src/modules/px4iofirmware/px4io.c
+27 -18
View File
@@ -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,
src/modules/px4iofirmware/registers.c
+198 -133
View File
@@ -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;
src/modules/px4iofirmware/safety.c
+3 -1
View File
@@ -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;
}
src/modules/px4iofirmware/serial.c
+30 -10
View File
@@ -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, &registers, &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 */