Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

SIM_GPS_GSOF - support dcol parser via new bidirectional data parsing #25541

Merged
merged 2 commits into from
Nov 28, 2023

Conversation

Ryanf55
Copy link
Collaborator

@Ryanf55 Ryanf55 commented Nov 15, 2023

Purpose

In #25525, the driver changes require active configuration parsing and response from the driver. That PR can't merge easily, and it wasn't trivial to implement because bidirectional serial simulators for GPS did not exist. This performs the leg work to set up the GPS as a full bidirectional capability.

  • Enable testing GPS configuration in CI.
  • Enable GSOF to implement configuration guarantees as part of initialization logic
  • Enable higher code coverage prior to my planned GSOF refactor of splitting into separate library for EAHRS support

How it works (physics, throttling, and updates)

Previous behavior

  1. A call to update_publish(const GPS_Data *d) in the backend builds the packet and immediately publishes, so this needs to be throttled
  2. A call to update_read(const GPS_Data *d) in the backend reads incoming data. Previously, it read one bit per loop, and would just throw them on the floor

New behavior

  1. The throttling of data writing now occurs in the GSOF simulator backend in addition to the front-end.
  2. The dcol parser now reads incoming configuration data, parses it, and acts upon some of the commands to enable/disable channels and set the output rates

How the GSOF parser works

I added a DCOL parser, which receives incoming configuration packets. Upon receipt of a successful packet, it spits back out an ACK. This will allow active configuration in #25525 to actually work.

The parser is pretty standard, using a switch statement, keeping track of parser state, and then I have a bunch of little routines for doing the parsing and utilities. A key important part is the fact that the GSOF simulator rates are now configured from ArduPilots driver, just like the real device, instead of the previous hard coded behavior of immediately outputting data streams. This now enforces whatever configuration packets are sent are probably valid. Coverage is not complete for all cases, but it at least covers the data channel enable/rate configuration that's currently in GSOF.

In order to handle scenarios where the simulator is missing configuration parsing, I want to let developers know with a a flow_of_control error. This is a good indication that you added new config in the driver without supporting it in the simulator, which is much better than silently failing.

As an aside, indent was totally off in SIM_GPS, so I fixed it.

Complications with SITL params interfering with GPS params

The GSOF driver is configuring the output rates with the APP File command, but then the ArduPilot simulator also gates write speeds with _sitl->gps_hertz[instance]. This param will interfere if it's lower than 10Hz because the GSOF driver should ideally be driven by GPS_RATE_MS parameter. In the land of GSOF in this PR, there is no reason SITL should also have a parameter. One idea is to make GPS_Backend::update(const GPS_Data &d) virtual, and override in the GSOF driver to skip the throttling. In Dev call, Tridge thought it would good for the driver to set the simulation behavior instead of separate SIM params since this is more like the real device.

Future work

  • Support baud rate commands from GSOF and change SITL's baud rate

Testing performed

  1. Autotest with ./Tools/autotest/autotest.py build.Copter test.CopterTests2b.GPSTypes
  2. GDB to inspect all logic is working correctly (happy path logic)

@Ryanf55 Ryanf55 changed the title Gsof sim - support dcol parser via new bidirectional data parsing SIM_GPS_GSOF - support dcol parser via new bidirectional data parsing Nov 15, 2023
libraries/SITL/SIM_GPS.cpp Outdated Show resolved Hide resolved
libraries/AP_Math/crc.h Outdated Show resolved Hide resolved
libraries/AP_Math/crc.h Outdated Show resolved Hide resolved
libraries/SITL/SIM_GPS.cpp Outdated Show resolved Hide resolved
@Ryanf55 Ryanf55 force-pushed the gsof-sim-support-dcol-parser branch from 2ce241b to 1c41f0d Compare November 21, 2023 07:06
* Implement DCOL command support for GSOF simulator
* Only send GSOF when enabled
* Publish only at the configured rate
* Only build GSOF packets when needed
  * This saves CPU
* Make physics and read loop run at full rate
  * The logic to rate-limit writes is now pushed to the backend
* Indent errors were fixed too

Signed-off-by: Ryan Friedman <[email protected]>
@@ -235,7 +354,8 @@ class GPS : public SerialDevice {

int ext_fifo_fd;

uint32_t last_update; // milliseconds
// The last time GPS data was written [mS]
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
// The last time GPS data was written [mS]
// The last time GPS data was written [ms]

@peterbarker peterbarker merged commit 35b52a4 into ArduPilot:master Nov 28, 2023
86 checks passed
@Ryanf55 Ryanf55 deleted the gsof-sim-support-dcol-parser branch November 28, 2023 05:03
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Projects
None yet
Development

Successfully merging this pull request may close these issues.

5 participants