From 9be8d6acc995526367bbf8e644dd7922db88ef60 Mon Sep 17 00:00:00 2001 From: ChristophTobler Date: Mon, 23 Oct 2017 10:32:26 +0200 Subject: [PATCH] add subscriber handler and status displays --- src/modules/events/CMakeLists.txt | 3 + src/modules/events/send_event.cpp | 21 ++-- src/modules/events/send_event.h | 6 +- src/modules/events/set_leds.cpp | 48 +++++++++ src/modules/events/status_display.cpp | 113 ++++++++++++++++++++++ src/modules/events/status_display.h | 101 +++++++++++++++++++ src/modules/events/subscriber_handler.cpp | 112 +++++++++++++++++++++ src/modules/events/subscriber_handler.h | 64 ++++++++++++ 8 files changed, 456 insertions(+), 12 deletions(-) create mode 100644 src/modules/events/set_leds.cpp create mode 100644 src/modules/events/status_display.cpp create mode 100644 src/modules/events/status_display.h create mode 100644 src/modules/events/subscriber_handler.cpp create mode 100644 src/modules/events/subscriber_handler.h diff --git a/src/modules/events/CMakeLists.txt b/src/modules/events/CMakeLists.txt index 72b0a27da7..13c5b16f99 100644 --- a/src/modules/events/CMakeLists.txt +++ b/src/modules/events/CMakeLists.txt @@ -43,6 +43,9 @@ px4_add_module( temperature_calibration/baro.cpp temperature_calibration/gyro.cpp temperature_calibration/task.cpp + subscriber_handler.cpp + status_display.cpp + set_leds.cpp DEPENDS platforms__common modules__uORB diff --git a/src/modules/events/send_event.cpp b/src/modules/events/send_event.cpp index 32f7782f8e..69698dda86 100644 --- a/src/modules/events/send_event.cpp +++ b/src/modules/events/send_event.cpp @@ -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; diff --git a/src/modules/events/send_event.h b/src/modules/events/send_event.h index 334cf842ac..46d0f14ce8 100644 --- a/src/modules/events/send_event.h +++ b/src/modules/events/send_event.h @@ -33,6 +33,9 @@ #pragma once +#include "subscriber_handler.h" +#include "status_display.h" + #include #include #include @@ -79,6 +82,7 @@ private: void answer_command(const vehicle_command_s &cmd, unsigned result); static struct work_s _work; - int _vehicle_command_sub = -1; + events::SubscriberHandler _subscriber_handler; + status::StatusDisplay _status_display; orb_advert_t _command_ack_pub = nullptr; }; diff --git a/src/modules/events/set_leds.cpp b/src/modules/events/set_leds.cpp new file mode 100644 index 0000000000..f589b1a08d --- /dev/null +++ b/src/modules/events/set_leds.cpp @@ -0,0 +1,48 @@ +/**************************************************************************** + * + * Copyright (c) 2017 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file set_leds.cpp + * Separate the set_leds() function + * + * @author Christoph Tobler + */ + +#include "status_display.h" + +using namespace status; + +void StatusDisplay::set_leds() +{ + // Put your LED handling here +} diff --git a/src/modules/events/status_display.cpp b/src/modules/events/status_display.cpp new file mode 100644 index 0000000000..1ab2739ceb --- /dev/null +++ b/src/modules/events/status_display.cpp @@ -0,0 +1,113 @@ +/**************************************************************************** + * + * Copyright (c) 2017 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file status_display.cpp + * Status Display: this decouples the LED and tune logic from the control logic in commander + * + * @author Simone Guscetti + * @author Beat Küng + * @editor Christoph Tobler + */ + +#include "status_display.h" +#include + +using namespace status; + +StatusDisplay::StatusDisplay(const events::SubscriberHandler &subscriber_handler) + : _subscriber_handler(subscriber_handler) +{ + // set the base color + _led_control.priority = 0; + _led_control.led_mask = 0xff; + _led_control.color = led_control_s::COLOR_CYAN; + _led_control.mode = led_control_s::MODE_ON; + publish(); + + _led_control.priority = 1; + _led_control.num_blinks = 0; // infinite blinking +} + +bool StatusDisplay::check_for_updates() +{ + bool got_updates = false; + + if (_subscriber_handler.battery_status_updated()) { + orb_copy(ORB_ID(battery_status), _subscriber_handler.get_battery_status_sub(), &_battery_status); + got_updates = true; + } + + if (_subscriber_handler.cpuload_updated()) { + orb_copy(ORB_ID(cpuload), _subscriber_handler.get_cpuload_sub(), &_cpu_load); + got_updates = true; + } + + if (_subscriber_handler.vehicle_status_flags_updated()) { + orb_copy(ORB_ID(vehicle_status_flags), _subscriber_handler.get_vehicle_status_flags_sub(), &_vehicle_status_flags); + got_updates = true; + } + + if (_subscriber_handler.vehicle_status_updated()) { + orb_copy(ORB_ID(vehicle_status), _subscriber_handler.get_vehicle_status_sub(), &_vehicle_status); + got_updates = true; + } + + if (_subscriber_handler.vehicle_attitude_updated()) { + orb_copy(ORB_ID(vehicle_attitude), _subscriber_handler.get_vehicle_attitude_sub(), &_vehicle_attitude); + got_updates = true; + } + + return got_updates; +} + +void StatusDisplay::process() +{ + if (!check_for_updates()) { + return; + } + + set_leds(); +} + +void StatusDisplay::publish() +{ + _led_control.timestamp = hrt_absolute_time(); + + if (_led_control_pub != nullptr) { + orb_publish(ORB_ID(led_control), _led_control_pub, &_led_control); + + } else { + _led_control_pub = orb_advertise_queue(ORB_ID(led_control), &_led_control, LED_UORB_QUEUE_LENGTH); + } +} diff --git a/src/modules/events/status_display.h b/src/modules/events/status_display.h new file mode 100644 index 0000000000..a0ed5160ce --- /dev/null +++ b/src/modules/events/status_display.h @@ -0,0 +1,101 @@ +/**************************************************************************** + * + * Copyright (c) 2017 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file status_display.h + * Status Display decouple the LED and tune form the original commander + * + * @author Simone Guscetti + */ + +#pragma once + +#include "subscriber_handler.h" + +#include + +#include +#include +#include +#include +#include +#include + +namespace status +{ + +class StatusDisplay +{ +public: + + StatusDisplay(const events::SubscriberHandler &subscriber_handler); + + /** regularily called to handle state updates */ + void process(); + +protected: + /** + * check for topic updates + * @return true if one or more topic got updated + */ + bool check_for_updates(); + + /** + * handle LED logic changes & call publish() + */ + void set_leds(); + + /** publish LED control */ + void publish(); + + // TODO: review if there is a better variant that allocate this in the memory + struct battery_status_s _battery_status = {}; + struct cpuload_s _cpu_load = {}; + struct vehicle_status_s _vehicle_status = {}; + struct vehicle_status_flags_s _vehicle_status_flags = {}; + struct vehicle_attitude_s _vehicle_attitude = {}; + + struct led_control_s _led_control = {}; + +private: + bool _old_gps_lock_valid = false; + bool _old_home_position_valid = false; + bool _low_battery = false; + bool _critical_battery = false; + int _old_nav_state = -1; + int _old_battery_status_warning = -1; + orb_advert_t _led_control_pub = nullptr; + const events::SubscriberHandler &_subscriber_handler; +}; + +} /* status */ diff --git a/src/modules/events/subscriber_handler.cpp b/src/modules/events/subscriber_handler.cpp new file mode 100644 index 0000000000..fe911c134f --- /dev/null +++ b/src/modules/events/subscriber_handler.cpp @@ -0,0 +1,112 @@ + +#include "subscriber_handler.h" + +#include + +using namespace events; + +void SubscriberHandler::subscribe() +{ + if (_battery_status_sub < 0) { + _battery_status_sub = orb_subscribe(ORB_ID(battery_status)); + } + + if (_cpuload_sub < 0) { + _cpuload_sub = orb_subscribe(ORB_ID(cpuload)); + } + + if (_vehicle_command_sub < 0) { + _vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command)); + } + + if (_vehicle_status_sub < 0) { + _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); + } + + if (_vehicle_status_flags_sub < 0) { + _vehicle_status_flags_sub = orb_subscribe(ORB_ID(vehicle_status_flags)); + } + + if (_vehicle_attitude_sub < 0) { + _vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + } +} + +void SubscriberHandler::unsubscribe() +{ + if (_battery_status_sub >= 0) { + orb_unsubscribe(_battery_status_sub); + _battery_status_sub = -1; + } + + if (_cpuload_sub >= 0) { + orb_unsubscribe(_cpuload_sub); + _cpuload_sub = -1; + } + + if (_vehicle_command_sub >= 0) { + orb_unsubscribe(_vehicle_command_sub); + _vehicle_command_sub = -1; + } + + if (_vehicle_status_sub >= 0) { + orb_unsubscribe(_vehicle_status_sub); + _vehicle_status_sub = -1; + } + + if (_vehicle_status_flags_sub >= 0) { + orb_unsubscribe(_vehicle_status_flags_sub); + _vehicle_status_flags_sub = -1; + } + + if (_vehicle_attitude_sub >= 0) { + orb_unsubscribe(_vehicle_attitude_sub); + _vehicle_attitude_sub = -1; + } +} + +void SubscriberHandler::check_for_updates() +{ + bool updated; + _update_bitfield = 0; + orb_check(_vehicle_command_sub, &updated); + + if (updated) { + _update_bitfield |= (uint32_t)StatusMask::VehicleCommand; + } + + updated = false; + orb_check(_vehicle_status_sub, &updated); + + if (updated) { + _update_bitfield |= (uint32_t)StatusMask::VehicleStatus; + } + + updated = false; + orb_check(_vehicle_status_flags_sub, &updated); + + if (updated) { + _update_bitfield |= (uint32_t)StatusMask::VehicleStatusFlags; + } + + updated = false; + orb_check(_battery_status_sub, &updated); + + if (updated) { + _update_bitfield |= (uint32_t)StatusMask::BatteryStatus; + } + + updated = false; + orb_check(_cpuload_sub, &updated); + + if (updated) { + _update_bitfield |= (uint32_t)StatusMask::CpuLoad; + } + + updated = false; + orb_check(_vehicle_attitude_sub, &updated); + + if (updated) { + _update_bitfield |= (uint32_t)StatusMask::VehicleAttitude; + } +} diff --git a/src/modules/events/subscriber_handler.h b/src/modules/events/subscriber_handler.h new file mode 100644 index 0000000000..a8fd3a28db --- /dev/null +++ b/src/modules/events/subscriber_handler.h @@ -0,0 +1,64 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace events +{ + +/** + * @class SubscriberHandler + * Contains a list of uORB subscriptions and maintains their update state. + */ +class SubscriberHandler +{ +public: + void subscribe(); + void unsubscribe(); + void check_for_updates(); + + int get_battery_status_sub() const { return _battery_status_sub; } + int get_cpuload_sub() const { return _cpuload_sub; } + int get_vehicle_command_sub() const { return _vehicle_command_sub; } + int get_vehicle_status_sub() const { return _vehicle_status_sub; } + int get_vehicle_status_flags_sub() const { return _vehicle_status_flags_sub; } + int get_vehicle_attitude_sub() const { return _vehicle_attitude_sub; } + // TODO: incorporate an add_topic method, this will push back the sub handler + // in the subscriber vector + + /* update checking methods */ + bool battery_status_updated() const { return _update_bitfield & (uint32_t)StatusMask::BatteryStatus; } + bool cpuload_updated() const { return _update_bitfield & (uint32_t)StatusMask::CpuLoad; } + bool vehicle_command_updated() const { return _update_bitfield & (uint32_t)StatusMask::VehicleCommand; } + bool vehicle_status_updated() const { return _update_bitfield & (uint32_t)StatusMask::VehicleStatus; } + bool vehicle_status_flags_updated() const { return _update_bitfield & (uint32_t)StatusMask::VehicleStatusFlags; } + bool vehicle_attitude_updated() const { return _update_bitfield & (uint32_t)StatusMask::VehicleAttitude; } + + +private: + enum class StatusMask : uint32_t { + VehicleCommand = (0x01 << 0), + VehicleStatus = (0x01 << 1), + VehicleStatusFlags = (0x01 << 2), + BatteryStatus = (0x01 << 3), + CpuLoad = (0x01 << 4), + VehicleAttitude = (0x01 << 5), + }; + + // TODO: incorporate the subscriber into a vector of int + int _battery_status_sub = -1; + int _cpuload_sub = -1; + int _vehicle_command_sub = -1; + int _vehicle_status_sub = -1; + int _vehicle_status_flags_sub = -1; + int _vehicle_attitude_sub = -1; + + uint32_t _update_bitfield = 0; +}; + +} /* events */