mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-06-30 06:30:35 +08:00
add subscriber handler and status displays
This commit is contained in:
committed by
Beat Küng
parent
ba11e0dc44
commit
9be8d6acc9
@@ -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;
|
||||
|
||||
|
||||
Reference in New Issue
Block a user