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

IO: Allow to pre-arm the non-throttle channels with the safety switch

This commit is contained in:
Lorenz Meier 2015-07-04 09:51:59 +02:00
parent 8bb9707f3f
commit 0ca6f46ef4
1 changed files with 18 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -62,6 +62,7 @@ extern "C" {
* Maximum interval in us before FMU signal is considered lost
*/
#define FMU_INPUT_DROP_LIMIT_US 500000
#define NAN_VALUE (0.0f/0.0f)
/* current servo arm/disarm state */
static bool mixer_servos_armed = false;
@ -243,12 +244,12 @@ mixer_tick(void)
in_mixer = false;
/* the pwm limit call takes care of out of band errors */
pwm_limit_calc(should_arm, mixed, r_setup_pwm_reverse, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
pwm_limit_calc(should_arm, should_arm_nothrottle, mixed, r_setup_pwm_reverse, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
/* clamp unused outputs to zero */
for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) {
r_page_servos[i] = 0;
outputs[i] = 0;
outputs[i] = 0.0f;
}
/* store normalized outputs */
@ -369,7 +370,14 @@ mixer_callback(uintptr_t handle,
}
}
/* motor spinup phase or only safety off, but not armed - lock throttle to zero */
/* limit output */
if (control > 1.0f) {
control = 1.0f;
} else if (control < -1.0f) {
control = -1.0f;
}
/* motor spinup phase - lock throttle to zero */
if ((pwm_limit.state == PWM_LIMIT_STATE_RAMP) || (should_arm_nothrottle && !should_arm)) {
if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE &&
control_index == actuator_controls_s::INDEX_THROTTLE) {
@ -380,11 +388,13 @@ mixer_callback(uintptr_t handle,
}
}
/* limit output */
if (control > 1.0f) {
control = 1.0f;
} else if (control < -1.0f) {
control = -1.0f;
/* only safety off, but not armed - set throttle as invalid */
if (should_arm_nothrottle && !should_arm) {
if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE &&
control_index == actuator_controls_s::INDEX_THROTTLE) {
/* mark the throttle as invalid */
control = NAN_VALUE;
}
}
return 0;