Go to most recent revision |
Blame |
Compare with Previous |
Last modification |
View Log
| RSS feed
//------------------------------------------------------------------------------
// _ _
// | | | |
// ___ _ __ ___ ___ _ _ ___| |_ ___ ___| |__
// / _ \ '_ ` _ \/ __| | | / __| __/ _ \/ __| '_ \.
// | __/ | | | | \__ \ |_| \__ \ || __/ (__| | | |
// \___|_| |_| |_|___/\__, |___/\__\___|\___|_| |_|
// __/ |
// |___/ 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++;
}
}