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

SIM_GPS: make writing of data w/out byteloss more efficient

This commit is contained in:
Peter Barker 2021-11-05 13:50:16 +11:00 committed by Peter Barker
parent 13bd2379cf
commit 306bd0a4ab
1 changed files with 9 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/SITL/SIM_GPS.cpp
View File

@ -72,16 +72,22 @@ ssize_t GPS::write_to_autopilot(const char *p, size_t size) const
}
#endif
const float byteloss = _sitl->gps_byteloss[instance];
// shortcut if we're not doing byteloss:
if (!is_positive(byteloss)) {
return SerialDevice::write_to_autopilot(p, size);
}
size_t ret = 0;
while (size--) {
if (_sitl->gps_byteloss[instance] > 0.0f) {
float r = ((((unsigned)random()) % 1000000)) / 1.0e4;
if (r < _sitl->gps_byteloss[instance]) {
if (r < byteloss) {
// lose the byte
p++;
continue;
}
}
const ssize_t pret = SerialDevice::write_to_autopilot(p, 1);
if (pret == 0) {
// no space?