void TestZigZag(double V_wind, double theta_wind) { double t, V_tas, V_gps, theta_gps, theta_glider; int i; for (i=0; i<=NUM_SAMPLES; i++) { t = i; V_tas = 20.0; theta_glider = sin(t*M_PI*2.0/NUM_SAMPLES)*30*DEGTORAD; double V_gps_x = V_tas * sin(theta_glider) - V_wind*sin(theta_wind); double V_gps_y = V_tas * cos(theta_glider) - V_wind*cos(theta_wind); V_gps = sqrt(V_gps_x*V_gps_x+V_gps_y*V_gps_y); theta_gps = atan2(V_gps_x,V_gps_y); myzigzag.AddPoint(t, V_tas, V_gps, theta_gps); } // ok, ready to calculate if (myzigzag.CheckSpread(t+1, 0.0)) { // data is ok to make an estimate double V_wind_estimate=1; double theta_wind_estimate=0; double percent_error; percent_error = myzigzag.StartSearch(V_wind_estimate, theta_wind_estimate); myzigzag.Estimate(&V_wind_estimate, &theta_wind_estimate, &percent_error); DebugStore("%2.1f %2.1f %03.0f %03.0f %2.1f # test zigzag\n", V_wind, V_wind_estimate, theta_wind/DEGTORAD, theta_wind_estimate/DEGTORAD, percent_error ); } }
int WindZigZagUpdate(const NMEA_INFO &basic, const DERIVED_INFO &derived, fixed &zzwindspeed, fixed &zzwindbearing) { static fixed tLastEstimate(-1); if (!basic.AirspeedAvailable) return 0; // TODO accuracy: correct TAS for vertical speed if dynamic pullup if ((basic.Time <= tLastEstimate) || (tLastEstimate == fixed_minus_one)) tLastEstimate = basic.Time - UPDATE_RATE; if (!WindZigZagCheckAirData(basic)) return 0; // ok to add a point myzigzag.AddPoint(basic.Time, basic.TrueAirspeed, basic.GroundSpeed, basic.TrackBearing); // don't update wind from zigzag more often than // every UPDATE_RATE seconds, so it is balanced with respect // to circling if (basic.Time < tLastEstimate + UPDATE_RATE) return 0; fixed V_wind_estimate = basic.wind.norm; Angle theta_wind_estimate = basic.wind.bearing; fixed percent_error = myzigzag.StartSearch(V_wind_estimate, theta_wind_estimate); // Check spread of zig-zag manoeuver if (!myzigzag.CheckSpread(basic.Time, percent_error)) return 0; fixed v_error = percent_error * basic.TrueAirspeed / 100; if (v_error < fixed_half) // don't refine search if error is small return 0; if (myzigzag.Estimate(V_wind_estimate, theta_wind_estimate, percent_error)) { // ok, we have made an update tLastEstimate = basic.Time; zzwindspeed = V_wind_estimate; zzwindbearing = theta_wind_estimate.value_degrees(); // calculate error quality int quality; //double pes = v_error/(V_SCALE/NUM_V_POINTS); //quality = iround(0.5+4.5/(1.0+percent_error*percent_error/30.0)); quality = max(1, 5 - iround(percent_error / 2)); if (derived.Circling) quality = max(1, quality / 2); // de-value updates in circling mode return quality; } return 0; }
int WindZigZagUpdate(NMEA_INFO* Basic, DERIVED_INFO* Calculated, double *zzwindspeed, double *zzwindbearing) { static double tLastEstimate = -1; if (!Basic->AirspeedAvailable) { return 0; } #if (WINDOWSPC>0) #ifdef DEBUG_ZIGZAG TestZigZagLoop(); #endif #endif // TODO accuracy: correct TAS for vertical speed if dynamic pullup if ((Basic->Time<= tLastEstimate)||(tLastEstimate==-1)) { tLastEstimate = Basic->Time-UPDATE_RATE; } if (!WindZigZagCheckAirData(Basic, Calculated)) { return 0; } // ok to add a point myzigzag.AddPoint(Basic->Time, Basic->TrueAirspeed, Basic->Speed, Basic->TrackBearing*DEGTORAD); #ifdef DEBUG_ZIGZAG_A DebugStore("%f %03.0f %03.0f %03.0f # zigpoint\n", Basic->Time, Basic->TrueAirspeed, Basic->Speed, Basic->TrackBearing); #endif // don't update wind from zigzag more often than // every UPDATE_RATE seconds, so it is balanced with respect // to circling if (Basic->Time<tLastEstimate+UPDATE_RATE) { return 0; } double V_wind_estimate = Calculated->WindSpeed; double theta_wind_estimate = Calculated->WindBearing*DEGTORAD; double percent_error = myzigzag.StartSearch(V_wind_estimate, theta_wind_estimate); // Check spread of zig-zag manoeuver if (!myzigzag.CheckSpread(Basic->Time, percent_error)) { return 0; } double v_error = percent_error*Basic->TrueAirspeed/100.0; if (v_error<0.5) { // don't refine search if error is small #ifdef DEBUG_ZIGZAG DebugStore("zigzag error small %02.0f %03.1f\n", percent_error, v_error); #endif return 0; } if (myzigzag.Estimate(&V_wind_estimate, &theta_wind_estimate, &percent_error)) { // ok, we have made an update tLastEstimate = Basic->Time; theta_wind_estimate /= DEGTORAD; *zzwindspeed = V_wind_estimate; *zzwindbearing = theta_wind_estimate; // calculate error quality int quality; // double pes = v_error/(V_SCALE/NUM_V_POINTS); //quality = iround(0.5+4.5/(1.0+percent_error*percent_error/30.0)); quality = max(1,5-iround(percent_error/2)); if (Calculated->Circling) { quality = max(1,quality/2); // de-value updates in circling mode } #ifdef DEBUG_ZIGZAG DebugStore("%f %3.1f %03.0f %3.1f %03.0f %f %d # zigzag\n", Basic->Time, V_wind_estimate, theta_wind_estimate, Calculated->WindSpeed, Calculated->WindBearing, percent_error, quality); #endif return quality; } else { #ifdef DEBUG_ZIGZAG DebugStore("zigzag estimate failed to improve\n"); #endif } return 0; }