| 0,0 → 1,287 |
|---|
| //------------------------------------------------------------------------------ |
| // _ _ |
| // | | | | |
| // ___ _ __ ___ ___ _ _ ___| |_ ___ ___| |__ |
| // / _ \ '_ ` _ \/ __| | | / __| __/ _ \/ __| '_ \. |
| // | __/ | | | | \__ \ |_| \__ \ || __/ (__| | | | |
| // \___|_| |_| |_|___/\__, |___/\__\___|\___|_| |_| |
| // __/ | |
| // |___/ 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++; |
| } |
| } |