diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
index 2c66cac9bbe92ea8cc84bb766c944c7c786cd311..462ce92deeca1f5e932a20a005eedd93326e4634 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 d279a8fd0b2cbb50361f2a7c22197b98c0289bff..8659f11d3f50d762b96613abae3e2bbb350a6812 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 ba0e3cf48e08dabcc023285859ce25182e35d0d6..795da1af7c6f9fe67f0d1e84c92c84c2c770a4dc 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 93715eb9dc908c66213624e569e396e7a2a6281a..34f44b5abf7136d105fdda34c5cac6f274627ae9 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++;
}