void
mixer_set_failsafe()
{
/*
* Check if a custom failsafe value has been written,
* or if the mixer is not ok and bail out.
*/
if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ||
!(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
return;
}
/* set failsafe defaults to the values for all inputs = 0 */
float outputs[PX4IO_SERVO_COUNT];
unsigned mixed;
if (REG_TO_FLOAT(r_setup_slew_max) > FLT_EPSILON) {
// maximum value the ouputs of the multirotor mixer are allowed to change in this cycle
// factor 2 is needed because actuator ouputs are in the range [-1,1]
float delta_out_max = 2.0f * 1000.0f * dt / (r_page_servo_control_max[0] - r_page_servo_control_min[0]) / REG_TO_FLOAT(
r_setup_slew_max);
mixer_group.set_max_delta_out_once(delta_out_max);
}
/* update parameter for mc thrust model if it updated */
if (update_mc_thrust_param) {
mixer_group.set_thrust_factor(REG_TO_FLOAT(r_setup_thr_fac));
update_mc_thrust_param = false;
}
/* mix */
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++) {
/* scale to servo output */
r_page_servo_failsafe[i] = (outputs[i] * 600.0f) + 1500;
}
/* disable the rest of the outputs */
for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) {
r_page_servo_failsafe[i] = 0;
}
}
void
mixer_tick(void)
{
/* check that we are receiving fresh data from the FMU */
if ((system_state.fmu_data_received_time == 0) ||
hrt_elapsed_time(&system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) {
/* too long without FMU input, time to go to failsafe */
if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) {
isr_debug(1, "AP RX timeout");
}
r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK);
r_status_alarms |= PX4IO_P_STATUS_ALARMS_FMU_LOST;
} else {
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;
}
/* default to failsafe mixing - it will be forced below if flag is set */
source = MIX_FAILSAFE;
/*
* Decide which set of controls we're using.
*/
/* Do not mix if we have raw PWM and FMU is ok. */
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) {
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) > 0) {
/* a channel based override has been
* triggered, with FMU active */
source = MIX_OVERRIDE_FMU_OK;
} else {
/* don't actually mix anything - copy values from r_page_direct_pwm */
source = MIX_NONE;
memcpy(r_page_servos, r_page_direct_pwm, sizeof(uint16_t)*PX4IO_SERVO_COUNT);
}
} else {
if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
/* mix from FMU controls */
source = MIX_FMU;
}
else if (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) {
if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) {
/* if allowed, mix from RC inputs directly up to available rc channels */
source = MIX_OVERRIDE_FMU_OK;
} else {
/* if allowed, mix from RC inputs directly */
source = MIX_OVERRIDE;
}
}
}
/*
* Decide whether the servos should be armed right now.
*
* We must be armed, and we must have a PWM source; either raw from
* FMU or from the mixer.
*
*/
should_arm = (
/* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
/* and IO is armed */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)
/* and FMU is armed */ && (
((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)
/* and there is valid input via or mixer */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK))
/* or direct PWM is set */ || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM)
/* or failsafe was set manually */ || ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM)
&& !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK))
)
);
should_arm_nothrottle = (
/* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
/* and IO is armed */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)
/* and there is valid input via or mixer */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK));
should_always_enable_pwm = (r_setup_arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE)
&& (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
&& (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK);
/*
* Check if failsafe termination is set - if yes,
* set the force failsafe flag once entering the first
* failsafe condition.
*/
if ( /* if we have requested flight termination style failsafe (noreturn) */
(r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) &&
/* and we ended up in a failsafe condition */
(source == MIX_FAILSAFE) &&
/* and we should be armed, so we intended to provide outputs */
should_arm &&
/* and FMU is initialized */
(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED)) {
r_setup_arming |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE;
}
/*
* Check if we should force failsafe - and do it if we have to
*/
if (r_setup_arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE) {
source = MIX_FAILSAFE;
}
/*
* Set failsafe status flag depending on mixing source
*/
if (source == MIX_FAILSAFE) {
r_status_flags |= PX4IO_P_STATUS_FLAGS_FAILSAFE;
} else {
r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FAILSAFE);
}
/*
* Set simple mixer trim values
* (there should be a "dirty" flag to indicate that r_page_servo_control_trim has changed)
*/
mixer_group.set_trims(r_page_servo_control_trim, PX4IO_SERVO_COUNT);
/*
* Run the mixers.
*/
if (source == MIX_FAILSAFE) {
/* copy failsafe values to the servo outputs */
for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
r_page_servos[i] = r_page_servo_failsafe[i];
/* safe actuators for FMU feedback */
r_page_actuators[i] = FLOAT_TO_REG((r_page_servos[i] - 1500) / 600.0f);
}
} else if (source != MIX_NONE && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)
&& !(r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN)) {
float outputs[PX4IO_SERVO_COUNT];
unsigned mixed;
if (REG_TO_FLOAT(r_setup_slew_max) > FLT_EPSILON) {
// maximum value the ouputs of the multirotor mixer are allowed to change in this cycle
// factor 2 is needed because actuator ouputs are in the range [-1,1]
float delta_out_max = 2.0f * 1000.0f * dt / (r_page_servo_control_max[0] - r_page_servo_control_min[0]) / REG_TO_FLOAT(
r_setup_slew_max);
mixer_group.set_max_delta_out_once(delta_out_max);
}
/* mix */
/* update parameter for mc thrust model if it updated */
if (update_mc_thrust_param) {
mixer_group.set_thrust_factor(REG_TO_FLOAT(r_setup_thr_fac));
update_mc_thrust_param = false;
}
/* mix */
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,
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.0f;
}
/* store normalized outputs */
for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
r_page_actuators[i] = FLOAT_TO_REG(outputs[i]);
}
}
/* set arming */
bool needs_to_arm = (should_arm || should_arm_nothrottle || should_always_enable_pwm);
/* lockdown means to send a valid pulse which disables the outputs */
if (r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN) {
needs_to_arm = true;
}
if (needs_to_arm && !mixer_servos_armed) {
/* need to arm, but not armed */
up_pwm_servo_arm(true);
mixer_servos_armed = true;
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);
isr_debug(5, "> PWM disabled");
}
if (mixer_servos_armed && (should_arm || should_arm_nothrottle)
&& !(r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN)) {
/* update the servo outputs. */
for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
up_pwm_servo_set(i, r_page_servos[i]);
}
/* set S.BUS1 or S.BUS2 outputs */
if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) {
sbus2_output(_sbus_fd, r_page_servos, PX4IO_SERVO_COUNT);
} else if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) {
sbus1_output(_sbus_fd, r_page_servos, PX4IO_SERVO_COUNT);
}
} else if (mixer_servos_armed && (should_always_enable_pwm
|| (r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN))) {
/* set the disarmed servo outputs. */
for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
up_pwm_servo_set(i, r_page_servo_disarmed[i]);
/* copy values into reporting register */
r_page_servos[i] = r_page_servo_disarmed[i];
}
/* set S.BUS1 or S.BUS2 outputs */
if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) {
sbus1_output(_sbus_fd, r_page_servo_disarmed, PX4IO_SERVO_COUNT);
}
if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) {
sbus2_output(_sbus_fd, r_page_servo_disarmed, PX4IO_SERVO_COUNT);
}
}
}
