Optic Flow using ArduCopter on Navio+

[MartinSnaith]

Hello,

Randy Mackay suggested this work would be of interest to the developers group.

In moving a research system to a Navio+ Raspberry Pi 2 platform things have gone well until I got to the Pulsedlight Lidar 

Lite sensor and the PX4Flow sensor. The Lidar now works acceptably once the i2c bus speed was reduced to 100kHz. Please see 

http://community.emlid.com/t/lidarlite/457/7 for details. 

However the PX4Flow still refuses to behave.

Two issues are apparent:

1. The i2c reads don't produce real data.

The integral read starting at register 0x16 produces fixed arbitrary numbers.

The sum read starting at register 0x0 produces a random sonar_interval reading and zeros otherwise.

I based the AP_OpticalFlowNavio driver on a combination of the working Lidar driver for the i2c HAL structures with Randy's 

PX4Flow.h and the Arduino PX4Flow library providing data structure guidance. After trying various delays then double checking 

sensor initialisation and address/register values the above results still stand. Checking for the presence of the sensor using 

i2cdetect shows its presence at the correct address.

In order to check that the PX4Flow writes correctly to i2c I used the Arduino library and an Arduino Uno to act as a hardware 

intermediary. While the register 0x16 read (updateIntegral) doesn't appear to work the register 0x0 read works just fine. 

Hooking this test rig up to the Navio via a USB (ttyACM0) brings me to the second issue.

2. With working PX4Flow data being provided via a USB serial driver I am able to populate the backend "state" struct. Currently 

this is running reliably at 2Hz. However the data doesn't appear to go any further up the software stack. Although I'm still 

digging.

Following Randy's advice I moved to the current ardupilot-master and repeated the above work.

Apart from loosing the "OpFlow Init Failed." message at first start up, everything else is as reported above.

Randy tells me a number of you are working on UNIX ports at present. I'd be grateful for any advice and suggestions.

Regards,

Martin

The specs of the Navio and software follow.

Linux image used:

Linux navio-rpi 3.18.9-rt5-v7+ #1 SMP PREEMPT RT Thu Mar 26 10:31:34 UTC 2015 armv7l

Source code:

Clone from ardupilot-master 27 July 2015

Built using standard: make -j4 navio

Cross compiler as per Emlid standard build.

ArduCopter:

APM:Copter V3.4-dev ()

Free RAM: 4096

FW Ver: 120

PX4Flow sensor: 

v1.3

Firmware updated 27/07/2015 using QCGroundControl

Camera focused to 3 metres as per standard setup instructions.

Optic flow data being delivered to QCGroundControl Analyze screen appears correct and predictable.

 

Hardware:

Raspberry Pi 2 Model B 1GB

Navio+

I2C bus speed set to 200kHz (PX4Flow supports 100-400 kHz )

Connection type:

I2C standard connector direct to Navio board.

All sensor hardware and connections pre tested using an Arduino based test rig.

No issues reported on test rig.

PX4Flow initialised and tested using USB to QCGroundControl

Power source: USB lead to Navio.

Test targets:

High contrast rich texture external scene.

Natural ambient lighting day time.

Low contrast low texture internal scene

Natural ambient lighting day time.

[Pierre Kancir]

Hi !

Erlerobotic makes it work on their elrebrain (Beaglebone black based). There is the working branch https://github.com/erlerobot/ardupilot/commits/px4flow (not up to date with master , be careful! )

I didn't achieved to make it work on my BBB but you may found useful informations !

[MartinSnaith]

Hello,

Thanks for the tip Pierre, however I had a look over the Optical Flow parts of the Erlerobotic source code and it all looks the same as the relevant code on ardupilot-master.
Perhaps Randy could confirm this if he has a moment?

Anyhoo. A quick update on Navio+ progress follows.

Issue 2 has gone away since I upped the sensor write rate to 5Hz. Which is hardly surprising since this frequency filter sits at the bottom of OpticalFlow.cpp

void OpticalFlow::update(void)
{
    if (backend != NULL) {
        backend->update();
    }
    // only healthy if the data is less than 0.5s old

    _flags.healthy = (hal.scheduler->millis() - _last_update_ms < 500);

}

So now I have sensor data from the arduino serial rig bound into the optflow object as in the exemplar code.
A debug output placed at the EKF update point in sensors.cpp (approx line 120) viz.

    // write to log and send to EKF if new data has arrived
    if (optflow.last_update() != last_of_update) { ...

     ...   ahrs.writeOptFlowMeas(flowQuality, flowRate, bodyRate, last_of_update);


Produces sensible looking data. So here's an example with the sensor rotating and translating:

Debug output for EKF update call:

SOF Flow Rate q x y: � : 10757:-796.813
SOF Body Rate q x y: � : -398.406:-6374.5

fields are:

Debug comment : surface quality as uint8_t : x_value : y_value

Equivalent input from flow sensor pre maths:
251,2.70,-0.20,-0.10,-1.60,8032,163,

fields are:

Interval(ms), flow_x, flow_y, body_x, body_y, timestamp(ms), surface_quality

And here's the following reading where the sensor was rotating only.

SOF Body Rate q x y:  : 796.813:-5179.28
251,0.00,0.00,0.20,-1.30,8283,0,

Notice the surface quality is now 0.

The inputs all follow the direction of motion of the sensor.
And the surface quality drops to zero for the internal low contrast test image as it should.

So this all looks pretty good. But the readings still do not show up in the status output on Mission Planner.
I'll dig further.

And no the i2c interface still isn't working.

Any help and suggestions warmly received.

Regards,

Martin

[MartinSnaith]

Hello,

After a couple of days of digging and trying alternatives there appears to be an issue with the parameter passing
between:

AP_AHRS_NavEKF::writeOptFlowMeas

and

AP_NavEKF::writeOptFlowMeas


the AP_AHRS_NavEKF::writeOptFlowMeas

simply calls the second method in the chosen EKF viz.

// write optical flow data to EKF
void  AP_AHRS_NavEKF::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, Vector2f &rawGyroRates, uint32_t &msecFlowMeas)
{

    EKF.writeOptFlowMeas(rawFlowQuality, rawFlowRates, rawGyroRates, msecFlowMeas);
}

