add subscriber handler and status displays

This commit is contained in:
ChristophTobler
2017-10-23 10:32:26 +02:00
committed by Beat Küng
parent ba11e0dc44
commit 9be8d6acc9
8 changed files with 456 additions and 12 deletions
+10 -11
View File
@@ -64,6 +64,7 @@ int SendEvent::task_spawn(int argc, char *argv[])
}
SendEvent::SendEvent()
: _status_display(_subscriber_handler)
{
}
@@ -73,7 +74,8 @@ int SendEvent::start()
return 0;
}
_vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command));
// subscribe to the topics
_subscriber_handler.subscribe();
// Kick off the cycling. We can call it directly because we're already in the work queue context
cycle();
@@ -105,33 +107,30 @@ SendEvent::cycle_trampoline(void *arg)
void SendEvent::cycle()
{
if (should_exit()) {
if (_vehicle_command_sub >= 0) {
orb_unsubscribe(_vehicle_command_sub);
_vehicle_command_sub = -1;
}
_subscriber_handler.unsubscribe();
exit_and_cleanup();
return;
}
_subscriber_handler.check_for_updates();
process_commands();
_status_display.process();
work_queue(LPWORK, &_work, (worker_t)&SendEvent::cycle_trampoline, this,
USEC2TICK(SEND_EVENT_INTERVAL_US));
}
void SendEvent::process_commands()
{
bool updated;
orb_check(_vehicle_command_sub, &updated);
if (!updated) {
if (!_subscriber_handler.vehicle_command_updated()) {
return;
}
struct vehicle_command_s cmd;
orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd);
orb_copy(ORB_ID(vehicle_command), _subscriber_handler.get_vehicle_command_sub(), &cmd);
bool got_temperature_calibration_command = false, accel = false, baro = false, gyro = false;