From 3d2d6c4fdcde8cac1fdaa995899b5936793a9cf8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 3 May 2017 22:44:24 +0200 Subject: [PATCH] IO: Add lazy atomic OR, AND and CLEAR functions for flags IO needs atomic updates of a few critical status flags, but doing these always atomic (with interrupts disabled) might cause a too high interrupts disabled interval. In order to avoid this only operations that change the state of the target variable are done with interrupts disabled, while operations without an effect on the target variable are not executed. --- src/modules/px4iofirmware/controls.c | 44 +++++++++++++++------------- src/modules/px4iofirmware/mixer.cpp | 22 +++++++------- src/modules/px4iofirmware/px4io.h | 6 ++++ src/modules/px4iofirmware/safety.c | 4 +-- 4 files changed, 42 insertions(+), 34 deletions(-) diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 2c66cac9bb..462ce92dee 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -116,7 +116,7 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool *rssi = st24_rssi; r_raw_rc_count = st24_channel_count; - PX4_CRITICAL_SECTION(r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_ST24); + PX4_ATOMIC_MODIFY_OR(r_status_flags, PX4IO_P_STATUS_FLAGS_RC_ST24); r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); } @@ -141,7 +141,7 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool /* not setting RSSI since SUMD does not provide one */ r_raw_rc_count = sumd_channel_count; - PX4_CRITICAL_SECTION(r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SUMD); + PX4_ATOMIC_MODIFY_OR(r_status_flags, PX4IO_P_STATUS_FLAGS_RC_SUMD); r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); if (sumd_failsafe_state) { @@ -233,15 +233,15 @@ controls_tick() (void)dsm_port_input(&rssi, &dsm_updated, &st24_updated, &sumd_updated); if (dsm_updated) { - PX4_CRITICAL_SECTION(r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM); + PX4_ATOMIC_MODIFY_OR(r_status_flags, PX4IO_P_STATUS_FLAGS_RC_DSM); } if (st24_updated) { - PX4_CRITICAL_SECTION(r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_ST24); + PX4_ATOMIC_MODIFY_OR(r_status_flags, PX4IO_P_STATUS_FLAGS_RC_ST24); } if (sumd_updated) { - PX4_CRITICAL_SECTION(r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SUMD); + PX4_ATOMIC_MODIFY_OR(r_status_flags, PX4IO_P_STATUS_FLAGS_RC_SUMD); } perf_end(c_gather_dsm); @@ -253,7 +253,7 @@ controls_tick() PX4IO_RC_INPUT_CHANNELS); if (sbus_updated) { - PX4_CRITICAL_SECTION(r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS); + PX4_ATOMIC_MODIFY_OR(r_status_flags, PX4IO_P_STATUS_FLAGS_RC_SBUS); unsigned sbus_rssi = RC_INPUT_RSSI_MAX; @@ -291,7 +291,7 @@ controls_tick() if (ppm_updated) { - PX4_CRITICAL_SECTION(r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM); + PX4_ATOMIC_MODIFY_OR(r_status_flags, PX4IO_P_STATUS_FLAGS_RC_PPM); r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); } @@ -424,7 +424,7 @@ controls_tick() } /* set RC OK flag, as we got an update */ - PX4_CRITICAL_SECTION(r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK); + PX4_ATOMIC_MODIFY_OR(r_status_flags, PX4IO_P_STATUS_FLAGS_RC_OK); r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_OK; /* if we have enough channels (5) to control the vehicle, the mapping is ok */ @@ -445,14 +445,16 @@ controls_tick() * If we haven't seen any new control data in 200ms, assume we * have lost input. */ - if (hrt_elapsed_time(&system_state.rc_channels_timestamp_received) > 200000) { + if (!rc_input_lost && hrt_elapsed_time(&system_state.rc_channels_timestamp_received) > 200000) { rc_input_lost = true; /* clear the input-kind flags here */ - PX4_CRITICAL_SECTION(r_status_flags &= ~( - PX4IO_P_STATUS_FLAGS_RC_PPM | - PX4IO_P_STATUS_FLAGS_RC_DSM | - PX4IO_P_STATUS_FLAGS_RC_SBUS)); + PX4_ATOMIC_MODIFY_CLEAR(r_status_flags, ( + PX4IO_P_STATUS_FLAGS_RC_PPM | + PX4IO_P_STATUS_FLAGS_RC_DSM | + PX4IO_P_STATUS_FLAGS_RC_SBUS | + PX4IO_P_STATUS_FLAGS_RC_ST24 | + PX4IO_P_STATUS_FLAGS_RC_SUMD)); } @@ -462,15 +464,15 @@ controls_tick() /* if we are in failsafe, clear the override flag */ if (r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE) { - PX4_CRITICAL_SECTION(r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE)); + PX4_ATOMIC_MODIFY_CLEAR(r_status_flags, PX4IO_P_STATUS_FLAGS_OVERRIDE); } /* this kicks in if the receiver is gone, but there is not on failsafe (indicated by separate flag) */ if (rc_input_lost) { /* Clear the RC input status flag, clear manual override flag */ - PX4_CRITICAL_SECTION(r_status_flags &= ~( - PX4IO_P_STATUS_FLAGS_OVERRIDE | - PX4IO_P_STATUS_FLAGS_RC_OK)); + PX4_ATOMIC_MODIFY_CLEAR(r_status_flags, ( + PX4IO_P_STATUS_FLAGS_OVERRIDE | + PX4IO_P_STATUS_FLAGS_RC_OK)); /* flag raw RC as lost */ r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_RC_OK); @@ -482,7 +484,7 @@ controls_tick() r_raw_rc_count = 0; /* Set the RC_LOST alarm */ - PX4_CRITICAL_SECTION(r_status_alarms |= PX4IO_P_STATUS_ALARMS_RC_LOST); + PX4_ATOMIC_MODIFY_OR(r_status_alarms, PX4IO_P_STATUS_ALARMS_RC_LOST); } /* @@ -525,14 +527,14 @@ controls_tick() } if (override) { - PX4_CRITICAL_SECTION(r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE); + PX4_ATOMIC_MODIFY_OR(r_status_flags, PX4IO_P_STATUS_FLAGS_OVERRIDE); } else { - PX4_CRITICAL_SECTION(r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE)); + PX4_ATOMIC_MODIFY_CLEAR(r_status_flags, PX4IO_P_STATUS_FLAGS_OVERRIDE); } } else { - PX4_CRITICAL_SECTION(r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE)); + PX4_ATOMIC_MODIFY_CLEAR(r_status_flags, PX4IO_P_STATUS_FLAGS_OVERRIDE); } } diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index d279a8fd0b..8659f11d3f 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -129,14 +129,14 @@ mixer_tick(void) isr_debug(1, "AP RX timeout"); } - PX4_CRITICAL_SECTION(r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK)); - PX4_CRITICAL_SECTION(r_status_alarms |= PX4IO_P_STATUS_ALARMS_FMU_LOST); + PX4_ATOMIC_MODIFY_CLEAR(r_status_flags, (PX4IO_P_STATUS_FLAGS_FMU_OK)); + PX4_ATOMIC_MODIFY_OR(r_status_alarms, PX4IO_P_STATUS_ALARMS_FMU_LOST); } else { - PX4_CRITICAL_SECTION(r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK); + PX4_ATOMIC_MODIFY_OR(r_status_flags, PX4IO_P_STATUS_FLAGS_FMU_OK); /* this flag is never cleared once OK */ - PX4_CRITICAL_SECTION(r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED); + PX4_ATOMIC_MODIFY_OR(r_status_flags, PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED); if (system_state.fmu_data_received_time > last_fmu_update) { new_fmu_data = true; @@ -231,7 +231,7 @@ mixer_tick(void) should_arm && /* and FMU is initialized */ (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED)) { - PX4_CRITICAL_SECTION(r_setup_arming |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE); + PX4_ATOMIC_MODIFY_OR(r_setup_arming, PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE); } /* @@ -245,10 +245,10 @@ mixer_tick(void) * Set failsafe status flag depending on mixing source */ if (source == MIX_FAILSAFE) { - PX4_CRITICAL_SECTION(r_status_flags |= PX4IO_P_STATUS_FLAGS_FAILSAFE); + PX4_ATOMIC_MODIFY_OR(r_status_flags, PX4IO_P_STATUS_FLAGS_FAILSAFE); } else { - PX4_CRITICAL_SECTION(r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FAILSAFE)); + PX4_ATOMIC_MODIFY_CLEAR(r_status_flags, (PX4IO_P_STATUS_FLAGS_FAILSAFE)); } /* @@ -336,14 +336,14 @@ mixer_tick(void) /* need to arm, but not armed */ up_pwm_servo_arm(true); mixer_servos_armed = true; - PX4_CRITICAL_SECTION(r_status_flags |= PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED); + PX4_ATOMIC_MODIFY_OR(r_status_flags, PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED); isr_debug(5, "> PWM enabled"); } else if (!needs_to_arm && mixer_servos_armed) { /* armed but need to disarm */ up_pwm_servo_arm(false); mixer_servos_armed = false; - PX4_CRITICAL_SECTION(r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)); + PX4_ATOMIC_MODIFY_CLEAR(r_status_flags, (PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)); isr_debug(5, "> PWM disabled"); } @@ -501,7 +501,7 @@ mixer_handle_text(const void *buffer, size_t length) } /* disable mixing, will be enabled once load is complete */ - PX4_CRITICAL_SECTION(r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_MIXER_OK)); + PX4_ATOMIC_MODIFY_CLEAR(r_status_flags, PX4IO_P_STATUS_FLAGS_MIXER_OK); /* abort if we're in the mixer - the caller is expected to retry */ if (in_mixer) { @@ -532,7 +532,7 @@ mixer_handle_text(const void *buffer, size_t length) /* check for overflow - this would be really fatal */ if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) { - PX4_CRITICAL_SECTION(r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK); + PX4_ATOMIC_MODIFY_CLEAR(r_status_flags, PX4IO_P_STATUS_FLAGS_MIXER_OK); return 0; } diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index ba0e3cf48e..795da1af7c 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -203,6 +203,12 @@ extern pwm_limit_t pwm_limit; #define PX4_CRITICAL_SECTION(cmd) { irqstate_t flags = px4_enter_critical_section(); cmd; px4_leave_critical_section(flags); } +#define PX4_ATOMIC_MODIFY_OR(target, modification) { if ((target | (modification)) != target) { PX4_CRITICAL_SECTION(target |= (modification)); } } + +#define PX4_ATOMIC_MODIFY_CLEAR(target, modification) { if ((target & ~(modification)) != target) { PX4_CRITICAL_SECTION(target &= ~(modification)); } } + +#define PX4_ATOMIC_MODIFY_AND(target, modification) { if ((target & (modification)) != target) { PX4_CRITICAL_SECTION(target &= (modification)); } } + /* * Mixer */ diff --git a/src/modules/px4iofirmware/safety.c b/src/modules/px4iofirmware/safety.c index 93715eb9dc..34f44b5abf 100644 --- a/src/modules/px4iofirmware/safety.c +++ b/src/modules/px4iofirmware/safety.c @@ -117,7 +117,7 @@ safety_check_button(void *arg) } else if (counter == ARM_COUNTER_THRESHOLD) { /* switch to armed state */ - PX4_CRITICAL_SECTION(r_status_flags |= PX4IO_P_STATUS_FLAGS_SAFETY_OFF); + PX4_ATOMIC_MODIFY_OR(r_status_flags, PX4IO_P_STATUS_FLAGS_SAFETY_OFF); counter++; } @@ -128,7 +128,7 @@ safety_check_button(void *arg) } else if (counter == ARM_COUNTER_THRESHOLD) { /* change to disarmed state and notify the FMU */ - PX4_CRITICAL_SECTION(r_status_flags &= ~PX4IO_P_STATUS_FLAGS_SAFETY_OFF); + PX4_ATOMIC_MODIFY_CLEAR(r_status_flags, PX4IO_P_STATUS_FLAGS_SAFETY_OFF); counter++; }