From 1279352a956e8660f09f8e328b737c8b253badac Mon Sep 17 00:00:00 2001 From: Laurent Lalanne Date: Wed, 22 May 2019 20:18:44 +0200 Subject: [PATCH 01/13] LP-457 Call set_fc_baud_from_settings only from/to DJI == avoid trouble for Ubxautoconfig. --- flight/modules/GPS/DJI.c | 2 ++ flight/modules/GPS/GPS.c | 26 +++++++++++--------------- 2 files changed, 13 insertions(+), 15 deletions(-) diff --git a/flight/modules/GPS/DJI.c b/flight/modules/GPS/DJI.c index a178e1cf2..07e8d85d6 100644 --- a/flight/modules/GPS/DJI.c +++ b/flight/modules/GPS/DJI.c @@ -382,6 +382,8 @@ uint32_t parse_dji_message(struct DJIPacket *dji, GPSPositionSensorData *gpsPosi gpsPosition->HDOP = 99.99f; gpsPosition->PDOP = 99.99f; gpsPosition->VDOP = 99.99f; + // clear out satellite data because DJI doesn't provide it + GPSSatellitesSetDefaults(GPSSatellitesHandle(), 0); djiInitialized = true; } diff --git a/flight/modules/GPS/GPS.c b/flight/modules/GPS/GPS.c index fd7eb9a05..31a174bae 100644 --- a/flight/modules/GPS/GPS.c +++ b/flight/modules/GPS/GPS.c @@ -571,15 +571,14 @@ static void gps_set_fc_baud_from_settings() uint8_t speed; // Retrieve settings + HwSettingsGPSSpeedGet(&speed); + #if defined(PIOS_INCLUDE_GPS_DJI_PARSER) && !defined(PIOS_GPS_MINIMAL) if (gpsSettings.DataProtocol == GPSSETTINGS_DATAPROTOCOL_DJI) { speed = HWSETTINGS_GPSSPEED_115200; - } else { -#endif - HwSettingsGPSSpeedGet(&speed); -#if defined(PIOS_INCLUDE_GPS_DJI_PARSER) && !defined(PIOS_GPS_MINIMAL) -} + } #endif + // set fc baud gps_set_fc_baud_from_arg(speed); } @@ -663,16 +662,19 @@ void AuxMagSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) #if defined(ANY_FULL_GPS_PARSER) void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev) { + GPSSettingsDataProtocolOptions previous_data_protocol = gpsSettings.DataProtocol; ubx_autoconfig_settings_t newconfig; GPSSettingsGet(&gpsSettings); #if defined(PIOS_INCLUDE_GPS_DJI_PARSER) - // each time there is a protocol change, set the baud rate - // so that DJI can be forced to 115200, but changing to another protocol will change the baud rate to the user specified value // note that changes to HwSettings GPS baud rate are detected in the HwSettings callback, - // but changing to/from DJI is effectively a baud rate change because DJI is forced to be 115200 - gps_set_fc_baud_from_settings(); // knows to force 115200 for DJI + // but changing to/from DJI in GPSSettings is effectively a baud rate change because DJI is forced to be 115200 + if (((gpsSettings.DataProtocol == GPSSETTINGS_DATAPROTOCOL_DJI) && (previous_data_protocol != GPSSETTINGS_DATAPROTOCOL_DJI)) || + ((gpsSettings.DataProtocol != GPSSETTINGS_DATAPROTOCOL_DJI) && (previous_data_protocol == GPSSETTINGS_DATAPROTOCOL_DJI))) { + // knows to force 115200 for DJI + gps_set_fc_baud_from_settings(); + } #endif // it's OK that ubx auto config is inited and ready to go, when GPS is disabled or running another protocol @@ -791,12 +793,6 @@ void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev) gps_ubx_autoconfig_set(&newconfig); } #endif -#if defined(PIOS_INCLUDE_GPS_DJI_PARSER) - if (gpsSettings.DataProtocol == GPSSETTINGS_DATAPROTOCOL_DJI) { - // clear out satellite data because DJI doesn't provide it - GPSSatellitesSetDefaults(GPSSatellitesHandle(), 0); - } -#endif } #endif /* defined(ANY_FULL_GPS_PARSER) */ /** From d2ecda6f9b5d1b4f3d017ea8d1599e6e6d5f60dd Mon Sep 17 00:00:00 2001 From: Laurent Lalanne Date: Wed, 22 May 2019 14:05:59 +0200 Subject: [PATCH 02/13] LP-457 Remove array, jumps directly to the expected gps speed after change --- flight/modules/GPS/ubx_autoconfig.c | 50 +++++++++++------------------ 1 file changed, 18 insertions(+), 32 deletions(-) diff --git a/flight/modules/GPS/ubx_autoconfig.c b/flight/modules/GPS/ubx_autoconfig.c index 1f692b6d1..89d6a6105 100644 --- a/flight/modules/GPS/ubx_autoconfig.c +++ b/flight/modules/GPS/ubx_autoconfig.c @@ -169,7 +169,8 @@ static volatile bool current_step_touched = false; // both the pointer and what it points to are volatile. Yuk. static volatile status_t *volatile status = 0; static uint8_t hwsettings_baud; -static uint8_t baud_to_try_index = 255; +static uint8_t baud_to_try = 0; +static uint8_t new_gps_speed = 255; // functions @@ -238,14 +239,10 @@ void gps_ubx_reset_sensor_type() // is this needed? // what happens if two tasks / threads try to do an XyzSet() at the same time? if (__sync_fetch_and_add(&mutex, 1) == 0) { - ubxHwVersion = -1; - baud_to_try_index -= 1; // undo postincrement and start with the one that was most recently successful - ubxSensorType = GPSPOSITIONSENSOR_SENSORTYPE_UNKNOWN; + ubxHwVersion = -1; + baud_to_try = new_gps_speed; + ubxSensorType = GPSPOSITIONSENSOR_SENSORTYPE_UNKNOWN; GPSPositionSensorSensorTypeSet(&ubxSensorType); - // make the sensor type / autobaud code time out immediately to send the request immediately - if (status) { - status->lastStepTimestampRaw += 0x8000000UL; - } } --mutex; } @@ -283,6 +280,9 @@ static void config_gps_baud(uint16_t *bytes_to_send) // Ask GPS to change it's speed status->working_packet.message.payload.cfg_prt.baudRate = hwsettings_gpsspeed_enum_to_baud(hwsettings_baud); *bytes_to_send = prepare_packet((UBXSentPacket_t *)&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_PRT, sizeof(ubx_cfg_prt_t)); + + // GPS will be found later at this new_gps_speed + new_gps_speed = hwsettings_baud; } @@ -569,34 +569,20 @@ void gps_ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send) // if AutoBaud or higher, do AutoBaud if (status->currentSettings.UbxAutoConfig >= GPSSETTINGS_UBXAUTOCONFIG_AUTOBAUD) { - uint8_t baud_to_try; - static uint8_t baud_array[] = { - HWSETTINGS_GPSSPEED_57600, - HWSETTINGS_GPSSPEED_9600, - HWSETTINGS_GPSSPEED_115200, - HWSETTINGS_GPSSPEED_38400, - HWSETTINGS_GPSSPEED_19200, - HWSETTINGS_GPSSPEED_230400, - HWSETTINGS_GPSSPEED_4800, - HWSETTINGS_GPSSPEED_2400 - }; - // first try HwSettings.GPSSpeed and then // get the next baud rate to try from the table, but skip over the value of HwSettings.GPSSpeed - do { - // index is inited to be out of bounds, which is interpreted as "currently defined baud rate" (= HwSettings.GPSSpeed) - if (baud_to_try_index >= sizeof(baud_array) / sizeof(baud_array[0])) { - HwSettingsGPSSpeedGet(&hwsettings_baud); - baud_to_try = hwsettings_baud; - baud_to_try_index = 0; - break; - } else { - baud_to_try = baud_array[baud_to_try_index++]; - } - // skip HwSettings.GPSSpeed when you run across it in the list - } while (baud_to_try == hwsettings_baud); + // new_gps_speed is not initialized yet + if (new_gps_speed > HWSETTINGS_GPSSPEED_230400) { + // try current speed set in HwSettings.GPSSpeed + HwSettingsGPSSpeedGet(&hwsettings_baud); + baud_to_try = hwsettings_baud; + new_gps_speed = baud_to_try; + } // set the FC (Revo) baud rate gps_set_fc_baud_from_arg(baud_to_try); + + // increase baud_to_try for next try + baud_to_try = (baud_to_try < HWSETTINGS_GPSSPEED_230400) ? baud_to_try + 1 : HWSETTINGS_GPSSPEED_2400; } // this code is executed even if ubxautoconfig is disabled From 7268af2d8c981ab165c411a1f56db74d43a99314 Mon Sep 17 00:00:00 2001 From: Laurent Lalanne Date: Thu, 23 May 2019 00:45:56 +0200 Subject: [PATCH 03/13] LP-457 Debug mode --- flight/modules/GPS/GPS.c | 13 +++++++++++++ flight/modules/GPS/UBX.c | 4 ++++ flight/modules/GPS/ubx_autoconfig.c | 10 ++++++++++ .../boards/revolution/firmware/inc/pios_config.h | 4 ++-- 4 files changed, 29 insertions(+), 2 deletions(-) diff --git a/flight/modules/GPS/GPS.c b/flight/modules/GPS/GPS.c index 31a174bae..8c2276523 100644 --- a/flight/modules/GPS/GPS.c +++ b/flight/modules/GPS/GPS.c @@ -413,6 +413,16 @@ static void gpsTask(__attribute__((unused)) void *parameters) GPSPositionSensorStatusOptions status = GPSPOSITIONSENSOR_STATUS_NOGPS; GPSPositionSensorStatusSet(&status); AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_ERROR); + + if (res == PARSER_ERROR) { + DEBUG_PRINTF(3, "NoGPS-ParserError\r"); + } + if ((timeNowMs - timeOfLastUpdateMs) >= GPS_TIMEOUT_MS) { + DEBUG_PRINTF(3, "NoGPS-TimeOUT:%d\r", (timeNowMs - timeOfLastUpdateMs)); + } + if (gpsSettings.DataProtocol == GPSSETTINGS_DATAPROTOCOL_UBX && gpspositionsensor.AutoConfigStatus == GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_ERROR) { + DEBUG_PRINTF(3, "NoGPS-AutoCfgError\r"); + } } // if we parsed at least one packet successfully // we aren't offline, but we still may not have a good enough fix to fly @@ -554,10 +564,13 @@ void gps_set_fc_baud_from_arg(uint8_t baud) // don't bother doing the baud change if it is actually the same // might drop less data if (previous_baud != baud) { + DEBUG_PRINTF(3, "SetBaud:%d - %d", baud, hwsettings_gpsspeed_enum_to_baud(baud)); previous_baud = baud; // Set Revo port hwsettings_baud PIOS_COM_ChangeBaud(PIOS_COM_GPS, hwsettings_gpsspeed_enum_to_baud(baud)); GPSPositionSensorBaudRateSet(&baud); + } else { + DEBUG_PRINTF(3, "SetBaud:%d - NoChange\r", baud); } } --mutex; diff --git a/flight/modules/GPS/UBX.c b/flight/modules/GPS/UBX.c index e8a79a3b5..bbbd95733 100644 --- a/flight/modules/GPS/UBX.c +++ b/flight/modules/GPS/UBX.c @@ -197,6 +197,7 @@ int parse_ubx_stream(uint8_t *rx, uint16_t len, char *gps_rx_buffer, GPSPosition restart_state = RESTART_NO_ERROR; } else { restart_state = RESTART_WITH_ERROR; + DEBUG_PRINTF(3, "UbxPayloadError: 0x%02x:0x%02x\r", ubx->header.class, ubx->header.id); } #endif // We won't see the end of the packet. Which means it is useless to do any further processing. @@ -248,6 +249,7 @@ int parse_ubx_stream(uint8_t *rx, uint16_t len, char *gps_rx_buffer, GPSPosition } else { gpsRxStats->gpsRxChkSumError++; restart_state = RESTART_WITH_ERROR; + DEBUG_PRINTF(3, "UbxCheckError\r"); break; } continue; @@ -547,6 +549,7 @@ static void parse_ubx_mon_ver(struct UBXPacket *ubx, __attribute__((unused)) GPS // send sensor type right now because on UBX NEMA we don't get a full set of messages // and we want to be able to see sensor type even on UBX NEMA GPS's GPSPositionSensorSensorTypeSet((uint8_t *)&ubxSensorType); + DEBUG_PRINTF(3, "ParseMonVer\r"); } static void parse_ubx_op_sys(struct UBXPacket *ubx, __attribute__((unused)) GPSPositionSensorData *GpsPosition) @@ -618,6 +621,7 @@ uint32_t parse_ubx_message(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosi // see OP GPSV9 comment in parse_ubx_stream() for further information status = GPSPOSITIONSENSOR_STATUS_NOFIX; GPSPositionSensorStatusSet(&status); + DEBUG_PRINTF(3, "Detected UBX\r"); } } return id; diff --git a/flight/modules/GPS/ubx_autoconfig.c b/flight/modules/GPS/ubx_autoconfig.c index 89d6a6105..4e739f126 100644 --- a/flight/modules/GPS/ubx_autoconfig.c +++ b/flight/modules/GPS/ubx_autoconfig.c @@ -263,6 +263,7 @@ static void config_reset(uint16_t *bytes_to_send) status->working_packet.message.payload.cfg_cfg.deviceMask = UBX_CFG_CFG_DEVICE_ALL; *bytes_to_send = prepare_packet((UBXSentPacket_t *)&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_CFG, sizeof(ubx_cfg_cfg_t)); + DEBUG_PRINTF(3, "CfgReset\r"); } @@ -283,6 +284,7 @@ static void config_gps_baud(uint16_t *bytes_to_send) // GPS will be found later at this new_gps_speed new_gps_speed = hwsettings_baud; + DEBUG_PRINTF(3, "CfgBaud: %d\r", new_gps_speed); } @@ -304,6 +306,7 @@ static void config_rate(uint16_t *bytes_to_send) status->working_packet.message.payload.cfg_rate.timeRef = 1; // 0 = UTC Time, 1 = GPS Time *bytes_to_send = prepare_packet((UBXSentPacket_t *)&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_RATE, sizeof(ubx_cfg_rate_t)); + DEBUG_PRINTF(3, "CfgRate: %dHz\r", rate); } @@ -316,6 +319,7 @@ static void config_nav(uint16_t *bytes_to_send) status->working_packet.message.payload.cfg_nav5.mask = UBX_CFG_NAV5_DYNMODEL + UBX_CFG_NAV5_FIXMODE; *bytes_to_send = prepare_packet((UBXSentPacket_t *)&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_NAV5, sizeof(ubx_cfg_nav5_t)); + DEBUG_PRINTF(3, "CfgNav\r"); } static void config_navx(uint16_t *bytes_to_send) @@ -325,6 +329,7 @@ static void config_navx(uint16_t *bytes_to_send) status->working_packet.message.payload.cfg_navx5.mask1 = UBX_CFG_NAVX5_AOP; *bytes_to_send = prepare_packet((UBXSentPacket_t *)&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_NAVX5, sizeof(ubx_cfg_navx5_t)); + DEBUG_PRINTF(3, "CfgNavX\r"); } @@ -350,6 +355,7 @@ static void config_sbas(uint16_t *bytes_to_send) UBX_CFG_SBAS_SCANMODE2; *bytes_to_send = prepare_packet((UBXSentPacket_t *)&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_SBAS, sizeof(ubx_cfg_sbas_t)); + DEBUG_PRINTF(3, "CfgSBAS\n"); } @@ -411,6 +417,7 @@ static void config_gnss(uint16_t *bytes_to_send) } *bytes_to_send = prepare_packet((UBXSentPacket_t *)&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_GNSS, sizeof(ubx_cfg_gnss_t)); + DEBUG_PRINTF(3, "CfgGNSS\r"); } @@ -424,6 +431,7 @@ static void config_save(uint16_t *bytes_to_send) status->working_packet.message.payload.cfg_cfg.deviceMask = UBX_CFG_CFG_DEVICE_ALL; *bytes_to_send = prepare_packet((UBXSentPacket_t *)&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_CFG, sizeof(ubx_cfg_cfg_t)); + DEBUG_PRINTF(3, "CfgSavenew: %d\r", new_gps_speed); } static void configure(uint16_t *bytes_to_send) @@ -583,6 +591,7 @@ void gps_ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send) // increase baud_to_try for next try baud_to_try = (baud_to_try < HWSETTINGS_GPSSPEED_230400) ? baud_to_try + 1 : HWSETTINGS_GPSSPEED_2400; + DEBUG_PRINTF(3, "IncreaseBaudToTry\r"); } // this code is executed even if ubxautoconfig is disabled @@ -619,6 +628,7 @@ void gps_ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send) // keep timeouts running properly, we (will have) just sent a packet that generates a reply set_current_step_if_untouched(INIT_STEP_WAIT_MON_VER_ACK); status->lastStepTimestampRaw = PIOS_DELAY_GetRaw(); + DEBUG_PRINTF(3, "Mon_verRefresh\r"); break; case INIT_STEP_WAIT_MON_VER_ACK: diff --git a/flight/targets/boards/revolution/firmware/inc/pios_config.h b/flight/targets/boards/revolution/firmware/inc/pios_config.h index 6694ff33d..f069c8fd2 100644 --- a/flight/targets/boards/revolution/firmware/inc/pios_config.h +++ b/flight/targets/boards/revolution/firmware/inc/pios_config.h @@ -37,8 +37,8 @@ * details. */ -/* #define PIOS_INCLUDE_DEBUG_CONSOLE */ -/* #define DEBUG_LEVEL 0 */ +#define PIOS_INCLUDE_DEBUG_CONSOLE +#define DEBUG_LEVEL 3 /* #define PIOS_ENABLE_DEBUG_PINS */ /* PIOS FreeRTOS support */ From e4f8352cee86e9a0097c62c1a19e53f0158eea82 Mon Sep 17 00:00:00 2001 From: Laurent Lalanne Date: Sat, 25 May 2019 19:00:05 +0200 Subject: [PATCH 04/13] LP-457 Enable sentences, even for low baudrates where rate is forced to 1Hz. Increase retries for 2400bds --- flight/modules/GPS/inc/ubx_autoconfig.h | 2 +- flight/modules/GPS/ubx_autoconfig.c | 63 ++++++++++++------------- 2 files changed, 30 insertions(+), 35 deletions(-) diff --git a/flight/modules/GPS/inc/ubx_autoconfig.h b/flight/modules/GPS/inc/ubx_autoconfig.h index 44045840c..0ea7ebbca 100644 --- a/flight/modules/GPS/inc/ubx_autoconfig.h +++ b/flight/modules/GPS/inc/ubx_autoconfig.h @@ -57,7 +57,7 @@ // timeout for a settings save, in case it has to erase flash? #define UBX_SAVE_WAIT_TIME (1000 * 1000) // max retries in case of timeout -#define UBX_MAX_RETRIES 5 +#define UBX_MAX_RETRIES 10 // max time for ubx parser to respond to MON_VER #define UBX_PARSER_TIMEOUT (950 * 1000) // pause between each unverifiable (no ack/nak) configuration step diff --git a/flight/modules/GPS/ubx_autoconfig.c b/flight/modules/GPS/ubx_autoconfig.c index 4e739f126..0a746fc2c 100644 --- a/flight/modules/GPS/ubx_autoconfig.c +++ b/flight/modules/GPS/ubx_autoconfig.c @@ -288,11 +288,12 @@ static void config_gps_baud(uint16_t *bytes_to_send) } -static void config_rate(uint16_t *bytes_to_send) +static void config_rate(uint16_t *bytes_to_send, bool min_rate) { memset((uint8_t *)status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_rate_t)); // if rate is less than 1 uses the highest rate for current hardware - uint16_t rate = status->currentSettings.navRate > 0 ? status->currentSettings.navRate : 99; + // force rate to 1Hz if min_rate = true + uint16_t rate = (min_rate) ? 1 : (status->currentSettings.navRate > 0) ? status->currentSettings.navRate : 99; if (ubxHwVersion < UBX_HW_VERSION_7 && rate > UBX_MAX_RATE) { rate = UBX_MAX_RATE; } else if (ubxHwVersion < UBX_HW_VERSION_8 && rate > UBX_MAX_RATE_VER7) { @@ -436,10 +437,13 @@ static void config_save(uint16_t *bytes_to_send) static void configure(uint16_t *bytes_to_send) { + // for low baudrates, force rate to 1Hz + // and apply UbxRate from GPS settings for higher baudrates + bool min_rate = (new_gps_speed <= HWSETTINGS_GPSSPEED_9600) ? true : false; + switch (status->lastConfigSent) { case LAST_CONFIG_SENT_START: - // increase message rates to 5 fixes per second - config_rate(bytes_to_send); + config_rate(bytes_to_send, min_rate); break; case LAST_CONFIG_SENT_START + 1: @@ -534,8 +538,6 @@ static void setGpsSettings() #endif /* if defined(AUTOBAUD_CONFIGURE_STORE_AND_DISABLE) */ -// 9600 baud and lower are not usable, and are best left at factory default -// if the user selects 9600 void gps_ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send) { *bytes_to_send = 0; @@ -560,7 +562,6 @@ void gps_ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send) // implying a 1 second send buffer and that it could be over 1 second before a reply is received // later uBlox versions dropped this 1 second constraint and drop data when the send buffer is full // and that could be even longer than 1 second - // send this more quickly and it will get a reply more quickly if a fixed percentage of replies are being dropped // wait for the normal reply timeout before sending it over and over if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) < UBX_PARSER_TIMEOUT) { @@ -682,8 +683,6 @@ void gps_ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send) // GPS was just reset, so GPS is running 9600 baud, and Revo is running whatever baud it was before case INIT_STEP_REVO_9600_BAUD: #if !defined(ALWAYS_RESET) - // if user requests a low baud rate then we just reset and leave it set to NMEA - // because low baud and high OP data rate doesn't play nice // if user requests that settings be saved, we will reset here too // that makes sure that all strange settings are reset to factory default // else these strange settings may persist because we don't reset all settings by hand @@ -744,39 +743,35 @@ void gps_ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send) status->lastConfigSent = LAST_CONFIG_SENT_START; // zero the retries for the first "enable sentence" status->retryCount = 0; - // skip enabling UBX sentences for low baud rates - // low baud rates are not usable, and higher data rates just makes it harder for this code to change the configuration - if (hwsettings_baud <= HWSETTINGS_GPSSPEED_9600) { - set_current_step_if_untouched(INIT_STEP_SAVE); - } else { - set_current_step_if_untouched(INIT_STEP_ENABLE_SENTENCES); - } + // enable UBX sentences + set_current_step_if_untouched(INIT_STEP_ENABLE_SENTENCES); // allow it enter the next state immmediately by not setting status->lastStepTimestampRaw = PIOS_DELAY_GetRaw(); break; case INIT_STEP_ENABLE_SENTENCES: + { + enable_sentences(bytes_to_send); + if (status->lastConfigSent == LAST_CONFIG_SENT_COMPLETED) { + // finished enabling sentences, now configure() needs to start at the beginning + status->lastConfigSent = LAST_CONFIG_SENT_START; + // zero the retries for the first "configure" + status->retryCount = 0; + set_current_step_if_untouched(INIT_STEP_CONFIGURE); + } else { + set_current_step_if_untouched(INIT_STEP_ENABLE_SENTENCES_WAIT_ACK); + status->lastStepTimestampRaw = PIOS_DELAY_GetRaw(); + } + break; + } + case INIT_STEP_CONFIGURE: { - bool step_configure = (status->currentStep == INIT_STEP_CONFIGURE); - if (step_configure) { - configure(bytes_to_send); - } else { - enable_sentences(bytes_to_send); - } - - // for some branches, allow it enter the next state immmediately by not setting status->lastStepTimestampRaw = PIOS_DELAY_GetRaw(); + configure(bytes_to_send); if (status->lastConfigSent == LAST_CONFIG_SENT_COMPLETED) { - if (step_configure) { - // zero retries for the next state that needs it (INIT_STEP_SAVE) - status->retryCount = 0; - set_current_step_if_untouched(INIT_STEP_SAVE); - } else { - // finished enabling sentences, now configure() needs to start at the beginning - status->lastConfigSent = LAST_CONFIG_SENT_START; - set_current_step_if_untouched(INIT_STEP_CONFIGURE); - } + status->retryCount = 0; + set_current_step_if_untouched(INIT_STEP_SAVE); } else { - set_current_step_if_untouched(step_configure ? INIT_STEP_CONFIGURE_WAIT_ACK : INIT_STEP_ENABLE_SENTENCES_WAIT_ACK); + set_current_step_if_untouched(INIT_STEP_CONFIGURE_WAIT_ACK); status->lastStepTimestampRaw = PIOS_DELAY_GetRaw(); } break; From 748dacf32ba1b1718e5fc9a838663a013117ae38 Mon Sep 17 00:00:00 2001 From: Laurent Lalanne Date: Sat, 25 May 2019 20:44:22 +0200 Subject: [PATCH 05/13] LP-457 No need to enable ubx_nav_status since it is not parsed --- flight/modules/GPS/ubx_autoconfig.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flight/modules/GPS/ubx_autoconfig.c b/flight/modules/GPS/ubx_autoconfig.c index 0a746fc2c..49f59444f 100644 --- a/flight/modules/GPS/ubx_autoconfig.c +++ b/flight/modules/GPS/ubx_autoconfig.c @@ -93,6 +93,7 @@ ubx_cfg_msg_t msg_config_ubx6[] = { { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_SBAS, .rate = 0 }, { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_TIMEGPS, .rate = 0 }, { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_VELECEF, .rate = 0 }, + { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_STATUS, .rate = 0 }, { .msgClass = UBX_CLASS_MON, .msgID = UBX_ID_MON_HW, .rate = 0 }, { .msgClass = UBX_CLASS_MON, .msgID = UBX_ID_MON_HW2, .rate = 0 }, @@ -108,7 +109,6 @@ ubx_cfg_msg_t msg_config_ubx6[] = { { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_POSLLH, .rate = 1 }, { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_DOP, .rate = 1 }, { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_SOL, .rate = 1 }, - { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_STATUS, .rate = 1 }, { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_VELNED, .rate = 1 }, { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_TIMEUTC, .rate = 1 }, { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_SVINFO, .rate = 10 }, From 9f723dd2ba2d295bad841cf5f24421387ef84325 Mon Sep 17 00:00:00 2001 From: Laurent Lalanne Date: Sat, 25 May 2019 21:49:48 +0200 Subject: [PATCH 06/13] LP-457 Increase timeout so GPS at 2400bds can be "used" --- flight/modules/GPS/GPS.c | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/flight/modules/GPS/GPS.c b/flight/modules/GPS/GPS.c index 8c2276523..8b880f69e 100644 --- a/flight/modules/GPS/GPS.c +++ b/flight/modules/GPS/GPS.c @@ -97,8 +97,9 @@ void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev); // **************** // Private constants -// GPS timeout is greater than 1000ms so that a stock GPS configuration can be used without timeout errors -#define GPS_TIMEOUT_MS 1250 +// GPS timeout is greater than 1000ms so that a stock GPS configuration +// or 2400bds can be used without timeout errors +#define GPS_TIMEOUT_MS 1800 // delay from detecting HomeLocation.Set == False before setting new homelocation // this prevent that a save with homelocation.Set = false triggered by gps ends saving From 1aced28613e711321e17f6addd7666098f4fcbc9 Mon Sep 17 00:00:00 2001 From: Laurent Lalanne Date: Sun, 26 May 2019 03:01:18 +0200 Subject: [PATCH 07/13] LP-457 Disable svinfo with low baudrates, reset GPSSat uavo --- flight/modules/GPS/ubx_autoconfig.c | 17 ++++++++++++++--- 1 file changed, 14 insertions(+), 3 deletions(-) diff --git a/flight/modules/GPS/ubx_autoconfig.c b/flight/modules/GPS/ubx_autoconfig.c index 49f59444f..6a1f3eaf8 100644 --- a/flight/modules/GPS/ubx_autoconfig.c +++ b/flight/modules/GPS/ubx_autoconfig.c @@ -145,6 +145,10 @@ ubx_cfg_msg_t msg_config_ubx7[] = { { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_SVINFO, .rate = 10 }, }; +ubx_cfg_msg_t msg_config_ubx_disable_svinfo[] = { + { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_SVINFO, .rate = 0 }, +}; + // private defines #define LAST_CONFIG_SENT_START (-1) @@ -480,7 +484,7 @@ static void configure(uint16_t *bytes_to_send) } -static void enable_sentences(__attribute__((unused)) uint16_t *bytes_to_send) +static void enable_sentences(__attribute__((unused)) uint16_t *bytes_to_send, bool disable_svinfo) { int8_t msg = status->lastConfigSent + 1; uint8_t msg_count = (ubxHwVersion >= UBX_HW_VERSION_7) ? @@ -489,11 +493,16 @@ static void enable_sentences(__attribute__((unused)) uint16_t *bytes_to_send) &msg_config_ubx7[0] : &msg_config_ubx6[0]; if (msg >= 0 && msg < msg_count) { - status->working_packet.message.payload.cfg_msg = msg_config[msg]; + status->working_packet.message.payload.cfg_msg = (msg == (msg_count - 1) && disable_svinfo) ? msg_config_ubx_disable_svinfo[0] : msg_config[msg]; *bytes_to_send = prepare_packet((UBXSentPacket_t *)&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_MSG, sizeof(ubx_cfg_msg_t)); } else { status->lastConfigSent = LAST_CONFIG_SENT_COMPLETED; } + + // Reset GPSSatellites UAVO + if (disable_svinfo && (status->lastConfigSent == LAST_CONFIG_SENT_COMPLETED) && (GPSSatellitesHandle() != NULL)) { + GPSSatellitesSetDefaults(GPSSatellitesHandle(), 0); + } } @@ -750,7 +759,9 @@ void gps_ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send) case INIT_STEP_ENABLE_SENTENCES: { - enable_sentences(bytes_to_send); + // for low baudrates, disable SVINFO + bool disable_svinfo = (new_gps_speed <= HWSETTINGS_GPSSPEED_9600) ? true : false; + enable_sentences(bytes_to_send, disable_svinfo); if (status->lastConfigSent == LAST_CONFIG_SENT_COMPLETED) { // finished enabling sentences, now configure() needs to start at the beginning status->lastConfigSent = LAST_CONFIG_SENT_START; From a67a374eaf13e416532a35c96abec05435155082 Mon Sep 17 00:00:00 2001 From: Laurent Lalanne Date: Sun, 26 May 2019 05:43:10 +0200 Subject: [PATCH 08/13] LP-457 Apply minimal rate before gps baud change - Add extra wait_time for mon_ver parsing --- flight/modules/GPS/ubx_autoconfig.c | 51 +++++++++++++++++++++++------ 1 file changed, 41 insertions(+), 10 deletions(-) diff --git a/flight/modules/GPS/ubx_autoconfig.c b/flight/modules/GPS/ubx_autoconfig.c index 6a1f3eaf8..5e0fc5763 100644 --- a/flight/modules/GPS/ubx_autoconfig.c +++ b/flight/modules/GPS/ubx_autoconfig.c @@ -44,6 +44,8 @@ typedef enum { INIT_STEP_WAIT_MON_VER_ACK, INIT_STEP_RESET_GPS, INIT_STEP_REVO_9600_BAUD, + INIT_STEP_SET_LOWRATE, + INIT_STEP_SET_LOWRATE_WAIT_ACK, INIT_STEP_GPS_BAUD, INIT_STEP_REVO_BAUD, INIT_STEP_ENABLE_SENTENCES, @@ -312,6 +314,9 @@ static void config_rate(uint16_t *bytes_to_send, bool min_rate) *bytes_to_send = prepare_packet((UBXSentPacket_t *)&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_RATE, sizeof(ubx_cfg_rate_t)); DEBUG_PRINTF(3, "CfgRate: %dHz\r", rate); + if (min_rate) { + status->lastConfigSent = LAST_CONFIG_SENT_COMPLETED; + } } @@ -441,14 +446,16 @@ static void config_save(uint16_t *bytes_to_send) static void configure(uint16_t *bytes_to_send) { - // for low baudrates, force rate to 1Hz - // and apply UbxRate from GPS settings for higher baudrates + // for low baudrates, keep rate to 1Hz and fall to config_nav + // apply UbxRate from GPS settings for higher baudrates bool min_rate = (new_gps_speed <= HWSETTINGS_GPSSPEED_9600) ? true : false; switch (status->lastConfigSent) { case LAST_CONFIG_SENT_START: - config_rate(bytes_to_send, min_rate); - break; + if (!min_rate) { + config_rate(bytes_to_send, min_rate); + break; + } case LAST_CONFIG_SENT_START + 1: config_nav(bytes_to_send); @@ -572,8 +579,14 @@ void gps_ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send) // later uBlox versions dropped this 1 second constraint and drop data when the send buffer is full // and that could be even longer than 1 second + uint32_t time_to_wait = UBX_PARSER_TIMEOUT; + // Add extra time to detect ubxHwVersion in case detection + // becomes more difficult due to a lower transmission rate + if (baud_to_try < HWSETTINGS_GPSSPEED_9600) { + time_to_wait += UBX_PARSER_TIMEOUT; + } // wait for the normal reply timeout before sending it over and over - if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) < UBX_PARSER_TIMEOUT) { + if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) < time_to_wait) { return; } @@ -715,12 +728,29 @@ void gps_ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send) // at most, we just set Revo baud and that doesn't send any data // fall through to next state // we can do that if we choose because we haven't sent any data in this state - // set_current_step_if_untouched(INIT_STEP_GPS_BAUD); + // set_current_step_if_untouched(INIT_STEP_SET_LOWRATE); // allow it enter the next state immmediately by not setting status->lastStepTimestampRaw = PIOS_DELAY_GetRaw(); // break; + case INIT_STEP_SET_LOWRATE: + // Here we set minimal baudrate *before* changing gps baudrate + config_rate(bytes_to_send, true); + if (status->lastConfigSent == LAST_CONFIG_SENT_COMPLETED) { + status->lastConfigSent = LAST_CONFIG_SENT_START; + status->retryCount = 0; + set_current_step_if_untouched(INIT_STEP_GPS_BAUD); + } else { + set_current_step_if_untouched(INIT_STEP_SET_LOWRATE_WAIT_ACK); + status->lastStepTimestampRaw = PIOS_DELAY_GetRaw(); + } + break; + // Revo and GPS are both at 9600 baud case INIT_STEP_GPS_BAUD: + // wait for previous step + if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) < UBX_UNVERIFIED_STEP_WAIT_TIME) { + return; + } // https://www.u-blox.com/images/downloads/Product_Docs/u-bloxM8_ReceiverDescriptionProtocolSpec_%28UBX-13003221%29_Public.pdf // It is possible to change the current communications port settings using a UBX-CFG-CFG message. This could // affect baud rate and other transmission parameters. Because there may be messages queued for transmission @@ -749,9 +779,6 @@ void gps_ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send) } // set the Revo GPS port baud rate to the (same) user configured value gps_set_fc_baud_from_arg(hwsettings_baud); - status->lastConfigSent = LAST_CONFIG_SENT_START; - // zero the retries for the first "enable sentence" - status->retryCount = 0; // enable UBX sentences set_current_step_if_untouched(INIT_STEP_ENABLE_SENTENCES); // allow it enter the next state immmediately by not setting status->lastStepTimestampRaw = PIOS_DELAY_GetRaw(); @@ -788,10 +815,12 @@ void gps_ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send) break; } + case INIT_STEP_SET_LOWRATE_WAIT_ACK: case INIT_STEP_ENABLE_SENTENCES_WAIT_ACK: case INIT_STEP_CONFIGURE_WAIT_ACK: // Wait for an ack from GPS { - bool step_configure = (status->currentStep == INIT_STEP_CONFIGURE_WAIT_ACK); + bool step_setlowrate = (status->currentStep == INIT_STEP_SET_LOWRATE_WAIT_ACK); + bool step_configure = (status->currentStep == INIT_STEP_CONFIGURE_WAIT_ACK); if (ubxLastAck.clsID == status->requiredAck.clsID && ubxLastAck.msgID == status->requiredAck.msgID) { // Continue with next configuration option // start retries over for the next setting to be sent @@ -813,6 +842,8 @@ void gps_ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send) // success or failure here, retries are handled elsewhere if (step_configure) { set_current_step_if_untouched(INIT_STEP_CONFIGURE); + } else if (step_setlowrate) { + set_current_step_if_untouched(INIT_STEP_SET_LOWRATE); } else { set_current_step_if_untouched(INIT_STEP_ENABLE_SENTENCES); } From 22d624f40b2f593b39689cbd0e581b20efbd5383 Mon Sep 17 00:00:00 2001 From: Laurent Lalanne Date: Sun, 26 May 2019 11:01:09 +0200 Subject: [PATCH 09/13] LP-457 Add PIOS_COM_ClearRxBuffer function --- flight/pios/common/pios_com.c | 22 ++++++++++++++++++++++ flight/pios/inc/pios_com.h | 1 + 2 files changed, 23 insertions(+) diff --git a/flight/pios/common/pios_com.c b/flight/pios/common/pios_com.c index 6bef10454..c1a05f359 100644 --- a/flight/pios/common/pios_com.c +++ b/flight/pios/common/pios_com.c @@ -296,6 +296,28 @@ int32_t PIOS_COM_ChangeBaud(uint32_t com_id, uint32_t baud) return 0; } + +/** + * Clear Rx buffer + * \param[in] port COM port + * \return -1 if port not available + * \return 0 on success + */ +int32_t PIOS_COM_ClearRxBuffer(uint32_t com_id) +{ + struct pios_com_dev *com_dev = (struct pios_com_dev *)com_id; + + if (!PIOS_COM_validate(com_dev)) { + /* Undefined COM port for this board (see pios_board.c) */ + return -1; + } + + fifoBuf_clearData(&com_dev->rx); + + return 0; +} + + int32_t PIOS_COM_ChangeConfig(uint32_t com_id, enum PIOS_COM_Word_Length word_len, enum PIOS_COM_Parity parity, enum PIOS_COM_StopBits stop_bits, uint32_t baud_rate) { struct pios_com_dev *com_dev = (struct pios_com_dev *)com_id; diff --git a/flight/pios/inc/pios_com.h b/flight/pios/inc/pios_com.h index 3e33775da..439df28c3 100644 --- a/flight/pios/inc/pios_com.h +++ b/flight/pios/inc/pios_com.h @@ -84,6 +84,7 @@ struct pios_com_driver { /* Public Functions */ extern int32_t PIOS_COM_Init(uint32_t *com_id, const struct pios_com_driver *driver, uint32_t lower_id, uint8_t *rx_buffer, uint16_t rx_buffer_len, uint8_t *tx_buffer, uint16_t tx_buffer_len); extern int32_t PIOS_COM_ChangeBaud(uint32_t com_id, uint32_t baud); +extern int32_t PIOS_COM_ClearRxBuffer(uint32_t com_id); extern int32_t PIOS_COM_ChangeConfig(uint32_t com_id, enum PIOS_COM_Word_Length word_len, enum PIOS_COM_Parity parity, enum PIOS_COM_StopBits stop_bits, uint32_t baud_rate); extern int32_t PIOS_COM_SetCtrlLine(uint32_t com_id, uint32_t mask, uint32_t state); extern int32_t PIOS_COM_RegisterCtrlLineCallback(uint32_t usart_id, pios_com_callback_ctrl_line ctrl_line_cb, uint32_t context); From 9211aa5aec5c620da8d272d691a5c0f4be5e4a7d Mon Sep 17 00:00:00 2001 From: Laurent Lalanne Date: Sun, 26 May 2019 11:02:40 +0200 Subject: [PATCH 10/13] LP-457 Clear Rx buffer after baudrate change --- flight/modules/GPS/GPS.c | 1 + 1 file changed, 1 insertion(+) diff --git a/flight/modules/GPS/GPS.c b/flight/modules/GPS/GPS.c index 8b880f69e..dc3ecda73 100644 --- a/flight/modules/GPS/GPS.c +++ b/flight/modules/GPS/GPS.c @@ -569,6 +569,7 @@ void gps_set_fc_baud_from_arg(uint8_t baud) previous_baud = baud; // Set Revo port hwsettings_baud PIOS_COM_ChangeBaud(PIOS_COM_GPS, hwsettings_gpsspeed_enum_to_baud(baud)); + PIOS_COM_ClearRxBuffer(PIOS_COM_GPS); GPSPositionSensorBaudRateSet(&baud); } else { DEBUG_PRINTF(3, "SetBaud:%d - NoChange\r", baud); From c39e242c61605aa293d33ed84b52e5a0e188cee3 Mon Sep 17 00:00:00 2001 From: Laurent Lalanne Date: Sun, 26 May 2019 13:24:02 +0200 Subject: [PATCH 11/13] LP-457 Update comments --- flight/modules/GPS/GPS.c | 2 +- flight/modules/GPS/ubx_autoconfig.c | 26 ++++++++++---------------- 2 files changed, 11 insertions(+), 17 deletions(-) diff --git a/flight/modules/GPS/GPS.c b/flight/modules/GPS/GPS.c index dc3ecda73..f2e69786b 100644 --- a/flight/modules/GPS/GPS.c +++ b/flight/modules/GPS/GPS.c @@ -563,12 +563,12 @@ void gps_set_fc_baud_from_arg(uint8_t baud) // can the code stand having two tasks/threads do an XyzSet() call at the same time? if (__sync_fetch_and_add(&mutex, 1) == 0) { // don't bother doing the baud change if it is actually the same - // might drop less data if (previous_baud != baud) { DEBUG_PRINTF(3, "SetBaud:%d - %d", baud, hwsettings_gpsspeed_enum_to_baud(baud)); previous_baud = baud; // Set Revo port hwsettings_baud PIOS_COM_ChangeBaud(PIOS_COM_GPS, hwsettings_gpsspeed_enum_to_baud(baud)); + // clear Rx buffer PIOS_COM_ClearRxBuffer(PIOS_COM_GPS); GPSPositionSensorBaudRateSet(&baud); } else { diff --git a/flight/modules/GPS/ubx_autoconfig.c b/flight/modules/GPS/ubx_autoconfig.c index 5e0fc5763..76c424795 100644 --- a/flight/modules/GPS/ubx_autoconfig.c +++ b/flight/modules/GPS/ubx_autoconfig.c @@ -107,7 +107,7 @@ ubx_cfg_msg_t msg_config_ubx6[] = { { .msgClass = UBX_CLASS_RXM, .msgID = UBX_ID_RXM_SVSI, .rate = 0 }, - // message to enable + // message to enable, keep SVINFO at end { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_POSLLH, .rate = 1 }, { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_DOP, .rate = 1 }, { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_SOL, .rate = 1 }, @@ -141,7 +141,7 @@ ubx_cfg_msg_t msg_config_ubx7[] = { { .msgClass = UBX_CLASS_RXM, .msgID = UBX_ID_RXM_SVSI, .rate = 0 }, - // message to enable + // message to enable, keep SVINFO at end { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_PVT, .rate = 1 }, { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_DOP, .rate = 1 }, { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_SVINFO, .rate = 10 }, @@ -315,6 +315,7 @@ static void config_rate(uint16_t *bytes_to_send, bool min_rate) *bytes_to_send = prepare_packet((UBXSentPacket_t *)&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_RATE, sizeof(ubx_cfg_rate_t)); DEBUG_PRINTF(3, "CfgRate: %dHz\r", rate); if (min_rate) { + // used for INIT_STEP_SET_LOWRATE step status->lastConfigSent = LAST_CONFIG_SENT_COMPLETED; } } @@ -500,6 +501,7 @@ static void enable_sentences(__attribute__((unused)) uint16_t *bytes_to_send, bo &msg_config_ubx7[0] : &msg_config_ubx6[0]; if (msg >= 0 && msg < msg_count) { + // last message is SVINFO, disable if needed status->working_packet.message.payload.cfg_msg = (msg == (msg_count - 1) && disable_svinfo) ? msg_config_ubx_disable_svinfo[0] : msg_config[msg]; *bytes_to_send = prepare_packet((UBXSentPacket_t *)&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_MSG, sizeof(ubx_cfg_msg_t)); } else { @@ -515,7 +517,7 @@ static void enable_sentences(__attribute__((unused)) uint16_t *bytes_to_send, bo #if defined(AUTOBAUD_CONFIGURE_STORE_AND_DISABLE) // permanently store our version of GPSSettings.UbxAutoConfig -// we use this to disable after AbConfigStoreAndDisable is complete +// we use this to disable after AutoBaudConfigureStoreAndDisable is complete static void setGpsSettings() { // trying to do this as perfectly as possible we must realize that they may have pressed Send on some fields @@ -574,10 +576,6 @@ void gps_ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send) // at low baud rates and high data rates the ubx gps simply must drop some outgoing data // this isn't really an error // and when a lot of data is being dropped, the MON VER reply often gets dropped - // on the other hand, uBlox documents that some versions discard data that is over 1 second old - // implying a 1 second send buffer and that it could be over 1 second before a reply is received - // later uBlox versions dropped this 1 second constraint and drop data when the send buffer is full - // and that could be even longer than 1 second uint32_t time_to_wait = UBX_PARSER_TIMEOUT; // Add extra time to detect ubxHwVersion in case detection @@ -590,19 +588,15 @@ void gps_ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send) return; } - // at this point we have already waited for the MON_VER reply to time out (except the first time where it times out without being sent) + // at this point we have already waited for the MON_VER reply to time out // and the fact we are here says that ubxHwVersion has not been set (it is set externally) // so this try at this baud rate has failed - // if we get here - // select the next baud rate, skipping ahead if new baud rate is HwSettings.GPSSpeed - // set Revo baud rate to current++ value (immediate change so we can send right after that) and send the MON_VER request - // baud rate search order are most likely matches first + // if we get here set Revo baud rate to the previous incremented value and send the MON_VER request // if AutoBaud or higher, do AutoBaud if (status->currentSettings.UbxAutoConfig >= GPSSETTINGS_UBXAUTOCONFIG_AUTOBAUD) { // first try HwSettings.GPSSpeed and then - // get the next baud rate to try from the table, but skip over the value of HwSettings.GPSSpeed - // new_gps_speed is not initialized yet + // at startup new_gps_speed is not initialized yet if (new_gps_speed > HWSETTINGS_GPSSPEED_230400) { // try current speed set in HwSettings.GPSSpeed HwSettingsGPSSpeedGet(&hwsettings_baud); @@ -612,7 +606,7 @@ void gps_ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send) // set the FC (Revo) baud rate gps_set_fc_baud_from_arg(baud_to_try); - // increase baud_to_try for next try + // increase baud_to_try for next try, in case ubxHwVersion is not set baud_to_try = (baud_to_try < HWSETTINGS_GPSSPEED_230400) ? baud_to_try + 1 : HWSETTINGS_GPSSPEED_2400; DEBUG_PRINTF(3, "IncreaseBaudToTry\r"); } @@ -747,7 +741,7 @@ void gps_ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send) // Revo and GPS are both at 9600 baud case INIT_STEP_GPS_BAUD: - // wait for previous step + // wait for previous step (INIT_STEP_SET_LOWRATE) if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) < UBX_UNVERIFIED_STEP_WAIT_TIME) { return; } From 05a358e505602651cdfe58ad91a8e0a81fe856c7 Mon Sep 17 00:00:00 2001 From: Laurent Lalanne Date: Sun, 26 May 2019 02:56:46 +0200 Subject: [PATCH 12/13] LP-457 Remove Debug --- flight/modules/GPS/GPS.c | 13 ------------- flight/modules/GPS/UBX.c | 4 ---- flight/modules/GPS/ubx_autoconfig.c | 11 +---------- .../boards/revolution/firmware/inc/pios_config.h | 4 ++-- 4 files changed, 3 insertions(+), 29 deletions(-) diff --git a/flight/modules/GPS/GPS.c b/flight/modules/GPS/GPS.c index f2e69786b..768dbc396 100644 --- a/flight/modules/GPS/GPS.c +++ b/flight/modules/GPS/GPS.c @@ -414,16 +414,6 @@ static void gpsTask(__attribute__((unused)) void *parameters) GPSPositionSensorStatusOptions status = GPSPOSITIONSENSOR_STATUS_NOGPS; GPSPositionSensorStatusSet(&status); AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_ERROR); - - if (res == PARSER_ERROR) { - DEBUG_PRINTF(3, "NoGPS-ParserError\r"); - } - if ((timeNowMs - timeOfLastUpdateMs) >= GPS_TIMEOUT_MS) { - DEBUG_PRINTF(3, "NoGPS-TimeOUT:%d\r", (timeNowMs - timeOfLastUpdateMs)); - } - if (gpsSettings.DataProtocol == GPSSETTINGS_DATAPROTOCOL_UBX && gpspositionsensor.AutoConfigStatus == GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_ERROR) { - DEBUG_PRINTF(3, "NoGPS-AutoCfgError\r"); - } } // if we parsed at least one packet successfully // we aren't offline, but we still may not have a good enough fix to fly @@ -564,15 +554,12 @@ void gps_set_fc_baud_from_arg(uint8_t baud) if (__sync_fetch_and_add(&mutex, 1) == 0) { // don't bother doing the baud change if it is actually the same if (previous_baud != baud) { - DEBUG_PRINTF(3, "SetBaud:%d - %d", baud, hwsettings_gpsspeed_enum_to_baud(baud)); previous_baud = baud; // Set Revo port hwsettings_baud PIOS_COM_ChangeBaud(PIOS_COM_GPS, hwsettings_gpsspeed_enum_to_baud(baud)); // clear Rx buffer PIOS_COM_ClearRxBuffer(PIOS_COM_GPS); GPSPositionSensorBaudRateSet(&baud); - } else { - DEBUG_PRINTF(3, "SetBaud:%d - NoChange\r", baud); } } --mutex; diff --git a/flight/modules/GPS/UBX.c b/flight/modules/GPS/UBX.c index bbbd95733..e8a79a3b5 100644 --- a/flight/modules/GPS/UBX.c +++ b/flight/modules/GPS/UBX.c @@ -197,7 +197,6 @@ int parse_ubx_stream(uint8_t *rx, uint16_t len, char *gps_rx_buffer, GPSPosition restart_state = RESTART_NO_ERROR; } else { restart_state = RESTART_WITH_ERROR; - DEBUG_PRINTF(3, "UbxPayloadError: 0x%02x:0x%02x\r", ubx->header.class, ubx->header.id); } #endif // We won't see the end of the packet. Which means it is useless to do any further processing. @@ -249,7 +248,6 @@ int parse_ubx_stream(uint8_t *rx, uint16_t len, char *gps_rx_buffer, GPSPosition } else { gpsRxStats->gpsRxChkSumError++; restart_state = RESTART_WITH_ERROR; - DEBUG_PRINTF(3, "UbxCheckError\r"); break; } continue; @@ -549,7 +547,6 @@ static void parse_ubx_mon_ver(struct UBXPacket *ubx, __attribute__((unused)) GPS // send sensor type right now because on UBX NEMA we don't get a full set of messages // and we want to be able to see sensor type even on UBX NEMA GPS's GPSPositionSensorSensorTypeSet((uint8_t *)&ubxSensorType); - DEBUG_PRINTF(3, "ParseMonVer\r"); } static void parse_ubx_op_sys(struct UBXPacket *ubx, __attribute__((unused)) GPSPositionSensorData *GpsPosition) @@ -621,7 +618,6 @@ uint32_t parse_ubx_message(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosi // see OP GPSV9 comment in parse_ubx_stream() for further information status = GPSPOSITIONSENSOR_STATUS_NOFIX; GPSPositionSensorStatusSet(&status); - DEBUG_PRINTF(3, "Detected UBX\r"); } } return id; diff --git a/flight/modules/GPS/ubx_autoconfig.c b/flight/modules/GPS/ubx_autoconfig.c index 76c424795..847960d3d 100644 --- a/flight/modules/GPS/ubx_autoconfig.c +++ b/flight/modules/GPS/ubx_autoconfig.c @@ -269,7 +269,6 @@ static void config_reset(uint16_t *bytes_to_send) status->working_packet.message.payload.cfg_cfg.deviceMask = UBX_CFG_CFG_DEVICE_ALL; *bytes_to_send = prepare_packet((UBXSentPacket_t *)&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_CFG, sizeof(ubx_cfg_cfg_t)); - DEBUG_PRINTF(3, "CfgReset\r"); } @@ -290,7 +289,6 @@ static void config_gps_baud(uint16_t *bytes_to_send) // GPS will be found later at this new_gps_speed new_gps_speed = hwsettings_baud; - DEBUG_PRINTF(3, "CfgBaud: %d\r", new_gps_speed); } @@ -313,7 +311,7 @@ static void config_rate(uint16_t *bytes_to_send, bool min_rate) status->working_packet.message.payload.cfg_rate.timeRef = 1; // 0 = UTC Time, 1 = GPS Time *bytes_to_send = prepare_packet((UBXSentPacket_t *)&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_RATE, sizeof(ubx_cfg_rate_t)); - DEBUG_PRINTF(3, "CfgRate: %dHz\r", rate); + if (min_rate) { // used for INIT_STEP_SET_LOWRATE step status->lastConfigSent = LAST_CONFIG_SENT_COMPLETED; @@ -330,7 +328,6 @@ static void config_nav(uint16_t *bytes_to_send) status->working_packet.message.payload.cfg_nav5.mask = UBX_CFG_NAV5_DYNMODEL + UBX_CFG_NAV5_FIXMODE; *bytes_to_send = prepare_packet((UBXSentPacket_t *)&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_NAV5, sizeof(ubx_cfg_nav5_t)); - DEBUG_PRINTF(3, "CfgNav\r"); } static void config_navx(uint16_t *bytes_to_send) @@ -340,7 +337,6 @@ static void config_navx(uint16_t *bytes_to_send) status->working_packet.message.payload.cfg_navx5.mask1 = UBX_CFG_NAVX5_AOP; *bytes_to_send = prepare_packet((UBXSentPacket_t *)&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_NAVX5, sizeof(ubx_cfg_navx5_t)); - DEBUG_PRINTF(3, "CfgNavX\r"); } @@ -366,7 +362,6 @@ static void config_sbas(uint16_t *bytes_to_send) UBX_CFG_SBAS_SCANMODE2; *bytes_to_send = prepare_packet((UBXSentPacket_t *)&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_SBAS, sizeof(ubx_cfg_sbas_t)); - DEBUG_PRINTF(3, "CfgSBAS\n"); } @@ -428,7 +423,6 @@ static void config_gnss(uint16_t *bytes_to_send) } *bytes_to_send = prepare_packet((UBXSentPacket_t *)&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_GNSS, sizeof(ubx_cfg_gnss_t)); - DEBUG_PRINTF(3, "CfgGNSS\r"); } @@ -442,7 +436,6 @@ static void config_save(uint16_t *bytes_to_send) status->working_packet.message.payload.cfg_cfg.deviceMask = UBX_CFG_CFG_DEVICE_ALL; *bytes_to_send = prepare_packet((UBXSentPacket_t *)&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_CFG, sizeof(ubx_cfg_cfg_t)); - DEBUG_PRINTF(3, "CfgSavenew: %d\r", new_gps_speed); } static void configure(uint16_t *bytes_to_send) @@ -608,7 +601,6 @@ void gps_ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send) // increase baud_to_try for next try, in case ubxHwVersion is not set baud_to_try = (baud_to_try < HWSETTINGS_GPSSPEED_230400) ? baud_to_try + 1 : HWSETTINGS_GPSSPEED_2400; - DEBUG_PRINTF(3, "IncreaseBaudToTry\r"); } // this code is executed even if ubxautoconfig is disabled @@ -645,7 +637,6 @@ void gps_ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send) // keep timeouts running properly, we (will have) just sent a packet that generates a reply set_current_step_if_untouched(INIT_STEP_WAIT_MON_VER_ACK); status->lastStepTimestampRaw = PIOS_DELAY_GetRaw(); - DEBUG_PRINTF(3, "Mon_verRefresh\r"); break; case INIT_STEP_WAIT_MON_VER_ACK: diff --git a/flight/targets/boards/revolution/firmware/inc/pios_config.h b/flight/targets/boards/revolution/firmware/inc/pios_config.h index f069c8fd2..6694ff33d 100644 --- a/flight/targets/boards/revolution/firmware/inc/pios_config.h +++ b/flight/targets/boards/revolution/firmware/inc/pios_config.h @@ -37,8 +37,8 @@ * details. */ -#define PIOS_INCLUDE_DEBUG_CONSOLE -#define DEBUG_LEVEL 3 +/* #define PIOS_INCLUDE_DEBUG_CONSOLE */ +/* #define DEBUG_LEVEL 0 */ /* #define PIOS_ENABLE_DEBUG_PINS */ /* PIOS FreeRTOS support */ From 82ca8fa643ad4522b4c82a80a6ea3a48562dfa02 Mon Sep 17 00:00:00 2001 From: Laurent Lalanne Date: Sun, 26 May 2019 17:05:45 +0200 Subject: [PATCH 13/13] LP-457 High baudrate fix like 230400bds where packets are dropped --- flight/modules/GPS/GPS.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flight/modules/GPS/GPS.c b/flight/modules/GPS/GPS.c index 768dbc396..544f1080f 100644 --- a/flight/modules/GPS/GPS.c +++ b/flight/modules/GPS/GPS.c @@ -127,7 +127,7 @@ void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev); // are run often enough. // GPS_LOOP_DELAY_MS on the other hand, should be less then 5.55 ms. A value set too high will cause data to be dropped. -#define GPS_LOOP_DELAY_MS 5 +#define GPS_LOOP_DELAY_MS 4 #define GPS_BLOCK_ON_NO_DATA_MS 20 #ifdef PIOS_GPS_SETS_HOMELOCATION