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

fixed px4io firmware read-modify-write bug

Browse Source
This commit is contained in:
Werner Stern
2017-05-02 11:50:42 +02:00
committed by Lorenz Meier
parent 65baf99832
commit dc6b688a6a
4 changed files with 35 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files

40
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;
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_ST24;
PX4_CRITICAL_SECTION(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;
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SUMD;
PX4_CRITICAL_SECTION(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) {
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM;
PX4_CRITICAL_SECTION(r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM);
}
if (st24_updated) {
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_ST24;
PX4_CRITICAL_SECTION(r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_ST24);
}
if (sumd_updated) {
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SUMD;
PX4_CRITICAL_SECTION(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) {
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS;
PX4_CRITICAL_SECTION(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) {
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM;
PX4_CRITICAL_SECTION(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 */
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK;
PX4_CRITICAL_SECTION(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 */
@@ -449,10 +449,10 @@ controls_tick()
rc_input_lost = true;
/* clear the input-kind flags here */
r_status_flags &= ~(
PX4IO_P_STATUS_FLAGS_RC_PPM |
PX4IO_P_STATUS_FLAGS_RC_DSM |
PX4IO_P_STATUS_FLAGS_RC_SBUS);
PX4_CRITICAL_SECTION(r_status_flags &= ~(
PX4IO_P_STATUS_FLAGS_RC_PPM |
PX4IO_P_STATUS_FLAGS_RC_DSM |
PX4IO_P_STATUS_FLAGS_RC_SBUS));
}
@@ -462,15 +462,15 @@ controls_tick()
/* if we are in failsafe, clear the override flag */
if (r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE) {
r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE);
PX4_CRITICAL_SECTION(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 */
r_status_flags &= ~(
PX4IO_P_STATUS_FLAGS_OVERRIDE |
PX4IO_P_STATUS_FLAGS_RC_OK);
PX4_CRITICAL_SECTION(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 +482,7 @@ controls_tick()
r_raw_rc_count = 0;
/* Set the RC_LOST alarm */
r_status_alarms |= PX4IO_P_STATUS_ALARMS_RC_LOST;
PX4_CRITICAL_SECTION(r_status_alarms |= PX4IO_P_STATUS_ALARMS_RC_LOST);
}
/*
@@ -525,14 +525,14 @@ controls_tick()
}
if (override) {
r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE;
PX4_CRITICAL_SECTION(r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE);
} else {
r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE);
PX4_CRITICAL_SECTION(r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE));
}
} else {
r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE);
PX4_CRITICAL_SECTION(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");
}
r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK);
r_status_alarms |= PX4IO_P_STATUS_ALARMS_FMU_LOST;
PX4_CRITICAL_SECTION(r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK));
PX4_CRITICAL_SECTION(r_status_alarms |= PX4IO_P_STATUS_ALARMS_FMU_LOST);
} else {
r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK;
PX4_CRITICAL_SECTION(r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK);
/* this flag is never cleared once OK */
r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED;
PX4_CRITICAL_SECTION(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)) {
r_setup_arming |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE;
PX4_CRITICAL_SECTION(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) {
r_status_flags |= PX4IO_P_STATUS_FLAGS_FAILSAFE;
PX4_CRITICAL_SECTION(r_status_flags |= PX4IO_P_STATUS_FLAGS_FAILSAFE);
} else {
r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FAILSAFE);
PX4_CRITICAL_SECTION(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;
r_status_flags |= PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED;
PX4_CRITICAL_SECTION(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;
r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED);
PX4_CRITICAL_SECTION(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 */
r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_MIXER_OK);
PX4_CRITICAL_SECTION(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)) {
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK;
PX4_CRITICAL_SECTION(r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK);
return 0;
}

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

@@ -201,6 +201,8 @@ extern pwm_limit_t pwm_limit;
#define CONTROL_PAGE_INDEX(_group, _channel) (_group * PX4IO_CONTROL_CHANNELS + _channel)
#define PX4_CRITICAL_SECTION(cmd) { irqstate_t flags = px4_enter_critical_section(); cmd; px4_leave_critical_section(flags); }
/*
* 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 */
r_status_flags |= PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
PX4_CRITICAL_SECTION(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 */
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
PX4_CRITICAL_SECTION(r_status_flags &= ~PX4IO_P_STATUS_FLAGS_SAFETY_OFF);
counter++;
}