static 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;
/* mix */
mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT);
/* 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;
}
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;
}
in_mixer = true;
int mixcount = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT);
*limits = mixer_group.get_saturation_status();
in_mixer = false;
return mixcount;
}
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_group.mix(&outputs[0], PX4IO_SERVO_COUNT, &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
PX4FMU::task_main()
{
/* force a reset of the update rate */
_current_update_rate = 0;
_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
_param_sub = orb_subscribe(ORB_ID(parameter_update));
/* advertise the mixed control outputs */
actuator_outputs_s outputs;
memset(&outputs, 0, sizeof(outputs));
#ifdef HRT_PPM_CHANNEL
// rc input, published to ORB
struct rc_input_values rc_in;
orb_advert_t to_input_rc = 0;
memset(&rc_in, 0, sizeof(rc_in));
rc_in.input_source = RC_INPUT_SOURCE_PX4FMU_PPM;
#endif
/* initialize PWM limit lib */
pwm_limit_init(&_pwm_limit);
update_pwm_rev_mask();
/* loop until killed */
while (!_task_should_exit) {
if (_groups_subscribed != _groups_required) {
subscribe();
_groups_subscribed = _groups_required;
/* force setting update rate */
_current_update_rate = 0;
}
/*
* Adjust actuator topic update rate to keep up with
* the highest servo update rate configured.
*
* We always mix at max rate; some channels may update slower.
*/
unsigned max_rate = (_pwm_default_rate > _pwm_alt_rate) ? _pwm_default_rate : _pwm_alt_rate;
if (_current_update_rate != max_rate) {
_current_update_rate = max_rate;
int update_rate_in_ms = int(1000 / _current_update_rate);
/* reject faster than 500 Hz updates */
if (update_rate_in_ms < 2) {
update_rate_in_ms = 2;
}
/* reject slower than 10 Hz updates */
if (update_rate_in_ms > 100) {
update_rate_in_ms = 100;
}
debug("adjusted actuator update interval to %ums", update_rate_in_ms);
for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (_control_subs[i] > 0) {
orb_set_interval(_control_subs[i], update_rate_in_ms);
}
}
// set to current max rate, even if we are actually checking slower/faster
_current_update_rate = max_rate;
}
/* sleep waiting for data, stopping to check for PPM
* input at 50Hz */
int ret = ::poll(_poll_fds, _poll_fds_num, CONTROL_INPUT_DROP_LIMIT_MS);
/* this would be bad... */
if (ret < 0) {
log("poll error %d", errno);
continue;
} else if (ret == 0) {
/* timeout: no control data, switch to failsafe values */
// warnx("no PWM: failsafe");
} else {
/* get controls for required topics */
unsigned poll_id = 0;
for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (_control_subs[i] > 0) {
if (_poll_fds[poll_id].revents & POLLIN) {
orb_copy(_control_topics[i], _control_subs[i], &_controls[i]);
}
poll_id++;
}
}
/* can we mix? */
if (_mixers != nullptr) {
unsigned num_outputs;
switch (_mode) {
case MODE_2PWM:
num_outputs = 2;
break;
case MODE_4PWM:
num_outputs = 4;
break;
case MODE_6PWM:
num_outputs = 6;
break;
case MODE_8PWM:
num_outputs = 8;
break;
default:
num_outputs = 0;
break;
}
/* do mixing */
outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs, NULL);
outputs.timestamp = hrt_absolute_time();
/* iterate actuators */
for (unsigned i = 0; i < num_outputs; i++) {
/* last resort: catch NaN and INF */
if ((i >= outputs.noutputs) ||
!isfinite(outputs.output[i])) {
/*
* Value is NaN, INF or out of band - set to the minimum value.
* This will be clearly visible on the servo status and will limit the risk of accidentally
* spinning motors. It would be deadly in flight.
*/
outputs.output[i] = -1.0f;
}
}
uint16_t pwm_limited[num_outputs];
/* the PWM limit call takes care of out of band errors and constrains */
pwm_limit_calc(_servo_armed, num_outputs, _reverse_pwm_mask, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit);
/* output to the servos */
for (unsigned i = 0; i < num_outputs; i++) {
up_pwm_servo_set(i, pwm_limited[i]);
}
/* publish mixed control outputs */
if (_outputs_pub != nullptr) {
_outputs_pub = orb_advertise_multi(ORB_ID(actuator_outputs), &outputs, &_actuator_output_topic_instance, ORB_PRIO_DEFAULT);
} else {
orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &outputs);
}
}
}
/* check arming state */
bool updated = false;
orb_check(_armed_sub, &updated);
if (updated) {
orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
/* update the armed status and check that we're not locked down */
bool set_armed = _armed.armed && !_armed.lockdown;
if (_servo_armed != set_armed)
_servo_armed = set_armed;
/* update PWM status if armed or if disarmed PWM values are set */
bool pwm_on = (_armed.armed || _num_disarmed_set > 0);
if (_pwm_on != pwm_on) {
_pwm_on = pwm_on;
up_pwm_servo_arm(pwm_on);
}
}
orb_check(_param_sub, &updated);
if (updated) {
parameter_update_s pupdate;
orb_copy(ORB_ID(parameter_update), _param_sub, &pupdate);
update_pwm_rev_mask();
}
#ifdef HRT_PPM_CHANNEL
// see if we have new PPM input data
if (ppm_last_valid_decode != rc_in.timestamp_last_signal) {
// we have a new PPM frame. Publish it.
rc_in.channel_count = ppm_decoded_channels;
if (rc_in.channel_count > RC_INPUT_MAX_CHANNELS) {
rc_in.channel_count = RC_INPUT_MAX_CHANNELS;
}
for (uint8_t i = 0; i < rc_in.channel_count; i++) {
rc_in.values[i] = ppm_buffer[i];
}
rc_in.timestamp_publication = ppm_last_valid_decode;
rc_in.timestamp_last_signal = ppm_last_valid_decode;
rc_in.rc_ppm_frame_length = ppm_frame_length;
rc_in.rssi = RC_INPUT_RSSI_MAX;
rc_in.rc_failsafe = false;
rc_in.rc_lost = false;
rc_in.rc_lost_frame_count = 0;
rc_in.rc_total_frame_count = 0;
/* lazily advertise on first publication */
if (to_input_rc == 0) {
to_input_rc = orb_advertise(ORB_ID(input_rc), &rc_in);
} else {
orb_publish(ORB_ID(input_rc), to_input_rc, &rc_in);
}
}
#endif
}
for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (_control_subs[i] > 0) {
::close(_control_subs[i]);
_control_subs[i] = -1;
}
}
::close(_armed_sub);
::close(_param_sub);
/* make sure servos are off */
up_pwm_servo_deinit();
log("stopping");
/* note - someone else is responsible for restoring the GPIO config */
/* tell the dtor that we are exiting */
_task = -1;
_exit(0);
}
void
mixer_tick(void)
{
/* check that we are receiving fresh data from the FMU */
if (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;
}
/* default to failsafe mixing */
source = MIX_FAILSAFE;
/*
* Decide which set of controls we're using.
*/
/* do not mix if RAW_PWM mode is on and FMU is good */
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) {
/* don't actually mix anything - we already have raw PWM values */
source = MIX_NONE;
} 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;
}
if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) &&
!(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED)) {
/* if allowed, mix from RC inputs directly */
source = MIX_OVERRIDE;
}
}
/*
* 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);
}
/*
* 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] = (r_page_servos[i] - 1500) / 600.0f;
}
} else if (source != MIX_NONE) {
float outputs[PX4IO_SERVO_COUNT];
unsigned mixed;
uint16_t ramp_promille;
/* update esc init state, but only if we are truely armed and not just PWM enabled */
if (mixer_servos_armed && should_arm) {
switch (esc_state) {
/* after arming, some ESCs need an initalization period, count the time from here */
case ESC_OFF:
esc_init_time = hrt_absolute_time();
esc_state = ESC_INIT;
break;
/* after waiting long enough for the ESC initialization, we can start with the ramp to start the ESCs */
case ESC_INIT:
if (hrt_elapsed_time(&esc_init_time) > ESC_INIT_TIME_US) {
esc_state = ESC_RAMP;
}
break;
/* then ramp until the min speed is reached */
case ESC_RAMP:
if (hrt_elapsed_time(&esc_init_time) > (ESC_INIT_TIME_US + ESC_RAMP_TIME_US)) {
esc_state = ESC_ON;
}
break;
case ESC_ON:
default:
break;
}
} else {
esc_state = ESC_OFF;
}
/* do the calculations during the ramp for all at once */
if(esc_state == ESC_RAMP) {
ramp_promille = (1000*(hrt_elapsed_time(&esc_init_time)-ESC_INIT_TIME_US))/ESC_RAMP_TIME_US;
}
/* mix */
mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT);
/* scale to PWM and update the servo outputs as required */
for (unsigned i = 0; i < mixed; i++) {
/* save actuator values for FMU readback */
r_page_actuators[i] = FLOAT_TO_REG(outputs[i]);
switch (esc_state) {
case ESC_INIT:
r_page_servos[i] = (outputs[i] * 600 + 1500);
break;
case ESC_RAMP:
r_page_servos[i] = (outputs[i]
* (ramp_promille*r_page_servo_control_max[i] + (1000-ramp_promille)*2100 - ramp_promille*r_page_servo_control_min[i] - (1000-ramp_promille)*900)/2/1000
+ (ramp_promille*r_page_servo_control_max[i] + (1000-ramp_promille)*2100 + ramp_promille*r_page_servo_control_min[i] + (1000-ramp_promille)*900)/2/1000);
break;
case ESC_ON:
r_page_servos[i] = (outputs[i]
* (r_page_servo_control_max[i] - r_page_servo_control_min[i])/2
+ (r_page_servo_control_max[i] + r_page_servo_control_min[i])/2);
break;
case ESC_OFF:
default:
break;
}
}
for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++)
r_page_servos[i] = 0;
}
/*
* 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.
*
* XXX correct behaviour for failsafe may require an additional case
* here.
*/
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)
)
);
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);
if ((should_arm || should_always_enable_pwm) && !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 ((!should_arm && !should_always_enable_pwm) && 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) {
/* update the servo outputs. */
for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++)
up_pwm_servo_set(i, r_page_servos[i]);
} else if (mixer_servos_armed && should_always_enable_pwm) {
/* set the idle servo outputs. */
for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++)
up_pwm_servo_set(i, r_page_servo_idle[i]);
}
}
/**
* Main loop function
*/
void
PCA9685::i2cpwm()
{
if (_mode == IOX_MODE_TEST_OUT) {
setPin(0, PCA9685_PWMCENTER);
_should_run = true;
} else if (_mode == IOX_MODE_OFF) {
_should_run = false;
} else {
if (!_mode_on_initialized) {
// Init PWM limits
pwm_limit_init(&_pwm_limit);
// Get arming state
_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
/* Subscribe to actuator groups */
subscribe();
/* set the uorb update interval lower than the driver pwm interval */
for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; ++i) {
orb_set_interval(_control_subs[i], 1000.0f / PCA9685_PWMFREQ - 5);
}
_mode_on_initialized = true;
}
/* check if anything updated */
int ret = ::poll(_poll_fds, _poll_fds_num, 0);
if (ret < 0) {
DEVICE_LOG("poll error %d", errno);
} else if (ret == 0) {
// warnx("no PWM: failsafe");
} else {
/* get controls for required topics */
unsigned poll_id = 0;
for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (_control_subs[i] > 0) {
if (_poll_fds[poll_id].revents & POLLIN) {
orb_copy(_control_topics[i], _control_subs[i], &_controls[i]);
}
}
poll_id++;
}
if (_mixers != nullptr) {
size_t num_outputs = actuator_outputs_s::NUM_ACTUATOR_OUTPUTS;
// do mixing
num_outputs = _mixers->mix(_outputs, num_outputs, NULL);
// disable unused ports by setting their output to NaN
for (size_t i = 0; i < sizeof(_outputs) / sizeof(_outputs[0]); i++) {
if (i >= num_outputs) {
_outputs[i] = NAN_VALUE;
}
}
// Finally, write servo values to motors
for (int i = 0; i < num_outputs; i++) {
uint16_t new_value = PCA9685_PWMCENTER +
(_outputs[i] * M_PI_F * PCA9685_SCALE);
// DEVICE_DEBUG("%d: current: %u, new %u, control %.2f", i, _current_values[i], new_value,
// (double)_controls[1].control[i]);
if (isfinite(new_value) &&
new_value >= PCA9685_PWMMIN &&
new_value <= PCA9685_PWMMAX) {
setPin(i, new_value);
_rates[i] = new_value;
}
}
}
}
bool updated;
// Update Arming state
orb_check(_armed_sub, &updated);
if (updated) {
orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
bool set_armed = (_armed.armed || _armed.prearmed) && !_armed.lockdown;
if (_servo_armed != set_armed) {
_servo_armed = set_armed;
}
}
// Update AUX controls update
// orb_check(_actuator_controls_sub, &updated);
// if (updated) {
// size_t num_outputs = actuator_outputs_s::NUM_ACTUATOR_OUTPUTS;
//
// // Get updated actuator
// // Only update actuator 1 for now
// orb_copy(ORB_ID(actuator_controls_1), _actuator_controls_sub, &_controls[1]);
//
//
// }
_should_run = true;
}
// check if any activity remains, else stop
if (!_should_run) {
_running = false;
return;
}
// re-queue ourselves to run again later
_running = true;
work_queue(LPWORK, &_work, (worker_t)&PCA9685::i2cpwm_trampoline, this, _i2cpwm_interval);
}
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 RAW_PWM mode is on and FMU is good */
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) {
/* don't actually mix anything - we already have raw PWM values */
source = MIX_NONE;
} 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;
}
if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) &&
!(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) &&
!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) &&
/* do not enter manual override if we asked for termination failsafe and FMU is lost */
!(r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE)) {
/* if allowed, mix from RC inputs directly */
source = MIX_OVERRIDE;
} else if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) &&
!(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) &&
(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;
}
}
/*
* 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_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);
}
/*
* 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)) {
float outputs[PX4IO_SERVO_COUNT];
unsigned mixed;
/* mix */
/* poor mans mutex */
in_mixer = true;
mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT, &r_mixer_limits);
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);
for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++)
r_page_servos[i] = 0;
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_always_enable_pwm);
/* check any conditions that prevent arming */
if (r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN) {
needs_to_arm = false;
}
if (!should_arm && !should_always_enable_pwm) {
needs_to_arm = false;
}
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) {
/* 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(r_page_servos, PX4IO_SERVO_COUNT);
} else if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) {
sbus1_output(r_page_servos, PX4IO_SERVO_COUNT);
}
} else if (mixer_servos_armed && should_always_enable_pwm) {
/* set the disarmed servo outputs. */
for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++)
up_pwm_servo_set(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(r_page_servos, PX4IO_SERVO_COUNT);
if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT)
sbus2_output(r_page_servos, PX4IO_SERVO_COUNT);
}
}
void
mixer_tick(void)
{
/* check that we are receiving fresh data from the FMU */
if (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 | PX4IO_P_STATUS_FLAGS_RAW_PWM);
r_status_alarms |= PX4IO_P_STATUS_ALARMS_FMU_LOST;
} else {
r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK;
}
/* default to failsafe mixing */
source = MIX_FAILSAFE;
/*
* Decide which set of controls we're using.
*/
/* do not mix if mixer is invalid or if RAW_PWM mode is on and FMU is good */
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) &&
!(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
/* don't actually mix anything - we already have raw PWM values or
not a valid mixer. */
source = MIX_NONE;
} 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;
}
if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
/* if allowed, mix from RC inputs directly */
source = MIX_OVERRIDE;
}
}
/*
* 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);
}
/*
* Run the mixers.
*/
if (source == MIX_FAILSAFE) {
/* copy failsafe values to the servo outputs */
for (unsigned i = 0; i < IO_SERVO_COUNT; i++) {
r_page_servos[i] = r_page_servo_failsafe[i];
/* safe actuators for FMU feedback */
r_page_actuators[i] = (r_page_servos[i] - 1500) / 600.0f;
}
} else if (source != MIX_NONE) {
float outputs[IO_SERVO_COUNT];
unsigned mixed;
/* mix */
mixed = mixer_group.mix(&outputs[0], IO_SERVO_COUNT);
/* scale to PWM and update the servo outputs as required */
for (unsigned i = 0; i < mixed; i++) {
/* save actuator values for FMU readback */
r_page_actuators[i] = FLOAT_TO_REG(outputs[i]);
/* scale to servo output */
r_page_servos[i] = (outputs[i] * 600.0f) + 1500;
}
for (unsigned i = mixed; i < IO_SERVO_COUNT; i++)
r_page_servos[i] = 0;
}
/*
* 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.
*
* XXX correct behaviour for failsafe may require an additional case
* here.
*/
bool should_arm = (
/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) &&
/* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) &&
/* there is valid input via direct PWM or mixer */ (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)) &&
/* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) &&
/* FMU is available or FMU is not available but override is an option */
((r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) || (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && (r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ))
);
if (should_arm && !mixer_servos_armed) {
/* need to arm, but not armed */
up_pwm_servo_arm(true);
mixer_servos_armed = true;
} else if (!should_arm && mixer_servos_armed) {
/* armed but need to disarm */
up_pwm_servo_arm(false);
mixer_servos_armed = false;
}
if (mixer_servos_armed) {
/* update the servo outputs. */
for (unsigned i = 0; i < IO_SERVO_COUNT; i++)
up_pwm_servo_set(i, r_page_servos[i]);
}
}
void
mixer_tick(void)
{
bool should_arm;
/*
* Decide which set of inputs we're using.
*/
if (system_state.mixer_use_fmu) {
/* we have recent control data from the FMU */
control_count = PX4IO_CONTROL_CHANNELS;
control_values = &system_state.fmu_channel_data[0];
/* check that we are receiving fresh data from the FMU */
if (!system_state.fmu_data_received) {
fmu_input_drops++;
/* too many frames without FMU input, time to go to failsafe */
if (fmu_input_drops >= FMU_INPUT_DROP_LIMIT) {
system_state.mixer_use_fmu = false;
}
} else {
fmu_input_drops = 0;
system_state.fmu_data_received = false;
}
} else if (system_state.rc_channels > 0) {
/* we have control data from an R/C input */
control_count = system_state.rc_channels;
control_values = &system_state.rc_channel_data[0];
} else {
/* we have no control input */
/* XXX builtin failsafe would activate here */
control_count = 0;
}
/*
* Run the mixers if we have any control data at all.
*/
if (control_count > 0) {
float outputs[IO_SERVO_COUNT];
unsigned mixed;
/* mix */
mixed = mixer_group.mix(&outputs[0], IO_SERVO_COUNT);
/* scale to PWM and update the servo outputs as required */
for (unsigned i = 0; i < IO_SERVO_COUNT; i++) {
if (i < mixed) {
/* scale to servo output */
system_state.servos[i] = (outputs[i] * 500.0f) + 1500;
} else {
/* set to zero to inhibit PWM pulse output */
system_state.servos[i] = 0;
}
/*
* If we are armed, update the servo output.
*/
if (system_state.armed && system_state.arm_ok)
up_pwm_servo_set(i, system_state.servos[i]);
}
}
/*
* Decide whether the servos should be armed right now.
*/
should_arm = system_state.armed && system_state.arm_ok && (control_count > 0);
if (should_arm && !mixer_servos_armed) {
/* need to arm, but not armed */
up_pwm_servo_arm(true);
mixer_servos_armed = true;
} else if (!should_arm && mixer_servos_armed) {
/* armed but need to disarm */
up_pwm_servo_arm(false);
mixer_servos_armed = false;
}
}
bool MixerTest::mixerTest()
{
/*
* PWM limit structure
*/
pwm_limit_t pwm_limit;
bool should_arm = false;
uint16_t r_page_servo_disarmed[output_max];
uint16_t r_page_servo_control_min[output_max];
uint16_t r_page_servo_control_max[output_max];
uint16_t r_page_servos[output_max];
uint16_t servo_predicted[output_max];
int16_t reverse_pwm_mask = 0;
bool load_ok = load_mixer(MIXER_PATH(IO_pass.mix), 8);
if (!load_ok) {
return load_ok;
}
/* execute the mixer */
float outputs[output_max];
unsigned mixed;
const int jmax = 5;
pwm_limit_init(&pwm_limit);
/* run through arming phase */
for (unsigned i = 0; i < output_max; i++) {
actuator_controls[i] = 0.1f;
r_page_servo_disarmed[i] = PWM_MOTOR_OFF;
r_page_servo_control_min[i] = PWM_DEFAULT_MIN;
r_page_servo_control_max[i] = PWM_DEFAULT_MAX;
}
//PX4_INFO("PRE-ARM TEST: DISABLING SAFETY");
/* mix */
should_prearm = true;
mixed = mixer_group.mix(&outputs[0], output_max, nullptr);
pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min,
r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
for (unsigned i = 0; i < mixed; i++) {
//fprintf(stderr, "pre-arm:\t %d: out: %8.4f, servo: %d \n", i, (double)outputs[i], (int)r_page_servos[i]);
if (i != actuator_controls_s::INDEX_THROTTLE) {
if (r_page_servos[i] < r_page_servo_control_min[i]) {
warnx("active servo < min");
return false;
}
} else {
if (r_page_servos[i] != r_page_servo_disarmed[i]) {
warnx("throttle output != 0 (this check assumed the IO pass mixer!)");
return false;
}
}
}
should_arm = true;
should_prearm = false;
/* simulate another orb_copy() from actuator controls */
for (unsigned i = 0; i < output_max; i++) {
actuator_controls[i] = 0.1f;
}
//PX4_INFO("ARMING TEST: STARTING RAMP");
unsigned sleep_quantum_us = 10000;
hrt_abstime starttime = hrt_absolute_time();
unsigned sleepcount = 0;
while (hrt_elapsed_time(&starttime) < INIT_TIME_US + RAMP_TIME_US + 2 * sleep_quantum_us) {
/* mix */
mixed = mixer_group.mix(&outputs[0], output_max, nullptr);
pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min,
r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
for (unsigned i = 0; i < mixed; i++) {
//fprintf(stderr, "ramp:\t %d: out: %8.4f, servo: %d \n", i, (double)outputs[i], (int)r_page_servos[i]);
/* check mixed outputs to be zero during init phase */
if (hrt_elapsed_time(&starttime) < INIT_TIME_US &&
r_page_servos[i] != r_page_servo_disarmed[i]) {
PX4_ERR("disarmed servo value mismatch: %d vs %d", r_page_servos[i], r_page_servo_disarmed[i]);
return false;
}
if (hrt_elapsed_time(&starttime) >= INIT_TIME_US &&
r_page_servos[i] + 1 <= r_page_servo_disarmed[i]) {
PX4_ERR("ramp servo value mismatch");
return false;
}
}
usleep(sleep_quantum_us);
sleepcount++;
if (sleepcount % 10 == 0) {
fflush(stdout);
}
}
//PX4_INFO("ARMING TEST: NORMAL OPERATION");
for (int j = -jmax; j <= jmax; j++) {
for (unsigned i = 0; i < output_max; i++) {
actuator_controls[i] = j / 10.0f + 0.1f * i;
r_page_servo_disarmed[i] = PWM_LOWEST_MIN;
r_page_servo_control_min[i] = PWM_DEFAULT_MIN;
r_page_servo_control_max[i] = PWM_DEFAULT_MAX;
}
/* mix */
mixed = mixer_group.mix(&outputs[0], output_max, nullptr);
pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min,
r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
//fprintf(stderr, "mixed %d outputs (max %d)", mixed, output_max);
for (unsigned i = 0; i < mixed; i++) {
servo_predicted[i] = 1500 + outputs[i] * (r_page_servo_control_max[i] - r_page_servo_control_min[i]) / 2.0f;
if (abs(servo_predicted[i] - r_page_servos[i]) > MIXER_DIFFERENCE_THRESHOLD) {
fprintf(stderr, "\t %d: %8.4f predicted: %d, servo: %d\n", i, (double)outputs[i], servo_predicted[i],
(int)r_page_servos[i]);
PX4_ERR("mixer violated predicted value");
return false;
}
}
}
//PX4_INFO("ARMING TEST: DISARMING");
starttime = hrt_absolute_time();
sleepcount = 0;
should_arm = false;
while (hrt_elapsed_time(&starttime) < 600000) {
/* mix */
mixed = mixer_group.mix(&outputs[0], output_max, nullptr);
pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min,
r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
for (unsigned i = 0; i < mixed; i++) {
//fprintf(stderr, "disarmed:\t %d: out: %8.4f, servo: %d \n", i, (double)outputs[i], (int)r_page_servos[i]);
/* check mixed outputs to be zero during init phase */
if (r_page_servos[i] != r_page_servo_disarmed[i]) {
PX4_ERR("disarmed servo value mismatch");
return false;
}
}
usleep(sleep_quantum_us);
sleepcount++;
if (sleepcount % 10 == 0) {
//printf(".");
//fflush(stdout);
}
}
//printf("\n");
//PX4_INFO("ARMING TEST: REARMING: STARTING RAMP");
starttime = hrt_absolute_time();
sleepcount = 0;
should_arm = true;
while (hrt_elapsed_time(&starttime) < 600000 + RAMP_TIME_US) {
/* mix */
mixed = mixer_group.mix(&outputs[0], output_max, nullptr);
pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min,
r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
for (unsigned i = 0; i < mixed; i++) {
/* predict value */
servo_predicted[i] = 1500 + outputs[i] * (r_page_servo_control_max[i] - r_page_servo_control_min[i]) / 2.0f;
/* check ramp */
//fprintf(stderr, "ramp:\t %d: out: %8.4f, servo: %d \n", i, (double)outputs[i], (int)r_page_servos[i]);
if (hrt_elapsed_time(&starttime) < RAMP_TIME_US &&
(r_page_servos[i] + 1 <= r_page_servo_disarmed[i] ||
r_page_servos[i] > servo_predicted[i])) {
PX4_ERR("ramp servo value mismatch");
return false;
}
/* check post ramp phase */
if (hrt_elapsed_time(&starttime) > RAMP_TIME_US &&
abs(servo_predicted[i] - r_page_servos[i]) > 2) {
printf("\t %d: %8.4f predicted: %d, servo: %d\n", i, (double)outputs[i], servo_predicted[i], (int)r_page_servos[i]);
PX4_ERR("mixer violated predicted value");
return false;
}
}
usleep(sleep_quantum_us);
sleepcount++;
if (sleepcount % 10 == 0) {
// printf(".");
// fflush(stdout);
}
}
return true;
}
void
TAP_ESC::cycle()
{
if (!_initialized) {
_current_update_rate = 0;
/* advertise the mixed control outputs, insist on the first group output */
_outputs_pub = orb_advertise(ORB_ID(actuator_outputs), &_outputs);
_esc_feedback_pub = orb_advertise(ORB_ID(esc_report), &_esc_feedback);
_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
_test_motor_sub = orb_subscribe(ORB_ID(test_motor));
_initialized = true;
}
if (_groups_subscribed != _groups_required) {
subscribe();
_groups_subscribed = _groups_required;
_current_update_rate = 0;
}
unsigned max_rate = _pwm_default_rate ;
if (_current_update_rate != max_rate) {
_current_update_rate = max_rate;
int update_rate_in_ms = int(1000 / _current_update_rate);
/* reject faster than 500 Hz updates */
if (update_rate_in_ms < 2) {
update_rate_in_ms = 2;
}
/* reject slower than 10 Hz updates */
if (update_rate_in_ms > 100) {
update_rate_in_ms = 100;
}
DEVICE_DEBUG("adjusted actuator update interval to %ums", update_rate_in_ms);
for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (_control_subs[i] > 0) {
orb_set_interval(_control_subs[i], update_rate_in_ms);
}
}
// set to current max rate, even if we are actually checking slower/faster
_current_update_rate = max_rate;
}
/* check if anything updated */
int ret = ::poll(_poll_fds, _poll_fds_num, 5);
/* this would be bad... */
if (ret < 0) {
DEVICE_LOG("poll error %d", errno);
} else { /* update even in the case of a timeout, to check for test_motor commands */
/* get controls for required topics */
unsigned poll_id = 0;
for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (_control_subs[i] > 0) {
if (_poll_fds[poll_id].revents & POLLIN) {
orb_copy(_control_topics[i], _control_subs[i], &_controls[i]);
}
poll_id++;
}
}
size_t num_outputs = _channels_count;
/* can we mix? */
if (_is_armed && _mixers != nullptr) {
/* do mixing */
num_outputs = _mixers->mix(&_outputs.output[0], num_outputs, NULL);
_outputs.noutputs = num_outputs;
_outputs.timestamp = hrt_absolute_time();
/* disable unused ports by setting their output to NaN */
for (size_t i = num_outputs; i < sizeof(_outputs.output) / sizeof(_outputs.output[0]); i++) {
_outputs.output[i] = NAN;
}
/* iterate actuators */
for (unsigned i = 0; i < num_outputs; i++) {
/* last resort: catch NaN, INF and out-of-band errors */
if (i < _outputs.noutputs &&
PX4_ISFINITE(_outputs.output[i]) &&
_outputs.output[i] >= -1.0f &&
_outputs.output[i] <= 1.0f) {
/* scale for PWM output 1000 - 2000us */
_outputs.output[i] = 1600 + (350 * _outputs.output[i]);
} else {
/*
* Value is NaN, INF or out of band - set to the minimum value.
* This will be clearly visible on the servo status and will limit the risk of accidentally
* spinning motors. It would be deadly in flight.
*/
_outputs.output[i] = RPMSTOPPED;
}
}
} else {
_outputs.noutputs = num_outputs;
_outputs.timestamp = hrt_absolute_time();
/* check for motor test commands */
bool test_motor_updated;
orb_check(_test_motor_sub, &test_motor_updated);
if (test_motor_updated) {
struct test_motor_s test_motor;
orb_copy(ORB_ID(test_motor), _test_motor_sub, &test_motor);
_outputs.output[test_motor.motor_number] = RPMSTOPPED + ((RPMMAX - RPMSTOPPED) * test_motor.value);
PX4_INFO("setting motor %i to %.1lf", test_motor.motor_number,
(double)_outputs.output[test_motor.motor_number]);
}
/* set the invalid values to the minimum */
for (unsigned i = 0; i < num_outputs; i++) {
if (!PX4_ISFINITE(_outputs.output[i])) {
_outputs.output[i] = RPMSTOPPED;
}
}
/* disable unused ports by setting their output to NaN */
for (size_t i = num_outputs; i < sizeof(_outputs.output) / sizeof(_outputs.output[0]); i++) {
_outputs.output[i] = NAN;
}
}
const unsigned esc_count = num_outputs;
float motor_out[TAP_ESC_MAX_MOTOR_NUM];
// We need to remap from the system default to what PX4's normal
// scheme is
if (num_outputs == 6) {
motor_out[0] = _outputs.output[3];
motor_out[1] = _outputs.output[0];
motor_out[2] = _outputs.output[4];
motor_out[3] = _outputs.output[2];
motor_out[4] = _outputs.output[1];
motor_out[5] = _outputs.output[5];
motor_out[6] = RPMSTOPPED;
motor_out[7] = RPMSTOPPED;
} else {
// Use the system defaults
for (int i = 0; i < esc_count; ++i) {
motor_out[i] = _outputs.output[i];
}
}
send_esc_outputs(motor_out, esc_count);
read_data_from_uart();
if (parse_tap_esc_feedback(&uartbuf, &_packet) == true) {
if (_packet.msg_id == ESCBUS_MSG_ID_RUN_INFO) {
RunInfoRepsonse &feed_back_data = _packet.d.rspRunInfo;
if (feed_back_data.channelID < esc_status_s::CONNECTED_ESC_MAX) {
_esc_feedback.esc[feed_back_data.channelID].esc_rpm = feed_back_data.speed;
// _esc_feedback.esc[feed_back_data.channelID].esc_voltage = feed_back_data.voltage;
_esc_feedback.esc[feed_back_data.channelID].esc_state = feed_back_data.ESCStatus;
_esc_feedback.esc[feed_back_data.channelID].esc_vendor = esc_status_s::ESC_VENDOR_TAP;
// printf("vol is %d\n",feed_back_data.voltage );
// printf("speed is %d\n",feed_back_data.speed );
_esc_feedback.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_SERIAL;
_esc_feedback.counter++;
_esc_feedback.esc_count = esc_count;
_esc_feedback.timestamp = hrt_absolute_time();
orb_publish(ORB_ID(esc_status), _esc_feedback_pub, &_esc_feedback);
}
}
}
/* and publish for anyone that cares to see */
orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &_outputs);
}
bool updated;
orb_check(_armed_sub, &updated);
if (updated) {
orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
if (_is_armed != _armed.armed) {
/* reset all outputs */
for (size_t i = 0; i < sizeof(_outputs.output) / sizeof(_outputs.output[0]); i++) {
_outputs.output[i] = NAN;
}
}
/* do not obey the lockdown value, as lockdown is for PWMSim */
_is_armed = _armed.armed;
}
}
void
FMUServo::task_main()
{
/* configure for PWM output */
switch (_mode) {
case MODE_2PWM:
/* multi-port with flow control lines as PWM */
/* XXX magic numbers */
up_pwm_servo_init(0x3);
break;
case MODE_4PWM:
/* multi-port as 4 PWM outs */
/* XXX magic numbers */
up_pwm_servo_init(0xf);
break;
case MODE_NONE:
/* we should never get here... */
break;
}
/* subscribe to objects that we are interested in watching */
_t_actuators = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
orb_set_interval(_t_actuators, 20); /* 50Hz update rate */
_t_armed = orb_subscribe(ORB_ID(actuator_armed));
orb_set_interval(_t_armed, 100); /* 10Hz update rate */
struct pollfd fds[2];
fds[0].fd = _t_actuators;
fds[0].events = POLLIN;
fds[1].fd = _t_armed;
fds[1].events = POLLIN;
unsigned num_outputs = (_mode == MODE_2PWM) ? 2 : 4;
log("starting");
/* loop until killed */
while (!_task_should_exit) {
/* sleep waiting for data, but no more than 100ms */
int ret = ::poll(&fds[0], 2, 1000);
/* this would be bad... */
if (ret < 0) {
log("poll error %d", errno);
usleep(1000000);
continue;
}
/* do we have a control update? */
if (fds[0].revents & POLLIN) {
float outputs[num_outputs];
/* get controls - must always do this to avoid spinning */
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls);
/* can we mix? */
if (_mixers != nullptr) {
/* do mixing */
_mixers->mix(&outputs[0], num_outputs);
/* iterate actuators */
for (unsigned i = 0; i < num_outputs; i++) {
/* scale for PWM output 900 - 2100us */
up_pwm_servo_set(i, 1500 + (600 * outputs[i]));
}
}
}
/* how about an arming update? */
if (fds[1].revents & POLLIN) {
struct actuator_armed_s aa;
/* get new value */
orb_copy(ORB_ID(actuator_armed), _t_armed, &aa);
/* update PMW servo armed status */
up_pwm_servo_arm(aa.armed);
}
}
::close(_t_actuators);
::close(_t_armed);
/* make sure servos are off */
up_pwm_servo_deinit();
log("stopping");
/* note - someone else is responsible for restoring the GPIO config */
/* tell the dtor that we are exiting */
_task = -1;
_exit(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)) {
source = MIX_NONE;
} 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 */
/* poor mans mutex */
in_mixer = true;
mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT, &r_mixer_limits);
in_mixer = false;
/* 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);
}
}
}
void
PX4FMU::cycle()
{
if (!_initialized) {
/* force a reset of the update rate */
_current_update_rate = 0;
_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
//_param_sub = orb_subscribe(ORB_ID(parameter_update));
/* initialize PWM limit lib */
pwm_limit_init(&_pwm_limit);
update_pwm_rev_mask();
#ifdef RC_SERIAL_PORT
_sbus_fd = sbus_init(RC_SERIAL_PORT, true);
#endif
_initialized = true;
}
if (_groups_subscribed != _groups_required) {
subscribe();
_groups_subscribed = _groups_required;
/* force setting update rate */
_current_update_rate = 0;
}
/*
* Adjust actuator topic update rate to keep up with
* the highest servo update rate configured.
*
* We always mix at max rate; some channels may update slower.
*/
unsigned max_rate = (_pwm_default_rate > _pwm_alt_rate) ? _pwm_default_rate : _pwm_alt_rate;
if (_current_update_rate != max_rate) {
_current_update_rate = max_rate;
int update_rate_in_ms = int(1000 / _current_update_rate);
/* reject faster than 500 Hz updates */
if (update_rate_in_ms < 2) {
update_rate_in_ms = 2;
}
/* reject slower than 10 Hz updates */
if (update_rate_in_ms > 100) {
update_rate_in_ms = 100;
}
//DEVICE_DEBUG("adjusted actuator update interval to %ums\n", update_rate_in_ms);
for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (_control_subs[i] > 0) {
orb_set_interval(_control_subs[i], update_rate_in_ms);
}
}
// set to current max rate, even if we are actually checking slower/faster
_current_update_rate = max_rate;
}
/* check if anything updated */
//从mkblctrl-blctrl电子模块驱动拿数据，貌似没用到，而且poll里面也在等待定时器导致定时器卡死
int ret = 0;//::poll(_poll_fds, _poll_fds_num, 0);
/* this would be bad... */
if (ret < 0) {
DEVICE_LOG("poll error %d\n", ret);
} else if (ret == 0) {
/* timeout: no control data, switch to failsafe values */
// warnx("no PWM: failsafe\n");
} else {
/* get controls for required topics */
unsigned poll_id = 0;
for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (_control_subs[i] > 0) {
if (_poll_fds[poll_id].revents & POLLIN) {
orb_copy(_control_topics[i], _control_subs[i], &_controls[i]);
}
poll_id++;
}
}
/* can we mix? */
if (_mixers != NULL) {
size_t num_outputs;
switch (_mode) {
case MODE_2PWM:
num_outputs = 2;
break;
case MODE_4PWM:
num_outputs = 4;
break;
case MODE_6PWM:
num_outputs = 6;
break;
case MODE_8PWM:
num_outputs = 8;
break;
default:
num_outputs = 0;
break;
}
/* do mixing */
float outputs[_max_actuators];
num_outputs = _mixers->mix(outputs, num_outputs, NULL);
/* disable unused ports by setting their output to NaN */
for (size_t i = 0; i < sizeof(outputs) / sizeof(outputs[0]); i++) {
if (i >= num_outputs) {
outputs[i] = NAN_VALUE;
}
}
uint16_t pwm_limited[_max_actuators];
/* the PWM limit call takes care of out of band errors, NaN and constrains */
pwm_limit_calc(_servo_armed, arm_nothrottle(), num_outputs, _reverse_pwm_mask, _disarmed_pwm, _min_pwm, _max_pwm,
outputs, pwm_limited, &_pwm_limit);
/* output to the servos */
for (size_t i = 0; i < num_outputs; i++) {
up_pwm_servo_set(i, pwm_limited[i]);
}
publish_pwm_outputs(pwm_limited, num_outputs);
}
}
/* check arming state */
bool updated = false;
orb_check(_armed_sub, &updated);
if (updated) {
orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
/* update the armed status and check that we're not locked down */
bool set_armed = (_armed.armed || _armed.prearmed) && !_armed.lockdown;
if (_servo_armed != set_armed) {
_servo_armed = set_armed;
}
/* update PWM status if armed or if disarmed PWM values are set */
bool pwm_on = (set_armed || _num_disarmed_set > 0);
if (_pwm_on != pwm_on) {
_pwm_on = pwm_on;
up_pwm_servo_arm(pwm_on);
}
}
/* TODO:F
orb_check(_param_sub, &updated);
if (updated) {
parameter_update_s pupdate;
orb_copy(ORB_ID(parameter_update), _param_sub, &pupdate);
update_pwm_rev_mask();
}
*/
bool rc_updated = false;
#ifdef RC_SERIAL_PORT
bool sbus_failsafe, sbus_frame_drop;
uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS];
uint16_t raw_rc_count;
bool sbus_updated = sbus_input(_sbus_fd, &raw_rc_values[0], &raw_rc_count, &sbus_failsafe, &sbus_frame_drop,
input_rc_s::RC_INPUT_MAX_CHANNELS);
if (sbus_updated) {
// we have a new PPM frame. Publish it.
_rc_in.channel_count = raw_rc_count;
if (_rc_in.channel_count > input_rc_s::RC_INPUT_MAX_CHANNELS) {
_rc_in.channel_count = input_rc_s::RC_INPUT_MAX_CHANNELS;
}
for (uint8_t i = 0; i < _rc_in.channel_count; i++) {
_rc_in.values[i] = raw_rc_values[i];
// pilot_info("value[%d]=%d\n", i, _rc_in.values[i]);
}
_rc_in.timestamp_publication = hrt_absolute_time();
_rc_in.timestamp_last_signal = _rc_in.timestamp_publication;
_rc_in.rc_ppm_frame_length = 0;
_rc_in.rssi = (!sbus_frame_drop) ? RC_INPUT_RSSI_MAX : 0;
_rc_in.rc_failsafe = false;
_rc_in.rc_lost = false;
_rc_in.rc_lost_frame_count = 0;
_rc_in.rc_total_frame_count = 0;
rc_updated = true;
}
#endif
#ifdef HRT_PPM_CHANNEL
// see if we have new PPM input data
if ((ppm_last_valid_decode != _rc_in.timestamp_last_signal) &&
ppm_decoded_channels > 3) {
// we have a new PPM frame. Publish it.
_rc_in.channel_count = ppm_decoded_channels;
if (_rc_in.channel_count > input_rc_s::RC_INPUT_MAX_CHANNELS) {
_rc_in.channel_count = input_rc_s::RC_INPUT_MAX_CHANNELS;
}
for (uint8_t i = 0; i < _rc_in.channel_count; i++) {
_rc_in.values[i] = ppm_buffer[i];
}
_rc_in.timestamp_publication = ppm_last_valid_decode;
_rc_in.timestamp_last_signal = ppm_last_valid_decode;
_rc_in.rc_ppm_frame_length = ppm_frame_length;
_rc_in.rssi = RC_INPUT_RSSI_MAX;
_rc_in.rc_failsafe = false;
_rc_in.rc_lost = false;
_rc_in.rc_lost_frame_count = 0;
_rc_in.rc_total_frame_count = 0;
rc_updated = true;
}
#endif
if (rc_updated) {
/* lazily advertise on first publication */
if (_to_input_rc == NULL) {
_to_input_rc = orb_advertise(ORB_ID(input_rc), &_rc_in);
} else {
orb_publish(ORB_ID(input_rc), _to_input_rc, &_rc_in);
}
}
// xTimerStart(_work, (2/portTICK_PERIOD_MS));
}
void
mixer_tick(void)
{
/* check that we are receiving fresh data from the FMU */
if (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;
}
/* default to failsafe mixing */
source = MIX_FAILSAFE;
/*
* Decide which set of controls we're using.
*/
/* do not mix if RAW_PWM mode is on and FMU is good */
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) {
/* don't actually mix anything - we already have raw PWM values */
source = MIX_NONE;
} 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;
}
if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) &&
!(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) &&
!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) {
/* if allowed, mix from RC inputs directly */
source = MIX_OVERRIDE;
} else if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) &&
!(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) &&
(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;
}
}
/*
* 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);
}
/*
* 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.
*
* XXX correct behaviour for failsafe may require an additional case
* here.
*/
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)
)
);
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);
/*
* 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)) {
float outputs[PX4IO_SERVO_COUNT];
unsigned mixed;
/* mix */
/* poor mans mutex */
in_mixer = true;
mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT);
in_mixer = false;
pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++)
r_page_servos[i] = 0;
for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
r_page_actuators[i] = FLOAT_TO_REG(outputs[i]);
}
}
if ((should_arm || should_always_enable_pwm) && !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 ((!should_arm && !should_always_enable_pwm) && 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) {
/* update the servo outputs. */
for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++)
up_pwm_servo_set(i, r_page_servos[i]);
} else if (mixer_servos_armed && should_always_enable_pwm) {
/* set the disarmed servo outputs. */
for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++)
up_pwm_servo_set(i, r_page_servo_disarmed[i]);
}
}
void
mixer_tick(void)
{
bool should_arm;
/* check that we are receiving fresh data from the FMU */
if ((hrt_absolute_time() - system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) {
/* too many frames without FMU input, time to go to failsafe */
system_state.mixer_manual_override = true;
system_state.mixer_fmu_available = false;
lib_lowprintf("RX timeout\n");
}
/*
* Decide which set of inputs we're using.
*/
/* this is for planes, where manual override makes sense */
if (system_state.manual_override_ok) {
/* if everything is ok */
if (!system_state.mixer_manual_override && system_state.mixer_fmu_available) {
/* we have recent control data from the FMU */
control_count = PX4IO_CONTROL_CHANNELS;
control_values = &system_state.fmu_channel_data[0];
} else if (system_state.rc_channels > 0) {
/* when override is on or the fmu is not available, but RC is present */
control_count = system_state.rc_channels;
sched_lock();
/* remap roll, pitch, yaw and throttle from RC specific to internal ordering */
rc_channel_data[ROLL] = system_state.rc_channel_data[system_state.rc_map[ROLL] - 1];
rc_channel_data[PITCH] = system_state.rc_channel_data[system_state.rc_map[PITCH] - 1];
rc_channel_data[YAW] = system_state.rc_channel_data[system_state.rc_map[YAW] - 1];
rc_channel_data[THROTTLE] = system_state.rc_channel_data[system_state.rc_map[THROTTLE] - 1];
//rc_channel_data[OVERRIDE] = system_state.rc_channel_data[system_state.rc_map[OVERRIDE] - 1];
/* get the remaining channels, no remapping needed */
for (unsigned i = 4; i < system_state.rc_channels; i++) {
rc_channel_data[i] = system_state.rc_channel_data[i];
}
/* scale the control inputs */
rc_channel_data[THROTTLE] = ((float)(rc_channel_data[THROTTLE] - system_state.rc_min[THROTTLE]) /
(float)(system_state.rc_max[THROTTLE] - system_state.rc_min[THROTTLE])) * 1000.0f + 1000;
if (rc_channel_data[THROTTLE] > 2000) {
rc_channel_data[THROTTLE] = 2000;
}
if (rc_channel_data[THROTTLE] < 1000) {
rc_channel_data[THROTTLE] = 1000;
}
// lib_lowprintf("Tmin: %d Ttrim: %d Tmax: %d T: %d \n",
// (int)(system_state.rc_min[THROTTLE]), (int)(system_state.rc_trim[THROTTLE]),
// (int)(system_state.rc_max[THROTTLE]), (int)(rc_channel_data[THROTTLE]));
control_values = &rc_channel_data[0];
sched_unlock();
} else {
/* we have no control input (no FMU, no RC) */
// XXX builtin failsafe would activate here
control_count = 0;
}
//lib_lowprintf("R: %d P: %d Y: %d T: %d \n", control_values[0], control_values[1], control_values[2], control_values[3]);
/* this is for multicopters, etc. where manual override does not make sense */
} else {
/* if the fmu is available whe are good */
if (system_state.mixer_fmu_available) {
control_count = PX4IO_CONTROL_CHANNELS;
control_values = &system_state.fmu_channel_data[0];
/* we better shut everything off */
} else {
control_count = 0;
}
}
/*
* Run the mixers if we have any control data at all.
*/
if (control_count > 0) {
float outputs[IO_SERVO_COUNT];
unsigned mixed;
/* mix */
mixed = mixer_group.mix(&outputs[0], IO_SERVO_COUNT);
/* scale to PWM and update the servo outputs as required */
for (unsigned i = 0; i < IO_SERVO_COUNT; i++) {
if (i < mixed) {
/* scale to servo output */
system_state.servos[i] = (outputs[i] * 500.0f) + 1500;
} else {
/* set to zero to inhibit PWM pulse output */
system_state.servos[i] = 0;
}
/*
* If we are armed, update the servo output.
*/
if (system_state.armed && system_state.arm_ok) {
up_pwm_servo_set(i, system_state.servos[i]);
}
}
}
/*
* Decide whether the servos should be armed right now.
* A sufficient reason is armed state and either FMU or RC control inputs
*/
should_arm = system_state.armed && system_state.arm_ok && (control_count > 0);
if (should_arm && !mixer_servos_armed) {
/* need to arm, but not armed */
up_pwm_servo_arm(true);
mixer_servos_armed = true;
} else if (!should_arm && mixer_servos_armed) {
/* armed but need to disarm */
up_pwm_servo_arm(false);
mixer_servos_armed = false;
}
}