- ao_flight_state = ao_flight_pad;
- start_latitude = ao_gps_data.longitude;
- start_longitude = ao_gps_data.latitude;
- start_altitude = ao_gps_data.altitude;
+
+ lat_sum += ao_gps_data.latitude;
+ lon_sum += ao_gps_data.longitude;
+ alt_sum += ao_gps_data.altitude;
+
+ if (++nsamples >= STARTUP_AVERAGE) {
+ ao_flight_state = ao_flight_pad;
+ ao_wakeup(&ao_flight_state);
+ start_latitude = lat_sum / nsamples;
+ start_longitude = lon_sum / nsamples;
+ start_altitude = alt_sum / nsamples;
+ }