yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 09:22:18 +00:00
Code Issues Packages Projects Releases Wiki Activity

px4iofirmware: convert atomic macro into methods

Browse Source 
Reduces flash size by 744 bytes.
CPU (loop time) is not affected.
This commit is contained in:
Beat Küng
2019-08-29 15:54:28 +02:00
committed by Lorenz Meier
parent 0f2a7ad2b7
commit d965b928f8
5 changed files with 59 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

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

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

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

21
src/modules/px4iofirmware/px4io.c
View File

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

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

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

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

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