ArduPlane patches for FENCE_ACTION DisarmMotor and GPS_FAIL

ArduPlane Patches for FENCE_ACTION DisarmMotor and GPS_FAIL_ACTION

Patch: Add FENCE_ACTION DisarmMotor and new FENCE_AUTOENABLE options

I've been experimenting with APM:Plane autonomous flight using a lightweight foam airplane, and was having good results until one day I had a fly-away. The plane could not complete one of its turns in the mission pattern, and instead flew off in a slow arc. Part of the mission had the plane going inverted and doing rolls, so one possible explanation is that the accelerometer lost its zero-level position. Other possibilities are that the flight-controller board mount came loose, or there was some kind of mechanical issue with the control surfaces.

Whatever the cause, it's clear that the flight controller did not have a good fallback response to the sensor and GPS input, which was showing that the plane was way off course. So, I've been working on modifications to improve this, and this "FenceActionDisarm" patch is one of two that I've implemented.

During the fly-away, the GPS position continued to be tracked and received via telemetry (see above pic). If a geo-fence was in place, a breach would have been detected and the plane would have attempted to fly back to the return point, but it was already attempting (and failing) to fly back, so this wouldn't have helped. What I see as needed is a new FENCE_ACTION option that can shut off the motor -- this can at least down the plane before it flies further away.

This patch adds a new FENCE_ACTION action called "DisarmMotor", where, after a geo-fence breach lasting more than five seconds is detected, the flight mode is changed to STABILIZED and the motor is disarmed. The idea is to have the plane glide down to the ground as gently as possible.

When testing, I found that I really wanted options for having the geo-fence be automatically enabled when entering AUTO mode or when arming the motor, so I added the following options to the FENCE_AUTOENABLE setting: AutoModeEnable, AutoModeEnableDisableFloorOnly, and MotorArmEnable.

I've successfully flown and tested this patch on my lightweight foam airplane. This patch has been submitted as diydrones/ardupilot Pull Request #3385. Below is a doc update.

----

Action on geofence breach (ArduPlane:FENCE_ACTION)

What to do on fence breach. If this is set to 0 then no action is taken, and geofencing is disabled. If this is set to 1 then the plane will enter GUIDED mode, with the target waypoint as the fence return point. If this is set to 2 then the fence breach is reported to the ground station, but no other action is taken. If set to 3 then the plane enters guided mode but the pilot retains manual throttle control. If set to 4, after a fence breach lasting more than five seconds the flight mode is changed to STABILIZED and the motor is disarmed (glide to ground).

Value     Meaning
    0     None
    1     GuidedMode
    2     ReportOnly
    3     GuidedModeThrPass
    4     DisarmMotor

----

Fence automatic enable (ArduPlane:FENCE_AUTOENABLE)

When set to 1, geofence automatically enables after an auto takeoff and automatically disables at the beginning of an auto landing. When on the ground before takeoff the fence is disabled. When set to 2, the fence autoenables after an auto takeoff, but only disables the fence floor during landing. When set to 3, geofence automatically enables after the AUTO flight mode is engaged and automatically disables at the beginning of an auto landing. When set to 4, the fence autoenables after the AUTO flight mode is engaged, but only disables the fence floor during landing. When set to 5, geofence enables after the motor is armed (and does not automatically disable later). It is recommended to not use this option for line of sight flying and use a fence enable channel instead.

Value     Meaning
    0     NoAutoEnable
    1     AutoTakeoffEnable
    2     AutoTakeoffEnableDisableFloorOnly
    3     AutoModeEnable
    4     AutoModeEnableDisableFloorOnly
    5     MotorArmEnable

Patch: Add GPS_FAIL_ACTION and XTRACK_FAIL_LIM parameters

This "GPSFailHandler" patch is the second of two patches I've implemented for better APM:Plane handling of possible fly-away situations. In tests that I've done with the current code, if the GPS fails (i.e., broken wire, lost fix) when flying in a GPS-dependent mode, the plane's heading becomes uncontrolled.

To address this, I've implemented a new parameter called GPS_FAIL_ACTION, which sets the action to be taken when a GPS failure is detected while in the AUTO, GUIDED, RTL or LOITER flight mode. A GPS failure is when its data connection or its fix is lost for more than five seconds. The available options are: Enter the CIRCLE flight mode (indefinitely), enter the CIRCLE flight mode for a period of time and then disarm the motor, or disarm the motor immediately after a GPS failure is detected (after the five seconds). While in CIRCLE mode, if a GPS fix is regained then the previous flight mode is restored.

