dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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.
This commit is contained in:
Lorenz Meier 2017-05-03 22:44:24 +02:00
parent dc6b688a6a
commit 3d2d6c4fdc
4 changed files with 42 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

44
src/modules/px4iofirmware/controls.c
View File

@ -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);
}
}

22
src/modules/px4iofirmware/mixer.cpp
View File

@ -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;
}

6
src/modules/px4iofirmware/px4io.h
View File

@ -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
*/

4
src/modules/px4iofirmware/safety.c
View File

@ -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++;
}