Some AVC2015 source code

[Thomas Roell]

Thought I would want to share this.

https://github.com/BizzaBoy/AVC2015-KK

This is what we were running on Killer Kitty in the 3rd heat. Verbatim, not cleaned up (so I left all the bugs in ;-)).


I thought it might be interesting as it has a few cool things in it (other than an EKF scheme that would need some more work):

- bare metal, no RTOS ARM Cortex-M4

- systick/pendsv handler based scheme for deferred IRQ processing (navigation in pendsv handler)

- a profiler system to judge which modules take what time

- a stack checker using the MPU to detect stack overflows and underflows

- a nice exception handler that detects crashes, saves system information, gets the rover back into neutral state and then waits for a debugger to connect

- full speed async data logging onto a MicroSD card

- some TFT display code, which proved to be very handy in testing

- a NMEA/UBX/SRF/OSP GPS parser that handles PPS time stamping as well as the dreaded leap second correction; should work with MTK, UBLOX and SIRF units (well, I tested a bunch of them); GPS and GPS+GLONASS

- time stamped sensor readings

- MPU9150 code that will read back the embedded AK8975 compass at a 100Hz rate (well, how fast can you read useless data ;-)) [there is some HMC5883 code as well]

- a playback.c utility that reads back the log files, sorts entries to a time line and the prints them; the utility also has a simulation pass that allows to play the sensor data into the ekf/navigation/guidance code (very useful for debugging)

- a magneto.c utility to a magnetometer calibration (external source)

- heavy use of ARMV7M atomic primitives

- use a ARMV7M bitband operations to atomically set output pins on a GPIO port (quite handy and easy).

- a 6 state EKF that uses proper time sorted EKF_PREDICT/EKF_CORRECT sequences (i.e. corrects for GPS latency).


So there is a bunch to look at, and perhaps get others started faster. Maybe somebody forks over some BMO055 dead reconing code to me to look at in exchange ;-)

- Thomas

[Ted Meyers]

Aww, the spirit of openness is really tempting me, but I still haven't won a first place and I'm getting so close.  Maybe, I'll think about it.  Though, my code is really rather simple, there's not much to it:  update position based on wheel encoder distance and heading, update pursuit point, check for waypoint, update steering and throttle, repeat.

Ted

[Thomas Roell]

Gotta climb on my high horse here ;-)

I did put that up there because it seems to be unfair not to share stuff that would help others to make use of infrastructure. Personally I found it difficult to deal with an RTOS (and debugging things with different stacks), not having stack checking, The safe, shadowed variables in tm4c123_receiver.c ? (make the fail-safe detect memory corruption ...). 

The grand plan was to have my son learn how to program, and being able to use this infrastructure to fool around with his own ideas. No need to reinvent the wheel. If you want to build a rover, coding the failsafe is just not something that needs to waste time.

If somebody likes it, and shares code the other way around, I'd really love to. As said, I'd love to see the BNO055 code ... but then again also BMI160, LSM6DS3 ... 

- Thomas

--
You received this message because you are subscribed to a topic in the Google Groups "diyrovers" group.
To unsubscribe from this topic, visit https://groups.google.com/d/topic/diyrovers/t0gTAqcGfSw/unsubscribe.
To unsubscribe from this group and all its topics, send an email to diyrovers+...@googlegroups.com.
To post to this group, send email to diyr...@googlegroups.com.
To view this discussion on the web visit https://groups.google.com/d/msgid/diyrovers/CADnZWKMusbe0cDhHV5axpf%2BudyirQafSPofcbC05UgiNBoNiYA%40mail.gmail.com.

For more options, visit https://groups.google.com/d/optout.

[Minuteman]

Wow. I have only a vague idea of what most of these things would be useful for. It sounds like your system could be used for much more than just the AVC competition.

My code has been available for a while, though not officially shared. Rich and I didn't go to any special lengths to hide it, and at some point we realized that others on this board had taken a look at it...and we were fine with that. If someone can wade through our twisted logic and unorthodox coding style, and can get something useful out of it, then go ahead. 

We tried to keep our cars and code as simple as possible. Here is the repository for those who haven't seen it:

https://github.com/rkburnside/BURNOUT

Nathan

[Thomas Roell]

Didn't really want to write too much about this. I could give a lot of lengthy explanations.

In any case I found a lot of source out there (like APM) seems to be geared towards plug and play, rather then experimentation. It's been done for a TI TM4C123 chip, but porting over to say Teensy 3.1 should be trivial.

- Thomas

--
You received this message because you are subscribed to a topic in the Google Groups "diyrovers" group.
To unsubscribe from this topic, visit https://groups.google.com/d/topic/diyrovers/t0gTAqcGfSw/unsubscribe.
To unsubscribe from this group and all its topics, send an email to diyrovers+...@googlegroups.com.
To post to this group, send email to diyr...@googlegroups.com.

To view this discussion on the web visit https://groups.google.com/d/msgid/diyrovers/14ed07ff-61f0-4101-aaba-6f28724553a4%40googlegroups.com.

