diff options
author | Julian Oes <joes@student.ethz.ch> | 2013-02-06 18:47:32 -0800 |
---|---|---|
committer | Julian Oes <joes@student.ethz.ch> | 2013-02-06 18:47:32 -0800 |
commit | 6ed5d97aea29a284015708a6089b7910afea8369 (patch) | |
tree | 1192387a09c9198f97b27a0b1c203cc42cd7844b /apps | |
parent | d962e6c403678e14a64a6b01be8773e98660bb24 (diff) | |
download | px4-firmware-6ed5d97aea29a284015708a6089b7910afea8369.tar.gz px4-firmware-6ed5d97aea29a284015708a6089b7910afea8369.tar.bz2 px4-firmware-6ed5d97aea29a284015708a6089b7910afea8369.zip |
Merged mtk16 and mtk19 helper classes, configure() now writes directly instead of buffering
Diffstat (limited to 'apps')
-rw-r--r-- | apps/drivers/drv_gps.h | 13 | ||||
-rw-r--r-- | apps/drivers/gps/gps.cpp | 53 | ||||
-rw-r--r-- | apps/drivers/gps/gps_helper.h | 3 | ||||
-rw-r--r-- | apps/drivers/gps/ubx.cpp | 62 | ||||
-rw-r--r-- | apps/drivers/gps/ubx.h | 11 |
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; |