File content as of revision 59:3e5e5b97ca75:

extern int angle_send;

void update_euler(double *z_arr, char*output, int &angle_send)
{
    lastpitch = 0.9 * lastpitch + 0.1 * (toDegrees(get_pitch()*3.5));
    lastroll = 0.9 * lastroll + 0.1 * toDegrees(get_roll()*3.5);

    static double x_;
    static double y_;
    static double z_;
    get_angle(z_,y_,x_);
    angle_send = 0;
    arr_pushback(z_/5.0, z_arr, 5);
    //z_summ = arr_sum((int)5, z_arr);
    //          z_summ *= 5;
    //        z_summ = constrains(z_summ,-45.0,45.0);

    sprintf(output,"$EULER,%f,%f,%f\r\n",x_,y_,z_);
    // bluetooth.puts(output);
    //pc.puts(output);
}

double new_bearing(double track_, int avgpos_, double *track_arr_, double *z_arr_)
{
    arr_pushback((track_-90)*-1,track_arr_,10);
    double track_avg_ = arr_avg(track_arr_,abs(avgpos_));
    //////////////////////////////////////////////////////////////////////////////////////////////////////////////
    //  double z_summ = z_arr_[0]*abs(avgpos_)*2;//arr_sum((int)5, z_arr_);///////////////////////////////////////
    //////////////////////////////////////////////////////////////////////////////////////////////////////////////
    double z_summ = z_arr_[0]*abs(kp);////////////////////////////////////////////////////////////////////////////
    //////////////////////////////////////////////////////////////////////////////////////////////////////////////
    z_summ = constrains(z_summ,-45.0,45.0);
    track_avg_ -= z_summ;
    // pc.printf("$NEWANGLE,%f,    %f,    %f\r\n",track_avg_, track_arr[0], z_summ);

    return track_avg_;
}

Point on_linePoint(double lookahead, Point pos, Point start, Point end, double bearing)
{
    Point forward;

    double mline = (end.GetY()-start.GetY())/(end.GetX()-start.GetX());
    double mp3 = - 1 / mline;
    double b = pos.GetY() - ( mp3 * pos.GetX());
    double bline = end.GetY()-mline*end.GetX();
    double x = (b-bline)/(mline-mp3);
    double y = mline * x + bline;

    double x5;
    double y5;
    get_latlon_byangle(x,y,lookahead,bearing,y5,x5);
    //  pc.printf("bearing,%f  %f  %f  %f  %f \r\n",x,y,x5,y5,bearing);
    forward.SetX(x5);
    forward.SetY(y5);

    return forward;
}

/*double bearing_to_line(double *z_array, double *track_array,double la,Point pos, Point start, Point end, double lookahead_time,double scl_, double avgp, Point forw)
{
    avgp = abs(avgp);
    //  double gyroDPS = z_array[0]*5;
    double antenna_avg = arr_avg(track_array,avgp);

    for ( int i = 0; i < avgp / 2; i++ ) {
        antenna_avg += z_array[i];
    }

    antenna_avg = angle_normal(antenna_avg, 0, 360);
    Point forward_pos = on_linePoint(la, pos,start,end,track_array[0]);

    // double bearing = atan2(cos(pos.GetY())*sin(forward_pos.GetY())-
    //            sin(pos.GetY())*cos(forward_pos.GetY())*cos(forward_pos.GetX()-pos.GetX()),
    //           sin(forward_pos.GetX()-pos.GetX())*cos(forward_pos.GetX()));
    // =ATAN2(COS(lat1)*SIN(lat2)-SIN(lat1)*COS(lat2)*COS(lon2-lon1),

//     double  y = sin(forward_pos.GetX()-pos.GetX()) * cos(forward_pos.GetX());
//double x = cos(pos.GetX())*sin(forward_pos.GetX()) - sin(pos.GetX())*cos(forward_pos.GetX())*cos(forward_pos.GetY()-pos.GetY());
//double bearing = ToDeg(atan2(y, x));
//bearing = angle_normal(bearing,-180,180);
    // SIN(lon2-lon1)*COS(lat2))

    //        XTD =asin(sin(dist_AD)*sin(crs_AD-crs_AB))
//(positive XTD means right of course, negative means left)
//(If the point A is the N. or S. Pole replace crs_AD-cr
//double bearing=asin(sqrt( (sin(dist_AD))^2 - (sin(XTD))^2 )/cos(XTD))

//XTD =asin(sin(dist_AD)*sin(crs_AD-crs_AB))

//b = arccos ( cos (90 - lat2) * cos (90 - lat1) + sin (90 - lat2) * sin (90 - lat1) * cos (lon2 - lon1) )
//A = arcsin ( sin (90 - lat2) * sin (lon2 - lon1) / sin (b) )

    double b = acos ( cos (ToRad(90 - forward_pos.GetX())) * cos (ToRad(90 - pos.GetX())) + sin (ToRad(90 - forward_pos.GetX())) * sin (ToRad(90 - pos.GetX())) * cos (ToRad(forward_pos.GetY() - pos.GetY()) ));
    double bearing = asin ( sin (ToRad(90 - forward_pos.GetX())) * sin (ToRad(forward_pos.GetY() - pos.GetY())) / sin (b) );

double c = acos ( cos (ToRad(90 - end.GetY())) * cos (ToRad(90 - start.GetY())) + sin (ToRad(90 - end.GetY())) * sin (ToRad(90 - start.GetY())) * cos (ToRad(end.GetX() - start.GetX())));
    double d = asin ( sin (ToRad(90 - end.GetY())) * sin (ToRad(end.GetX() - start.GetX())) / sin (c) );

double dot = ToRad(pos.GetX())*ToRad(forward_pos.GetX()) + ToRad(pos.GetY())*ToRad(forward_pos.GetX());//      # dot product
double det = ToRad(pos.GetX())*ToRad(forward_pos.GetY()) - ToRad(pos.GetY())*ToRad(forward_pos.GetX());//      # determinant
double angl = atan2(det, dot); 
//dot = x1*x2 + y1*y2      # dot product
//det = x1*y2 - y1*x2      # determinant
//angle = atan2(det, dot) 
    pc.printf("POS,%f   %f   %f   %f\r\n",forward_pos.GetX(),forward_pos.GetY(),pos.GetX(),pos.GetY());

    double heading_to_line = CalculateHeading(pos.GetX(), pos.GetY(), forward_pos.GetX(), forward_pos.GetY());
    double error = heading_err(antenna_avg, heading_to_line);

//double error_angle = ToDeg(asin((forward_pos.GetX()-pos.GetX())/(sqrt((forward_pos.GetX()-pos.GetX())*(forward_pos.GetX()-pos.GetX())+(forward_pos.GetY()-pos.GetY())*(forward_pos.GetY()-pos.GetY())))));
 //   heading_to_line = angle_normal(heading_to_line,-180,180);
   // pc.printf("ANGLES,%f   %f    %f    %f     %f   %f    %f \r\n",error_angle,antenna_avg,heading_to_line,error, ToDeg(bearing), ToDeg(d), ToDeg(angl));
   //     pc.printf("ANGLES,%f   %f    %f    %f     %f   %f    %f \r\n",error_angle,antenna_avg,heading_to_line,error, ToDeg(bearing), ToDeg(d), ToDeg(angl));

    return heading_to_line;
}*/