[AS-5] added bt_cfg command.

11 files changed, 532 insertions, 22 deletions

diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk
index 8959379c0..fdb7e2396 100644
--- a/makefiles/config_px4fmu-v2_default.mk
+++ b/makefiles/config_px4fmu-v2_default.mk
@@ -157,6 +157,7 @@ MODULES		+= lib/launchdetection
 # Hardware test
 #MODULES			+= examples/hwtest
 MODULES				+= modules/serial_echo
+MODULES				+= modules/bt_cfg
 
 #
 # Transitional support - add commands from the NuttX export archive.
diff --git a/src/modules/bt_cfg/at.hpp b/src/modules/bt_cfg/at.hpp
new file mode 100644
index 000000000..7616c6a4e
--- /dev/null
+++ b/src/modules/bt_cfg/at.hpp
@@ -0,0 +1,148 @@
+#pragma once
+
+#include <unistd.h>
+
+#include <cstdlib>
+#include <cstring>
+
+// #include <algorithm>
+// #include <iterator>
+#include <utility>
+
+// #include "string_algorithms.hpp"
+
+namespace AT {
+
+constexpr size_t reply_buffer_size = 128;
+constexpr size_t reply_read_retries = 25;
+constexpr useconds_t reply_read_delay = 100 * 1000;
+
+// template <typename ForwardIt>
+// std::pair<bool, bool> // ok/err received, it was ok
+// check_reply_ok_err(ForwardIt first, ForwardIt last) {
+// 	bool found = false;
+// 	bool ok = false;
+// 	if (first != last) {
+// 		auto crlf = find_crlf(first, last);
+// 		while (crlf != last) {
+// 			auto dist = std::distance(first, crlf);
+// 			ok = dist == 2 and std::equal(first, crlf, "OK");
+// 			found = ok or (dist == 5 and std::equal(first, crlf, "ERROR"));
+//
+// 			if (found) break;
+//
+// 			std::advance(crlf, 2);
+// 			first = crlf;
+// 			crlf = find_crlf(first, last);
+// 		}
+// 	}
+// 	return std::make_pair(found, ok);
+// }
+//
+// template <typename Device>
+// std::pair<bool, bool> // ok/err received, it was ok
+// wait_ok_error(Device &f) {
+// 	auto retries = reply_read_retries;
+// 	bool found = false;
+// 	bool ok = false;
+// 	char buf[reply_buffer_size];
+// 	ssize_t s = 0, n = 0;
+// 	usleep(reply_read_delay);
+// 	while (not found || retries > 0) {
+// 		s = read(f, buf + n, sizeof(buf) - n);
+// 		if (s < 0) break;
+// 		if (s == 0) {
+// 			usleep(reply_read_delay);
+// 			--retries;
+// 			s = read(f, buf + n, sizeof(buf) - n);
+// 			if (s <= 0) break;
+// 		}
+// 		n += s;
+// 		std::tie(found, ok) = check_reply_ok_err(buf, buf + n);
+// 	}
+// 	if (not found and n > 0)
+// 		std::tie(found, ok) = check_reply_ok_err(buf, buf + n);
+// 	return std::make_pair(found, ok);
+// }
+
+template <typename Device>
+std::pair<bool, bool> // ok/err received, it was ok
+wait_ok_error(Device &f) {
+	auto retries = reply_read_retries;
+	bool found = false;
+	bool ok = false;
+	char buf[reply_buffer_size + 1];
+	ssize_t s = 0;
+       	size_t n = 0;
+	usleep(reply_read_delay);
+	while (not found || retries > 0) {
+		s = read(f, buf + n, reply_buffer_size - n);
+		if (s < 0) break;
+		if (s == 0) {
+			usleep(reply_read_delay);
+			--retries;
+			s = read(f, buf + n, reply_buffer_size - n);
+			if (s <= 0) break;
+		}
+		n += s;
+		buf[n] = '\0';
+		ok = std::strstr(buf, "\r\nOK\r\n");
+		found = ok or std::strstr(buf, "\r\nERROR\r\n");
+	}
+	if (not found and n > 0) {
+		buf[n] = '\0';
+		ok = std::strstr(buf, "\r\nOK\r\n");
+		found = ok or std::strstr(buf, "\r\nERROR\r\n");
+	}
+	return std::make_pair(found, ok);
+}
+
+template <typename Device>
+inline bool
+write_char(Device &f, char ch) {
+	return write(f, &ch, 1) == 1;
+}
+
+template <typename Device>
+inline bool
+exec_cmd(Device &f, const char cmd[], std::size_t len) {
+	return write(f, cmd, len) == len and write_char(f, '\r')
+		and wait_ok_error(f).second;
+}
+
+template <typename Device>
+inline bool
+exec_cmd(Device &f, const char * const cmd) {
+	return exec_cmd(f, cmd, std::strlen(cmd));
+}
+
+template <typename Device>
+inline bool
+exec_reset(Device &f) {
+	static const char at_reset[] = "AT*AMWS=0,0,0,0,1,0";
+	const ssize_t len = sizeof(at_reset) - 1;
+	bool ok = write(f, at_reset, len) == len and write_char(f, '\r');
+	if (ok) sleep(1);
+	return ok;
+}
+
+template <typename Device>
+inline bool
+switch_to_at_mode(Device &f) {
+	sleep(1);
+	static const char escape_seq[3] = {'/', '/', '/'};
+	bool ok = write(f, escape_seq, sizeof(escape_seq));
+	if (ok) {
+		sleep(1);
+		ok = exec_cmd(f, "AT");
+	}
+	return ok;
+}
+
+template <typename Device>
+inline bool
+switch_to_data_mode(Device &f) {
+	return exec_cmd(f, "AT*ADDM");
+}
+
+} // end of namespace AT
diff --git a/src/modules/bt_cfg/main.cpp b/src/modules/bt_cfg/main.cpp
new file mode 100644
index 000000000..9ad41884e
--- /dev/null
+++ b/src/modules/bt_cfg/main.cpp
@@ -0,0 +1,97 @@
+#include <nuttx/config.h>
+
+#include <fcntl.h>
+#include <stdlib.h>
+
+#include <cstdio>
+#include <cstring>
+#include <ctime>
+
+extern "C" __EXPORT int
+main(int argc, const char * const * argv);
+
+#include "at.hpp"
+
+namespace bt_cfg {
+
+#include "read_write_log.hpp"
+#include "unique_file.hpp"
+
+#ifdef FORCE_SERIAL_TERMIOS_RAW
+# include "serial_config.hpp"
+#endif
+
+using namespace std;
+
+int
+open_serial_default(const char name[]) {
+	int fd = ::open(name, O_RDWR | O_NONBLOCK | O_NOCTTY);
+	if (fd == -1) {
+		perror("open");
+		exit(1);
+	}
+
+#ifdef FORCE_SERIAL_TERMIOS_RAW
+	if (not (serial_set_raw(fd) and serial_set_speed(fd, B57600))) {
+		fprintf(stderr, "failed setting raw mode and speed\n");
+		exit(1);
+	}
+#endif
+
+	return fd;
+}
+
+template <typename Device>
+inline void
+read_junk(Device &f) {
+	char buf[32];
+	ssize_t s;
+	do {
+		s = ::read(f, s, sizeof(s));
+	} while (s > 0);
+}
+
+inline bool
+streq(const char *a, const char *b) {
+	return not std::strcmp(a, b);
+}
+
+} // end of namespace bt_cfg
+
+int
+main(int argc, char const * const * argv)
+{
+	using namespace bt_cfg;
+	using namespace std;
+
+	if (argc < 3) {
+		fprintf(stderr,"Usage: %s tty commands\n", argv[0]);
+		return 1;
+	}
+
+	unique_file d = open_serial_default(argv[1]);
+
+	DevLog f { d.get(), 2 };
+
+	bool ok;
+	const char * const *p;
+	for (p = argv + 2; *p; ++p) {
+		const char *cmd = *p;
+
+		if (streq(cmd, "+at"))
+			ok = AT::switch_to_at_mode(f);
+		else if (streq(cmd, "-at"))
+			ok = AT::switch_to_data_mode(f);
+		else if (streq(cmd, "reset"))
+			ok = AT::exec_reset(f);
+		else
+			ok = AT::exec_cmd(f, cmd);
+
+		if (not ok) {
+			fprintf(stderr, "Failed at command '%s'.\n", cmd);
+			return 1;
+		}
+	}
+
+	return 0;
+}
diff --git a/src/modules/bt_cfg/module.mk b/src/modules/bt_cfg/module.mk
new file mode 100644
index 000000000..68304cb89
--- /dev/null
+++ b/src/modules/bt_cfg/module.mk
@@ -0,0 +1,42 @@
+############################################################################
+#
+#   Copyright (c) 2012, 2013 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.
+#
+############################################################################
+
+#
+# Basic example application
+#
+
+MODULE_COMMAND		= bt_cfg
+
+SRCS			= main.cpp
+
+CXXFLAGS		+= -std=c++11 -Dmain=${MODULE_COMMAND}_main
diff --git a/src/modules/bt_cfg/read_write_log.hpp b/src/modules/bt_cfg/read_write_log.hpp
new file mode 100644
index 000000000..930249dee
--- /dev/null
+++ b/src/modules/bt_cfg/read_write_log.hpp
@@ -0,0 +1,41 @@
+#pragma once
+
+#include <sys/types.h>
+
+// #include "repr.hpp"
+
+struct DevLog {
+	int dev;
+	int log;
+
+	DevLog(int d, int l) : dev {d}, log {l} {}
+
+	friend ssize_t
+	read(DevLog &self, void *buf, size_t buf_size)
+	{
+		ssize_t r = read(self.dev, buf, buf_size);
+		if (r > 0) { write_log(self, true, buf, r); }
+		return r;
+	}
+
+	friend ssize_t
+	write(DevLog &self, const void *buf, size_t buf_size)
+	{
+		ssize_t r = write(self.dev, buf, buf_size);
+		if (r > 0) { write_log(self, false, buf, buf_size); }
+		return r;
+	}
+
+	friend void
+	write_log(DevLog &self, bool rw, const void *buf, size_t buf_size)
+	{
+		write(self.log, buf, buf_size);
+
+		// static const char r[]          = "read   ";
+		// static const char w[sizeof(r)] = "write  ";
+		//
+		// write(self.log, rw ? r : w, sizeof(r) - 1);
+		// write_repr(self.log, buf, buf_size);
+		// write(self.log, "\n", 2);
+	}
+};
diff --git a/src/modules/bt_cfg/serial_config.hpp b/src/modules/bt_cfg/serial_config.hpp
new file mode 100644
index 000000000..523ea938c
--- /dev/null
+++ b/src/modules/bt_cfg/serial_config.hpp
@@ -0,0 +1,37 @@
+#pragma once
+
+#include <termios.h>
+
+void
+termios_raw_mode(struct termios & mode) {
+	mode.c_iflag = 0;
+	mode.c_oflag = 0;
+	mode.c_lflag = 0;
+
+	mode.c_cflag &= ~CSIZE;
+	mode.c_cflag |= CS8;
+	mode.c_cflag &= ~CSTOPB;
+	mode.c_cflag &= ~(PARENB | PARODD);
+	mode.c_cflag |= CREAD;
+	mode.c_cflag |= CLOCAL;
+}
+
+bool
+serial_set_raw(int fd) {
+	struct termios mode;
+	bool ok = tcgetattr(fd, &mode) == 0;
+	if (ok)
+	{
+		termios_raw_mode(mode);
+		ok = tcsetattr(fd, TCSANOW, &mode) == 0;
+	}
+	return ok;
+}
+
+bool
+serial_set_speed(int fd, speed_t speed) {
+	struct termios mode;
+	return tcgetattr(fd, &mode) == 0
+		and cfsetspeed(&mode, speed) == 0
+		and tcsetattr(fd, TCSANOW, &mode) == 0;
+}
diff --git a/src/modules/bt_cfg/unique_file.hpp b/src/modules/bt_cfg/unique_file.hpp
new file mode 100644
index 000000000..0735f7389
--- /dev/null
+++ b/src/modules/bt_cfg/unique_file.hpp
@@ -0,0 +1,36 @@
+#pragma once
+
+#include <unistd.h>
+
+class unique_file {
+private:
+	int fd;
+
+public:
+	inline
+	unique_file() : fd(-1) {}
+
+	inline
+	unique_file(int fd) : fd(fd) {}
+
+	inline
+	unique_file(unique_file && other) : fd(other.fd) {
+		other.fd = -1;
+	}
+
+	inline
+	~unique_file() {
+		if (fd > -1) close(fd);
+	}
+
+	inline
+	int get() { return fd; }
+
+	friend inline std::size_t
+	read(unique_file & uf, void * buf, size_t buf_size)
+	{ return ::read(uf.fd, buf, buf_size); }
+
+	friend inline std::size_t
+	write(unique_file & uf, const void * buf, size_t buf_size)
+	{ return ::write(uf.fd, buf, buf_size); }
+};
diff --git a/src/modules/serial_echo/serial_echo.cpp b/src/modules/serial_echo/main.cpp
index 29bf59901..e36821276 100644
--- a/src/modules/serial_echo/serial_echo.cpp
+++ b/src/modules/serial_echo/main.cpp
@@ -36,35 +36,64 @@
 
 #include <fcntl.h>
 #include <poll.h>
