Linux: Added commands to show threads and devices

The list_tasks and list_devices commands will show
lists of running px4 threads and created virtual device nodes.

The list_builtins command was removes and the list of commands
will be shown if return is pressed.

Signed-off-by: Mark Charlebois <charlebm@gmail.com>

Tools/linux_apps.py

@@ -44,16 +44,25 @@ print
 print """
 #include <string>
 #include <map>
+
+#define __EXPORT 
+
+#include <px4_tasks.h>
+#include <px4_posix.h>
+
 using namespace std;
 
+extern void px4_show_devices(void);
+
 extern "C" {
 """
 for app in apps:
 	print "extern int "+app+"_main(int argc, char *argv[]);"
 
 print """
-static int list_builtins_main(int argc, char *argv[]);
 static int shutdown_main(int argc, char *argv[]);
+static int list_tasks_main(int argc, char *argv[]);
+static int list_devices_main(int argc, char *argv[]);
 }
 
 
@@ -66,21 +75,20 @@ static map<string,px4_main_t> app_map(void)
 for app in apps:
 	print '\tapps["'+app+'"] = '+app+'_main;'
 
-print '\tapps["list_builtins"] = list_builtins_main;'
 print '\tapps["shutdown"] = shutdown_main;'
+print '\tapps["list_tasks"] = list_tasks_main;'
+print '\tapps["list_devices"] = list_devices_main;'
 print """
 	return apps;
 }
 
 map<string,px4_main_t> apps = app_map();
 
-static int list_builtins_main(int argc, char *argv[])
+static void list_builtins(void)
 {
 	cout << "Builtin Commands:" << endl;
 	for (map<string,px4_main_t>::iterator it=apps.begin(); it!=apps.end(); ++it)
 		cout << '\t' << it->first << endl;
-
-	return 0;
 }
 
 static int shutdown_main(int argc, char *argv[])
@@ -88,5 +96,17 @@ static int shutdown_main(int argc, char *argv[])
 	cout << "Shutting down" << endl;
 	exit(0);
 }