When I had a fly-away while experimenting with APM:Plane autonomous flight, I noticed in the telemetry data that the 'xtrack_error' values clearly indicated when the problem occurred (see above pic). To make use of this, I've also implemented a new parameter called XTRACK_FAIL_LIM: When this setting is greater than zero, the fight mode is AUTO, GUIDED, RTL or LOITER, and the navigation crosstrack error (xtrack_error) is larger than this setting for more than five seconds, the action configured by GPS_FAIL_ACTION will be taken. In my testing I found that an XTRACK_FAIL_LIM value of 200 is effective for detecting failures.

I've successfully flown and tested this patch on my lightweight foam airplane. This patch has been submitted as diydrones/ardupilot Pull Request #3386. Below is a doc update.

For those who would like to test GPS failures, below is code that I used at the top of the 'update_GPS_10Hz()' function in the "ArduPlane.cpp" file. When the RC channel 5 input is less than 1100, the GPS data connection is disabled:

    // *** temp DEBUG test ***

    static bool last_dtstflg = (hal.rcin->read(4)
> 500 && hal.rcin->read(4) < 1100);

    bool dtstflg = (hal.rcin->read(4) > 500
&& hal.rcin->read(4) < 1100);

    if (dtstflg != last_dtstflg) {

        gps.lock_port(0, dtstflg);

        last_dtstflg = dtstflg;

        if (dtstflg) {

           
gcs_send_text(MAV_SEVERITY_WARNING,
                                           
"DEBUG gps-fail simulate FAIL");

        } else {

           
gcs_send_text(MAV_SEVERITY_WARNING,

                                           
"DEBUG gps-fail simulate OK");

        }

    }

----

Action on GPS Fail (ArduPlane:GPS_FAIL_ACTION)

Action to be taken when a GPS failure is detected while in the AUTO, GUIDED, RTL or LOITER flight mode. A GPS failure is when its fix is lost for more than five seconds. If 0 then no action. If 1 then change to CIRCLE flight mode (and restore the previous flight mode if a GPS fix is regained). If 2 through 7 then change to CIRCLE flight mode for the given time, and then, if a GPS fix has not been regained, disarm the motor. If a GPS fix is regained then the previous flight mode is restored. If 8 then change to STABILIZED flight mode and disarm the motor immediately (glide to ground).

Value    Meaning
    0    NoAction
    1    Circle
    2    Circle5SecDisarm
    3    Circle10SecDisarm
    4    Circle30SecDisarm
    5    Circle1MinDisarm
    6    Circle2MinDisarm
    7    Circle5MinDisarm
    8    DisarmMotor

----

Crosstrack Failure Limit (ArduPlane:XTRACK_FAIL_LIM)
When this setting is greater than zero, the fight mode is AUTO, GUIDED, RTL or LOITER, and the navigation crosstrack error (xtrack_error) is larger than this setting for more than five seconds, the action configured by GPS_FAIL_ACTION will be taken. The setting is specified in meters. A large crosstrack error can indicate sensor or mechanical failure. A value of 200 (meters) is effective for detecting failures.

These files are for a build with both patches applied to the APM:Plane v3.4.0 release code:
Firmware file for PX4-V2 (Pixhawk) boards: ArduPlane-3.4.0rel_etFenceGPSfHdlr_px4-v2_20151224.px4
Firmware file for PX4-V1 (PX4MiniAIO) boards: ArduPlane-3.4.0rel_etFenceGPSfHdlr_px4-v1_20151224.px4
Complete source code fileset: ArduPlane-3.4.0rel_etFenceGPSfHdlr_20151224.zip
Code on GitHub: https://github.com/ethomas997/ardupilot/tree/ArduPlane-3.4.0rel_etFenceGPSfHdlr

These files are for a build with both patches applied to the APM:Plane v3.5.0beta1 code:
Firmware file for PX4-V2 (Pixhawk) boards: ArduPlane-3.5.0beta1_etFenceGPSfHdlr_20160123_px4v2.px4
Firmware file for PX4-V1 (PX4MiniAIO) boards: ArduPlane-3.5.0beta1_etFenceGPSfHdlr_20160123_px4v1.px4
Complete source code fileset: ArduPlane-3.5.0beta1_etFenceGPSfHdlr_20160123.zip
Code on GitHub: https://github.com/ethomas997/ardupilot/tree/ArduPlane-3.5.0beta1_etFenceGPSfHdlr

The firmware files may be uploaded to the flight controller using the Mission Planner (under "Install Firmware" | "Load custom firmware").

Note: These firmware files should be considered beta-test releases and are to be used at your own risk

If you have questions/comments or find these patches useful, please let me know.

Click here to contact me

Back to etheli.com home page