- state->pad_lat = state->pad_lat_total / state->npad;
- state->pad_lon = state->pad_lon_total / state->npad;
- state->pad_alt = state->pad_alt_total / state->npad;
+ if (state->npad > 1) {
+ state->pad_lat = (state->pad_lat * 31 + data->lat) / 32.0;
+ state->pad_lon = (state->pad_lon * 31 + data->lon) / 32.0;
+ state->pad_alt = (state->pad_alt * 31 + data->alt) / 32.0;
+ } else {
+ state->pad_lat = data->lat;
+ state->pad_lon = data->lon;
+ state->pad_alt = data->alt;
+ }