Maybe fix GPS recovery - update lastUpdateTime on first reading after signal recovery #11144
Conversation
…l recovery When GPS signal is lost and recovered, the position estimator's lastUpdateTime was only being updated on subsequent readings (when isFirstGPSUpdate was false). This caused the GPS validity timeout check to fail on the first reading after recovery, resulting in altitude and distance-to-home values getting stuck at zero. The fix moves the lastUpdateTime update outside the isFirstGPSUpdate check so it is updated on every GPS reading, including the first one after recovery. Fixes iNavFlight#11049
PR Compliance Guide 🔍All compliance sections have been disabled in the configurations. |
![qodo-code-review[bot]](https://avatars.githubusercontent.com/in/484649?s=60&v=4){kind=small}
| def consume_response(board): | ||
| """Consume MSP response to prevent buffer overflow.""" | ||
| try: | ||
| dataHandler = board.receive_msg() | ||
| if dataHandler: | ||
| board.process_recv_data(dataHandler) | ||
| except: | ||
| pass |
There was a problem hiding this comment.
Suggestion: In the consume_response function, replace the bare except: with specific exception handling for expected errors like ConnectionRefusedError and TimeoutError, while logging any other unexpected exceptions to improve debuggability. [general, importance: 6]
| def consume_response(board): | |
| """Consume MSP response to prevent buffer overflow.""" | |
| try: | |
| dataHandler = board.receive_msg() | |
| if dataHandler: | |
| board.process_recv_data(dataHandler) | |
| except: | |
| pass | |
| def consume_response(board): | |
| """Consume MSP response to prevent buffer overflow.""" | |
| try: | |
| dataHandler = board.receive_msg() | |
| if dataHandler: | |
| board.process_recv_data(dataHandler) | |
| except (ConnectionRefusedError, TimeoutError): | |
| # Expected errors if the flight controller is not ready, can be ignored. | |
| pass | |
| except Exception as e: | |
| # Log other unexpected errors to aid in debugging. | |
| print(f"Warning: Unexpected error in consume_response: {e}") |
| def setup_receiver_type(board): | ||
| """Set receiver type to MSP.""" | ||
| # Read current config | ||
| board.send_RAW_msg(MSP_RX_CONFIG, data=[]) | ||
| time.sleep(0.2) | ||
| dataHandler = board.receive_msg() | ||
| data = dataHandler.get('dataView', []) if dataHandler else [] | ||
|
|
||
| if data and len(data) >= 24: | ||
| current_data = list(data) | ||
| else: | ||
| # Use defaults | ||
| current_data = [0] * 24 | ||
| current_data[1], current_data[2] = 0x6C, 0x07 # maxcheck = 1900 | ||
| current_data[3], current_data[4] = 0xDC, 0x05 # midrc = 1500 | ||
| current_data[5], current_data[6] = 0x4C, 0x04 # mincheck = 1100 | ||
| current_data[8], current_data[9] = 0x75, 0x03 # rx_min_usec = 885 | ||
| current_data[10], current_data[11] = 0x43, 0x08 # rx_max_usec = 2115 | ||
|
|
||
| current_data[23] = RX_TYPE_MSP | ||
| board.send_RAW_msg(MSP_SET_RX_CONFIG, data=current_data[:24]) | ||
| consume_response(board) |
There was a problem hiding this comment.
Suggestion: In setup_receiver_type, replace the fixed time.sleep(0.2) with a polling loop that waits for the MSP response with a timeout. This makes the test setup more reliable by handling variable response times from the flight controller. [general, importance: 7]
| def setup_receiver_type(board): | |
| """Set receiver type to MSP.""" | |
| # Read current config | |
| board.send_RAW_msg(MSP_RX_CONFIG, data=[]) | |
| time.sleep(0.2) | |
| dataHandler = board.receive_msg() | |
| data = dataHandler.get('dataView', []) if dataHandler else [] | |
| if data and len(data) >= 24: | |
| current_data = list(data) | |
| else: | |
| # Use defaults | |
| current_data = [0] * 24 | |
| current_data[1], current_data[2] = 0x6C, 0x07 # maxcheck = 1900 | |
| current_data[3], current_data[4] = 0xDC, 0x05 # midrc = 1500 | |
| current_data[5], current_data[6] = 0x4C, 0x04 # mincheck = 1100 | |
| current_data[8], current_data[9] = 0x75, 0x03 # rx_min_usec = 885 | |
| current_data[10], current_data[11] = 0x43, 0x08 # rx_max_usec = 2115 | |
| current_data[23] = RX_TYPE_MSP | |
| board.send_RAW_msg(MSP_SET_RX_CONFIG, data=current_data[:24]) | |
| consume_response(board) | |
| def setup_receiver_type(board): | |
| """Set receiver type to MSP.""" | |
| # Read current config | |
| board.send_RAW_msg(MSP_RX_CONFIG, data=[]) | |
| # Poll for response with a timeout | |
| start_time = time.time() | |
| dataHandler = None | |
| while time.time() - start_time < 1.0: # 1 second timeout | |
| dataHandler = board.receive_msg() | |
| if dataHandler: | |
| break | |
| time.sleep(0.05) | |
| data = dataHandler.get('dataView', []) if dataHandler else [] | |
| if data and len(data) >= 24: | |
| current_data = list(data) | |
| else: | |
| # Use defaults | |
| current_data = [0] * 24 | |
| current_data[1], current_data[2] = 0x6C, 0x07 # maxcheck = 1900 | |
| current_data[3], current_data[4] = 0xDC, 0x05 # midrc = 1500 | |
| current_data[5], current_data[6] = 0x4C, 0x04 # mincheck = 1100 | |
| current_data[8], current_data[9] = 0x75, 0x03 # rx_min_usec = 885 | |
| current_data[10], current_data[11] = 0x43, 0x08 # rx_max_usec = 2115 | |
| current_data[23] = RX_TYPE_MSP | |
| board.send_RAW_msg(MSP_SET_RX_CONFIG, data=current_data[:24]) | |
| consume_response(board) |
| def query_distance_to_home(board): | ||
| """Query MSP_COMP_GPS to get distance to home.""" | ||
| board.send_RAW_msg(MSP_COMP_GPS, data=[]) | ||
| dataHandler = board.receive_msg() | ||
| if dataHandler: | ||
| board.process_recv_data(dataHandler) | ||
| return board.GPS_DATA.get('distanceToHome', None) |
There was a problem hiding this comment.
Suggestion: In query_distance_to_home, replace the immediate message read with a polling loop that has a timeout. This makes the function more resilient to latency and ensures it waits for the correct MSP response, improving the reliability of the test's validation logic. [general, importance: 8]
| def query_distance_to_home(board): | |
| """Query MSP_COMP_GPS to get distance to home.""" | |
| board.send_RAW_msg(MSP_COMP_GPS, data=[]) | |
| dataHandler = board.receive_msg() | |
| if dataHandler: | |
| board.process_recv_data(dataHandler) | |
| return board.GPS_DATA.get('distanceToHome', None) | |
| def query_distance_to_home(board): | |
| """Query MSP_COMP_GPS to get distance to home.""" | |
| board.send_RAW_msg(MSP_COMP_GPS, data=[]) | |
| # Poll for response with a timeout | |
| start_time = time.time() | |
| dataHandler = None | |
| while time.time() - start_time < 0.5: # 500ms timeout | |
| dataHandler = board.receive_msg() | |
| if dataHandler and dataHandler.get('code') == MSP_COMP_GPS: | |
| board.process_recv_data(dataHandler) | |
| return board.GPS_DATA.get('distanceToHome', None) | |
| time.sleep(0.01) # Short sleep to prevent busy-waiting | |
| return None |
| # Calculate movement per update (at 50Hz) | ||
| # 30 mph = 13.41 m/s, at 50Hz = 0.27m per update | ||
| meters_per_update = FLIGHT_SPEED_CMS / 100.0 / 50.0 # m/s / Hz | ||
| units_per_update = int(meters_per_update / METERS_PER_UNIT) |
There was a problem hiding this comment.
Suggestion: Avoid truncating the per-update movement to an integer to preserve realistic motion; keep the accumulator in float and only quantize when forming the MSP payload. [Learned best practice, importance: 6]
| units_per_update = int(meters_per_update / METERS_PER_UNIT) | |
| METERS_PER_UNIT = 0.0000111 | |
| ... | |
| units_per_update = meters_per_update / METERS_PER_UNIT # keep float | |
| ... | |
| current_lat_f = float(base_lat) | |
| current_lon_f = float(base_lon) | |
| ... | |
| # in the loop | |
| current_lat_f += units_per_update | |
| current_lat = int(round(current_lat_f)) | |
| current_lon = int(round(current_lon_f)) |
|
It's not immediately obvious why this would work. I'd expect the estimated positions to freeze on the last values when the GPS fix is lost with only velocities resetting to 0. In which case it doesn't explain why home distance and altitude reset to zero in #11049. Also the jump in flight distance to 214km would require a large glitch in XY velocity ?. There's a lot of interaction going on here so did you definitively work out how |
sensei-hacker
changed the title
sensei-hacker
changed the title
It was "try fixing the first thing that looks wrong-ish, then run a test". And the test incorrectly indicated that it fixed all the problems. |
2 participants
User description
Summary
posEstimator.gps.lastUpdateTime = currentTimeUs;outside theif (!isFirstGPSUpdate)block so it updates on every GPS reading, including the first one after recovery