Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-08 18:52:46 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

Merged mtk16 and mtk19 helper classes, configure() now writes directly instead of buffering

Browse Source
This commit is contained in:
Julian Oes
2013-02-06 18:47:32 -08:00
parent d962e6c403
commit 6ed5d97aea
5 changed files with 41 additions and 101 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/drivers/drv_gps.h
+1 -12
View File
@@ -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 */
apps/drivers/gps/gps.cpp
+6 -47
View File
@@ -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");
apps/drivers/gps/gps_helper.h
+2 -1
View File
@@ -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;
};
apps/drivers/gps/ubx.cpp
+24 -38
View File
@@ -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");
}
apps/drivers/gps/ubx.h
+8 -3
View File
@@ -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;