Deamonized GPS app, fixed GPS issues, reworking RC input

5 files changed, 127 insertions, 13 deletions

diff --git a/apps/gps/gps.c b/apps/gps/gps.c
index 6d3ff0d6b..309b9a17a 100644
--- a/apps/gps/gps.c
+++ b/apps/gps/gps.c
@@ -58,6 +58,10 @@
 #include <v1.0/common/mavlink.h>
 #include <mavlink/mavlink_log.h>
 
+static bool thread_should_exit = false;		/**< Deamon exit flag */
+static bool thread_running = false;		/**< Deamon status flag */
+static int deamon_task;				/**< Handle of deamon task / thread */
+
 /**
  * GPS module readout and publishing.
  * 
@@ -68,6 +72,26 @@
  */
 __EXPORT int gps_main(int argc, char *argv[]);
 
+/**
+ * Mainloop of deamon.
+ */
+int gps_thread_main(int argc, char *argv[]);
+
+/**
+ * Print the correct usage.
+ */
+static void usage(const char *reason);
+
+
+static void
+usage(const char *reason)
+{
+	if (reason)
+		fprintf(stderr, "%s\n", reason);
+	fprintf(stderr, "\tusage: %s -d devicename -b baudrate -m mode\n\tmodes are:\n\t\tubx\n\t\tmtkcustom\n\t\tnmea\n\t\tall\n");
+	exit(1);
+}
+
 /****************************************************************************
  * Definitions
  ****************************************************************************/
@@ -137,11 +161,56 @@ void setup_port(char *device, int speed, int *fd)
 }
 
 
