Merge branch 'master' of github.com:PX4/Firmware into pca8574

1 files changed, 20 insertions, 15 deletions

diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index a902bdf2f..5342ccf78 100644
--- a/src/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013, 2014 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
@@ -56,6 +56,7 @@
 #include <arch/board/board.h>
 #include <drivers/drv_hrt.h>
 #include <drivers/device/i2c.h>
+#include <systemlib/systemlib.h>
 #include <systemlib/perf_counter.h>
 #include <systemlib/scheduling_priorities.h>
 #include <systemlib/err.h>
@@ -63,6 +64,8 @@
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_gps_position.h>
 
+#include <board_config.h>
+
 #include "ubx.h"
 #include "mtk.h"
 
@@ -76,12 +79,6 @@
 #endif
 static const int ERROR = -1;
 
-#ifndef CONFIG_SCHED_WORKQUEUE
-# error This requires CONFIG_SCHED_WORKQUEUE.
-#endif
-
-
-
 class GPS : public device::CDev
 {
 public:
@@ -209,7 +206,8 @@ GPS::init()
 		goto out;
 
 	/* start the GPS driver worker task */
-	_task = task_create("gps", SCHED_PRIORITY_SLOW_DRIVER, 2048, (main_t)&GPS::task_main_trampoline, nullptr);
+	_task = task_spawn_cmd("gps", SCHED_DEFAULT,
+				SCHED_PRIORITY_SLOW_DRIVER, 2000, (main_t)&GPS::task_main_trampoline, nullptr);
 
 	if (_task < 0) {
 		warnx("task start failed: %d", errno);
@@ -276,14 +274,14 @@ GPS::task_main()
 			_report.timestamp_position = hrt_absolute_time();
 			_report.lat = (int32_t)47.378301e7f;
 			_report.lon = (int32_t)8.538777e7f;
-			_report.alt = (int32_t)400e3f;
+			_report.alt = (int32_t)1200e3f;
 			_report.timestamp_variance = hrt_absolute_time();
 			_report.s_variance_m_s = 10.0f;
 			_report.p_variance_m = 10.0f;
 			_report.c_variance_rad = 0.1f;
 			_report.fix_type = 3;
-			_report.eph_m = 3.0f;
-			_report.epv_m = 7.0f;
+			_report.eph_m = 0.9f;
+			_report.epv_m = 1.8f;
 			_report.timestamp_velocity = hrt_absolute_time();
 			_report.vel_n_m_s = 0.0f;
 			_report.vel_e_m_s = 0.0f;
@@ -421,7 +419,14 @@ GPS::task_main()
 void
 GPS::cmd_reset()
 {
-	//XXX add reset?
+#ifdef GPIO_GPS_NRESET
+	warnx("Toggling GPS reset pin");
+	stm32_configgpio(GPIO_GPS_NRESET);
+	stm32_gpiowrite(GPIO_GPS_NRESET, 0);
+	usleep(100);
+	stm32_gpiowrite(GPIO_GPS_NRESET, 1);
+	warnx("Toggled GPS reset pin");
+#endif
 }
 
 void
@@ -443,10 +448,10 @@ GPS::print_info()
 	warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK");
 
 	if (_report.timestamp_position != 0) {
-		warnx("position lock: %dD, satellites: %d, last update: %fms ago", (int)_report.fix_type,
-				_report.satellites_visible, (hrt_absolute_time() - _report.timestamp_position) / 1000.0f);
+		warnx("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report.fix_type,
+				_report.satellites_visible, (double)(hrt_absolute_time() - _report.timestamp_position) / 1000.0f);
 		warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt);
-		warnx("eph: %.2fm, epv: %.2fm", _report.eph_m, _report.epv_m);
+		warnx("eph: %.2fm, epv: %.2fm", (double)_report.eph_m, (double)_report.epv_m);
 		warnx("rate position: \t%6.2f Hz", (double)_Helper->get_position_update_rate());
 		warnx("rate velocity: \t%6.2f Hz", (double)_Helper->get_velocity_update_rate());
 		warnx("rate publication:\t%6.2f Hz", (double)_rate);