+
+static int list_tasks_main(int argc, char *argv[])
+{
+	px4_show_tasks();
+	return 0;
+}
+
+static int list_devices_main(int argc, char *argv[])
+{
+	px4_show_devices();
+	return 0;
+}
 """
 

src/drivers/device/vdev.cpp

@@ -454,5 +454,16 @@ VDev *VDev::getDev(const char *path)
 	return NULL;
 }
 
+void VDev::showDevices()
+{
+	int i=0;
+	printf("Devices:\n");
+	for (; i<PX4_MAX_DEV; ++i) {
+		if (devmap[i]) {
+			printf("   %s\n", devmap[i]->name);
+		}
+	}
+}
+
 } // namespace device
 

src/drivers/device/vdev.h

@@ -64,10 +64,10 @@ struct px4_dev_handle_t {
 	int fd;
 	int flags;
 	void *priv;
-	void *cdev;
+	void *vdev;
 
-	px4_dev_handle_t() : fd(-1), flags(0), priv(NULL), cdev(NULL) {}
+	px4_dev_handle_t() : fd(-1), flags(0), priv(NULL), vdev(NULL) {}
-	px4_dev_handle_t(int f, void *c, int d) : fd(d), flags(f), priv(NULL), cdev(c) {}
+	px4_dev_handle_t(int f, void *c, int d) : fd(d), flags(f), priv(NULL), vdev(c) {}
 };
 
 /**
@@ -333,6 +333,7 @@ public:
 	virtual int	ioctl(px4_dev_handle_t *handlep, int cmd, unsigned long arg);
 
 	static VDev *getDev(const char *path);
+	static void showDevices(void);
 
 protected:
 

src/drivers/device/vdev_posix.cpp

@@ -117,7 +117,7 @@ int px4_close(int fd)
 {
 	int ret;
 	if (valid_fd(fd)) {
-		VDev *dev = (VDev *)(filemap[fd]->cdev);
+		VDev *dev = (VDev *)(filemap[fd]->vdev);
 		PX4_DEBUG("px4_close fd = %d\n", fd);
 		ret = dev->close(filemap[fd]);
 		filemap[fd] = NULL;
@@ -135,7 +135,7 @@ ssize_t px4_read(int fd, void *buffer, size_t buflen)
 {
 	int ret;
 	if (valid_fd(fd)) {
-		VDev *dev = (VDev *)(filemap[fd]->cdev);
+		VDev *dev = (VDev *)(filemap[fd]->vdev);
 		PX4_DEBUG("px4_read fd = %d\n", fd);
 		ret = dev->read(filemap[fd], (char *)buffer, buflen);
 	}
@@ -152,7 +152,7 @@ ssize_t px4_write(int fd, const void *buffer, size_t buflen)
 {
 	int ret = PX4_ERROR;
         if (valid_fd(fd)) {
-		VDev *dev = (VDev *)(filemap[fd]->cdev);
+		VDev *dev = (VDev *)(filemap[fd]->vdev);
 		PX4_DEBUG("px4_write fd = %d\n", fd);
 		ret = dev->write(filemap[fd], (const char *)buffer, buflen);
 	}
@@ -169,7 +169,7 @@ int px4_ioctl(int fd, int cmd, unsigned long arg)
 {
 	int ret = PX4_ERROR;
         if (valid_fd(fd)) {
-		VDev *dev = (VDev *)(filemap[fd]->cdev);
+		VDev *dev = (VDev *)(filemap[fd]->vdev);
 		PX4_DEBUG("px4_ioctl fd = %d\n", fd);
 		ret = dev->ioctl(filemap[fd], cmd, arg);
 	}
@@ -206,7 +206,7 @@ int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout)
 		// If fd is valid
 		if (valid_fd(fds[i].fd))
 		{
-			VDev *dev = (VDev *)(filemap[fds[i].fd]->cdev);;
+			VDev *dev = (VDev *)(filemap[fds[i].fd]->vdev);;
 			PX4_DEBUG("px4_poll: VDev->poll(setup) %d\n", fds[i].fd);
 			ret = dev->poll(filemap[fds[i].fd], &fds[i], true);
 
@@ -250,7 +250,7 @@ int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout)
 			// If fd is valid
 			if (valid_fd(fds[i].fd))
 			{
-				VDev *dev = (VDev *)(filemap[fds[i].fd]->cdev);;
+				VDev *dev = (VDev *)(filemap[fds[i].fd]->vdev);;
 				PX4_DEBUG("px4_poll: VDev->poll(teardown) %d\n", fds[i].fd);
 				ret = dev->poll(filemap[fds[i].fd], &fds[i], false);
 	
@@ -269,5 +269,10 @@ cleanup:
 	return count;
 }
 
+void px4_show_devices()
+{
+	VDev::showDevices();
+}
+
 }
 

src/platforms/linux/main.cpp

@@ -48,6 +48,7 @@ using namespace std;
 typedef int (*px4_main_t)(int argc, char *argv[]);
 
 #include "apps.h"
+#include "px4_middleware.h"
 
 void run_cmd(const vector<string> &appargs);
 void run_cmd(const vector<string> &appargs) {
@@ -62,12 +63,12 @@ void run_cmd(const vector<string> &appargs) {
 			++i;
 		}
 		arg[i] = (char *)0;
-		//cout << command << " " << i  << endl;
 		apps[command](i,(char **)arg);
 	}
 	else
 	{
 		cout << "Invalid command" << endl;
+		list_builtins();
 	}
 }
 
@@ -85,21 +86,16 @@ int main(int argc, char **argv)
 	if (argc == 2) {
 		ifstream infile(argv[1]);
 
-		//vector<string> tokens;
-
 		for (string line; getline(infile, line, '\n'); ) {
 			process_line(line);
 		}
 	}
 
 	string mystr;
-	vector<string> appargs(2);
-
-	appargs[0] = "list_builtins";
 	
-	while(1) {
+	px4::init(argc, argv, "mainapp");
-		run_cmd(appargs);
 
+	while(1) {
 		cout << "Enter a command and its args:" << endl;
 		getline (cin,mystr);
 		process_line(mystr);

src/platforms/linux/px4_layer/px4_linux_tasks.c

@@ -42,24 +42,28 @@
 #include <stdlib.h>
 #include <errno.h>
 #include <stdbool.h>
+#include <signal.h>
 #include <fcntl.h>
 #include <sched.h>
-#include <signal.h>
 #include <unistd.h>
-#include <float.h>
 #include <string.h>
 
 #include <sys/stat.h>
 #include <sys/types.h>
 
-//#include <systemlib/systemlib.h>
-
 #include <px4_tasks.h>
 
 #define MAX_CMD_LEN 100
 
 #define PX4_MAX_TASKS 100
-static pthread_t taskmap[PX4_MAX_TASKS] = {};
+typedef struct 
+{
+	pthread_t pid;
+	const char *name;
+	bool isused;
+} task_entry;
+
+static task_entry taskmap[PX4_MAX_TASKS] = {};
 
 typedef struct 
 {
@@ -69,15 +73,16 @@ typedef struct
 	// strings are allocated after the 
 } pthdata_t;
 
-void entry_adapter ( void *ptr );
+static void entry_adapter ( void *ptr )
-void entry_adapter ( void *ptr )
 {
 	pthdata_t *data;            
 	data = (pthdata_t *) ptr;  
 
 	data->entry(data->argc, data->argv);
 	free(ptr);
-	pthread_exit(0); 
+	printf("Before px4_task_exit\n");
+	px4_task_exit(0); 
+	printf("After px4_task_exit\n");
 } 
 
 void
@@ -170,9 +175,10 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int
 	printf("pthread_create task=%lu rv=%d\n",(unsigned long)task, rv);
 
 	for (i=0; i<PX4_MAX_TASKS; ++i) {
-		// FIXME - precludes pthread task to have an ID of 0
+		if (taskmap[i].isused == false) {
-		if (taskmap[i] == 0) {
+			taskmap[i].pid = task;
-			taskmap[i] = task;
+			taskmap[i].name = name;
+			taskmap[i].isused = true;
 			break;
 		}
 	}
@@ -186,21 +192,22 @@ int px4_task_delete(px4_task_t id)
 {
 	int rv = 0;
 	pthread_t pid;
-	//printf("Called px4_task_delete\n");
+	printf("Called px4_task_delete\n");
 
-	if (id < PX4_MAX_TASKS && taskmap[id] != 0)
+	if (id < PX4_MAX_TASKS && taskmap[id].isused)
-		pid = taskmap[id];
+		pid = taskmap[id].pid;
 	else
 		return -EINVAL;
 
 	// If current thread then exit, otherwise cancel
         if (pthread_self() == pid) {
+		taskmap[id].isused = false;
 		pthread_exit(0);
 	} else {
 		rv = pthread_cancel(pid);
 	}
 
-	taskmap[id] = 0;
+	taskmap[id].isused = false;
 
 	return rv;
 }
@@ -212,14 +219,15 @@ void px4_task_exit(int ret)
 
 	// Get pthread ID from the opaque ID
 	for (i=0; i<PX4_MAX_TASKS; ++i) {
-		// FIXME - precludes pthread task to have an ID of 0
+		if (taskmap[i].pid == pid) {
-		if (taskmap[i] == pid) {
+			taskmap[i].isused = false;
-			taskmap[i] = 0;
 			break;
 		}
 	}
 	if (i>=PX4_MAX_TASKS) 
 		printf("px4_task_exit: self task not found!\n");
+	else
+		printf("px4_task_exit: %s\n", taskmap[i].name);
 
 	pthread_exit((void *)(unsigned long)ret);
 }
@@ -229,7 +237,7 @@ void px4_killall(void)
 	//printf("Called px4_killall\n");
 	for (int i=0; i<PX4_MAX_TASKS; ++i) {
 		// FIXME - precludes pthread task to have an ID of 0
-		if (taskmap[i] != 0) {
+		if (taskmap[i].isused == true) {
 			px4_task_delete(i);
 		}
 	}
@@ -241,8 +249,8 @@ int px4_task_kill(px4_task_t id, int sig)
 	pthread_t pid;
 	//printf("Called px4_task_delete\n");
 
-	if (id < PX4_MAX_TASKS && taskmap[id] != 0)
+	if (id < PX4_MAX_TASKS && taskmap[id].pid != 0)
-		pid = taskmap[id];
+		pid = taskmap[id].pid;
 	else
 		return -EINVAL;
 
@@ -251,3 +259,21 @@ int px4_task_kill(px4_task_t id, int sig)
 
 	return rv;
 }
+
+void px4_show_tasks()
+{
+	int idx;
+	int count = 0;
+
+	printf("Active Tasks:\n");
+	for (idx=0; idx < PX4_MAX_TASKS; idx++)
+	{
+		if (taskmap[idx].isused) {
+			printf("   %-10s %lu\n", taskmap[idx].name, taskmap[idx].pid);
+			count++;
+		}
+	}
+	if (count == 0)
+		printf("   No running tasks\n");
+
+}

src/platforms/px4_posix.h

@@ -83,5 +83,6 @@ __EXPORT ssize_t	px4_read(int fd, void *buffer, size_t buflen);
 __EXPORT ssize_t	px4_write(int fd, const void *buffer, size_t buflen);
 __EXPORT int		px4_ioctl(int fd, int cmd, unsigned long arg);
 __EXPORT int		px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout);
+__EXPORT void		px4_show_devices(void);
 
 __END_DECLS

src/platforms/px4_tasks.h

@@ -95,5 +95,8 @@ __EXPORT int px4_task_kill(px4_task_t pid, int sig);
 /** Exit current task with return value **/
 __EXPORT void px4_task_exit(int ret);
 
+/** Show a list of running tasks **/
+__EXPORT void px4_show_tasks(void);
+
 __END_DECLS