However most of the time within the second method the optical flow info data is corrupted:

The flowQuality is an arbitrary, almost constant integer with the flow and gyro Vectors all set to zero.

Debug ouptut using working sensor data as previously described with the "NavEKF Q filter pass" debug is at approx line: 4420 in AP_NavEKF.cpp
just after:

    // don't use data with a low quality indicator or extreme rates (helps catch corrupt sensor data)
    if ((rawFlowQuality > 0) && rawFlowRates.length() < 4.2f && rawGyroRates.length() < 4.2f) {

DEBUG:

Write to ahrs Flow Rate q x y: 132 : 5179.28:1195.22
Write to ahrs Body Rate q x y: 132 : 10358.6:-15537.8
In AP_AHRS_NavEKF::writeOptFlowMeas q x y t: 132 : 5179.28:1195.22:19539
In AP_AHRS_NavEKF::writeOptFlowMeas q x y t: 132 : 10358.6:-15537.8:19539
NavEKF Q filter pass 199 : 0.000000 : 0.000000

As a double check I substituted for parameter passing using extern variables declared at file scope in AP_AHRS_NavEKF.cpp
then in system.cpp and got exactly the same result.

DEBUG:

Write to ahrs Flow Rate q x y: 132 : 5179.28:1195.22
Write to ahrs Body Rate q x y: 132 : 10358.6:-15537.8
In AP_AHRS_NavEKF::writeOptFlowMeas q x y t: 132 : 5179.28:1195.22:19539
In AP_AHRS_NavEKF::writeOptFlowMeas q x y t: 132 : 10358.6:-15537.8:19539
In AP_AHRS_NavEKF::writeOptFlowMeas GLOBAL q x y t: 132 : 5179.28:1195.22:19539
In AP_AHRS_NavEKF::writeOptFlowMeas GLOBAL q x y t: 132 : 10358.6:-15537.8:19539
NavEKF Q filter pass 199 : 0.000000 : 0.000000


Ok so now I'm stumped. Here are some guesses.

Something dumb in my debug approach perhaps? Likely.
Or:
Stack frame issue, interrupt issue, thread issue, race type condition? Not so likely given the extern test and the application structure.
Memory management issue in the Emlid Linux kernel? Possible.

Unfortunately it looks like I'm going to have to move to yet another platform unless one of you clever people out there can find/suggest a solution.

Regards,

Martin
 


On Tuesday, 28 July 2015 14:42:36 UTC+1, MartinSnaith wrote:

[MartinSnaith]

Hello,

In order to sanity check my findings using the Navio+ I substituted a Pixhawk board on the same copter with the same sensors.

After a bit of setup following the appropriate wiki, which mainly involved waiting for the logs to down load, the sensor produced the following setup data:

A series of simple test flights produced a stable hover and a controllable Loiter in a wooded environment with varying sun and 

dappled shade.

Only real issue is:

Compass variance needs reset before each flight, I'll add an external compass for the next series of tests.

Interestingly the lidar rangefinder showed none of the outliers found in the Navio tests.

Randy has just posted a fix for the optic flow sensor on the Linux ports (Hurrah!) so I'll try that on the Navio test rig and post the results.

 

On hols for a few weeks now.

Regards,

Martin

[Randy Mackay]

     I haven't flight tested the NAVIO+ with OptFlow but during bench tests I also saw the somewhat crazy looking values appearing although I saw the values in the MP's status screen (they appear as opt_m_x, opt_m_y, opt_x, opt_y and opt_qua)... so I think there is indeed some corruption going on somewhere.

-Randy

On Saturday, August 8, 2015 5:50 AM, MartinSnaith <martin...@pridepath.co.uk> wrote:

Hello,

In order to sanity check my findings using the Navio+ I substituted a Pixhawk board on the same copter with the same sensors.

After a bit of setup following the appropriate wiki, which mainly involved waiting for the logs to down load, the sensor produced the following setup data:

A series of simple test flights produced a stable hover and a controllable Loiter in a wooded environment with varying sun and 

dappled shade.

Only real issue is:

Compass variance needs reset before each flight, I'll add an external compass for the next series of tests.

Interestingly the lidar rangefinder showed none of the outliers found in the Navio tests.

Randy has just posted a fix for the optic flow sensor on the Linux ports (Hurrah!) so I'll try that on the Navio test rig and post the results.

 

On hols for a few weeks now.

Regards,

Martin

On Tuesday, July 28, 2015 at 2:42:36 PM UTC+1, MartinSnaith wrote:

Hello,

Randy Mackay suggested this work would be of interest to the developers group.

In moving a research system to a Navio+ Raspberry Pi 2 platform things have gone well until I got to the Pulsedlight Lidar 

Lite sensor and the PX4Flow sensor. The Lidar now works acceptably once the i2c bus speed was reduced to 100kHz. Please see 

http://community.emlid.com/t/ lidarlite/457/7 for details. 

http://community.emlid.com/t/ lidarlite/457/7 for details. 

http://community.emlid.com/t/ lidarlite/457/7 for details. 

--
You received this message because you are subscribed to the Google Groups "drones-discuss" group.
To unsubscribe from this group and stop receiving emails from it, send an email to drones-discus...@googlegroups.com.
For more options, visit https://groups.google.com/d/optout.