diff --git a/sw/airborne/estimator.c b/sw/airborne/estimator.c index ca25719e44..589f558238 100644 --- a/sw/airborne/estimator.c +++ b/sw/airborne/estimator.c @@ -1,10 +1,10 @@ /* * Paparazzi autopilot $Id$ - * + * * Copyright (C) 2004-2005 Pascal Brisset, Antoine Drouin * * This file is part of paparazzi. - * + * * paparazzi is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 2, or (at your option) @@ -18,7 +18,7 @@ * You should have received a copy of the GNU General Public License * along with paparazzi; see the file COPYING. If not, write to * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. + * Boston, MA 02111-1307, USA. * */ @@ -229,8 +229,7 @@ void estimator_update_state_gps( void ) { estimator_psi = atan2f(w_ve, w_vn); if (estimator_psi < 0.) estimator_psi += 2 * M_PI; -#ifdef EXTRA_DOWNLINK_DEVICE +#ifdef EXTRA_DOWNLINK_DEVICE DOWNLINK_SEND_ATTITUDE(ExtraPprzTransport,&estimator_phi,&estimator_psi,&estimator_theta); -#endif +#endif } - diff --git a/sw/airborne/gps_nmea.c b/sw/airborne/gps_nmea.c index 8c8df2b3dd..141d97a918 100644 --- a/sw/airborne/gps_nmea.c +++ b/sw/airborne/gps_nmea.c @@ -1,5 +1,5 @@ /* - * + * * Copyright (C) 2008 Marcus Wolschon * * This file is part of paparazzi. @@ -17,11 +17,11 @@ * You should have received a copy of the GNU General Public License * along with paparazzi; see the file COPYING. If not, write to * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. + * Boston, MA 02111-1307, USA. * */ -/** +/** * file gps_nmea.c * brief Parser for the NMEA protocol * @@ -34,7 +34,7 @@ */ #include -#include +#include #include #ifdef LINUX // do debug-output if run on the linux-target @@ -101,7 +101,7 @@ void ubxsend_cfg_rst(uint16_t bbr , uint8_t reset_mode) { #include "uart.h" void gps_configure_uart ( void ) { - //UbxSend_CFG_PRT(0x01, 0x0, 0x0, 0x000008D0, GPS_BAUD, UBX_PROTO_MASK, UBX_PROTO_MASK, 0x0, 0x0); + //UbxSend_CFG_PRT(0x01, 0x0, 0x0, 0x000008D0, GPS_BAUD, UBX_PROTO_MASK, UBX_PROTO_MASK, 0x0, 0x0); //while (GpsUartRunning) ; /* FIXME */ GpsUartInitParam( UART_BAUD(GPS_BAUD), UART_8N1, UART_FIFO_8); } @@ -124,7 +124,7 @@ int nmea_msg_len = 0; int GpsFixValid() { return gps_pos_available; -} +} /** * parse GPGSA-nmea-messages stored in @@ -199,7 +199,7 @@ void parse_nmea_GPRMC() { printf("parse_nmea_GPRMC() - skipping incomplete message\n"); #endif return; - } + } } // get warning @@ -210,7 +210,7 @@ void parse_nmea_GPRMC() { printf("parse_nmea_GPRMC() - skipping incomplete message\n"); #endif return; - } + } } // get lat // ignored @@ -220,7 +220,7 @@ void parse_nmea_GPRMC() { printf("parse_nmea_GPRMC() - skipping incomplete message\n"); #endif return; - } + } } // get North/South // ignored @@ -230,7 +230,7 @@ void parse_nmea_GPRMC() { printf("parse_nmea_GPRMC() - skipping incomplete message\n"); #endif return; - } + } } // get lon // ignored @@ -240,7 +240,7 @@ void parse_nmea_GPRMC() { printf("parse_nmea_GPRMC() - skipping incomplete message\n"); #endif return; - } + } } // get eath/west // ignored @@ -250,7 +250,7 @@ void parse_nmea_GPRMC() { printf("parse_nmea_GPRMC() - skipping incomplete message\n"); #endif return; - } + } } // get speed double speed = strtod(&nmea_msg_buf[i], &endptr); @@ -264,7 +264,7 @@ void parse_nmea_GPRMC() { printf("parse_nmea_GPRMC() - skipping incomplete message\n"); #endif return; - } + } } @@ -284,7 +284,7 @@ void parse_nmea_GPGGA() { if(nmea_msg_buf[i]==',' && nmea_msg_buf[i+1]==',') { #ifdef LINUX printf("parse_nmea_GPGGA() - skipping empty message\n"); -#endif +#endif return; } @@ -294,8 +294,8 @@ void parse_nmea_GPGGA() { if (i >= nmea_msg_len) { #ifdef LINUX printf("parse_nmea_GPGGA() - skipping incomplete message\n"); -#endif - return; +#endif + return; } } @@ -310,24 +310,24 @@ void parse_nmea_GPGGA() { if (i >= nmea_msg_len) { #ifdef LINUX printf("parse_nmea_GPGGA() - skipping incomplete message\n"); -#endif - return; - } +#endif + return; + } } - + // correct latitute for N/S if(nmea_msg_buf[i] == 'S') lat = -lat; while(nmea_msg_buf[i++] != ',') { // next field: longitude if (i >= nmea_msg_len) - return; + return; } gps_lat = lat * 1e7; // convert to fixed-point #ifdef LINUX printf("parse_nmea_GPGGA() - lat=%f gps_lat=%i\n", lat, gps_lat); -#endif - +#endif + // get longitude [ddmm.mmmmm] double lon = strtod(&nmea_msg_buf[i], &endptr); // convert to pure degrees [dd.dddd] format @@ -337,21 +337,21 @@ void parse_nmea_GPGGA() { //GpsInfo.PosLLA.lon.f *= (M_PI/180); while(nmea_msg_buf[i++] != ',') { // next field: E/W indicator if (i >= nmea_msg_len) - return; + return; } - + // correct latitute for E/W if(nmea_msg_buf[i] == 'W') lon = -lon; while(nmea_msg_buf[i++] != ',') { // next field: position fix status if (i >= nmea_msg_len) - return; + return; } - + gps_lon = lon * 1e7; // convert to fixed-point #ifdef LINUX printf("parse_nmea_GPGGA() - lon=%f gps_lon=%i\n", lon, gps_lon); -#endif +#endif latlong_utm_of(RadOfDeg(lat), RadOfDeg(lon), nav_utm_zone0); @@ -367,69 +367,69 @@ void parse_nmea_GPGGA() { gps_pos_available = TRUE; #ifdef LINUX printf("parse_nmea_GPGGA() - gps_pos_available == true\n"); -#endif +#endif } else { gps_pos_available = FALSE; #ifdef LINUX printf("parse_nmea_GPGGA() - gps_pos_available == false\n"); -#endif +#endif } while(nmea_msg_buf[i++] != ',') { // next field: satellites used if (i >= nmea_msg_len) { #ifdef LINUX printf("parse_nmea_GPGGA() - skipping incomplete message\n"); -#endif - return; - } +#endif + return; + } } - + // get number of satellites used in GPS solution gps_numSV = atoi(&nmea_msg_buf[i]); #ifdef LINUX printf("parse_nmea_GPGGA() - gps_numSatlitesUsed=%i\n", gps_numSV); -#endif +#endif while(nmea_msg_buf[i++] != ',') { // next field: HDOP (horizontal dilution of precision) if (i >= nmea_msg_len) { #ifdef LINUX printf("parse_nmea_GPGGA() - skipping incomplete message\n"); -#endif - return; - } +#endif + return; + } } while(nmea_msg_buf[i++] != ',') { // next field: altitude if (i >= nmea_msg_len) { #ifdef LINUX printf("parse_nmea_GPGGA() - skipping incomplete message\n"); -#endif - return; - } +#endif + return; + } } - + // get altitude (in meters) double alt = strtod(&nmea_msg_buf[i], &endptr); gps_alt = alt * 10; #ifdef LINUX printf("parse_nmea_GPGGA() - gps_alt=%i\n", gps_alt); -#endif +#endif while(nmea_msg_buf[i++] != ',') { // next field: altitude units, always 'M' if (i >= nmea_msg_len) - return; + return; } while(nmea_msg_buf[i++] != ',') { // next field: geoid seperation if (i >= nmea_msg_len) - return; + return; } while(nmea_msg_buf[i++] != ',') { // next field: seperation units if (i >= nmea_msg_len) - return; + return; } while(nmea_msg_buf[i++] != ',') { // next field: DGPS age if (i >= nmea_msg_len) - return; + return; } while(nmea_msg_buf[i++] != ',') { // next field: DGPS station ID if (i >= nmea_msg_len) - return; + return; } //while(nmea_msg_buf[i++] != '*'); // next field: checksum } @@ -445,27 +445,27 @@ void parse_gps_msg( void ) { nmea_msg_buf[nmea_msg_len] = 0; #ifdef LINUX printf("parse_gps_msg() - parsing RMC gps-message \"%s\"\n",nmea_msg_buf); -#endif +#endif parse_nmea_GPRMC(); } else if(nmea_msg_len > 7 && !strncmp(nmea_msg_buf + 1 , "$GPGGA", 6)) { nmea_msg_buf[nmea_msg_len] = 0; #ifdef LINUX printf("parse_gps_msg() - parsing GGA gps-message \"%s\"\n",nmea_msg_buf); -#endif +#endif parse_nmea_GPGGA(); } else if(nmea_msg_len > 7 && !strncmp(nmea_msg_buf + 1 , "$GPGSA", 6)) { nmea_msg_buf[nmea_msg_len] = 0; #ifdef LINUX printf("parse_gps_msg() - parsing GSA gps-message \"%s\"\n",nmea_msg_buf); -#endif +#endif parse_nmea_GPGSA(); } else { nmea_msg_buf[nmea_msg_len] = 0; #ifdef LINUX printf("parse_gps_msg() - ignoring unknown gps-message \"%s\" len=%i\n",nmea_msg_buf, nmea_msg_len); -#endif +#endif } // reset message-buffer diff --git a/sw/airborne/gps_ubx.c b/sw/airborne/gps_ubx.c index 8e9310fc5a..0e8efc5ac9 100644 --- a/sw/airborne/gps_ubx.c +++ b/sw/airborne/gps_ubx.c @@ -1,6 +1,6 @@ /* * Paparazzi mcu0 $Id$ - * + * * Copyright (C) 2003 Pascal Brisset, Antoine Drouin * * This file is part of paparazzi. @@ -18,7 +18,7 @@ * You should have received a copy of the GNU General Public License * along with paparazzi; see the file COPYING. If not, write to * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. + * Boston, MA 02111-1307, USA. * */ @@ -27,7 +27,7 @@ */ #include -#include +#include #include #ifdef FMS_PERIODIC_FREQ @@ -35,7 +35,7 @@ #include //for baudrate #include "fms_serial_port.h" -#endif +#endif #include "flight_plan.h" #include "uart.h" @@ -47,7 +47,7 @@ #ifdef GPS_TIMESTAMP #include "sys_time.h" #define MSEC_PER_WEEK (1000*60*60*24*7) -#endif +#endif #define UbxInitCheksum() { send_ck_a = send_ck_b = 0; } #define UpdateChecksum(c) { send_ck_a += c; send_ck_b += send_ck_a; } @@ -69,7 +69,7 @@ } -/** Includes macros generated from ubx.xml */ +/** Includes macros generated from ubx.xml */ #include "ubx_protocol.h" @@ -169,8 +169,8 @@ void gps_configure_uart ( void ) { uint8_t loop=0; while (GpsUartRunning) { //doesn't work unless some printfs are used, so : - if (loop<9) { - printf("."); loop++; + if (loop<9) { + printf("."); loop++; } else { printf("\b"); loop--; } @@ -195,7 +195,7 @@ void gps_configure_uart ( void ) { #ifdef USER_GPS_CONFIGURE #include USER_GPS_CONFIGURE -#else +#else static bool_t user_gps_configure(bool_t cpt) { switch (cpt) { case 0: @@ -222,14 +222,14 @@ static bool_t user_gps_configure(bool_t cpt) { UbxSend_CFG_SBAS(0x00, 0x00, 0x00, 0x00, 0x00); break; case 7: - UbxSend_CFG_RATE(0x00FA, 0x0001, 0x0000); + UbxSend_CFG_RATE(0x00FA, 0x0001, 0x0000); return FALSE; } return TRUE; /* Continue, except for the last case */ } #endif // ! USER_GPS_CONFIGURE -/* GPS configuration. Must be called on ack message reception while +/* GPS configuration. Must be called on ack message reception while gps_status_config < GPS_CONFIG_DONE */ void gps_configure ( void ) { if (ubx_class == UBX_ACK_ID) { @@ -265,7 +265,7 @@ void parse_gps_msg( void ) { gps_hmsl = UBX_NAV_POSLLH_HMSL(ubx_msg_buf); latlong_utm_of(RadOfDeg(gps_lat/1e7), RadOfDeg(gps_lon/1e7), nav_utm_zone0); - + gps_utm_east = latlong_utm_x * 100; gps_utm_north = latlong_utm_y * 100; gps_alt = UBX_NAV_POSLLH_HMSL(ubx_msg_buf) / 10; @@ -276,7 +276,7 @@ void parse_gps_msg( void ) { gps_utm_north = UBX_NAV_POSUTM_NORTH(ubx_msg_buf); uint8_t hem = UBX_NAV_POSUTM_HEM(ubx_msg_buf); if (hem == UTM_HEM_SOUTH) - gps_utm_north -= 1000000000; /* Subtract false northing: -10000km */ + gps_utm_north -= 1000000000; /* Subtract false northing: -10000km */ gps_alt = UBX_NAV_POSUTM_ALT(ubx_msg_buf); gps_utm_zone = UBX_NAV_POSUTM_ZONE(ubx_msg_buf); #endif @@ -286,7 +286,7 @@ void parse_gps_msg( void ) { gps_climb = - UBX_NAV_VELNED_VEL_D(ubx_msg_buf); gps_course = UBX_NAV_VELNED_Heading(ubx_msg_buf) / 10000; gps_itow = UBX_NAV_VELNED_ITOW(ubx_msg_buf); - + gps_pos_available = TRUE; /* The 3 UBX messages are sent in one rafale */ } else if (ubx_id == UBX_NAV_SOL_ID) { #ifdef GPS_TIMESTAMP @@ -307,12 +307,12 @@ void parse_gps_msg( void ) { gps_nb_channels = Min(UBX_NAV_SVINFO_NCH(ubx_msg_buf), GPS_NB_CHANNELS); uint8_t i; for(i = 0; i < gps_nb_channels; i++) { - gps_svinfos[i].svid = UBX_NAV_SVINFO_SVID(ubx_msg_buf, i); - gps_svinfos[i].flags = UBX_NAV_SVINFO_Flags(ubx_msg_buf, i); - gps_svinfos[i].qi = UBX_NAV_SVINFO_QI(ubx_msg_buf, i); - gps_svinfos[i].cno = UBX_NAV_SVINFO_CNO(ubx_msg_buf, i); - gps_svinfos[i].elev = UBX_NAV_SVINFO_Elev(ubx_msg_buf, i); - gps_svinfos[i].azim = UBX_NAV_SVINFO_Azim(ubx_msg_buf, i); + gps_svinfos[i].svid = UBX_NAV_SVINFO_SVID(ubx_msg_buf, i); + gps_svinfos[i].flags = UBX_NAV_SVINFO_Flags(ubx_msg_buf, i); + gps_svinfos[i].qi = UBX_NAV_SVINFO_QI(ubx_msg_buf, i); + gps_svinfos[i].cno = UBX_NAV_SVINFO_CNO(ubx_msg_buf, i); + gps_svinfos[i].elev = UBX_NAV_SVINFO_Elev(ubx_msg_buf, i); + gps_svinfos[i].azim = UBX_NAV_SVINFO_Azim(ubx_msg_buf, i); } } } @@ -351,7 +351,7 @@ void parse_ubx( uint8_t c ) { case GOT_CLASS: ubx_id = c; ubx_status++; - break; + break; case GOT_ID: ubx_len = c; ubx_status++; @@ -385,7 +385,7 @@ void parse_ubx( uint8_t c ) { goto error; } return; - error: + error: restart: ubx_status = UNINIT; return; @@ -417,4 +417,3 @@ uint32_t itow_from_ticks(uint32_t clock_ticks) return itow_now; } #endif - diff --git a/sw/airborne/gps_ubx.h b/sw/airborne/gps_ubx.h index 5f8ce21afe..e75ebfda9f 100644 --- a/sw/airborne/gps_ubx.h +++ b/sw/airborne/gps_ubx.h @@ -1,6 +1,6 @@ /* * Paparazzi autopilot $Id$ - * + * * Copyright (C) 2004-2006 Pascal Brisset, Antoine Drouin * * This file is part of paparazzi. @@ -18,7 +18,7 @@ * You should have received a copy of the GNU General Public License * along with paparazzi; see the file COPYING. If not, write to * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. + * Boston, MA 02111-1307, USA. * */ diff --git a/sw/airborne/gps_xsens.c b/sw/airborne/gps_xsens.c index 9d7ce28301..b08f114268 100644 --- a/sw/airborne/gps_xsens.c +++ b/sw/airborne/gps_xsens.c @@ -1,10 +1,10 @@ /* * $Id$ - * + * * Copyright (C) 2005 Pascal Brisset, Antoine Drouin * * This file is part of paparazzi. - * + * * paparazzi is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 2, or (at your option) @@ -18,7 +18,7 @@ * You should have received a copy of the GNU General Public License * along with paparazzi; see the file COPYING. If not, write to * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. + * Boston, MA 02111-1307, USA. * */ @@ -72,4 +72,3 @@ void gps_init( void ) {} void gps_configure( void ) {} void parse_gps_msg( void ) {} void gps_configure_uart( void ) {} - diff --git a/sw/airborne/gps_xsens.h b/sw/airborne/gps_xsens.h index 95878f56d4..2395793e5a 100644 --- a/sw/airborne/gps_xsens.h +++ b/sw/airborne/gps_xsens.h @@ -1,6 +1,6 @@ /* * Paparazzi autopilot $Id: gps_ubx.h 4659 2010-03-10 16:55:04Z mmm $ - * + * * Copyright (C) 2004-2006 Pascal Brisset, Antoine Drouin * * This file is part of paparazzi. @@ -18,7 +18,7 @@ * You should have received a copy of the GNU General Public License * along with paparazzi; see the file COPYING. If not, write to * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. + * Boston, MA 02111-1307, USA. * */