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 main()
{
vector<int> input = {1, 1};
ZigZag z;
int res = z.longestZigZag(input);
cout << res << endl;
return 0;
}
int main()
{
ZigZag z;
vector<int>v{ 374, 40, 854, 203, 203, 156, 362, 279, 812, 955,
600, 947, 978, 46, 100, 953, 670, 862, 568, 188,
67, 669, 810, 704, 52, 861, 49, 640, 370, 908,
477, 245, 413, 109, 659, 401, 483, 308, 609, 120,
249, 22, 176, 279, 23, 22, 617, 462, 459, 244 };
cout<<z.longestZigZag(v)<<endl;
return 0;
}
int main(int argc, char *argv[])
{
ZigZag obj;
vector<int> sequence;
sequence.push_back(1);
sequence.push_back(17);
sequence.push_back(5);
sequence.push_back(10);
sequence.push_back(13);
sequence.push_back(15);
sequence.push_back(10);
sequence.push_back(5);
sequence.push_back(16);
sequence.push_back(8);
cout<<obj.longestZigZag(sequence)<<endl;
}
int main()
{
vector<int> v ;
v.push_back(1);
v.push_back(2);
v.push_back(3);
v.push_back(4);
v.push_back(5);
v.push_back(6);
/*v.push_back(7);
v.push_back(8);
v.push_back(9);*/
ZigZag z;
cout<<"z.longestZigZag(v) "<<z.longestZigZag(v)<<endl;
}
// BEGIN KAWIGIEDIT TESTING
// Generated by KawigiEdit 2.1.4 (beta) modified by pivanof
bool KawigiEdit_RunTest(int testNum, vector <int> p0, bool hasAnswer, int p1) {
cout << "Test " << testNum << ": [" << "{";
for (int i = 0; int(p0.size()) > i; ++i) {
if (i > 0) {
cout << ",";
}
cout << p0[i];
}
cout << "}";
cout << "]" << endl;
ZigZag *obj;
int answer;
obj = new ZigZag();
clock_t startTime = clock();
answer = obj->longestZigZag(p0);
clock_t endTime = clock();
delete obj;
bool res;
res = true;
cout << "Time: " << double(endTime - startTime) / CLOCKS_PER_SEC << " seconds" << endl;
if (hasAnswer) {
cout << "Desired answer:" << endl;
cout << "\t" << p1 << endl;
}
cout << "Your answer:" << endl;
cout << "\t" << answer << endl;
if (hasAnswer) {
res = answer == p1;
}
if (!res) {
cout << "DOESN'T MATCH!!!!" << endl;
} else if (double(endTime - startTime) / CLOCKS_PER_SEC >= 2) {
cout << "FAIL the timeout" << endl;
res = false;
} else if (hasAnswer) {
cout << "Match :-)" << endl;
} else {
cout << "OK, but is it right?" << endl;
}
cout << "" << endl;
return res;
}
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;
}
// BEGIN CUT HERE
void main( int argc, char* argv[] ) {
{
int sequenceARRAY[] = { 1, 7, 4, 9, 2, 5 };
vector <int> sequence( sequenceARRAY, sequenceARRAY+ARRSIZE(sequenceARRAY) );
ZigZag theObject;
eq(0, theObject.longestZigZag(sequence),6);
}
{
int sequenceARRAY[] = { 1, 17, 5, 10, 13, 15, 10, 5, 16, 8 };
vector <int> sequence( sequenceARRAY, sequenceARRAY+ARRSIZE(sequenceARRAY) );
ZigZag theObject;
eq(1, theObject.longestZigZag(sequence),7);
}
{
int sequenceARRAY[] = { 44 };
vector <int> sequence( sequenceARRAY, sequenceARRAY+ARRSIZE(sequenceARRAY) );
ZigZag theObject;
eq(2, theObject.longestZigZag(sequence),1);
}
{
int sequenceARRAY[] = { 1, 2, 3, 4, 5, 6, 7, 8, 9 };
vector <int> sequence( sequenceARRAY, sequenceARRAY+ARRSIZE(sequenceARRAY) );
ZigZag theObject;
eq(3, theObject.longestZigZag(sequence),2);
}
{
int sequenceARRAY[] = { 70, 55, 13, 2, 99, 2, 80, 80, 80, 80, 100, 19, 7, 5, 5, 5, 1000, 32, 32 };
vector <int> sequence( sequenceARRAY, sequenceARRAY+ARRSIZE(sequenceARRAY) );
ZigZag theObject;
eq(4, theObject.longestZigZag(sequence),8);
}
{
int sequenceARRAY[] = {
374, 40, 854, 203, 203, 156, 362, 279, 812, 955,
600, 947, 978, 46, 100, 953, 670, 862, 568, 188,
67, 669, 810, 704, 52, 861, 49, 640, 370, 908,
477, 245, 413, 109, 659, 401, 483, 308, 609, 120,
249, 22, 176, 279, 23, 22, 617, 462, 459, 244 }
;
vector <int> sequence( sequenceARRAY, sequenceARRAY+ARRSIZE(sequenceARRAY) );
ZigZag theObject;
eq(5, theObject.longestZigZag(sequence),36);
}
}
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;
}
