Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-05 06:54:49 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

[modules] fix gps_ubx_ucenter debug print

Browse Source 
- use DEBUG_PRINT macro (that is only active if DEBUG_GPS_UBX_UCENTER) instead of printf
- add missing default case
This commit is contained in:
Felix Ruess
2014-03-03 19:09:26 +01:00
parent 161c25e5a0
commit 8f575cd053
1 changed files with 40 additions and 55 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/modules/gps/gps_ubx_ucenter.c
+40 -55
View File
@@ -32,6 +32,12 @@
#include "subsystems/datalink/downlink.h" #include "subsystems/datalink/downlink.h"
#include <stdio.h> #include <stdio.h>
#if DEBUG_GPS_UBX_UCENTER
#define DEBUG_PRINT(...) printf(__VA_ARGS__)
#else
#define DEBUG_PRINT(...) {}
#endif
////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////
// //
@@ -97,30 +103,25 @@ void gps_ubx_ucenter_periodic(void)
gps_ubx_ucenter.status = GPS_UBX_UCENTER_STATUS_CONFIG; gps_ubx_ucenter.status = GPS_UBX_UCENTER_STATUS_CONFIG;
gps_ubx_ucenter.cnt = 0; gps_ubx_ucenter.cnt = 0;
#if DEBUG_GPS_UBX_UCENTER #if DEBUG_GPS_UBX_UCENTER
if (gps_ubx_ucenter.baud_init > 0) if (gps_ubx_ucenter.baud_init > 0) {
{ DEBUG_PRINT("Initial ublox baudrate found: %u\n", gps_ubx_ucenter.baud_init);
printf("Initial ublox baudrate found: %u\n", gps_ubx_ucenter.baud_init);
} }
else else {
{ DEBUG_PRINT("WARNING: Unable to determine the ublox baudrate. Autoconfiguration is unlikely to work.\n");
printf("WARNING: Unable to determine the ublox baudrate. Autoconfiguration is unlikely to work.\n");
} }
#endif #endif
} }
else else {
{
gps_ubx_ucenter.cnt++; gps_ubx_ucenter.cnt++;
} }
break; break;
// Send Configuration // Send Configuration
case GPS_UBX_UCENTER_STATUS_CONFIG: case GPS_UBX_UCENTER_STATUS_CONFIG:
if (gps_ubx_ucenter_configure(gps_ubx_ucenter.cnt) == FALSE) if (gps_ubx_ucenter_configure(gps_ubx_ucenter.cnt) == FALSE) {
{
gps_ubx_ucenter.status = GPS_UBX_UCENTER_STATUS_STOPPED; gps_ubx_ucenter.status = GPS_UBX_UCENTER_STATUS_STOPPED;
gps_ubx_ucenter.cnt = 0; gps_ubx_ucenter.cnt = 0;
} }
else else {
{
gps_ubx_ucenter.cnt++; gps_ubx_ucenter.cnt++;
} }
break; break;
@@ -147,15 +148,11 @@ void gps_ubx_ucenter_event(void)
case UBX_ACK_ID: case UBX_ACK_ID:
if (gps_ubx.msg_id == UBX_ACK_ACK_ID) { if (gps_ubx.msg_id == UBX_ACK_ACK_ID) {
gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_ACK; gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_ACK;
#if DEBUG_GPS_UBX_UCENTER DEBUG_PRINT("ACK\n");
printf("ACK\n");
#endif
} }
else { else {
gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_NACK; gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_NACK;
#if DEBUG_GPS_UBX_UCENTER DEBUG_PRINT("NACK\n");
printf("NACK\n");
#endif
} }
break; break;
case UBX_MON_ID: case UBX_MON_ID:
@@ -168,10 +165,9 @@ void gps_ubx_ucenter_event(void)
gps_ubx_ucenter.hw_ver_h += 10*(UBX_MON_VER_c(gps_ubx.msg_buf,32) - '0'); gps_ubx_ucenter.hw_ver_h += 10*(UBX_MON_VER_c(gps_ubx.msg_buf,32) - '0');
gps_ubx_ucenter.hw_ver_l = UBX_MON_VER_c(gps_ubx.msg_buf,37) - '0'; gps_ubx_ucenter.hw_ver_l = UBX_MON_VER_c(gps_ubx.msg_buf,37) - '0';
gps_ubx_ucenter.hw_ver_l += 10*(UBX_MON_VER_c(gps_ubx.msg_buf,36) - '0'); gps_ubx_ucenter.hw_ver_l += 10*(UBX_MON_VER_c(gps_ubx.msg_buf,36) - '0');
#if DEBUG_GPS_UBX_UCENTER
printf("ublox sw_ver: %u.%u\n", gps_ubx_ucenter.sw_ver_h, gps_ubx_ucenter.sw_ver_l); DEBUG_PRINT("ublox sw_ver: %u.%u\n", gps_ubx_ucenter.sw_ver_h, gps_ubx_ucenter.sw_ver_l);
printf("ublox hw_ver: %u.%u\n", gps_ubx_ucenter.hw_ver_h, gps_ubx_ucenter.hw_ver_l); DEBUG_PRINT("ublox hw_ver: %u.%u\n", gps_ubx_ucenter.hw_ver_h, gps_ubx_ucenter.hw_ver_l);
#endif
} }
break; break;
case UBX_CFG_ID: case UBX_CFG_ID:
@@ -179,12 +175,13 @@ void gps_ubx_ucenter_event(void)
gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_CFG_PRT; gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_CFG_PRT;
gps_ubx_ucenter.port_id = UBX_CFG_PRT_PortId(gps_ubx.msg_buf,0); gps_ubx_ucenter.port_id = UBX_CFG_PRT_PortId(gps_ubx.msg_buf,0);
gps_ubx_ucenter.baud_run = UBX_CFG_PRT_Baudrate(gps_ubx.msg_buf,0); gps_ubx_ucenter.baud_run = UBX_CFG_PRT_Baudrate(gps_ubx.msg_buf,0);
#if DEBUG_GPS_UBX_UCENTER
printf("gps_ubx_ucenter.baud_run: %u\n", gps_ubx_ucenter.baud_run); DEBUG_PRINT("gps_ubx_ucenter.baud_run: %u\n", gps_ubx_ucenter.baud_run);
printf("gps_ubx_ucenter.port_id: %u\n", gps_ubx_ucenter.port_id); DEBUG_PRINT("gps_ubx_ucenter.port_id: %u\n", gps_ubx_ucenter.port_id);
#endif
} }
break; break;
default:
break;
} }
} }
@@ -244,8 +241,7 @@ static bool_t gps_ubx_ucenter_autobaud(uint8_t nr)
gps_ubx_ucenter_config_port_poll(); gps_ubx_ucenter_config_port_poll();
break; break;
case 3: case 3:
if (gps_ubx_ucenter.reply == GPS_UBX_UCENTER_REPLY_ACK) if (gps_ubx_ucenter.reply == GPS_UBX_UCENTER_REPLY_ACK) {
{
gps_ubx_ucenter.baud_init = gps_ubx_ucenter.baud_run; gps_ubx_ucenter.baud_init = gps_ubx_ucenter.baud_run;
return FALSE; return FALSE;
} }
@@ -254,8 +250,7 @@ static bool_t gps_ubx_ucenter_autobaud(uint8_t nr)
gps_ubx_ucenter_config_port_poll(); gps_ubx_ucenter_config_port_poll();
break; break;
case 4: case 4:
if (gps_ubx_ucenter.reply == GPS_UBX_UCENTER_REPLY_ACK) if (gps_ubx_ucenter.reply == GPS_UBX_UCENTER_REPLY_ACK) {
{
gps_ubx_ucenter.baud_init = gps_ubx_ucenter.baud_run; gps_ubx_ucenter.baud_init = gps_ubx_ucenter.baud_run;
return FALSE; return FALSE;
} }
@@ -264,8 +259,7 @@ static bool_t gps_ubx_ucenter_autobaud(uint8_t nr)
gps_ubx_ucenter_config_port_poll(); gps_ubx_ucenter_config_port_poll();
break; break;
case 5: case 5:
if (gps_ubx_ucenter.reply == GPS_UBX_UCENTER_REPLY_ACK) if (gps_ubx_ucenter.reply == GPS_UBX_UCENTER_REPLY_ACK) {
{
gps_ubx_ucenter.baud_init = gps_ubx_ucenter.baud_run; gps_ubx_ucenter.baud_init = gps_ubx_ucenter.baud_run;
return FALSE; return FALSE;
} }
@@ -274,8 +268,7 @@ static bool_t gps_ubx_ucenter_autobaud(uint8_t nr)
gps_ubx_ucenter_config_port_poll(); gps_ubx_ucenter_config_port_poll();
break; break;
case 6: case 6:
if (gps_ubx_ucenter.reply == GPS_UBX_UCENTER_REPLY_ACK) if (gps_ubx_ucenter.reply == GPS_UBX_UCENTER_REPLY_ACK) {
{
gps_ubx_ucenter.baud_init = gps_ubx_ucenter.baud_run; gps_ubx_ucenter.baud_init = gps_ubx_ucenter.baud_run;
return FALSE; return FALSE;
} }
@@ -284,8 +277,7 @@ static bool_t gps_ubx_ucenter_autobaud(uint8_t nr)
gps_ubx_ucenter_config_port_poll(); gps_ubx_ucenter_config_port_poll();
break; break;
case 7: case 7:
if (gps_ubx_ucenter.reply == GPS_UBX_UCENTER_REPLY_ACK) if (gps_ubx_ucenter.reply == GPS_UBX_UCENTER_REPLY_ACK) {
{
gps_ubx_ucenter.baud_init = gps_ubx_ucenter.baud_run; gps_ubx_ucenter.baud_init = gps_ubx_ucenter.baud_run;
return FALSE; return FALSE;
} }
@@ -345,12 +337,10 @@ static bool_t gps_ubx_ucenter_autobaud(uint8_t nr)
static inline void gps_ubx_ucenter_config_nav(void) static inline void gps_ubx_ucenter_config_nav(void)
{ {
//New ublox firmware v5 or higher uses CFG_NAV5 message, CFG_NAV is no longer available //New ublox firmware v5 or higher uses CFG_NAV5 message, CFG_NAV is no longer available
if (gps_ubx_ucenter.sw_ver_h < 5) if (gps_ubx_ucenter.sw_ver_h < 5) {
{
UbxSend_CFG_NAV(NAV_DYN_AIRBORNE_2G, 3, 16, 24, 20, 5, 0, 0x3C, 0x3C, 0x14, 0x03E8 ,0x0000, 0x0, 0x17, 0x00FA, 0x00FA, 0x0064, 0x012C, 0x000F, 0x00, 0x00); UbxSend_CFG_NAV(NAV_DYN_AIRBORNE_2G, 3, 16, 24, 20, 5, 0, 0x3C, 0x3C, 0x14, 0x03E8 ,0x0000, 0x0, 0x17, 0x00FA, 0x00FA, 0x0064, 0x012C, 0x000F, 0x00, 0x00);
} }
else else {
{
UbxSend_CFG_NAV5(NAV5_MASK, GPS_UBX_NAV5_DYNAMICS, NAV5_3D_ONLY, IGNORED, IGNORED, NAV5_DEFAULT_MIN_ELEV, RESERVED, UbxSend_CFG_NAV5(NAV5_MASK, GPS_UBX_NAV5_DYNAMICS, NAV5_3D_ONLY, IGNORED, IGNORED, NAV5_DEFAULT_MIN_ELEV, RESERVED,
NAV5_DEFAULT_PDOP_MASK, NAV5_DEFAULT_TDOP_MASK, NAV5_DEFAULT_P_ACC, NAV5_DEFAULT_T_ACC, NAV5_DEFAULT_PDOP_MASK, NAV5_DEFAULT_TDOP_MASK, NAV5_DEFAULT_P_ACC, NAV5_DEFAULT_T_ACC,
NAV5_DEFAULT_STATIC_HOLD_THRES, RESERVED, RESERVED, RESERVED, RESERVED); NAV5_DEFAULT_STATIC_HOLD_THRES, RESERVED, RESERVED, RESERVED, RESERVED);
@@ -397,7 +387,7 @@ static inline void gps_ubx_ucenter_config_port(void)
#ifdef GPS_I2C #ifdef GPS_I2C
UbxSend_CFG_PRT(gps_ubx_ucenter.port_id, 0x0, 0x0, GPS_I2C_SLAVE_ADDR, 0x0, UBX_PROTO_MASK, UBX_PROTO_MASK, 0x0, 0x0); UbxSend_CFG_PRT(gps_ubx_ucenter.port_id, 0x0, 0x0, GPS_I2C_SLAVE_ADDR, 0x0, UBX_PROTO_MASK, UBX_PROTO_MASK, 0x0, 0x0);
#else #else
printf("WARNING: Please include the gps_i2c module.\n"); DEBUG_PRINT("WARNING: Please include the gps_i2c module.\n");
#endif #endif
break; break;
// UART Interface // UART Interface
@@ -410,10 +400,10 @@ static inline void gps_ubx_ucenter_config_port(void)
UbxSend_CFG_PRT(gps_ubx_ucenter.port_id, 0x0, 0x0, 0x0, 0x0, UBX_PROTO_MASK, UBX_PROTO_MASK, 0x0, 0x0); UbxSend_CFG_PRT(gps_ubx_ucenter.port_id, 0x0, 0x0, 0x0, 0x0, UBX_PROTO_MASK, UBX_PROTO_MASK, 0x0, 0x0);
break; break;
case GPS_PORT_SPI: case GPS_PORT_SPI:
printf("WARNING: ublox SPI port is currently not supported.\n"); DEBUG_PRINT("WARNING: ublox SPI port is currently not supported.\n");
break; break;
default: default:
printf("WARNING: Unknown ublox port id: %u\n", gps_ubx_ucenter.port_id); DEBUG_PRINT("WARNING: Unknown ublox port id: %u\n", gps_ubx_ucenter.port_id);
break; break;
} }
} }
@@ -440,9 +430,7 @@ static inline void gps_ubx_ucenter_config_sbas(void)
static bool_t gps_ubx_ucenter_configure(uint8_t nr) static bool_t gps_ubx_ucenter_configure(uint8_t nr)
{ {
#if DEBUG_GPS_UBX_UCENTER DEBUG_PRINT("gps_ubx_ucenter_configure nr: %u\n",nr);
printf("gps_ubx_ucenter_configure nr: %u\n",nr);
#endif
// Store the reply of the last configuration step and reset // Store the reply of the last configuration step and reset
if (nr < GPS_UBX_UCENTER_CONFIG_STEPS) if (nr < GPS_UBX_UCENTER_CONFIG_STEPS)
@@ -455,13 +443,11 @@ static bool_t gps_ubx_ucenter_configure(uint8_t nr)
break; break;
case 1: case 1:
#if DEBUG_GPS_UBX_UCENTER #if DEBUG_GPS_UBX_UCENTER
if (gps_ubx_ucenter.reply != GPS_UBX_UCENTER_REPLY_ACK) if (gps_ubx_ucenter.reply != GPS_UBX_UCENTER_REPLY_ACK) {
{ DEBUG_PRINT("ublox did not acknowledge port configuration.\n");
printf("ublox did not acknowledge port configuration.\n");
} }
else else {
{ DEBUG_PRINT("Changed ublox baudrate to: %u\n", UART_SPEED(UBX_GPS_BAUD));
printf("Changed ublox baudrate to: %u\n", UART_SPEED(UBX_GPS_BAUD));
} }
#endif #endif
// Now the GPS baudrate should have changed // Now the GPS baudrate should have changed
@@ -547,9 +533,8 @@ static bool_t gps_ubx_ucenter_configure(uint8_t nr)
// Debug Downlink the result of all configuration steps: see messages // Debug Downlink the result of all configuration steps: see messages
// To view, enable DEBUG message in your telemetry configuration .xml // To view, enable DEBUG message in your telemetry configuration .xml
DOWNLINK_SEND_DEBUG(DefaultChannel, DefaultDevice,GPS_UBX_UCENTER_CONFIG_STEPS,gps_ubx_ucenter.replies); DOWNLINK_SEND_DEBUG(DefaultChannel, DefaultDevice,GPS_UBX_UCENTER_CONFIG_STEPS,gps_ubx_ucenter.replies);
for (int i = 0; i < GPS_UBX_UCENTER_CONFIG_STEPS; i++) for (int i = 0; i < GPS_UBX_UCENTER_CONFIG_STEPS; i++) {
{ DEBUG_PRINT("%u\n", gps_ubx_ucenter.replies[i]);
printf("%u\n", gps_ubx_ucenter.replies[i]);
} }
#endif #endif
return FALSE; return FALSE;