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;
}
