Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_GPS: Rename SBR1/SBR2 to SBRH/SBRM and change format 

Add (sender_id, msg_len) in SBRM message
Add index/pages in SBRH/SBRM messages
Change format to integers to avoid GCS string parsing
Decrease data size in SBRH
This commit is contained in:
ebethon 2017-06-26 16:41:24 +01:00 committed by Francisco Ferreira
parent 0b5dad33f2
commit 4779a8f1f1
2 changed files with 36 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
libraries/AP_GPS/AP_GPS_SBP.cpp
View File

@ -413,7 +413,6 @@ AP_GPS_SBP::logging_log_raw_sbp(uint16_t msg_type,
uint16_t sender_id, uint16_t sender_id,
uint8_t msg_len, uint8_t msg_len,
uint8_t *msg_buff) { uint8_t *msg_buff) {
if (gps._DataFlash == nullptr || !gps._DataFlash->logging_started()) { if (gps._DataFlash == nullptr || !gps._DataFlash->logging_started()) {
return; return;
} }
@ -424,30 +423,37 @@ AP_GPS_SBP::logging_log_raw_sbp(uint16_t msg_type,
} }
uint64_t time_us = AP_HAL::micros64(); uint64_t time_us = AP_HAL::micros64();
uint8_t pages = 1;
struct log_SbpRAW1 pkt = { if (msg_len > 48) {
LOG_PACKET_HEADER_INIT(LOG_MSG_SBPRAW1), pages += (msg_len - 48) / 104 + 1;
}
struct log_SbpRAWH pkt = {
LOG_PACKET_HEADER_INIT(LOG_MSG_SBPRAWH),
time_us : time_us, time_us : time_us,
msg_type : msg_type, msg_type : msg_type,
sender_id : sender_id, sender_id : sender_id,
index : 1,
pages : pages,
msg_len : msg_len, msg_len : msg_len,
}; };
memcpy(pkt.data1, msg_buff, MIN(msg_len,64)); memcpy(pkt.data, msg_buff, MIN(msg_len, 48));
gps._DataFlash->WriteBlock(&pkt, sizeof(pkt)); gps._DataFlash->WriteBlock(&pkt, sizeof(pkt));
if (msg_len > 64) { for (uint8_t i = 0; i < pages - 1; i++) {
struct log_SbpRAWM pkt2 = {
struct log_SbpRAW2 pkt2 = { LOG_PACKET_HEADER_INIT(LOG_MSG_SBPRAWM),
LOG_PACKET_HEADER_INIT(LOG_MSG_SBPRAW2),
time_us : time_us, time_us : time_us,
msg_type : msg_type, msg_type : msg_type,
sender_id : sender_id,
index : uint8_t(i + 2),
pages : pages,
msg_len : msg_len,
}; };
memcpy(pkt2.data2, &msg_buff[64], msg_len - 64); memcpy(pkt2.data, &msg_buff[48 + i * 104], MIN(msg_len - (48 + i * 104), 104));
gps._DataFlash->WriteBlock(&pkt2, sizeof(pkt2)); gps._DataFlash->WriteBlock(&pkt2, sizeof(pkt2));
} }
}; };
#endif // SBP_HW_LOGGING #endif // SBP_HW_LOGGING

25
libraries/AP_GPS/AP_GPS_SBP2.cpp
View File

@ -459,24 +459,35 @@ AP_GPS_SBP2::logging_log_raw_sbp(uint16_t msg_type,
} }
uint64_t time_us = AP_HAL::micros64(); uint64_t time_us = AP_HAL::micros64();
uint8_t pages = 1;
struct log_SbpRAW1 pkt = { if (msg_len > 48) {
LOG_PACKET_HEADER_INIT(LOG_MSG_SBPRAW1), pages += (msg_len - 48) / 104 + 1;
}
struct log_SbpRAWH pkt = {
LOG_PACKET_HEADER_INIT(LOG_MSG_SBPRAWH),
time_us : time_us, time_us : time_us,
msg_type : msg_type, msg_type : msg_type,
sender_id : sender_id, sender_id : sender_id,
index : 1,
pages : pages,
msg_len : msg_len, msg_len : msg_len,
}; };
memcpy(pkt.data1, msg_buff, MIN(msg_len,64)); memcpy(pkt.data, msg_buff, MIN(msg_len, 48));
gps._DataFlash->WriteBlock(&pkt, sizeof(pkt)); gps._DataFlash->WriteBlock(&pkt, sizeof(pkt));
if (msg_len > 64) { for (uint8_t i = 0; i < pages - 1; i++) {
struct log_SbpRAW2 pkt2 = { struct log_SbpRAWM pkt2 = {
LOG_PACKET_HEADER_INIT(LOG_MSG_SBPRAW2), LOG_PACKET_HEADER_INIT(LOG_MSG_SBPRAWM),
time_us : time_us, time_us : time_us,
msg_type : msg_type, msg_type : msg_type,
sender_id : sender_id,
index : uint8_t(i + 2),
pages : pages,
msg_len : msg_len,
}; };
memcpy(pkt2.data2, &msg_buff[64], MIN(msg_len - 64,192)); memcpy(pkt2.data, &msg_buff[48 + i * 104], MIN(msg_len - (48 + i * 104), 104));
gps._DataFlash->WriteBlock(&pkt2, sizeof(pkt2)); gps._DataFlash->WriteBlock(&pkt2, sizeof(pkt2));
} }
}; };