bizhang_-obav/src/drivers/qshell/qurt/qshell.cpp

/****************************************************************************
 *
 *   Copyright (c) 2016 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 qshell.cpp
 * Listener for shell commands from posix
 *
 * @author Nicolas de Palezieux <ndepal@gmail.com>
 */

#include "qshell.h"

#include <px4_log.h>
#include <px4_time.h>
#include <px4_posix.h>
#include <px4_defines.h>
#include <dspal_platform.h>

#include <unistd.h>
#include <stdio.h>
#include <string.h>
#include <iostream>
#include <fstream>
#include <sstream>
#include <signal.h>
#include <stdio.h>
#include <stdlib.h>

#include <uORB/topics/qshell_retval.h>
#include <drivers/drv_hrt.h>
#include "DriverFramework.hpp"

#define MAX_ARGS 8 // max number of whitespace separated args after app name

px4::AppState QShell::appState;

QShell::QShell()
{
	init_app_map(m_apps);
}

int QShell::main()
{
	appState.setRunning(true);
	int sub_qshell_req = orb_subscribe(ORB_ID(qshell_req));

	if (sub_qshell_req == PX4_ERROR) {
		PX4_ERR("Error subscribing to qshell_req topic");
		return -1;
	}

	px4_pollfd_struct_t fds[1] = {};
	fds[0].fd = sub_qshell_req;
	fds[0].events = POLLIN;

	orb_advert_t qshell_pub = nullptr;

	while (!appState.exitRequested()) {

		int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 1000);

		if (pret > 0 && fds[0].revents & POLLIN) {

			orb_copy(ORB_ID(qshell_req), sub_qshell_req, &m_qshell_req);

			PX4_INFO("qshell gotten: %s", m_qshell_req.cmd);
			char current_char;
			std::string arg;
			std::vector<std::string> appargs;

			for (unsigned str_idx = 0; str_idx < m_qshell_req.strlen; str_idx++) {
				current_char = m_qshell_req.cmd[str_idx];

				if (isspace(current_char)) { // split at spaces
					if (arg.length()) {
						appargs.push_back(arg);
						arg = "";
					}

				} else {
					arg += current_char;
				}
			}

			appargs.push_back(arg);  // push last argument

			struct qshell_retval_s retval;
			retval.return_value = run_cmd(appargs);
			retval.return_sequence = m_qshell_req.request_sequence;

			if (retval.return_value) {
				PX4_ERR("Failed to execute command: %s", m_qshell_req.cmd);

			} else {
				PX4_INFO("Ok executing command: %s", m_qshell_req.cmd);
			}

			int instance;
			orb_publish_auto(ORB_ID(qshell_retval), &qshell_pub, &retval, &instance, ORB_PRIO_DEFAULT);

		} else if (pret == 0) {
			// Timing out is fine.
		} else {
			// Something is wrong.
			usleep(10000);
		}
	}

	appState.setRunning(false);
	return 0;
}

int QShell::run_cmd(const std::vector<std::string> &appargs)
{
	// command is appargs[0]
	std::string command = appargs[0];

	if (command.compare("help") == 0) {
		list_builtins(m_apps);
		return 0;
	}

	//replaces app.find with iterator code to avoid null pointer exception
	for (apps_map_type::iterator it = m_apps.begin(); it != m_apps.end(); ++it) {
		if (it->first == command) {
			// one for command name, one for null terminator
			const char *arg[MAX_ARGS + 2];

			unsigned int i = 0;

			if (appargs.size() > MAX_ARGS + 1) {
				PX4_ERR("%d too many arguments in run_cmd", appargs.size() - (MAX_ARGS + 1));
				return 1;
			}

			while (i < appargs.size() && appargs[i].c_str()[0] != '\0') {
				arg[i] = (char *)appargs[i].c_str();
				PX4_DEBUG("  arg%d = '%s'\n", i, arg[i]);
				++i;
			}

			arg[i] = (char *)0;

			//PX4_DEBUG_PRINTF(i);
			if (m_apps[command] == NULL) {
				PX4_ERR("Null function !!\n");

			} else {
				return m_apps[command](i, (char **)arg);
			}

		}
	}

	PX4_ERR("Command %s not found", command.c_str());
	return 1;
}