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 GpsZiel_X = 0;
long GpsZiel_Y = 0;
long gps_dx;
long gps_dy;
long gps_vx;
long gps_vy;
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++;
  }    
}