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

PX4IO: Robustify firmware for mixer load operation 

This change makes the mixer load and reset operation closer to thread-safe. It was guarded one-way before and in only one location. This change ensures that its being locked out from both directions. The accesses to the locking variables still need work because they are non-atomic.
This commit is contained in:
Lorenz Meier 2016-12-26 12:57:23 +01:00
parent 5b70522541
commit 074e666b95
3 changed files with 30 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -66,10 +66,10 @@ extern "C" {
#define NAN_VALUE (0.0f/0.0f)
/* current servo arm/disarm state */
static bool mixer_servos_armed = false;
static bool should_arm = false;
static bool should_arm_nothrottle = false;
static bool should_always_enable_pwm = false;
static volatile bool mixer_servos_armed = false;
static volatile bool should_arm = false;
static volatile bool should_arm_nothrottle = false;
static volatile bool should_always_enable_pwm = false;
static volatile bool in_mixer = false;
extern int _sbus_fd;
@ -82,15 +82,33 @@ enum mixer_source {
MIX_FAILSAFE,
MIX_OVERRIDE_FMU_OK
};
static mixer_source source;
static volatile mixer_source source;
static int mixer_callback(uintptr_t handle,
uint8_t control_group,
uint8_t control_index,
float &control);
static int mixer_mix_threadsafe(float *outputs, volatile uint16_t *limits);
static MixerGroup mixer_group(mixer_callback, 0);
int mixer_mix_threadsafe(float *outputs, volatile uint16_t *limits)
{
/* poor mans mutex */
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) == 0) {
return 0;
}
uint16_t mixer_limits = 0;
in_mixer = true;
int mixcount = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT, &mixer_limits);
*limits = mixer_limits;
in_mixer = false;
return mixcount;
}
void
mixer_tick(void)
{
@ -255,10 +273,7 @@ mixer_tick(void)
}
/* mix */
/* poor mans mutex */
in_mixer = true;
mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT, &r_mixer_limits);
in_mixer = false;
mixed = mixer_mix_threadsafe(&outputs[0], &r_mixer_limits);
/* the pwm limit call takes care of out of band errors */
pwm_limit_calc(should_arm, should_arm_nothrottle, mixed, r_setup_pwm_reverse, r_page_servo_disarmed,
@ -549,7 +564,8 @@ mixer_set_failsafe()
}
/* mix */
mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT, &r_mixer_limits);
mixed = mixer_mix_threadsafe(&outputs[0], &r_mixer_limits);
/* scale to PWM and update the servo outputs as required */
for (unsigned i = 0; i < mixed; i++) {

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

@ -77,7 +77,7 @@
/*
* Registers.
*/
extern uint16_t r_page_status[]; /* PX4IO_PAGE_STATUS */
extern volatile uint16_t r_page_status[]; /* PX4IO_PAGE_STATUS */
extern uint16_t r_page_actuators[]; /* PX4IO_PAGE_ACTUATORS */
extern uint16_t r_page_servos[]; /* PX4IO_PAGE_SERVOS */
extern uint16_t r_page_raw_rc_input[]; /* PX4IO_PAGE_RAW_RC_INPUT */
@ -85,7 +85,7 @@ extern uint16_t r_page_rc_input[]; /* PX4IO_PAGE_RC_INPUT */
extern uint16_t r_page_adc[]; /* PX4IO_PAGE_RAW_ADC_INPUT */
extern volatile uint16_t r_page_setup[]; /* PX4IO_PAGE_SETUP */
extern volatile uint16_t r_page_controls[]; /* PX4IO_PAGE_CONTROLS */
extern uint16_t r_page_controls[]; /* PX4IO_PAGE_CONTROLS */
extern uint16_t r_page_rc_input_config[]; /* PX4IO_PAGE_RC_INPUT_CONFIG */
extern uint16_t r_page_servo_failsafe[]; /* PX4IO_PAGE_FAILSAFE_PWM */
extern uint16_t r_page_servo_control_min[]; /* PX4IO_PAGE_CONTROL_MIN_PWM */

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

@ -83,7 +83,7 @@ static const uint16_t r_page_config[] = {
*
* Status values.
*/
uint16_t r_page_status[] = {
volatile uint16_t r_page_status[] = {
[PX4IO_P_STATUS_FREEMEM] = 0,
[PX4IO_P_STATUS_CPULOAD] = 0,
[PX4IO_P_STATUS_FLAGS] = 0,
@ -214,7 +214,7 @@ volatile uint16_t r_page_setup[] = {
*
* Control values from the FMU.
*/
volatile uint16_t r_page_controls[PX4IO_CONTROL_GROUPS * PX4IO_CONTROL_CHANNELS];
uint16_t r_page_controls[PX4IO_CONTROL_GROUPS * PX4IO_CONTROL_CHANNELS];
/*
* PAGE 102 does not have a buffer.