[Robert Bouwens]

hi thomas,

thanks for sharing!
have you some timing details for the ekf?
maybe this runs well on simpler stm32f103.
will test it soon.

cheers
robert

[Thomas Roell]

I am certain it will run on stm32f103. I seem to recall it took less than 5% of the time on Cortex-M4F. Basically in the noise level. Prediction was run with 100Hz, and Correction with 10Hz.

It's not that great and I need to rework some things. For example I am using 0 ... 360 degrees (well 0 ... 2 * PI) instead of -180 ... 180. That kills an LSB on the gyro data.

A lot of the math could be simplified by taking advantage of known 0.0 and 1.0 factors in the matrics. I wanted to do that, but given that there was enough headroom in terms of processor cycles, I simply was tooo lazy. One thing to try is to use "double" instead of "float" ...

Overall I am not happy. I selected a RPM sensor that was reading the EMF feedback (Eagle Tree RPM Sensor, well the HobbyWing equivalent on aliexpress.com ;-)). That resulted in a HUGE variance on the distance reading and a lot of cross-correction on the Z-Axis gyro bias. 

- Thomas

To view this discussion on the web visit https://groups.google.com/d/msgid/diyrovers/7e0aba06-af70-4bc0-a0fe-a062976ae03a%40googlegroups.com.

[wholder]

I finally had some free time to look over your code.  Very extensive bit of work!  I'm curious about a few things:

1. Whey did you decide to parse messages from so many different GPS units?

2. Can you explain a bit about how your EKF code works and how you handle GPS latency?  It seems like you call two different methods in your EKF code depending on whether the rover is moving, or not.  i'm curious about what this does...

3. I'm also trying to understand the coordinate system you use in navigation.c.  What units are you using?

Wayne

[Thomas Roell]

Comments embedded.

On Thursday, December 17, 2015 at 3:12:15 AM UTC-7, wholder wrote:

I finally had some free time to look over your code.  Very extensive bit of work!  I'm curious about a few things:

1. Whey did you decide to parse messages from so many different GPS units?

I wanted to get the concept right. So I looked at what I could buy. At the end of the day SiRF does not matter, and MediaTek units have inferior output, so yes there is a lot of extra code that is not relevant anymore. The other aspect was that I want to get NMEA correct with support for GLONASS ... 

Ok, I am an engineer, to the honest answer is "because I could".

 

2. Can you explain a bit about how your EKF code works and how you handle GPS latency?  It seems like you call two different methods in your EKF code depending on whether the rover is moving, or not.  i'm curious about what this does...

A loaded question ;-) Let me answer in reverse order.

The GPS contribution to the EKF is using a speed threshold. If the rover is slow or stationary, the magnetometer is used for the "course" input. The reason is simple. If the rover is too slow, the GPS cannot compute a reliable course, let alone a speed. The "esve" and "ecve" (expected speed velocity error, expected course velocity error) parameters from the GPS show that quite nicely. The alternative would have been to simply not use speed/course in the EKF_CORRECT logic for those cases, but I had not explored that due to time constraints.

The EKF itself is simple. It uses a 6 state model, x/y/speed/course/rpm_scale/gyro_bias. The code has provisions for a gyro_scale (7 state model), but that did not work out any better in practice. Main issue for the rover was the utter uselessness of the rpm sensor. Next version shall go back to a hall sensor based approach. The EKF_PREDICT step simply uses the rpm sensor reading and the gyro z-axis (rate of turn) to advance x/y/speed/course. The EKF_CORRECT step uses x/y/speed/course from the GPS (plus magnetometer) to correct mainly rpm_scale and gyro_bias. There is really nothing fancy about the EKF.

But now this is where the code goes off into the deep end ;-)

First off all sensor readings are time-stamped relative to an internal wallclock, in my case running at 80MHz. The GPS PPS plus is also recorded relative to this wallclock. Hence for GPS data I know when it the past the measurement had happened. The ACCEL/GYRO output of the MPU9150 is using the DRDY output as a time trigger, and then the conversion latency is subtracted to get the real measurement time (n.b. that the ACCEL preceeds timewise the GYRO). The MAG is handled somewhat differently in that the measurement is triggered by the MCU instead of by the  AK8975 (or HMC5883L). The RPM sensor reading are taken at face value with regards to time. 

The idea is now that the GPS triggers a EKF_CORRECT step with 10Hz. For every EKF_CORRECT step there are 10 EKF_PREDICT steps (i.e. 100Hz or 10ms). That means that sensor reading for those 10ms ranges are sorted into buckets and then averaged and those averages fed into EKF_PREDICT, but in the correct sequences with the EKF_CORRECT steps. So in reality you wait for GPS data (of EKF_CORRECT), and then walk throu the table to call EKF_PREDICT for all full buckets (remember that if the GPS has say a 70ms latency there are probably 7 calls to EKF_PREDICT outstanding). It all sounds awefully complex, but at the end of the day it's rather simple, as you just have to find out into what bucket a sensor reading goes into, and then which bucket is full (i.e. next sensor readings will hit another bucket). With some less precise logic one could avoid using the DRDY pulse of the MPU9150 and use a MCU driven model ... again, thought about it, didn't explore it.

