Subversion Repositories FlightCtrl

//------------------------------------------------------------------------------

//                                  _            _     

//                                 | |          | |    

//      ___ _ __ ___  ___ _   _ ___| |_ ___  ___| |__  

//     / _ \ '_ ` _ \/ __| | | / __| __/ _ \/ __| '_ \. 

//    |  __/ | | | | \__ \ |_| \__ \ ||  __/ (__| | | |

//     \___|_| |_| |_|___/\__, |___/\__\___|\___|_| |_|

//                         __/ |                       

//                        |___/    Engineering

//

// Filename:    GPS.c

// Description: 

//              

// Author:      Martin Steppuhn

// History:     15.06.2007 Initial version

//              

//------------------------------------------------------------------------------

/**** Includes ****************************************************************/

#include "std_c.h"

#include "main.h"

#include "fc.h"

#include "gps_ubx.h"

/**** Preprocessing directives (#define) **************************************/

/**** Type definitions (typedef) **********************************************/

/**** Global constants ********************************************************/

/**** Global variables ********************************************************/

signed int GPS_Nick = 0;

signed int GPS_Roll = 0;

/**** Local constants  ********************************************************/

/**** Local variables *********************************************************/

signed int test_nick = 0;

signed int test_roll = 0;

long GpsAktuell_X = 0;

long GpsAktuell_Y = 0;

long GpsAktuell_Z = 0;

long GpsZiel_X = 0;

long GpsZiel_Y = 0;

long gps_dx;

long gps_dy;

long gps_dz;

long gps_vx;

long gps_vy;

long gps_vz;

bool  gps_hold;

uint8 print_pos;

/**** Local function prototypes ***********************************************/

void print_uint16(uint16 value,uint8 width);

void print_int16(int16 value,uint8 width);

void print_string(char *s);

//------------------------------------------------------------------------------

// Name:      GPS_Neutral

// Function:  

//            

// Parameter: 

// Return:    

//------------------------------------------------------------------------------

void GPS_Neutral(void)

{

  GpsZiel_X = GpsAktuell_X;

  GpsZiel_Y = GpsAktuell_Y;

  GPS_Nick  = 0;

  GPS_Roll  = 0;

 // LED_GPS_DATA_OFF;

  //LED_GPS_FIX_OFF;

  //LED_GPS_DATA_ON;

}

//------------------------------------------------------------------------------

// Name:      gps_trace

// Function:  

//            

// Parameter: 

// Return:    

//------------------------------------------------------------------------------

void gps_trace(void)

{

  print_pos = 0;

  print_uint16(nav_sol.gpsfix,1);      

  print_string(" ");

  print_int16(gps_dy,7);

  print_int16(gps_dx,7);

  print_string(" | ");

  print_int16(gps_vy,7);

        print_int16(gps_vx,7);

  print_string(" | ");

  print_int16(GPS_Roll,7);

  print_int16(GPS_Nick,7);

  print_string(" # ");

  print_int16(StickRoll,7);

  print_int16(StickNick,7);

  print_string("\r\n");

  UebertragungAbgeschlossen = 0;

  UDR = SendeBuffer[0];             // Start

}

//------------------------------------------------------------------------------

// Name:      gps_main

// Function:  

//            

// Parameter: 

// Return:    

//------------------------------------------------------------------------------

void GPS_Main(void)

{

  if((Poti3 > 125) && (nav_sol.gpsfix) && (gps_hold == false))    // Enable Hold

  {

    GpsZiel_X = GpsAktuell_X;

    GpsZiel_Y = GpsAktuell_Y;

        gps_hold = true;    

  }

  if(Poti3 < 125)                   // Disable Hold

  {

    GpsZiel_X = 0;

    GpsZiel_Y = 0;

    GPS_Nick  = 0;

    GPS_Roll  = 0;

        gps_hold = false;

  }

  if(ubx.update)

  {

    ubx.update = false;

    ubx_decode(&ubx);

    if(nav_sol.update)

    {

      //trace_nav_sol();

      nav_sol.update = false;

      if(nav_sol.gpsfix)        

      {

        LED_GPS_FIX_TOGGLE;

        LED_GPS_DATA_OFF;

      }

      else

      {

        LED_GPS_DATA_TOGGLE;

        LED_GPS_FIX_OFF;

      }

      //=== new Position ====================

      GpsAktuell_X = nav_sol.ecef_x;

      GpsAktuell_Y = nav_sol.ecef_y;

      gps_dx = GpsZiel_X - GpsAktuell_X;    // Süden +

      gps_dy = GpsZiel_Y - GpsAktuell_Y;        // Osten +      

      gps_vx = -nav_sol.ecefvx;    // cm/s ECEF X velocity  

      gps_vy = -nav_sol.ecefvy;    // cm/s ECEF Y velocity  

      if(gps_hold)

                  {    

        GPS_Nick = ((gps_dx * Poti1) / 512) + ((gps_vx * Poti2) / 128);

        GPS_Roll = ((gps_dy * Poti1) / 512) + ((gps_vy * Poti2) / 128);

            }

            else

            {

                    GPS_Nick = 0;

                GPS_Roll = 0;

        }

       //=====================================

      }

          gps_trace();

    }

}

//==============================================================================

//==============================================================================

//==============================================================================

//

//==============================================================================

//==============================================================================

//==============================================================================

//------------------------------------------------------------------------------

// Name:      

// Function:  

//            

// Parameter: 

// Return:    

//------------------------------------------------------------------------------

void print_uint16(uint16 value,uint8 width)

{

        uint8 i,s[10];

        for (i = 0; i < width; i++)

        {

                s[width - i - 1] = '0' + (value % 10);

                value /= 10;

        }

        for (i=0; i<(width - 1); i++)           // Overwrite trailing Zeros

        {

                if (s[i] == '0')        s[i] = ' ';

                        else break;

        }

        for (i=0; i<width; i++) SendeBuffer[print_pos++] = s[i];                // Output String

//      for (i=0; i<width; i++) putchar(s[i]);          // Output String

}

//------------------------------------------------------------------------------

// Name:      

// Function:  

//            

// Parameter: 

// Return:    

//------------------------------------------------------------------------------

void print_int16(int16 value,uint8 width)

{

        uint8 i,s[10];

        bool  neg;

  neg = false;

        if(value < 0) { value = -value; neg = true;     }

        for (i = 0; i < width; i++)

        {

                s[width - i - 1] = '0' + (value % 10);

                value /= 10;

        }

        for (i=0; i<(width - 1); i++)           // Overwrite trailing Zeros

        {

                if (s[i] == '0')        s[i] = ' ';

                else                   

                {

          if(neg)  s[i-1] = '-';

                        break;

    }                                          

        }

        for (i=0; i<width; i++) SendeBuffer[print_pos++] = s[i];                // Output String

//      for (i=0; i<width; i++) putchar(s[i]);          // Output String

}

//------------------------------------------------------------------------------

// Name:      

// Function:  

//            

// Parameter: 

// Return:    

//------------------------------------------------------------------------------

void print_string(char *s)

{

  while(*s)

  {

    SendeBuffer[print_pos++] = *s;

    s++;

  }    

}