From d965b928f8ef4985c0799e8ce64df4d9c4912a2b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Thu, 29 Aug 2019 15:54:28 +0200 Subject: [PATCH] px4iofirmware: convert atomic macro into methods Reduces flash size by 744 bytes. CPU (loop time) is not affected. --- src/modules/px4iofirmware/controls.c | 44 ++++++++++++++-------------- src/modules/px4iofirmware/mixer.cpp | 22 +++++++------- src/modules/px4iofirmware/px4io.c | 21 +++++++++++++ src/modules/px4iofirmware/px4io.h | 8 ++--- src/modules/px4iofirmware/safety.c | 4 +-- 5 files changed, 59 insertions(+), 40 deletions(-) diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 557ddd7f4d..fc50afe03a 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -130,7 +130,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_ATOMIC_MODIFY_OR(r_status_flags, PX4IO_P_STATUS_FLAGS_RC_ST24); + 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); } @@ -155,7 +155,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_ATOMIC_MODIFY_OR(r_status_flags, PX4IO_P_STATUS_FLAGS_RC_SUMD); + 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) { @@ -246,7 +246,7 @@ controls_tick() PX4IO_RC_INPUT_CHANNELS); if (sbus_updated) { - PX4_ATOMIC_MODIFY_OR(r_status_flags, PX4IO_P_STATUS_FLAGS_RC_SBUS); + atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_RC_SBUS); unsigned sbus_rssi = RC_INPUT_RSSI_MAX; @@ -284,7 +284,7 @@ controls_tick() if (ppm_updated) { - PX4_ATOMIC_MODIFY_OR(r_status_flags, PX4IO_P_STATUS_FLAGS_RC_PPM); + 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); } @@ -299,15 +299,15 @@ controls_tick() (void)dsm_port_input(&_rssi, &dsm_updated, &st24_updated, &sumd_updated); if (dsm_updated) { - PX4_ATOMIC_MODIFY_OR(r_status_flags, PX4IO_P_STATUS_FLAGS_RC_DSM); + atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_RC_DSM); } if (st24_updated) { - PX4_ATOMIC_MODIFY_OR(r_status_flags, PX4IO_P_STATUS_FLAGS_RC_ST24); + atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_RC_ST24); } if (sumd_updated) { - PX4_ATOMIC_MODIFY_OR(r_status_flags, PX4IO_P_STATUS_FLAGS_RC_SUMD); + atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_RC_SUMD); } perf_end(c_gather_dsm); @@ -439,7 +439,7 @@ controls_tick() } /* set RC OK flag, as we got an update */ - PX4_ATOMIC_MODIFY_OR(r_status_flags, PX4IO_P_STATUS_FLAGS_RC_OK); + 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 */ @@ -464,12 +464,12 @@ controls_tick() rc_input_lost = true; /* clear the input-kind flags here */ - 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)); + 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)); } @@ -479,15 +479,15 @@ controls_tick() /* if we are in failsafe, clear the override flag */ if (r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE) { - PX4_ATOMIC_MODIFY_CLEAR(r_status_flags, PX4IO_P_STATUS_FLAGS_OVERRIDE); + 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_ATOMIC_MODIFY_CLEAR(r_status_flags, ( - PX4IO_P_STATUS_FLAGS_OVERRIDE | - PX4IO_P_STATUS_FLAGS_RC_OK)); + 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); @@ -499,7 +499,7 @@ controls_tick() r_raw_rc_count = 0; /* Set the RC_LOST alarm */ - PX4_ATOMIC_MODIFY_OR(r_status_alarms, PX4IO_P_STATUS_ALARMS_RC_LOST); + atomic_modify_or(&r_status_alarms, PX4IO_P_STATUS_ALARMS_RC_LOST); } /* @@ -547,14 +547,14 @@ controls_tick() } if (override) { - PX4_ATOMIC_MODIFY_OR(r_status_flags, PX4IO_P_STATUS_FLAGS_OVERRIDE); + atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_OVERRIDE); } else { - PX4_ATOMIC_MODIFY_CLEAR(r_status_flags, PX4IO_P_STATUS_FLAGS_OVERRIDE); + atomic_modify_clear(&r_status_flags, PX4IO_P_STATUS_FLAGS_OVERRIDE); } } else { - PX4_ATOMIC_MODIFY_CLEAR(r_status_flags, PX4IO_P_STATUS_FLAGS_OVERRIDE); + 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 6be8fcd8cc..1f53f62f74 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -132,14 +132,14 @@ mixer_tick(void) isr_debug(1, "AP RX timeout"); } - 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); + atomic_modify_clear(&r_status_flags, (PX4IO_P_STATUS_FLAGS_FMU_OK)); + atomic_modify_or(&r_status_alarms, PX4IO_P_STATUS_ALARMS_FMU_LOST); } else { - PX4_ATOMIC_MODIFY_OR(r_status_flags, PX4IO_P_STATUS_FLAGS_FMU_OK); + atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_FMU_OK); /* this flag is never cleared once OK */ - PX4_ATOMIC_MODIFY_OR(r_status_flags, PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED); + atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED); if (fmu_data_received_time > last_fmu_update) { new_fmu_data = true; @@ -232,7 +232,7 @@ mixer_tick(void) (source == MIX_DISARMED) && /* and if we ended up not changing the default mixer */ should_arm && /* and we should be armed, so we intended to provide outputs */ (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED)) { /* and FMU is initialized */ - PX4_ATOMIC_MODIFY_OR(r_setup_arming, PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE); /* then FMU is dead -> terminate flight */ + atomic_modify_or(&r_setup_arming, PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE); /* then FMU is dead -> terminate flight */ } /* @@ -246,10 +246,10 @@ mixer_tick(void) * Set failsafe status flag depending on mixing source */ if (source == MIX_FAILSAFE) { - PX4_ATOMIC_MODIFY_OR(r_status_flags, PX4IO_P_STATUS_FLAGS_FAILSAFE); + atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_FAILSAFE); } else { - PX4_ATOMIC_MODIFY_CLEAR(r_status_flags, (PX4IO_P_STATUS_FLAGS_FAILSAFE)); + atomic_modify_clear(&r_status_flags, (PX4IO_P_STATUS_FLAGS_FAILSAFE)); } /* @@ -352,14 +352,14 @@ mixer_tick(void) /* need to arm, but not armed */ up_pwm_servo_arm(true); mixer_servos_armed = true; - PX4_ATOMIC_MODIFY_OR(r_status_flags, PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED); + 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_ATOMIC_MODIFY_CLEAR(r_status_flags, (PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)); + atomic_modify_clear(&r_status_flags, (PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)); isr_debug(5, "> PWM disabled"); } @@ -572,7 +572,7 @@ mixer_handle_text(const void *buffer, size_t length) } /* disable mixing, will be enabled once load is complete */ - PX4_ATOMIC_MODIFY_CLEAR(r_status_flags, PX4IO_P_STATUS_FLAGS_MIXER_OK); + atomic_modify_clear(&r_status_flags, PX4IO_P_STATUS_FLAGS_MIXER_OK); /* set the update flags to dirty so we reload those values after a mixer change */ update_trims = true; @@ -607,7 +607,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_ATOMIC_MODIFY_CLEAR(r_status_flags, PX4IO_P_STATUS_FLAGS_MIXER_OK); + atomic_modify_clear(&r_status_flags, PX4IO_P_STATUS_FLAGS_MIXER_OK); return 0; } diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index 7fab14894a..e55b03f606 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -92,6 +92,27 @@ static void heartbeat_blink(void); static void ring_blink(void); static void update_mem_usage(void); +void atomic_modify_or(volatile uint16_t *target, uint16_t modification) +{ + if ((*target | modification) != *target) { + PX4_CRITICAL_SECTION(*target |= modification); + } +} + +void atomic_modify_clear(volatile uint16_t *target, uint16_t modification) +{ + if ((*target & ~modification) != *target) { + PX4_CRITICAL_SECTION(*target &= ~modification); + } +} + +void atomic_modify_and(volatile uint16_t *target, uint16_t modification) +{ + if ((*target & modification) != *target) { + PX4_CRITICAL_SECTION(*target &= modification); + } +} + /* * add a debug message to be printed on the console */ diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 9f3171adc4..0a9d664ef0 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -186,11 +186,9 @@ extern output_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)); } } +void atomic_modify_or(volatile uint16_t *target, uint16_t modification); +void atomic_modify_clear(volatile uint16_t *target, uint16_t modification); +void atomic_modify_and(volatile uint16_t *target, uint16_t modification); /* * Mixer diff --git a/src/modules/px4iofirmware/safety.c b/src/modules/px4iofirmware/safety.c index 09d65ef61d..9239460e36 100644 --- a/src/modules/px4iofirmware/safety.c +++ b/src/modules/px4iofirmware/safety.c @@ -119,7 +119,7 @@ safety_check_button(void *arg) } else if (counter == ARM_COUNTER_THRESHOLD) { /* switch to armed state */ - PX4_ATOMIC_MODIFY_OR(r_status_flags, PX4IO_P_STATUS_FLAGS_SAFETY_OFF); + atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_SAFETY_OFF); counter++; } @@ -130,7 +130,7 @@ safety_check_button(void *arg) } else if (counter == ARM_COUNTER_THRESHOLD) { /* change to disarmed state and notify the FMU */ - PX4_ATOMIC_MODIFY_CLEAR(r_status_flags, PX4IO_P_STATUS_FLAGS_SAFETY_OFF); + atomic_modify_clear(&r_status_flags, PX4IO_P_STATUS_FLAGS_SAFETY_OFF); counter++; }