forked from Archive/PX4-Autopilot
IO: Allow to pre-arm the non-throttle channels with the safety switch
This commit is contained in:
parent
8bb9707f3f
commit
0ca6f46ef4
|
@ -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;
|
||||
|
|
Loading…
Reference in New Issue