void run()
{
while (1) {
node_spin_once();
chThdSleepMilliseconds(1);
}
}
int UavcanNode::run()
{
(void)pthread_mutex_lock(&_node_mutex);
// XXX figure out the output count
_output_count = 2;
_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
_test_motor_sub = orb_subscribe(ORB_ID(test_motor));
_actuator_direct_sub = orb_subscribe(ORB_ID(actuator_direct));
memset(&_outputs, 0, sizeof(_outputs));
/*
* Set up the time synchronization
*/
const int slave_init_res = _time_sync_slave.start();
if (slave_init_res < 0) {
warnx("Failed to start time_sync_slave");
_task_should_exit = true;
}
/* When we have a system wide notion of time update (i.e the transition from the initial
* System RTC setting to the GPS) we would call uavcan_stm32::clock::setUtc() when that
* happens, but for now we use adjustUtc with a correction of the hrt so that the
* time bases are the same
*/
uavcan_stm32::clock::adjustUtc(uavcan::UtcDuration::fromUSec(hrt_absolute_time()));
_master_timer.setCallback(TimerCallback(this, &UavcanNode::handle_time_sync));
_master_timer.startPeriodic(uavcan::MonotonicDuration::fromMSec(1000));
const int busevent_fd = ::open(uavcan_stm32::BusEvent::DevName, 0);
if (busevent_fd < 0) {
warnx("Failed to open %s", uavcan_stm32::BusEvent::DevName);
_task_should_exit = true;
}
/*
* XXX Mixing logic/subscriptions shall be moved into UavcanEscController::update();
* IO multiplexing shall be done here.
*/
_node.setModeOperational();
/*
* This event is needed to wake up the thread on CAN bus activity (RX/TX/Error).
* Please note that with such multiplexing it is no longer possible to rely only on
* the value returned from poll() to detect whether actuator control has timed out or not.
* Instead, all ORB events need to be checked individually (see below).
*/
add_poll_fd(busevent_fd);
/*
* setup poll to look for actuator direct input if we are
* subscribed to the topic
*/
if (_actuator_direct_sub != -1) {
_actuator_direct_poll_fd_num = add_poll_fd(_actuator_direct_sub);
}
while (!_task_should_exit) {
switch (_fw_server_action) {
case Start:
_fw_server_status = start_fw_server();
break;
case Stop:
_fw_server_status = stop_fw_server();
break;
case CheckFW:
_fw_server_status = request_fw_check();
break;
case None:
default:
break;
}
// update actuator controls subscriptions if needed
if (_groups_subscribed != _groups_required) {
subscribe();
_groups_subscribed = _groups_required;
}
// Mutex is unlocked while the thread is blocked on IO multiplexing
(void)pthread_mutex_unlock(&_node_mutex);
perf_end(_perfcnt_esc_mixer_total_elapsed); // end goes first, it's not a mistake
const int poll_ret = ::poll(_poll_fds, _poll_fds_num, PollTimeoutMs);
perf_begin(_perfcnt_esc_mixer_total_elapsed);
(void)pthread_mutex_lock(&_node_mutex);
node_spin_once(); // Non-blocking
bool new_output = false;
// this would be bad...
if (poll_ret < 0) {
DEVICE_LOG("poll error %d", errno);
continue;
} else {
// get controls for required topics
bool controls_updated = false;
for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (_control_subs[i] > 0) {
if (_poll_fds[_poll_ids[i]].revents & POLLIN) {
controls_updated = true;
orb_copy(_control_topics[i], _control_subs[i], &_controls[i]);
}
}
}
/*
see if we have any direct actuator updates
*/
if (_actuator_direct_sub != -1 &&
(_poll_fds[_actuator_direct_poll_fd_num].revents & POLLIN) &&
orb_copy(ORB_ID(actuator_direct), _actuator_direct_sub, &_actuator_direct) == OK &&
!_test_in_progress) {
if (_actuator_direct.nvalues > actuator_outputs_s::NUM_ACTUATOR_OUTPUTS) {
_actuator_direct.nvalues = actuator_outputs_s::NUM_ACTUATOR_OUTPUTS;
}
memcpy(&_outputs.output[0], &_actuator_direct.values[0],
_actuator_direct.nvalues * sizeof(float));
_outputs.noutputs = _actuator_direct.nvalues;
new_output = true;
}
// can we mix?
if (_test_in_progress) {
memset(&_outputs, 0, sizeof(_outputs));
if (_test_motor.motor_number < actuator_outputs_s::NUM_ACTUATOR_OUTPUTS) {
_outputs.output[_test_motor.motor_number] = _test_motor.value * 2.0f - 1.0f;
_outputs.noutputs = _test_motor.motor_number + 1;
}
new_output = true;
} else if (controls_updated && (_mixers != nullptr)) {
// XXX one output group has 8 outputs max,
// but this driver could well serve multiple groups.
unsigned num_outputs_max = 8;
// Do mixing
_outputs.noutputs = _mixers->mix(&_outputs.output[0], num_outputs_max, NULL);
new_output = true;
}
}
if (new_output) {
// iterate actuators, checking for valid values
for (uint8_t i = 0; i < _outputs.noutputs; i++) {
// last resort: catch NaN, INF and out-of-band errors
if (!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;
}
// never go below min
if (_outputs.output[i] < -1.0f) {
_outputs.output[i] = -1.0f;
}
// never go above max
if (_outputs.output[i] > 1.0f) {
_outputs.output[i] = 1.0f;
}
}
// Output to the bus
_outputs.timestamp = hrt_absolute_time();
perf_begin(_perfcnt_esc_mixer_output_elapsed);
_esc_controller.update_outputs(_outputs.output, _outputs.noutputs);
perf_end(_perfcnt_esc_mixer_output_elapsed);
}
// Check motor test state
bool updated = false;
orb_check(_test_motor_sub, &updated);
if (updated) {
orb_copy(ORB_ID(test_motor), _test_motor_sub, &_test_motor);
// Update the test status and check that we're not locked down
_test_in_progress = (_test_motor.value > 0);
_esc_controller.arm_single_esc(_test_motor.motor_number, _test_in_progress);
}
// Check arming state
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 and motor
// test is not running
bool set_armed = _armed.armed && !_armed.lockdown && !_test_in_progress;
arm_actuators(set_armed);
}
}
(void)::close(busevent_fd);
teardown();
warnx("exiting.");
exit(0);
}
int UavcanNode::run()
{
get_node().setRestartRequestHandler(&restart_request_handler);
while (init_indication_controller(get_node()) < 0) {
::syslog(LOG_INFO, "UAVCAN: Indication controller init failed\n");
::sleep(1);
}
while (init_sim_controller(get_node()) < 0) {
::syslog(LOG_INFO, "UAVCAN: sim controller init failed\n");
::sleep(1);
}
(void)pthread_mutex_lock(&_node_mutex);
/*
* Set up the time synchronization
*/
const int slave_init_res = _time_sync_slave.start();
if (slave_init_res < 0) {
warnx("Failed to start time_sync_slave");
_task_should_exit = true;
}
const unsigned PollTimeoutMs = 50;
const int busevent_fd = ::open(uavcan_stm32::BusEvent::DevName, 0);
if (busevent_fd < 0) {
warnx("Failed to open %s", uavcan_stm32::BusEvent::DevName);
_task_should_exit = true;
}
/* If we had an RTC we would call uavcan_stm32::clock::setUtc()
* but for now we use adjustUtc with a correction of 0
*/
// uavcan_stm32::clock::adjustUtc(uavcan::UtcDuration::fromUSec(0));
_node.setModeOperational();
/*
* This event is needed to wake up the thread on CAN bus activity (RX/TX/Error).
* Please note that with such multiplexing it is no longer possible to rely only on
* the value returned from poll() to detect whether actuator control has timed out or not.
* Instead, all ORB events need to be checked individually (see below).
*/
add_poll_fd(busevent_fd);
uint32_t start_tick = clock_systimer();
while (!_task_should_exit) {
// Mutex is unlocked while the thread is blocked on IO multiplexing
(void)pthread_mutex_unlock(&_node_mutex);
const int poll_ret = ::poll(_poll_fds, _poll_fds_num, PollTimeoutMs);
(void)pthread_mutex_lock(&_node_mutex);
node_spin_once(); // Non-blocking
// this would be bad...
if (poll_ret < 0) {
PX4_ERR("poll error %d", errno);
continue;
} else {
// Do Something
}
if (clock_systimer() - start_tick > TICK_PER_SEC) {
start_tick = clock_systimer();
resources("Udate:");
/*
* Printing the slave status information once a second
*/
const bool active = _time_sync_slave.isActive(); // Whether it can sync with a remote master
const int master_node_id = _time_sync_slave.getMasterNodeID().get(); // Returns an invalid Node ID if (active == false)
const long msec_since_last_adjustment = (_node.getMonotonicTime() - _time_sync_slave.getLastAdjustmentTime()).toMSec();
const uavcan::UtcTime utc = uavcan_stm32::clock::getUtc();
syslog(LOG_INFO, "Time:%lld\n"
" Time sync slave status:\n"
" Active: %d\n"
" Master Node ID: %d\n"
" Last clock adjustment was %ld ms ago\n",
utc .toUSec(), int(active), master_node_id, msec_since_last_adjustment);
syslog(LOG_INFO, "UTC %lu sec Rate corr: %fPPM Jumps: %lu Locked: %i\n\n",
static_cast<unsigned long>(utc.toMSec() / 1000),
static_cast<double>(uavcan_stm32::clock::getUtcRateCorrectionPPM()),
uavcan_stm32::clock::getUtcJumpCount(),
int(uavcan_stm32::clock::isUtcLocked()));
}
}
teardown();
warnx("exiting.");
exit(0);
}