Ok, moving on to the next level of convolution. Every time a EKF_CORRECT step is done the current location is updated (navigation_correct()). Every time a sensor bucket is full, this location is updated in navigation_predict(). That means there is a current prediction available that is ignoring future pending EKF_CORRECT steps (remember that GPS data arrives with a latency). So if the control code (ah, nice name for the servo update code) wants to know the current x/y/speed/course, it can call navigation_location() with the current time, and will get a fairly exact position (but out of order with the EKF code !!!).

One should note that this was done with a GPS primary focus. The rpm/gyro readings are used to correct/smooth out the GPS errors to get a smooth path. This was the whole downfall last year. The GPS position drifted over time, so that the waypoints shifted relative to the physical obstacles, which meant that the rover would hit them with fairly good accurancy ;-). I have a fix for that now though, one that I knew before last years AVC, but was to ignorant to put in ;-) Another thing I wanted to explore was to use x/y accel and z gyro ...

 

3. I'm also trying to understand the coordinate system you use in navigation.c.  What units are you using?

Simple flat earth model. Units are in meters. Flat earth is LLA translated relative to a local reference points. That is rather exact within a 10km (or so) radius around the reference point. Check the code in LLA2HOME() (sets the reference point) and LLA2NED() which converts the current LLA into a NED (north/east/down) representation. Waypoints are recorded in LLA form, but converted into flat earth NED ... The key idea is to use a local reference coordinate system to be able to use single precision floats, which Cortex-M4F does in hardware.

[Wayne Holder]

Thanks for taking the time to write such a detailed response!  I understand the "engineer" answer to my question about your GOS code.  I did a similar deep dive into writing C and Java code for the uBlox Binary protocol.  

I think I understand your explanation about how you handle GPS latency, but let me try stating back what I thought I read.  That is, you make a note of what point in time the GPS reading is taken by means of the PPS signal.  You then save the other measurements needed to do the prediction step into a series of buckets (table of the structure navigation_table_entry_t)  Then, when the GPS measurement is finally available, you look back into this table and find the point corresponding to when the PPS signal indicated the GPS measurement was taken and then run the prediction steps forward from this point using the GPS data.  Is that correct?

Wayne

--
You received this message because you are subscribed to the Google Groups "diyrovers" group.
To unsubscribe from this group and stop receiving emails from it, send an email to diyrovers+...@googlegroups.com.

To post to this group, send email to diyr...@googlegroups.com.

To view this discussion on the web visit https://groups.google.com/d/msgid/diyrovers/0d42c4d0-4a5c-4d4e-843d-d4064d700aee%40googlegroups.com.

[Thomas Roell]

On Thursday, December 17, 2015 at 3:55:05 PM UTC-7, wholder wrote:

Thanks for taking the time to write such a detailed response!  I understand the "engineer" answer to my question about your GOS code.  I did a similar deep dive into writing C and Java code for the uBlox Binary protocol.  

Yes, uBlox binary is fun ;-) 

 

I think I understand your explanation about how you handle GPS latency, but let me try stating back what I thought I read.  That is, you make a note of what point in time the GPS reading is taken by means of the PPS signal.

The PPS signal arrives at the full second (weekno/tow). If you get 2 pluses back to back you know the relative offset to your wallclock and the period. This is then used to translate the weekno/tow of the UBX packets to the wallclock. Same scheme with NMEA, except that UTC is used. 

 

  You then save the other measurements needed to do the prediction step into a series of buckets (table of the structure navigation_table_entry_t)

Correct. The only tricky part is that the table is really only 64 entries, as it acts like a fifo. This is actually oversided, but this way it can compensate for missing GPS reports without overflowing.

 

 Then, when the GPS measurement is finally available, you look back into this table and find the point corresponding to when the PPS signal indicated the GPS measurement was taken and then run the prediction steps forward from this point using the GPS data.  Is that correct?

Pretty much. Except that the PPS signal is not used for the GPS position reports themselfs, as they are tagged with the wallclock already (in gps_location()) . What the PPS time is used for in the navigation module is to predict a time boundary when a future GPS report will show up. The problem is that you need to define those buckets. A second has 100 buckets, and 10 of them will align with the GPS reports. You could use one reference point (one PPS pulse) to have a single origin, but I found that the wallclock on the MCU drifts over time. So what the code does, is to correct the buckets boundaries using the more frequent PPS pulses. navigation_sequence() shows how that is being used to find the bucket sequence for a given "tick" (local wallclock time stamp). navigation_threshold() computes conversely the ending wallclock for a given bucket.

Sorry for not adding more comments. It all seem so obvious back then ... and now I'm left scratching my head.