aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--apps/drivers/drv_gps.h13
-rw-r--r--apps/drivers/gps/gps.cpp53
-rw-r--r--apps/drivers/gps/gps_helper.h3
-rw-r--r--apps/drivers/gps/ubx.cpp62
-rw-r--r--apps/drivers/gps/ubx.h11
5 files changed, 41 insertions, 101 deletions
diff --git a/apps/drivers/drv_gps.h b/apps/drivers/drv_gps.h
index dfde115ef..67248efcd 100644
--- a/apps/drivers/drv_gps.h
+++ b/apps/drivers/drv_gps.h
@@ -50,8 +50,7 @@
typedef enum {
GPS_DRIVER_MODE_UBX = 0,
- GPS_DRIVER_MODE_MTK19,
- GPS_DRIVER_MODE_MTK16,
+ GPS_DRIVER_MODE_MTK,
GPS_DRIVER_MODE_NMEA,
} gps_driver_mode_t;
@@ -67,14 +66,4 @@ ORB_DECLARE(gps);
#define _GPSIOCBASE (0x2800) //TODO: arbitrary choice...
#define _GPSIOC(_n) (_IOC(_GPSIOCBASE, _n))
-/** configure ubx mode */
-#define GPS_CONFIGURE_UBX _GPSIOC(0)
-
-/** configure mtk mode */
-#define GPS_CONFIGURE_MTK19 _GPSIOC(1)
-#define GPS_CONFIGURE_MTK16 _GPSIOC(2)
-
-/** configure mtk mode */
-#define GPS_CONFIGURE_NMEA _GPSIOC(3)
-
#endif /* _DRV_GPS_H */
diff --git a/apps/drivers/gps/gps.cpp b/apps/drivers/gps/gps.cpp
index 45f18158f..3e1aca810 100644
--- a/apps/drivers/gps/gps.cpp
+++ b/apps/drivers/gps/gps.cpp
@@ -72,6 +72,7 @@
#include <uORB/topics/vehicle_gps_position.h>
#include "ubx.h"
+#include "mtk.h"
#define SEND_BUFFER_LENGTH 100
#define TIMEOUT 1000000 //1s
@@ -238,30 +239,6 @@ GPS::ioctl(struct file *filp, int cmd, unsigned long arg)
int ret = OK;
switch (cmd) {
- case GPS_CONFIGURE_UBX:
- if (_mode != GPS_DRIVER_MODE_UBX) {
- _mode = GPS_DRIVER_MODE_UBX;
- _mode_changed = true;
- }
- break;
- case GPS_CONFIGURE_MTK19:
- if (_mode != GPS_DRIVER_MODE_MTK19) {
- _mode = GPS_DRIVER_MODE_MTK19;
- _mode_changed = true;
- }
- break;
- case GPS_CONFIGURE_MTK16:
- if (_mode != GPS_DRIVER_MODE_MTK16) {
- _mode = GPS_DRIVER_MODE_MTK16;
- _mode_changed = true;
- }
- break;
- case GPS_CONFIGURE_NMEA:
- if (_mode != GPS_DRIVER_MODE_NMEA) {
- _mode = GPS_DRIVER_MODE_NMEA;
- _mode_changed = true;
- }
- break;
case SENSORIOCRESET:
cmd_reset();
break;
@@ -275,19 +252,7 @@ GPS::ioctl(struct file *filp, int cmd, unsigned long arg)
void
GPS::config()
{
- int length = 0;
- uint8_t send_buffer[SEND_BUFFER_LENGTH];
-
- _Helper->configure(send_buffer, length, SEND_BUFFER_LENGTH, _baudrate_changed, _baudrate);
-
- /* The config message is sent sent at the old baudrate */
- if (length > 0) {
-
- if (length != ::write(_serial_fd, send_buffer, length)) {
- debug("write config failed");
- return;
- }
- }
+ _Helper->configure(_serial_fd, _baudrate_changed, _baudrate);
/* Sometimes the baudrate needs to be changed, for instance UBX with factory settings are changed
* from 9600 to 38400
@@ -356,11 +321,8 @@ GPS::task_main()
case GPS_DRIVER_MODE_UBX:
_Helper = new UBX();
break;
- case GPS_DRIVER_MODE_MTK19:
- //_Helper = new MTK19();
- break;
- case GPS_DRIVER_MODE_MTK16:
- //_Helper = new MTK16();
+ case GPS_DRIVER_MODE_MTK:
+ _Helper = new MTK();
break;
case GPS_DRIVER_MODE_NMEA:
//_Helper = new NMEA();
@@ -530,11 +492,8 @@ GPS::print_info()
case GPS_DRIVER_MODE_UBX:
warnx("protocol: UBX");
break;
- case GPS_DRIVER_MODE_MTK19:
- warnx("protocol: MTK 1.9");
- break;
- case GPS_DRIVER_MODE_MTK16:
- warnx("protocol: MTK 1.6");
+ case GPS_DRIVER_MODE_MTK:
+ warnx("protocol: MTK");
break;
case GPS_DRIVER_MODE_NMEA:
warnx("protocol: NMEA");
diff --git a/apps/drivers/gps/gps_helper.h b/apps/drivers/gps/gps_helper.h
index 176b7eba8..576692c2a 100644
--- a/apps/drivers/gps/gps_helper.h
+++ b/apps/drivers/gps/gps_helper.h
@@ -36,12 +36,13 @@
/* @file U-Blox protocol definitions */
#ifndef GPS_HELPER_H
+#define GPS_HELPER_H
class GPS_Helper
{
public:
virtual void reset(void) = 0;
- virtual void configure(uint8_t *buffer, int &length, const unsigned max_length, bool &baudrate_changed, unsigned &baudrate) = 0;
+ virtual void configure(const int &fd, bool &baudrate_changed, unsigned &baudrate) = 0;
virtual void parse(uint8_t, struct vehicle_gps_position_s*, bool &config_needed, bool &pos_updated) = 0;
};
diff --git a/apps/drivers/gps/ubx.cpp b/apps/drivers/gps/ubx.cpp
index a82327175..8e2396564 100644
--- a/apps/drivers/gps/ubx.cpp
+++ b/apps/drivers/gps/ubx.cpp
@@ -68,11 +68,8 @@ UBX::reset()
}
void
-UBX::configure(uint8_t *buffer, int &length, const unsigned max_length, bool &baudrate_changed, unsigned &baudrate)
+UBX::configure(const int &fd, bool &baudrate_changed, unsigned &baudrate)
{
- /* make sure the buffer, where the message is written to, is long enough */
- assert(sizeof(type_gps_bin_cfg_prt_packet_t)+2 <= max_length);
-
/* Only send a new config message when we got the ACK of the last one,
* otherwise we might not configure all the messages because the ACK comes from an older/previos CFD command
* reason being that the ACK only includes CFG-MSG but not to which NAV MSG it belongs.
@@ -105,16 +102,7 @@ UBX::configure(uint8_t *buffer, int &length, const unsigned max_length, bool &ba
cfg_prt_packet.inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK;
cfg_prt_packet.outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK;
- /* Calculate the checksum now */
- addChecksumToMessage((uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet));
-
- /* Start with the two sync bytes */
- buffer[0] = UBX_SYNC1;
- buffer[1] = UBX_SYNC2;
- /* Copy it to the buffer that will be written back in the main gps driver */
- memcpy(&(buffer[2]), &cfg_prt_packet, sizeof(cfg_prt_packet));
- /* Set the length of the packet (plus the 2 sync bytes) */
- length = sizeof(cfg_prt_packet)+2;
+ sendConfigPacket(fd, (uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet));
} else if (_config_state == UBX_CONFIG_STATE_PRT_NEW_BAUDRATE) {
@@ -131,12 +119,7 @@ UBX::configure(uint8_t *buffer, int &length, const unsigned max_length, bool &ba
cfg_prt_packet.inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK;
cfg_prt_packet.outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK;
- addChecksumToMessage((uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet));
-
- buffer[0] = UBX_SYNC1;
- buffer[1] = UBX_SYNC2;
- memcpy(&(buffer[2]), &cfg_prt_packet, sizeof(cfg_prt_packet));
- length = sizeof(cfg_prt_packet)+2;
+ sendConfigPacket(fd, (uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet));
/* If the new baudrate will be different from the current one, we should report that back to the driver */
if (UBX_CFG_PRT_PAYLOAD_BAUDRATE != baudrate) {
@@ -160,12 +143,7 @@ UBX::configure(uint8_t *buffer, int &length, const unsigned max_length, bool &ba
cfg_rate_packet.navRate = UBX_CFG_RATE_PAYLOAD_NAVRATE;
cfg_rate_packet.timeRef = UBX_CFG_RATE_PAYLOAD_TIMEREF;
- addChecksumToMessage((uint8_t*)&cfg_rate_packet, sizeof(cfg_rate_packet));
-
- buffer[0] = UBX_SYNC1;
- buffer[1] = UBX_SYNC2;
- memcpy(&(buffer[2]), &cfg_rate_packet, sizeof(cfg_rate_packet));
- length = sizeof(cfg_rate_packet)+2;
+ sendConfigPacket(fd, (uint8_t*)&cfg_rate_packet, sizeof(cfg_rate_packet));
} else if (_config_state == UBX_CONFIG_STATE_NAV5) {
/* send a NAV5 message to set the options for the internal filter */
@@ -179,12 +157,7 @@ UBX::configure(uint8_t *buffer, int &length, const unsigned max_length, bool &ba
cfg_nav5_packet.dynModel = UBX_CFG_NAV5_PAYLOAD_DYNMODEL;
cfg_nav5_packet.fixMode = UBX_CFG_NAV5_PAYLOAD_FIXMODE;
- addChecksumToMessage((uint8_t*)&cfg_nav5_packet, sizeof(cfg_nav5_packet));
-
- buffer[0] = UBX_SYNC1;
- buffer[1] = UBX_SYNC2;
- memcpy(&(buffer[2]), &cfg_nav5_packet, sizeof(cfg_nav5_packet));
- length = sizeof(cfg_nav5_packet)+2;
+ sendConfigPacket(fd, (uint8_t*)&cfg_nav5_packet, sizeof(cfg_nav5_packet));
} else {
/* Catch the remaining config states here, they all need the same packet type */
@@ -233,12 +206,7 @@ UBX::configure(uint8_t *buffer, int &length, const unsigned max_length, bool &ba
break;
}
- addChecksumToMessage((uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet));
-
- buffer[0] = UBX_SYNC1;
- buffer[1] = UBX_SYNC2;
- memcpy(&(buffer[2]), &cfg_msg_packet, sizeof(cfg_msg_packet));
- length = sizeof(cfg_msg_packet)+2;
+ sendConfigPacket(fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet));
}
}
}
@@ -779,3 +747,21 @@ UBX::addChecksumToMessage(uint8_t* message, const unsigned length)
message[length-2] = ck_a;
message[length-1] = ck_b;
}
+
+void
+UBX::sendConfigPacket(const int &fd, uint8_t *packet, const unsigned length)
+{
+ ssize_t ret = 0;
+
+ /* Calculate the checksum now */
+ addChecksumToMessage(packet, length);
+
+ const uint8_t sync_bytes[] = {UBX_SYNC1, UBX_SYNC2};
+
+ /* Start with the two sync bytes */
+ ret += write(fd, sync_bytes, sizeof(sync_bytes));
+ ret += write(fd, packet, length);
+
+ if (ret != (int)length + (int)sizeof(sync_bytes)) // XXX is there a neater way to get rid of the unsigned signed warning?
+ warnx("ubx: config write fail");
+}
diff --git a/apps/drivers/gps/ubx.h b/apps/drivers/gps/ubx.h
index a23bb5502..c420e83b9 100644
--- a/apps/drivers/gps/ubx.h
+++ b/apps/drivers/gps/ubx.h
@@ -40,7 +40,6 @@
#include "gps_helper.h"
-
#define UBX_SYNC1 0xB5
#define UBX_SYNC2 0x62
@@ -341,7 +340,7 @@ public:
UBX();
~UBX();
void reset(void);
- void configure(uint8_t *buffer, int &length, const unsigned max_length, bool &baudrate_changed, unsigned &baudrate);
+ void configure(const int &fd, bool &baudrate_changed, unsigned &baudrate);
void parse(uint8_t, struct vehicle_gps_position_s*, bool &config_needed, bool &pos_updated);
private:
@@ -358,7 +357,13 @@ private:
/**
* Add the two checksum bytes to an outgoing message
*/
- void addChecksumToMessage(uint8_t*, const unsigned);
+ void addChecksumToMessage(uint8_t* message, const unsigned length);
+
+ /**
+ * Helper to send a config packet
+ */
+ void sendConfigPacket(const int &fd, uint8_t *packet, const unsigned length);
+
ubx_config_state_t _config_state;
bool _waiting_for_ack;
ubx_decode_state_t _decode_state;
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-06 04:08:39 +0000