fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-20 18:53:34 +08:00
Code Issues 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/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");
}