-#include <unistd.h>
+#include <stdlib.h>
 
 #include <cstdio>
-#include <cstdlib>
 #include <cstring>
 
+extern "C" __EXPORT int
+main(int argc, char const * const * const argv);
+
+namespace serial_echo {
+
+#include "unique_file.hpp"
+
+#ifdef FORCE_SERIAL_TERMIOS_RAW
+# include "serial_config.hpp"
+#endif
+
 
 using namespace std;
 
 
-extern "C" __EXPORT int
-main(int argc, char ** const argv);
+int
+open_serial_default(const char name[]) {
+	int fd = open(name, O_RDWR | O_NONBLOCK | O_NOCTTY);
+	if (fd == -1) {
+		perror("open");
+		exit(1);
+	}
 
+#ifdef FORCE_SERIAL_TERMIOS_RAW
+	if (not (serial_set_raw(fd) and serial_set_speed(fd, B57600))) {
+		fprintf(stderr, "failed setting raw mode and speed\n");
+		exit(1);
+	}
+#endif
+
+	return fd;
+}
 
 void
 echo_forever(int dev) {
 	struct pollfd poll_dev;
 	poll_dev.fd = dev;
 
-	char buf[32];
+	static char buf[128];
 	ssize_t n = 0;
 	ssize_t sr, sw;
+
+	fprintf(stderr, ".");
+	fflush(stderr);
 	while (true) {
 		poll_dev.events = POLLIN;
 		if (n > 0) poll_dev.events |= POLLOUT;
 
-		while (poll(&poll_dev, 1, 10000/*ms*/) != 1)
-			/* wait forever */ ;
-		
+		while (poll(&poll_dev, 1, 10000/*ms*/) != 1) {
+			fprintf(stderr, ".");
+			fflush(stderr);
+		}
+
 		sr = 0;
 		sw = 0;
 
@@ -76,20 +105,19 @@ echo_forever(int dev) {
 		if (n > 0) {
 			sw = write(dev, buf, n);
 			if (sw > 0)
-       				if (sw == n) n = 0;
-				else {
+				if (sw == n) { n = 0; }
+				else
+				{
 					n -= sw;
 					memcpy(buf, buf + sw, n);
 				}
 		}
 
-		if (n > 0 or sr > 0 or sw > 0)
-			fprintf(stderr, "n %d sr %d sw %d\n", n, sr, sw);
+		//if (n > 0 or sr > 0 or sw > 0)
+		//	fprintf(stderr, "n %d sr %d sw %d\n", n, sr, sw);
 	}
-
 }
 
-
 void
 usage(const char * name)
 {
@@ -100,16 +128,23 @@ usage(const char * name)
 	exit(1);
 }
 
+} // end of namespace serial_echo
+
 int
-main(int argc, char ** const argv)
+main(int argc, char const * const * const argv)
 {
+	using namespace serial_echo;
+
 	if (argc != 2) usage(argv[0]);
 
-	int dev = open(argv[1], O_RDWR | O_NONBLOCK | O_NOCTTY );
-	if (dev < 0) {
-		perror("open");
-		usage(argv[0]);
-	}
+	fprintf(stderr, "open(\"%s\")\n", argv[1]); fflush(stderr);
+	unique_file dev = open_serial_default(argv[1]);
+	fprintf(stderr, "open -> %d\n", dev.get()); fflush(stderr);
+
+	fprintf(stderr, "starting forever loop... bye\n"); fflush(stderr);
+	echo_forever(dev.get());
+
+	fprintf(stderr, "quit unexpectedly\n");
 
-	echo_forever(dev);
+	return 0;
 }
diff --git a/src/modules/serial_echo/module.mk b/src/modules/serial_echo/module.mk
index 2c1f93719..a8a2ce36a 100644
--- a/src/modules/serial_echo/module.mk
+++ b/src/modules/serial_echo/module.mk
@@ -39,4 +39,4 @@ MODULE_COMMAND		= serial_echo
 
 CXXFLAGS		+= -std=c++11 -Dmain=$(MODULE_COMMAND)_main
 
-SRCS			= serial_echo.cpp
+SRCS			= main.cpp
diff --git a/src/modules/serial_echo/serial_config.hpp b/src/modules/serial_echo/serial_config.hpp
new file mode 100644
index 000000000..523ea938c
--- /dev/null
+++ b/src/modules/serial_echo/serial_config.hpp
@@ -0,0 +1,37 @@
+#pragma once
+
+#include <termios.h>
+
+void
+termios_raw_mode(struct termios & mode) {
+	mode.c_iflag = 0;
+	mode.c_oflag = 0;
+	mode.c_lflag = 0;
+
+	mode.c_cflag &= ~CSIZE;
+	mode.c_cflag |= CS8;
+	mode.c_cflag &= ~CSTOPB;
+	mode.c_cflag &= ~(PARENB | PARODD);
+	mode.c_cflag |= CREAD;
+	mode.c_cflag |= CLOCAL;
+}
+
+bool
+serial_set_raw(int fd) {
+	struct termios mode;
+	bool ok = tcgetattr(fd, &mode) == 0;
+	if (ok)
+	{
+		termios_raw_mode(mode);
+		ok = tcsetattr(fd, TCSANOW, &mode) == 0;
+	}
+	return ok;
+}
+
+bool
+serial_set_speed(int fd, speed_t speed) {
+	struct termios mode;
+	return tcgetattr(fd, &mode) == 0
+		and cfsetspeed(&mode, speed) == 0
+		and tcsetattr(fd, TCSANOW, &mode) == 0;
+}
diff --git a/src/modules/serial_echo/unique_file.hpp b/src/modules/serial_echo/unique_file.hpp
new file mode 100644
index 000000000..0735f7389
--- /dev/null
+++ b/src/modules/serial_echo/unique_file.hpp
@@ -0,0 +1,36 @@
+#pragma once
+
+#include <unistd.h>
+
+class unique_file {
+private:
+	int fd;
+
+public:
+	inline
+	unique_file() : fd(-1) {}
+
+	inline
+	unique_file(int fd) : fd(fd) {}
+
+	inline
+	unique_file(unique_file && other) : fd(other.fd) {
+		other.fd = -1;
+	}
+
+	inline
+	~unique_file() {
+		if (fd > -1) close(fd);
+	}
+
+	inline
+	int get() { return fd; }
+
+	friend inline std::size_t
+	read(unique_file & uf, void * buf, size_t buf_size)
+	{ return ::read(uf.fd, buf, buf_size); }
+
+	friend inline std::size_t
+	write(unique_file & uf, const void * buf, size_t buf_size)
+	{ return ::write(uf.fd, buf, buf_size); }
+};