-/*
- * Main function of gps app.
+/**
+ * The deamon app only briefly exists to start
+ * the background job. The stack size assigned in the
+ * Makefile does only apply to this management task.
+ * 
+ * The actual stack size should be set in the call
+ * to task_create().
  */
 int gps_main(int argc, char *argv[])
 {
+	if (argc < 1)
+		usage("missing command");
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (thread_running) {
+			printf("gps already running\n");
+			/* this is not an error */
+			exit(0);
+		}
+
+		thread_should_exit = false;
+		deamon_task = task_create("gps", SCHED_PRIORITY_DEFAULT, 4096, gps_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL);
+		thread_running = true;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		thread_should_exit = true;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (thread_running) {
+			printf("\gps is running\n");
+		} else {
+			printf("\tgps not started\n");
+		}
+		exit(0);
+	}
+
+	usage("unrecognized command");
+	exit(1);
+}
+
+/*
+ * Main function of gps app.
+ */
+int gps_thread_main(int argc, char *argv[]) {
+
 	/* welcome message */
 	printf("[gps] Initialized. Searching for GPS receiver..\n");
 
@@ -163,7 +232,7 @@ int gps_main(int argc, char *argv[])
 	/* read arguments */
 	int i;
 
-	for (i = 1; i < argc; i++) {
+	for (i = 0; i < argc; i++) {
 		if (strcmp(argv[i], "-h") == 0 || strcmp(argv[i], "--help") == 0) { //device set
 			printf(commandline_usage, argv[0]);
 			return 0;
@@ -294,7 +363,7 @@ int gps_main(int argc, char *argv[])
 	}
 
 
-	while (true) {
+	while (!thread_should_exit) {
 		/* Infinite retries or break if retry == false */
 
 		/* Loop over all configurations of baud rate and protocol */
@@ -482,7 +551,9 @@ int gps_main(int argc, char *argv[])
 
 	close(mavlink_fd);
 
-	return ERROR;
+	printf("[gps] exiting.\n");
+
+	return 0;
 }
 
 int open_port(char *port)
diff --git a/apps/gps/mtk.c b/apps/gps/mtk.c
index 0333c7100..290fa6129 100644
--- a/apps/gps/mtk.c
+++ b/apps/gps/mtk.c
@@ -121,7 +121,7 @@ int mtk_parse(uint8_t b,  char *gps_rx_buffer)
 			if (mtk_state->ck_a == packet->ck_a && mtk_state->ck_b == packet->ck_b) {
 				mtk_gps->lat = packet->latitude * 10; // mtk: degrees*1e6, mavlink/ubx: degrees*1e7
 				mtk_gps->lon = packet->longitude * 10; // mtk: degrees*1e6, mavlink/ubx: degrees*1e7
-				mtk_gps->alt = (int32_t)packet->msl_altitude * 10; // conversion from centimeters to millimeters, and from uint32_t to int16_t
+				mtk_gps->alt = (int32_t)(packet->msl_altitude * 10); // conversion from centimeters to millimeters, and from uint32_t to int16_t
 				mtk_gps->fix_type = packet->fix_type;
 				mtk_gps->eph = packet->hdop;
 				mtk_gps->epv = 65535; //unknown in mtk custom mode
@@ -311,14 +311,14 @@ void *mtk_loop(void *arg)
 	/* advertise GPS topic */
 	struct vehicle_gps_position_s mtk_gps_d;
 	mtk_gps = &mtk_gps_d;
-	orb_advert_t gps_handle = orb_advertise(ORB_ID(vehicle_gps_position), &mtk_gps);
+	orb_advert_t gps_handle = orb_advertise(ORB_ID(vehicle_gps_position), mtk_gps);
 
 	while (1) {
 		/* Parse a message from the gps receiver */
 		if (OK == read_gps_mtk(fd, gps_rx_buffer, MTK_BUFFER_SIZE)) {
 
 			/* publish new GPS position */
-			orb_publish(ORB_ID(vehicle_gps_position), gps_handle, &mtk_gps);
+			orb_publish(ORB_ID(vehicle_gps_position), gps_handle, mtk_gps);
 
 		} else {
 			break;
diff --git a/apps/gps/nmea_helper.c b/apps/gps/nmea_helper.c
index 54912b6d3..fccc7414a 100644
--- a/apps/gps/nmea_helper.c
+++ b/apps/gps/nmea_helper.c
@@ -202,8 +202,8 @@ void *nmea_loop(void *arg)
 			nmea_gps->timestamp = hrt_absolute_time();
 			nmea_gps->time_gps_usec =  epoch * 1e6 + info->utc.hsec * 1e4;
 			nmea_gps->fix_type = (uint8_t)info->fix;
-			nmea_gps->lat = (int32_t)ndeg2degree(info->lat) * 1e7;
-			nmea_gps->lon = (int32_t)ndeg2degree(info->lon) * 1e7;
+			nmea_gps->lat = (int32_t)(ndeg2degree(info->lat) * 1e7);
+			nmea_gps->lon = (int32_t)(ndeg2degree(info->lon) * 1e7);
 			nmea_gps->alt = (int32_t)(info->elv * 1e3);
 			nmea_gps->eph = (uint16_t)(info->HDOP * 100); //TODO:test scaling
 			nmea_gps->epv = (uint16_t)(info->VDOP * 100); //TODO:test scaling
diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c
index b65e701db..36d444209 100644
--- a/apps/mavlink/mavlink.c
+++ b/apps/mavlink/mavlink.c
@@ -1704,7 +1704,7 @@ int mavlink_main(int argc, char *argv[])
 		}
 
 		thread_should_exit = false;
-		mavlink_task = task_create("mavlink", SCHED_PRIORITY_DEFAULT, 4400, mavlink_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL);
+		mavlink_task = task_create("mavlink", SCHED_PRIORITY_DEFAULT, 6000, mavlink_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL);
 		exit(0);
 	}
 
diff --git a/apps/px4/ground_estimator/ground_estimator.c b/apps/px4/ground_estimator/ground_estimator.c
index 0bd8f09f1..bbb0564ee 100644
--- a/apps/px4/ground_estimator/ground_estimator.c
+++ b/apps/px4/ground_estimator/ground_estimator.c
@@ -41,6 +41,9 @@
 #include <unistd.h>
 #include <stdio.h>
 
+#include <uORB/uORB.h>
+#include <uORB/topics/sensor_combined.h>
+
 static bool thread_should_exit = false;		/**< ground_estimator exit flag */
 static bool thread_running = false;		/**< ground_estimator status flag */
 static int ground_estimator_task;				/**< Handle of ground_estimator task / thread */
@@ -64,9 +67,49 @@ int ground_estimator_thread_main(int argc, char *argv[]) {
 
 	printf("[ground_estimator] starting\n");
 
+	/* subscribe to raw data */
+	int sub_raw = orb_subscribe(ORB_ID(sensor_combined));
+	bool publishing = false;
+
+	/* advertise attitude */
+	struct debug_key_value_s dbg = { .key = "velx      ", .value = 0.0f };
+	orb_advert_t pub_att = orb_advertise(ORB_ID(debug_key_value), &dbg);
+
+	float position[3] = {};
+	float velocity[3] = {};
+
+	uint64_t last_time = 0;
+
 	while (!thread_should_exit) {
-		printf("Hello ground_estimator!\n");
-		sleep(10);
+		
+		/* wait for sensor update */
+		int ret = poll(fds, 1, 1000);
+
+		if (ret < 0) {
+			/* XXX this is seriously bad - should be an emergency */
+		} else if (ret == 0) {
+			/* XXX this means no sensor data - should be critical or emergency */
+			printf("[attitude estimator bm] WARNING: Not getting sensor data - sensor app running?\n");
+		} else {
+			struct sensor_combined_s s;
+			orb_copy(ORB_ID(sensor_combined), sub_raw, &s);
+
+			uint64_t dt = s.timestamp - last_time;
+
+			/* Integration */
+			position[0] += velocity[0] * dt;
+			position[1] += velocity[1] * dt;
+			position[2] += velocity[2] * dt;
+
+			velocity[0] += velocity[0] * 0.01f + 0.99f * s.acceleration_m_s2[0] * dt;
+			velocity[1] += velocity[1] * 0.01f + 0.99f * s.acceleration_m_s2[1] * dt;
+			velocity[2] += velocity[2] * 0.01f + 0.99f * s.acceleration_m_s2[2] * dt;
+
+			dbg.value = position[0];
+
+			orb_publish(ORB_ID(debug_key_value), pub_dbg, &dbg);
+		}
+
 	}
 
 	printf("[ground_estimator] exiting.\n");