aboutsummaryrefslogtreecommitdiff
path: root/src/drivers/gps
diff options
context:
space:
mode:
Diffstat (limited to 'src/drivers/gps')
-rw-r--r--src/drivers/gps/gps.cpp35
-rw-r--r--src/drivers/gps/gps_helper.cpp4
-rw-r--r--src/drivers/gps/gps_helper.h8
-rw-r--r--src/drivers/gps/module.mk2
-rw-r--r--src/drivers/gps/mtk.cpp10
-rw-r--r--src/drivers/gps/ubx.cpp47
-rw-r--r--src/drivers/gps/ubx.h28
7 files changed, 100 insertions, 34 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);
diff --git a/src/drivers/gps/gps_helper.cpp b/src/drivers/gps/gps_helper.cpp
index 2360ff39b..3b92f1bf4 100644
--- a/src/drivers/gps/gps_helper.cpp
+++ b/src/drivers/gps/gps_helper.cpp
@@ -56,7 +56,7 @@ GPS_Helper::get_velocity_update_rate()
return _rate_vel;
}
-float
+void
GPS_Helper::reset_update_rates()
{
_rate_count_vel = 0;
@@ -64,7 +64,7 @@ GPS_Helper::reset_update_rates()
_interval_rate_start = hrt_absolute_time();
}
-float
+void
GPS_Helper::store_update_rates()
{
_rate_vel = _rate_count_vel / (((float)(hrt_absolute_time() - _interval_rate_start)) / 1000000.0f);
diff --git a/src/drivers/gps/gps_helper.h b/src/drivers/gps/gps_helper.h
index cfb9e0d43..d14a95afe 100644
--- a/src/drivers/gps/gps_helper.h
+++ b/src/drivers/gps/gps_helper.h
@@ -46,13 +46,17 @@
class GPS_Helper
{
public:
+
+ GPS_Helper() {};
+ virtual ~GPS_Helper() {};
+
virtual int configure(unsigned &baud) = 0;
virtual int receive(unsigned timeout) = 0;
int set_baudrate(const int &fd, unsigned baud);
float get_position_update_rate();
float get_velocity_update_rate();
- float reset_update_rates();
- float store_update_rates();
+ void reset_update_rates();
+ void store_update_rates();
protected:
uint8_t _rate_count_lat_lon;
diff --git a/src/drivers/gps/module.mk b/src/drivers/gps/module.mk
index 82c67d40a..eb382c4b2 100644
--- a/src/drivers/gps/module.mk
+++ b/src/drivers/gps/module.mk
@@ -41,3 +41,5 @@ SRCS = gps.cpp \
gps_helper.cpp \
mtk.cpp \
ubx.cpp
+
+MODULE_STACKSIZE = 1200
diff --git a/src/drivers/gps/mtk.cpp b/src/drivers/gps/mtk.cpp
index c90ecbe28..680f00d97 100644
--- a/src/drivers/gps/mtk.cpp
+++ b/src/drivers/gps/mtk.cpp
@@ -249,12 +249,18 @@ MTK::handle_message(gps_mtk_packet_t &packet)
warnx("mtk: unknown revision");
_gps_position->lat = 0;
_gps_position->lon = 0;
+
+ // Indicate this data is not usable and bail out
+ _gps_position->eph_m = 1000.0f;
+ _gps_position->epv_m = 1000.0f;
+ _gps_position->fix_type = 0;
+ return;
}
_gps_position->alt = (int32_t)(packet.msl_altitude * 10); // from cm to mm
_gps_position->fix_type = packet.fix_type;
- _gps_position->eph_m = packet.hdop; // XXX: Check this because eph_m is in m and hdop is without unit
- _gps_position->epv_m = 0.0; //unknown in mtk custom mode
+ _gps_position->eph_m = packet.hdop / 100.0f; // from cm to m
+ _gps_position->epv_m = _gps_position->eph_m; // unknown in mtk custom mode, so we cheat with eph
_gps_position->vel_m_s = ((float)packet.ground_speed) * 1e-2f; // from cm/s to m/s
_gps_position->cog_rad = ((float)packet.heading) * M_DEG_TO_RAD_F * 1e-2f; //from deg *100 to rad
_gps_position->satellites_visible = packet.satellites;
diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp
index 8a2afecb7..19cf5beec 100644
--- a/src/drivers/gps/ubx.cpp
+++ b/src/drivers/gps/ubx.cpp
@@ -164,7 +164,7 @@ UBX::configure(unsigned &baudrate)
send_config_packet(_fd, (uint8_t *)&cfg_rate_packet, sizeof(cfg_rate_packet));
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
- warnx("ubx: configuration failed: RATE");
+ warnx("CFG FAIL: RATE");
return 1;
}
@@ -185,7 +185,7 @@ UBX::configure(unsigned &baudrate)
send_config_packet(_fd, (uint8_t *)&cfg_nav5_packet, sizeof(cfg_nav5_packet));
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
- warnx("ubx: configuration failed: NAV5");
+ warnx("CFG FAIL: NAV5");
return 1;
}
@@ -194,35 +194,42 @@ UBX::configure(unsigned &baudrate)
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_POSLLH, 1);
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
- warnx("ubx: msg rate configuration failed: NAV POSLLH");
+ warnx("MSG CFG FAIL: NAV POSLLH");
return 1;
}
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_TIMEUTC, 1);
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
- warnx("ubx: msg rate configuration failed: NAV TIMEUTC");
+ warnx("MSG CFG FAIL: NAV TIMEUTC");
return 1;
}
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SOL, 1);
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
- warnx("ubx: msg rate configuration failed: NAV SOL");
+ warnx("MSG CFG FAIL: NAV SOL");
return 1;
}
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_VELNED, 1);
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
- warnx("ubx: msg rate configuration failed: NAV VELNED");
+ warnx("MSG CFG FAIL: NAV VELNED");
return 1;
}
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, 5);
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
- warnx("ubx: msg rate configuration failed: NAV SVINFO");
+ warnx("MSG CFG FAIL: NAV SVINFO");
+ return 1;
+ }
+
+ configure_message_rate(UBX_CLASS_MON, UBX_MESSAGE_MON_HW, 1);
+
+ if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
+ warnx("MSG CFG FAIL: MON HW");
return 1;
}
@@ -274,7 +281,7 @@ UBX::receive(unsigned timeout)
if (ret < 0) {
/* something went wrong when polling */
- warnx("ubx: poll error");
+ warnx("poll error");
return -1;
} else if (ret == 0) {
@@ -310,7 +317,7 @@ UBX::receive(unsigned timeout)
/* abort after timeout if no useful packets received */
if (time_started + timeout * 1000 < hrt_absolute_time()) {
- warnx("ubx: timeout - no useful messages");
+ warnx("timeout - no useful messages");
return -1;
}
}
@@ -383,7 +390,7 @@ UBX::parse_char(uint8_t b)
return 1; // message received successfully
} else {
- warnx("ubx: checksum wrong");
+ warnx("checksum wrong");
decode_init();
return -1;
}
@@ -392,7 +399,7 @@ UBX::parse_char(uint8_t b)
_rx_count++;
} else {
- warnx("ubx: buffer full");
+ warnx("buffer full");
decode_init();
return -1;
}
@@ -566,6 +573,24 @@ UBX::handle_message()
break;
}
+ case UBX_CLASS_MON: {
+ switch (_message_id) {
+ case UBX_MESSAGE_MON_HW: {
+
+ struct gps_bin_mon_hw_packet *p = (struct gps_bin_mon_hw_packet*) _rx_buffer;
+
+ _gps_position->noise_per_ms = p->noisePerMS;
+ _gps_position->jamming_indicator = p->jamInd;
+
+ ret = 1;
+ break;
+ }
+
+ default:
+ break;
+ }
+ }
+
default:
break;
}
diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h
index 79a904f4a..5cf47b60b 100644
--- a/src/drivers/gps/ubx.h
+++ b/src/drivers/gps/ubx.h
@@ -56,6 +56,7 @@
//#define UBX_CLASS_RXM 0x02
#define UBX_CLASS_ACK 0x05
#define UBX_CLASS_CFG 0x06
+#define UBX_CLASS_MON 0x0A
/* MessageIDs (the ones that are used) */
#define UBX_MESSAGE_NAV_POSLLH 0x02
@@ -72,6 +73,8 @@
#define UBX_MESSAGE_CFG_RATE 0x08
#define UBX_MESSAGE_CFG_NAV5 0x24
+#define UBX_MESSAGE_MON_HW 0x09
+
#define UBX_CFG_PRT_LENGTH 20
#define UBX_CFG_PRT_PAYLOAD_PORTID 0x01 /**< UART1 */
#define UBX_CFG_PRT_PAYLOAD_MODE 0x000008D0 /**< 0b0000100011010000: 8N1 */
@@ -210,6 +213,27 @@ typedef struct {
uint8_t ck_b;
} gps_bin_nav_velned_packet_t;
+struct gps_bin_mon_hw_packet {
+ uint32_t pinSel;
+ uint32_t pinBank;
+ uint32_t pinDir;
+ uint32_t pinVal;
+ uint16_t noisePerMS;
+ uint16_t agcCnt;
+ uint8_t aStatus;
+ uint8_t aPower;
+ uint8_t flags;
+ uint8_t __reserved1;
+ uint32_t usedMask;
+ uint8_t VP[25];
+ uint8_t jamInd;
+ uint16_t __reserved3;
+ uint32_t pinIrq;
+ uint32_t pulLH;
+ uint32_t pullL;
+};
+
+
//typedef struct {
// int32_t time_milliseconds; /**< Measurement integer millisecond GPS time of week */
// int16_t week; /**< Measurement GPS week number */
@@ -319,7 +343,7 @@ typedef enum {
//typedef type_gps_bin_ubx_state gps_bin_ubx_state_t;
#pragma pack(pop)
-#define RECV_BUFFER_SIZE 500 //The NAV-SOL messages really need such a big buffer
+#define RECV_BUFFER_SIZE 300 //The NAV-SOL messages really need such a big buffer
class UBX : public GPS_Helper
{
@@ -383,7 +407,7 @@ private:
uint8_t _message_class;
uint8_t _message_id;
unsigned _payload_size;
- uint8_t _disable_cmd_last;
+ hrt_abstime _disable_cmd_last;
};
#endif /* UBX_H_ */
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-24 16:46:51 +0000