void task_main(int argc, char *argv[])
{
_is_running = true;
if (uart_initialize(_device) < 0) {
PX4_ERR("Failed to initialize UART.");
return;
}
// Subscribe for orb topics
_controls_sub = orb_subscribe(ORB_ID(actuator_controls_0));
_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
// Start disarmed
_armed.armed = false;
_armed.prearmed = false;
// Set up poll topic
px4_pollfd_struct_t fds[1];
fds[0].fd = _controls_sub;
fds[0].events = POLLIN;
/* Don't limit poll intervall for now, 250 Hz should be fine. */
//orb_set_interval(_controls_sub, 10);
// Set up mixer
if (initialize_mixer(MIXER_FILENAME) < 0) {
PX4_ERR("Mixer initialization failed.");
return;
}
pwm_limit_init(&_pwm_limit);
// TODO XXX: this is needed otherwise we crash in the callback context.
_rc_pub = orb_advertise(ORB_ID(input_rc), &_rc);
// Main loop
while (!_task_should_exit) {
int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 10);
/* Timed out, do a periodic check for _task_should_exit. */
if (pret == 0) {
continue;
}
/* This is undesirable but not much we can do. */
if (pret < 0) {
PX4_WARN("poll error %d, %d", pret, errno);
/* sleep a bit before next try */
usleep(100000);
continue;
}
if (fds[0].revents & POLLIN) {
orb_copy(ORB_ID(actuator_controls_0), _controls_sub, &_controls);
_outputs.timestamp = _controls.timestamp;
/* do mixing */
_outputs.noutputs = _mixer->mix(_outputs.output, 0 /* not used */, NULL);
/* disable unused ports by setting their output to NaN */
for (size_t i = _outputs.noutputs; i < sizeof(_outputs.output) / sizeof(_outputs.output[0]); i++) {
_outputs.output[i] = NAN;
}
const uint16_t reverse_mask = 0;
// TODO FIXME: these should probably be params
const uint16_t disarmed_pwm[4] = {900, 900, 900, 900};
const uint16_t min_pwm[4] = {1230, 1230, 1230, 1230};
const uint16_t max_pwm[4] = {1900, 1900, 1900, 1900};
uint16_t pwm[4];
// TODO FIXME: pre-armed seems broken
pwm_limit_calc(_armed.armed, false/*_armed.prearmed*/, _outputs.noutputs, reverse_mask,
disarmed_pwm, min_pwm, max_pwm, _outputs.output, pwm, &_pwm_limit);
send_outputs_mavlink(pwm, 4);
if (_outputs_pub != nullptr) {
orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &_outputs);
} else {
_outputs_pub = orb_advertise(ORB_ID(actuator_outputs), &_outputs);
}
}
bool updated;
orb_check(_armed_sub, &updated);
if (updated) {
orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
}
}
uart_deinitialize();
orb_unsubscribe(_controls_sub);
orb_unsubscribe(_armed_sub);
_is_running = false;
}
void task_main(int argc, char *argv[])
{
PX4_INFO("enter uart_esc task_main");
_outputs_pub = nullptr;
parameters_init();
esc = UartEsc::get_instance();
if (esc == NULL) {
PX4_ERR("failed to new UartEsc instance");
} else if (esc->initialize((enum esc_model_t)_parameters.model,
_device, _parameters.baudrate) < 0) {
PX4_ERR("failed to initialize UartEsc");
} else {
// Subscribe for orb topics
_controls_sub = orb_subscribe(ORB_ID(actuator_controls_0)); // single group for now
_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
_param_sub = orb_subscribe(ORB_ID(parameter_update));
// initialize publication structures
memset(&_outputs, 0, sizeof(_outputs));
// set up poll topic and limit poll interval
px4_pollfd_struct_t fds[1];
fds[0].fd = _controls_sub;
fds[0].events = POLLIN;
//orb_set_interval(_controls_sub, 10); // max actuator update period, ms
// set up mixer
if (initialize_mixer(MIXER_FILENAME) < 0) {
PX4_ERR("Mixer initialization failed.");
_task_should_exit = true;
}
// Main loop
while (!_task_should_exit) {
int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
/* timed out - periodic check for _task_should_exit */
if (pret == 0) {
continue;
}
/* this is undesirable but not much we can do - might want to flag unhappy status */
if (pret < 0) {
PX4_WARN("poll error %d, %d", pret, errno);
/* sleep a bit before next try */
usleep(100000);
continue;
}
// Handle new actuator controls data
if (fds[0].revents & POLLIN) {
// Grab new controls data
orb_copy(ORB_ID(actuator_controls_0), _controls_sub, &_controls);
// Mix to the outputs
_outputs.timestamp = hrt_absolute_time();
int16_t motor_rpms[UART_ESC_MAX_MOTORS];
if (_armed.armed) {
_outputs.noutputs = mixer->mix(&_outputs.output[0],
actuator_controls_s::NUM_ACTUATOR_CONTROLS,
NULL);
// Make sure we support only up to UART_ESC_MAX_MOTORS motors
if (_outputs.noutputs > UART_ESC_MAX_MOTORS) {
PX4_ERR("Unsupported motors %d, up to %d motors supported",
_outputs.noutputs, UART_ESC_MAX_MOTORS);
continue;
}
// Send outputs to the ESCs
for (unsigned outIdx = 0; outIdx < _outputs.noutputs; outIdx++) {
// map -1.0 - 1.0 outputs to RPMs
motor_rpms[outIdx] = (int16_t)(((_outputs.output[outIdx] + 1.0) / 2.0) *
(esc->max_rpm() - esc->min_rpm()) + esc->min_rpm());
}
uart_esc_rotate_motors(motor_rpms, _outputs.noutputs);
} else {
_outputs.noutputs = UART_ESC_MAX_MOTORS;
for (unsigned outIdx = 0; outIdx < _outputs.noutputs; outIdx++) {
motor_rpms[outIdx] = 0;
_outputs.output[outIdx] = -1.0;
}
}
esc->send_rpms(&motor_rpms[0], _outputs.noutputs);
// TODO-JYW: TESTING-TESTING
// MAINTAIN FOR REFERENCE, COMMENT OUT FOR RELEASE BUILDS
// static int count=0;
// count++;
// if (!(count % 100)) {
// PX4_DEBUG(" ");
// PX4_DEBUG("Time t: %13lu, Armed: %d ",(unsigned long)_outputs.timestamp,_armed.armed);
// PX4_DEBUG("Act Controls: 0: %+8.4f, 1: %+8.4f, 2: %+8.4f, 3: %+8.4f ",_controls.control[0],_controls.control[1],_controls.control[2],_controls.control[3]);
// PX4_DEBUG("Act Outputs : 0: %+8.4f, 1: %+8.4f, 2: %+8.4f, 3: %+8.4f ",_outputs.output[0],_outputs.output[1],_outputs.output[2],_outputs.output[3]);
// }
// TODO-JYW: TESTING-TESTING
/* Publish mixed control outputs */
if (_outputs_pub != nullptr) {
orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &_outputs);
} else {
_outputs_pub = orb_advertise(ORB_ID(actuator_outputs), &_outputs);
}
}
// Check for updates in other subscripions
bool updated = false;
orb_check(_armed_sub, &updated);
if (updated) {
orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
PX4_DEBUG("arming status updated, _armed.armed: %d", _armed.armed);
}
orb_check(_param_sub, &updated);
if (updated) {
// Even though we are only interested in the update status of the parameters, copy
// the subscription to clear the update status.
orb_copy(ORB_ID(parameter_update), _param_sub, &_param_update);
parameters_update();
}
}
}
PX4_WARN("closing uart_esc");
delete esc;
}
