Skip to content

Commit

Permalink
px4iofirmware: convert atomic macro into methods
Browse files Browse the repository at this point in the history 
Reduces flash size by 744 bytes.
CPU (loop time) is not affected.
  • Loading branch information
@bkueng @LorenzMeier
bkueng authored and LorenzMeier committed Nov 16, 2019
1 parent 0f2a7ad commit d965b92
Show file tree
Hide file tree
Showing 5 changed files with 59 additions and 40 deletions.
44 changes: 22 additions & 22 deletions src/modules/px4iofirmware/controls.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}
Expand All @@ -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) {
Expand Down Expand Up @@ -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;

Expand Down Expand Up @@ -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);
}
Expand All @@ -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);
Expand Down Expand Up @@ -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 */
Expand All @@ -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));

}

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

/*
Expand Down Expand Up @@ -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);
}
}

Expand Down
22 changes: 11 additions & 11 deletions src/modules/px4iofirmware/mixer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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 */
}

/*
Expand All @@ -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));
}

/*
Expand Down Expand Up @@ -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");
}

Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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;
}

Expand Down
21 changes: 21 additions & 0 deletions src/modules/px4iofirmware/px4io.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
*/
Expand Down
8 changes: 3 additions & 5 deletions src/modules/px4iofirmware/px4io.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
4 changes: 2 additions & 2 deletions src/modules/px4iofirmware/safety.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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++;
}

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

Expand Down

0 comments on commit d965b92

Please sign in to comment.