Skip to content
Snippets Groups Projects
Commit 3d2d6c4f authored by Lorenz Meier's avatar Lorenz Meier
Browse files

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.
parent dc6b688a
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
src/modules/px4iofirmware/controls.c
+ 23
− 21
View file @ 3d2d6c4f
......@@ -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);
}
}
......
src/modules/px4iofirmware/mixer.cpp
+ 11
− 11
View file @ 3d2d6c4f
......@@ -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;
}
......
src/modules/px4iofirmware/px4io.h
+ 6
− 0
View file @ 3d2d6c4f
......@@ -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
*/
......
src/modules/px4iofirmware/safety.c
+ 2
− 2
View file @ 3d2d6c4f
......@@ -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++;
}
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment