Penelope

She exists for two purposes only:

  • produce a system overcoming the I²C and network changes in Jessie / Stretch after March 2017
  • use up the reams of the new spare parts acquired over the years of drone development.

It’s been more expensive in time and money than I’d hoped, primarily because of the cost and delayed shipping of the Garmin LiDAR-Lite v3HP.

I’m only showing a stable hover as that is infinitely harder than anything else:  accelerometer noise and drift over temperature and time, integrated for velocity and again for distance means that after a few seconds, errors in the integrated velocity and distance are very wrong and increasing rapidly, and only the extra sensors of ground-facing LiDAR and video constrain this increasing drift errors.

She weighs 4.1kg which is more than I’d like due to battery usage, but the only over-heavy bits are the black Lacrosse-ball feet at 0.6kg for the four – this makes her only a few grams lighter than Hermione.

All she’s missing compared to Hermione is the obstacle-avoidance due to the fact the KickStarter Scanse Sweep team have shut down.  Given the obstacle-avoidance concept has been proven, I’m not out to find an equivalent.

Barring an Archimedes “Eureka!” bath moment, this is genuinely the end-of-the-line for my RPi piDrones.

The code has been updated on GitHub as a result.

Garmin LiDAR-Lite v3HP working…

but at a price.  Here’s the python classes:

v3

####################################################################################################
#
#  Garmin LiDAR-Lite v3 range finder
#
####################################################################################################
class GLLv3:
    i2c = None

    __GLL_ACQ_COMMAND       = 0x00
    __GLL_STATUS            = 0x01
    __GLL_SIG_COUNT_VAL     = 0x02
    __GLL_ACQ_CONFIG_REG    = 0x04
    __GLL_VELOCITY          = 0x09
    __GLL_PEAK_CORR         = 0x0C
    __GLL_NOISE_PEAK        = 0x0D
    __GLL_SIGNAL_STRENGTH   = 0x0E
    __GLL_FULL_DELAY_HIGH   = 0x0F
    __GLL_FULL_DELAY_LOW    = 0x10
    __GLL_OUTER_LOOP_COUNT  = 0x11
    __GLL_REF_COUNT_VAL     = 0x12
    __GLL_LAST_DELAY_HIGH   = 0x14
    __GLL_LAST_DELAY_LOW    = 0x15
    __GLL_UNIT_ID_HIGH      = 0x16
    __GLL_UNIT_ID_LOW       = 0x17
    __GLL_I2C_ID_HIGHT      = 0x18
    __GLL_I2C_ID_LOW        = 0x19
    __GLL_I2C_SEC_ADDR      = 0x1A
    __GLL_THRESHOLD_BYPASS  = 0x1C
    __GLL_I2C_CONFIG        = 0x1E
    __GLL_COMMAND           = 0x40
    __GLL_MEASURE_DELAY     = 0x45
    __GLL_PEAK_BCK          = 0x4C
    __GLL_CORR_DATA         = 0x52
    __GLL_CORR_DATA_SIGN    = 0x53
    __GLL_ACQ_SETTINGS      = 0x5D
    __GLL_POWER_CONTROL     = 0x65

    def __init__(self, address=0x62, rate=10):
        self.i2c = I2C(address)
        self.rate = rate

        #-------------------------------------------------------------------------------------------
        # Set to continuous sampling after initial read.
        #-------------------------------------------------------------------------------------------
        self.i2c.write8(self.__GLL_OUTER_LOOP_COUNT, 0xFF)

        #-------------------------------------------------------------------------------------------
        # Set the sampling frequency as 2000 / Hz:
        # 10Hz = 0xc8
        # 20Hz = 0x64
        # 100Hz = 0x14
        #-------------------------------------------------------------------------------------------
        self.i2c.write8(self.__GLL_MEASURE_DELAY, int(2000 / rate))

        #-------------------------------------------------------------------------------------------
        # Include receiver bias correction 0x04
        #AB! 0x04 | 0x01 should cause (falling edge?) GPIO_GLL_DR_INTERRUPT.  Can GPIO handle this?
        #-------------------------------------------------------------------------------------------
        self.i2c.write8(self.__GLL_ACQ_COMMAND, 0x04 | 0x01)

        #-------------------------------------------------------------------------------------------
        # Acquisition config register:
        # 0x01 Data ready interrupt
        # 0x20 Take sampling rate from MEASURE_DELAY
        #-------------------------------------------------------------------------------------------
        self.i2c.write8(self.__GLL_ACQ_CONFIG_REG, 0x21)


    def read(self):
        #-------------------------------------------------------------------------------------------
        # Distance is in cm hence the 100s to convert to meters.
        # Velocity is in cm between consecutive reads; sampling rate converts these to a velocity.
        # Reading the list from 0x8F seems to get the previous reading, probably cached for the sake
        # of calculating the velocity next time round.
        #-------------------------------------------------------------------------------------------
        dist1 = self.i2c.readU8(self.__GLL_FULL_DELAY_HIGH)
        dist2 = self.i2c.readU8(self.__GLL_FULL_DELAY_LOW)
        distance = (dist1 << 8) + dist2

        if distance == 1:
            raise ValueError("GLL out of range")

        return distance / 100

v3HP

####################################################################################################
#
#  Garmin LiDAR-Lite v3HP range finder
#
####################################################################################################
class GLLv3HP:
    i2c = None

    __GLL_ACQ_COMMAND       = 0x00
    __GLL_STATUS            = 0x01
    __GLL_SIG_COUNT_VAL     = 0x02
    __GLL_ACQ_CONFIG_REG    = 0x04
    __GLL_LEGACY_RESET_EN   = 0x06
    __GLL_SIGNAL_STRENGTH   = 0x0E
    __GLL_FULL_DELAY_HIGH   = 0x0F
    __GLL_FULL_DELAY_LOW    = 0x10
    __GLL_REF_COUNT_VAL     = 0x12
    __GLL_UNIT_ID_HIGH      = 0x16
    __GLL_UNIT_ID_LOW       = 0x17
    __GLL_I2C_ID_HIGHT      = 0x18
    __GLL_I2C_ID_LOW        = 0x19
    __GLL_I2C_SEC_ADDR      = 0x1A
    __GLL_THRESHOLD_BYPASS  = 0x1C
    __GLL_I2C_CONFIG        = 0x1E
    __GLL_PEAK_STACK_HIGH   = 0x26
    __GLL_PEAK_STACK_LOW    = 0x27
    __GLL_COMMAND           = 0x40
    __GLL_HEALTHY_STATUS    = 0x48
    __GLL_CORR_DATA         = 0x52
    __GLL_CORR_DATA_SIGN    = 0x53
    __GLL_POWER_CONTROL     = 0x65

    def __init__(self, address=0x62):
        self.i2c = I2C(address)

        self.i2c.write8(self.__GLL_SIG_COUNT_VAL, 0x80)
        self.i2c.write8(self.__GLL_ACQ_CONFIG_REG, 0x08)
        self.i2c.write8(self.__GLL_REF_COUNT_VAL, 0x05)
        self.i2c.write8(self.__GLL_THRESHOLD_BYPASS, 0x00)

    def read(self):
        acquired = False

        # Trigger acquisition
        self.i2c.write8(self.__GLL_ACQ_COMMAND, 0x01)

        # Poll acquired?
        while not acquired:
            acquired = not (self.i2c.readU8(self.__GLL_STATUS) & 0x01)
        else:    
            dist1 = self.i2c.readU8(self.__GLL_FULL_DELAY_HIGH)
            dist2 = self.i2c.readU8(self.__GLL_FULL_DELAY_LOW)
            distance = (dist1 << 8) + dist2

        return distance / 100

The need for the v3HP is that its I2C conforms to an I2C  deviant added to the Raspberry Pi in March 2017.  It is also smaller and theoretically higher precision.

Trouble is, it comes with several problems:

  • $10 more than the v3
  • radically modified I2C registers missing several beneficial options in the v3 resulting in potentially less efficiency.
  • poor documentation meaning I was pointed at the GitHub sample to work out how to use it.

On the plus side, Penelope has now had a successful first live flight as a result but at a price.  I’m in two minds now whether to get one for Zoe when the only benefit for her is physical size.

Garmin LiDAR-Lite v3 vs. v3HP

Here’s Hermione, photographed with the Pi NoIR camera, showing the Garmin LiDAR-Lite v3 IR laser beam on the ground:

H's LiDAR beam

H’s LiDAR beam

Here’s Penelope’s equivalent:

Penelope's lack of laser

Penelope’s lack of laser

They are running identical code but Hermione gets data back over I2C for the distance whereas Penelope gets ‘1’ meaning no laser signal received, either for out-of-range, bad reflection direction, or in this case, the IR laser is not running.  There is no way to manager the laser in soft- or hardware.

Not a great start for the LiDAR-Lite v3HP 🙁


P.S. I’ve found and updated the different between the I2C registers between the GLL v3 and v3HP. However none of these mention control of the laser, and with the GLL v3HP using the new I2C registers code, the laser is still not showing.   😥


P.P.S. It seems the new v3HP I2C registers don’t support a couple of functions I was relying on:

  1. set sampling rate to 10Hz so I could always read the latest height from the ground when the lateral pixel distance was know.
  2.  always able to read the latest values automatically – this seems now to require a WRITE then a READ whereas previous, only a READ was required for the latest setting.

On the plus side it’s more accurate (apparent) though without an operating laser, that a moot point!

Refining Zoe, Reinstating Penelope, Retiring Hermione

The Garmin LiDAR Lite v3HP has arrived from RobotShop.

Garmin LiDAR-Lite v3HP

Garmin LiDAR-Lite v3HP

Strictly speaking, I don’t need it for Zoe nor Hermione, the previous version works well, but there are some benefits of the latest version:

  • it’s smaller, so sits under Zoe without the LiDAR virtually touching the ground
  • its data ready interrupt now works with Rapsberry Pi GPIO
  • it’s compatible with the Linux Stretch variant of the I²C protocol.

The latter means I can finally finish Penelope; I’ll be blogging on it as it progresses.

For Zoe, I’m doing various refinement, including a PCB to incorporate the new LiDAR.  I’ve also made a custom case foam for her; my plan here is I can show her off at events like the Cotswold Raspberry Jam.

Zoe's custom case

Zoe’s custom case

Hermione on the other hand has now become an art-statue only; the only physical difference between her and Penelope is the Scanse Sweep whose creators have just shut down hence no link to their site! I don’t want to further develop object avoidance to the extent of maze tracking when there are no spares available to allow for the inevitable crashes!  She is a very pretty statue though, isn’t she!

Hermione statue

Hermione statue

Penelope’s progress

I’ve done nothing on Penelope since my introductory post, but now I have reason to proceed:the new Raspberry Pi 3 B+ is already on its way courtesy of pimoroni combined with the new Garmin LiDAR-Lite v3HP and the new Raspian Stretch O/S.  Together these make the motivation I need though only in the background; my main focus is object avoidance with Hermione, and I hope to post on the first results imminently.  Oddly, what’s holding things back is hardware not software: construction of the obstacle.

“Penelope” or “Lucy”…

…as in the Amazon series “Lucifer”?  I’ll stick with Ms. Pitstop despite the colour scheme; Lucifer never shows up on Tuesdays.

Separated at birth?

Separated at birth?

She’s still pending the new version of the Garmin LIDAR-Lite v3HP – the lower-profile, higher-accuracy version of Hermione and Zoes’ height tracking LiDAR, She’s also waiting for a new PCB so she can have a buzzer, though that’s not holding her back in the same way.  She’ll intentionally not have a Scance Sweep as it’s very very expensive for a non-critical sensor.

My intent had been to make her lower profile, sleek and stealthy to enable longer flights per battery hence the shorter legs, and lower hat and the 13 x 4.4 CF props (compared to ‘H’ 12 x 4.4 Beechwoods). However her hat and feet prevent this – the feet are true lacrosse balls, so heavier than Hermione’s indoor ones, and her salad bowl top also seems heavier.  Overall then ‘H’ weighs in at 4.8kg all installed, and Penelope 4.7kg.  Thus the main benefit is likely she’ll be nippier due to slightly more power from the lighter, larger CF props combined with the raised centre of gravity.  And in fact, this raised CoG and lighter, larger props may well reduce the power needed – we shall see.

In the background, I am working on the “Pond Problem”: fusing GPS distance / direction with the other sensors.  Code’s nigh on complete but I’m yet to convince myself it will work well enough to test it immediately over the local gravel lakes.

Refinement time.

Sorry it’s been quiet; the weather’s been awful, so no videos to show.  Instead, I’ve been tinkering to ensure the code is as good as it can be prior to moving on to object avoidance / maze tracking.

Zoe is back to life once more to help with the “face the direction you’re flying” tweak testing as these don’t quite work yet.  She’s back as my first few attempts with ‘H’ kept breaking props.  First job for ‘Z’ was to have her run the same code as Hermione but with the autopilot moved back to inline to reduce the number of processes as possible for her single CPU Pi0W in comparison with Hermione’s 4 CPU 3B.

Then

  • I’ve started running the code with ‘sudo python -O ./qc.py’ to enable optimisation.  This disable assertion checking, and hopefully other stuff for better performance.
  • I’ve tweaked the Butterworth parameters to track gravity changes faster as Zoe’s IMU is exposed to the cold winds and her accelerometer values rise rapidly.
  • I’ve refining the Garmin LiDAR-Lite V3 to cope with occasional ‘no reading’ triggered caused by no laser reflection detected; this does happen occasionally (and correctly) if she’s tilted and the surface bouncing the laser points the wrong way.
  • I’d also hoped to add a “data ready interrupt” to the LiDAR to reduce the number of I2C requests made; however, the interrupts still don’t happens despite trying all 12 config options. I think the problem is Garmin’s so I’m awaiting a response from them on whether this flaw is fixed in a new model to be launched in the next few weeks .  In the meantime, I only call the GLL I2C when there’s video results which need the GLL vertical height to convert the video pixel movements into horizontal movement in meters.

Having added and tested all the above sequentially, the net result was failure: less bad a failure than previously, but failure nonetheless; the video tracking lagged in order to avoid the IMU FIFO overflowing.  So in the end, I changed Zoe’s video resolution to 240 pixels² @ 10 fps (‘H’ is at 320 pixel² @ 10 fps, and she now can hover on the grass which means I can get on with the “face where you’re going” code.

I do think all the other changes listed are valid and useful, and as a result, I’ve updated the code on GitHub.

In passing, I had also been investigating whether the magnetometer could be used to back up pitch, roll and yaw angles long term, but that’s an abject failure; with the props on idle prior to takeoff, it works fine giving the orientation to feed to the GPS tracking process, but once airborne, the magnetometer values shift by ±40° and varies depending which way she’s going while facing in the same direction.

Finally found the fecking wobbles

I ‘think’ there are two factors: the colder temperature reduced the power of the LiPo; this then made the system a little less able to react to distance errors, causing it to rotate more to correct horizontal distance drift, and this in turn exposed a long term bug that completely failed to compensate for horizontal distance changes due to changes in pitch / roll angles (the code was there, but had a crass bug).

The cool LiPo has been fixed with a walkers / skiers pocket-warmer strapped firmly on top, keeping it lovely and cosy.

The crass video horizontal tracking error has been fix also.  As a result, GitHub has been updated, and naively once more I can continue working on the GPS tracking.

More on mapping

It’s been a frustrating week – despite lovely weather, lots broke, and once each was fixed, something else would break.  To top is all, an update to the latest version of Jessie yesterday locked the RPi as soon as I kicked off a passive flight.  I backed this ‘upgrade’ out as a result.  I now I have everything back and working, confirm by hover and 10m lateral flights this morning, although the latter aborted half-way through with an I2C error.  Underlying it all is power to the GLL and RPi 3B – I was seeing lots of brown-out LED flashes from the B3 and lots of I2C and GLL errors.  I’m consider swapping back to a 2B+ overclocked to 1.2GHz as a result.

In the meantime I have been looking at mapping in more detail as it’s complex and it needs breaking down into easy pieces.  Here’s the general idea:

Polystyrene block layout

Polystyrene block layout

Each polystyrene block is 2.5m long, 1.25m high and 10cm thick.  They are pinned together with screw-in camping tent pegs.  The plan is to

  • fly 10m  at 1m height without the ‘maze’, logging compass and GPS to check the results, in particular to see whether
    • GPS can be gently fused with RPi ground facing motion tracking to enhance lateral motion distance measurements
    • compass can be fused with IMU gyro yaw rate to enforce a better linear flight
  • fly 10m without the ‘maze’ again but with fused compass and GPS (assuming the above is OK)
  • add the ‘maze’ and fly in a straight 10m line from bottom to top again as a sanity check
  • add the Sweep and log it’s contents when doing the same 10m again
  • build the flight map in Excel based upon GPS, compass and sweep logs – the results should look like the map with the addition of what garden clutter lies beyond the end of each exit from the ‘maze’
  • add a new mapping process to do dynamically what has been done in Excel above
  • add object avoidance from Sweep and repeat – this is the hardest bit as it introduces dynamic updates to preconfigured flight plans
  • add ‘maze’ tracking code to reach a target GPS position, nominally the center of the ‘maze’ – this stage requires further thought to break it down further.

MPU-9250 vs. Garmin LiDAR Lite

I had hoped yesterday to get going with Sweep integration, with a sanity check flight beforehand just to ensure all is running well – I can’t afford to have crashes with sweep installed.

And sure enough, Hermione crashed.  In the middle of the climbing phase of the flight, she suddenly leapt into the air, and the protection code killed her at the point her height exceeded the flight plan height by 50cm.  At the speed she was climbing, she continued to rise to a couple more meters before crashing down into a shrub bed, luckily minimising damage to components I had spares for.

A second mandatory flight to collect diagnostics (and more crash damage) revealed a conflict over I2C by the IMU and ground facing LiDAR.  The LiDAR won, and the IMU started seeing gravity as just about 0g.  This isn’t the first time this has happened, and I’ve tried various guessed solutions to fix it.

Accelerometer vs. Garmin LiDAR Lite

Accelerometer vs. Garmin LiDAR Lite

The left graph is height: blue is Garmin and is right; orange is the target – what should be happening, and grey is double integrated acceleration which is a very close match to Garmin right  up to the point it  all goes very wrong.  Looking in more detail at the right graph shows the accelerometer results dropped just before 3.5s and about 0.5s before hover would have started.

This ain’t my code; best guess is an interaction over I2C of the LiDAR and IMU, and the IMU loses.  I’ve seen similar IMU damage before, and without more detail, my only option is to add a new one and try again.