|
|
|
|
@ -50,6 +50,7 @@
@@ -50,6 +50,7 @@
|
|
|
|
|
#include "px4_middleware.h" |
|
|
|
|
#include "DriverFramework.hpp" |
|
|
|
|
#include <termios.h> |
|
|
|
|
#include <sys/stat.h> |
|
|
|
|
|
|
|
|
|
namespace px4 |
|
|
|
|
{ |
|
|
|
|
@ -62,6 +63,12 @@ typedef int (*px4_main_t)(int argc, char *argv[]);
@@ -62,6 +63,12 @@ typedef int (*px4_main_t)(int argc, char *argv[]);
|
|
|
|
|
|
|
|
|
|
#define CMD_BUFF_SIZE 100 |
|
|
|
|
|
|
|
|
|
#ifdef PATH_MAX |
|
|
|
|
const unsigned path_max_len = PATH_MAX; |
|
|
|
|
#else |
|
|
|
|
const unsigned path_max_len = 1024; |
|
|
|
|
#endif |
|
|
|
|
|
|
|
|
|
static bool _ExitFlag = false; |
|
|
|
|
|
|
|
|
|
static struct termios orig_term; |
|
|
|
|
@ -85,6 +92,96 @@ extern "C" {
@@ -85,6 +92,96 @@ extern "C" {
|
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
static inline bool fileExists(const string &name) |
|
|
|
|
{ |
|
|
|
|
struct stat buffer; |
|
|
|
|
return (stat(name.c_str(), &buffer) == 0); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
static inline bool dirExists(const string &path) |
|
|
|
|
{ |
|
|
|
|
struct stat info; |
|
|
|
|
|
|
|
|
|
if (stat(path.c_str(), &info) != 0) { |
|
|
|
|
return false; |
|
|
|
|
|
|
|
|
|
} else if (info.st_mode & S_IFDIR) { |
|
|
|
|
return true; |
|
|
|
|
|
|
|
|
|
} else { |
|
|
|
|
return false; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
static inline void touch(const string &name) |
|
|
|
|
{ |
|
|
|
|
fstream fs; |
|
|
|
|
fs.open(name, ios::out); |
|
|
|
|
fs.close(); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
static int mkpath(const char *path, mode_t mode); |
|
|
|
|
|
|
|
|
|
static int do_mkdir(const char *path, mode_t mode) |
|
|
|
|
{ |
|
|
|
|
struct stat st; |
|
|
|
|
int status = 0; |
|
|
|
|
|
|
|
|
|
if (stat(path, &st) != 0) { |
|
|
|
|
/* Directory does not exist. EEXIST for race condition */ |
|
|
|
|
if (mkdir(path, mode) != 0 && errno != EEXIST) { |
|
|
|
|
status = -1; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
} else if (!S_ISDIR(st.st_mode)) { |
|
|
|
|
errno = ENOTDIR; |
|
|
|
|
status = -1; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
return (status); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
/**
|
|
|
|
|
** mkpath - ensure all directories in path exist |
|
|
|
|
** Algorithm takes the pessimistic view and works top-down to ensure |
|
|
|
|
** each directory in path exists, rather than optimistically creating |
|
|
|
|
** the last element and working backwards. |
|
|
|
|
*/ |
|
|
|
|
static int mkpath(const char *path, mode_t mode) |
|
|
|
|
{ |
|
|
|
|
char *pp; |
|
|
|
|
char *sp; |
|
|
|
|
int status; |
|
|
|
|
char *copypath = strdup(path); |
|
|
|
|
|
|
|
|
|
status = 0; |
|
|
|
|
pp = copypath; |
|
|
|
|
|
|
|
|
|
while (status == 0 && (sp = strchr(pp, '/')) != 0) { |
|
|
|
|
if (sp != pp) { |
|
|
|
|
/* Neither root nor double slash in path */ |
|
|
|
|
*sp = '\0'; |
|
|
|
|
status = do_mkdir(copypath, mode); |
|
|
|
|
*sp = '/'; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
pp = sp + 1; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
if (status == 0) { |
|
|
|
|
status = do_mkdir(path, mode); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
free(copypath); |
|
|
|
|
return (status); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
static string pwd() |
|
|
|
|
{ |
|
|
|
|
char temp[path_max_len]; |
|
|
|
|
return (getcwd(temp, path_max_len) ? string(temp) : string("")); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
static void print_prompt() |
|
|
|
|
{ |
|
|
|
|
cout.flush(); |
|
|
|
|
@ -134,12 +231,13 @@ static void run_cmd(const vector<string> &appargs, bool exit_on_fail, bool silen
@@ -134,12 +231,13 @@ static void run_cmd(const vector<string> &appargs, bool exit_on_fail, bool silen
|
|
|
|
|
static void usage() |
|
|
|
|
{ |
|
|
|
|
|
|
|
|
|
cout << "./px4 [-d] [startup_config] -h" << std::endl; |
|
|
|
|
cout << "./px4 [-d] data_directory startup_config [-h]" << endl; |
|
|
|
|
cout << " -d - Optional flag to run the app in daemon mode and does not listen for user input." << |
|
|
|
|
std::endl; |
|
|
|
|
cout << " This is needed if px4 is intended to be run as a upstart job on linux" << std::endl; |
|
|
|
|
cout << "<startup_config> - config file for starting/stopping px4 modules" << std::endl; |
|
|
|
|
cout << " -h - help/usage information" << std::endl; |
|
|
|
|
endl; |
|
|
|
|
cout << " This is needed if px4 is intended to be run as a upstart job on linux" << endl; |
|
|
|
|
cout << "<data_directory> - directory where romfs and posix-configs are located" << endl; |
|
|
|
|
cout << "<startup_config> - config file for starting/stopping px4 modules" << endl; |
|
|
|
|
cout << " -h - help/usage information" << endl; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
static void process_line(string &line, bool exit_on_fail) |
|
|
|
|
@ -202,52 +300,131 @@ int main(int argc, char **argv)
@@ -202,52 +300,131 @@ int main(int argc, char **argv)
|
|
|
|
|
set_cpu_scaling(); |
|
|
|
|
|
|
|
|
|
int index = 1; |
|
|
|
|
char *commands_file = nullptr; |
|
|
|
|
string commands_file = ""; |
|
|
|
|
int positional_arg_count = 0; |
|
|
|
|
string data_path = ""; |
|
|
|
|
string node_name = ""; |
|
|
|
|
|
|
|
|
|
// parse arguments
|
|
|
|
|
while (index < argc) { |
|
|
|
|
//cout << "arg: " << index << " : " << argv[index] << endl;
|
|
|
|
|
|
|
|
|
|
if (argv[index][0] == '-') { |
|
|
|
|
// the arg starts with -
|
|
|
|
|
if (strcmp(argv[index], "-d") == 0) { |
|
|
|
|
if (strncmp(argv[index], "-d", 2) == 0) { |
|
|
|
|
daemon_mode = true; |
|
|
|
|
|
|
|
|
|
} else if (strcmp(argv[index], "-h") == 0) { |
|
|
|
|
} else if (strncmp(argv[index], "-h", 2) == 0) { |
|
|
|
|
usage(); |
|
|
|
|
return 0; |
|
|
|
|
|
|
|
|
|
} else if (strcmp(argv[index], "-c") == 0) { |
|
|
|
|
} else if (strncmp(argv[index], "-c", 2) == 0) { |
|
|
|
|
chroot_on = true; |
|
|
|
|
|
|
|
|
|
} else { |
|
|
|
|
PX4_WARN("Unknown/unhandled parameter: %s", argv[index]); |
|
|
|
|
PX4_ERR("Unknown/unhandled parameter: %s", argv[index]); |
|
|
|
|
return 1; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
} else if (!strncmp(argv[index], "__", 2)) { |
|
|
|
|
//cout << "ros argument" << endl;
|
|
|
|
|
|
|
|
|
|
// ros arguments
|
|
|
|
|
if (!strncmp(argv[index], "__name:=", 8)) { |
|
|
|
|
string name_arg = argv[index]; |
|
|
|
|
node_name = name_arg.substr(8); |
|
|
|
|
cout << "node name: " << node_name << endl; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
} else { |
|
|
|
|
// this is an argument that does not have '-' prefix; treat it like a file name
|
|
|
|
|
ifstream infile(argv[index]); |
|
|
|
|
//cout << "positional argument" << endl;
|
|
|
|
|
|
|
|
|
|
if (infile.good()) { |
|
|
|
|
infile.close(); |
|
|
|
|
commands_file = argv[index]; |
|
|
|
|
positional_arg_count += 1; |
|
|
|
|
|
|
|
|
|
} else { |
|
|
|
|
PX4_WARN("Error opening file: %s", argv[index]); |
|
|
|
|
return -1; |
|
|
|
|
if (positional_arg_count == 1) { |
|
|
|
|
data_path = argv[index]; |
|
|
|
|
cout << "data path: " << data_path << endl; |
|
|
|
|
|
|
|
|
|
} else if (positional_arg_count == 2) { |
|
|
|
|
commands_file = argv[index]; |
|
|
|
|
cout << "commands file: " << commands_file << endl; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
++index; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
if (positional_arg_count != 2) { |
|
|
|
|
PX4_ERR("Error expected 2 position arguments, got %d", positional_arg_count); |
|
|
|
|
usage(); |
|
|
|
|
return -1; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
if (commands_file.size() < 1) { |
|
|
|
|
PX4_ERR("Error commands file not specified"); |
|
|
|
|
return -1; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
if (!fileExists(commands_file)) { |
|
|
|
|
PX4_ERR("Error opening commands file, does not exist: %s", commands_file.c_str()); |
|
|
|
|
return -1; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// create sym-links
|
|
|
|
|
vector<string> path_sym_links; |
|
|
|
|
path_sym_links.push_back("ROMFS"); |
|
|
|
|
path_sym_links.push_back("posix-configs"); |
|
|
|
|
path_sym_links.push_back("test_data"); |
|
|
|
|
|
|
|
|
|
for (int i = 0; i < path_sym_links.size(); i++) { |
|
|
|
|
string path_sym_link = path_sym_links[i]; |
|
|
|
|
//cout << "path sym link: " << path_sym_link << endl;
|
|
|
|
|
string src_path = data_path + "/" + path_sym_link; |
|
|
|
|
string dest_path = pwd() + "/" + path_sym_link; |
|
|
|
|
|
|
|
|
|
PX4_DEBUG("Creating symlink %s -> %s", src_path.c_str(), dest_path.c_str()); |
|
|
|
|
|
|
|
|
|
if (dirExists(path_sym_link)) { continue; } |
|
|
|
|
|
|
|
|
|
// create sym-links
|
|
|
|
|
int ret = symlink(src_path.c_str(), dest_path.c_str()); |
|
|
|
|
|
|
|
|
|
if (ret != 0) { |
|
|
|
|
PX4_ERR("Error creating symlink %s -> %s", |
|
|
|
|
src_path.c_str(), dest_path.c_str()); |
|
|
|
|
return ret; |
|
|
|
|
|
|
|
|
|
} else { |
|
|
|
|
PX4_DEBUG("Successfully created symlink %s -> %s", |
|
|
|
|
src_path.c_str(), dest_path.c_str()); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// setup rootfs
|
|
|
|
|
const string eeprom_path = "./rootfs/eeprom/"; |
|
|
|
|
const string microsd_path = "./rootfs/fs/microsd/"; |
|
|
|
|
|
|
|
|
|
if (!fileExists(eeprom_path + "parameters")) { |
|
|
|
|
cout << "creating new parameters file" << endl; |
|
|
|
|
mkpath(eeprom_path.c_str(), S_IRUSR | S_IWUSR | S_IXUSR); |
|
|
|
|
touch(eeprom_path + "parameters"); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
if (!fileExists(microsd_path + "dataman")) { |
|
|
|
|
cout << "creating new dataman file" << endl; |
|
|
|
|
mkpath(microsd_path.c_str(), S_IRUSR | S_IWUSR | S_IXUSR); |
|
|
|
|
touch(microsd_path + "dataman"); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// initialize
|
|
|
|
|
DriverFramework::Framework::initialize(); |
|
|
|
|
px4::init_once(); |
|
|
|
|
|
|
|
|
|
px4::init(argc, argv, "px4"); |
|
|
|
|
|
|
|
|
|
// if commandfile is present, process the commands from the file
|
|
|
|
|
if (commands_file != nullptr) { |
|
|
|
|
ifstream infile(commands_file); |
|
|
|
|
if (commands_file.size() != 0) { |
|
|
|
|
ifstream infile(commands_file.c_str()); |
|
|
|
|
|
|
|
|
|
if (infile.is_open()) { |
|
|
|
|
for (string line; getline(infile, line, '\n');) { |
|
|
|
|
@ -261,7 +438,7 @@ int main(int argc, char **argv)
@@ -261,7 +438,7 @@ int main(int argc, char **argv)
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
} else { |
|
|
|
|
PX4_WARN("Error opening file: %s", commands_file); |
|
|
|
|
PX4_ERR("Error opening commands file: %s", commands_file.c_str()); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
@ -270,12 +447,6 @@ int main(int argc, char **argv)
@@ -270,12 +447,6 @@ int main(int argc, char **argv)
|
|
|
|
|
// this is not an attempt to secure the environment,
|
|
|
|
|
// rather, to replicate a deployed file system.
|
|
|
|
|
|
|
|
|
|
#ifdef PATH_MAX |
|
|
|
|
const unsigned path_max_len = PATH_MAX; |
|
|
|
|
#else |
|
|
|
|
const unsigned path_max_len = 1024; |
|
|
|
|
#endif |
|
|
|
|
|
|
|
|
|
char pwd_path[path_max_len]; |
|
|
|
|
const char *folderpath = "/rootfs/"; |
|
|
|
|
|
|
|
|
|
|
Lorenz Meier
9 years ago
committed by
GitHub