add subscriber handler and status displays

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

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;
 

src/modules/events/send_event.h

@@ -33,6 +33,9 @@
 
 #pragma once
 
+#include "subscriber_handler.h"
+#include "status_display.h"
+
 #include <px4_workqueue.h>
 #include <px4_module.h>
 #include <uORB/topics/vehicle_command.h>
@@ -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;
 };

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 <christoph@px4.io>
+ */
+
+#include "status_display.h"
+
+using namespace status;
+
+void StatusDisplay::set_leds()
+{
+	// Put your LED handling here
+}

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 <simone@px4.io>
+ * @author Beat Küng <beat-kueng@gmx.net>
+ * @editor Christoph Tobler <christoph@px4.io>
+ */
+
+#include "status_display.h"
+#include <drivers/drv_led.h>
+
+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);
+	}
+}

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 <simone@px4.io>
+ */
+
+#pragma once
+
+#include "subscriber_handler.h"
+
+#include <drivers/drv_hrt.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/battery_status.h>
+#include <uORB/topics/cpuload.h>
+#include <uORB/topics/led_control.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_status_flags.h>
+
+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 */

src/modules/events/subscriber_handler.cpp

@@ -0,0 +1,112 @@
+
+#include "subscriber_handler.h"
+
+#include <px4_log.h>
+
+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;
+	}
+}

src/modules/events/subscriber_handler.h

@@ -0,0 +1,64 @@
+#pragma once
+
+#include <uORB/uORB.h>
+#include <uORB/topics/battery_status.h>
+#include <uORB/topics/cpuload.h>
+#include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_status_flags.h>
+#include <uORB/topics/vehicle_attitude.h>
+
+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 */