MP tracker AZtoMAV/ELtoMAV discontinuities (SITL rover) SOLVED

Webillo · August 7, 2021, 11:16am

Testing in SITL on a different circuit:
CRUISE_SPEED=5 (too fast; waypoints missed)
CRUISE_SPEED=2

Twice per lap AZtoMAV/ELtoMAV go to zero and back to normal, moreless in front and near tracker location:

When that happens, the camera starts to be moved away, and the Pololu servo measurements show arrows and dots separated (thanks to the speed/acceleration servo limitations), that try to converge:

BTW1, on above simulations MissionPlanner.MainV2.comPort.MAV.cs.TrackerLocation.Alt is set with a script (4m above rover). I yet don’t see how to modify it on MP.

BTW2, it would be desirable to have servo outputs right after GPS lock.

MP build 1.3.7833.26333.

Michael_Oborne · August 8, 2021, 10:50am

there is a deadzone if you are within i think it was 5m of the base.

this was to prevent extreme movement of the tracker.

github.com

ArduPilot/MissionPlanner/blob/master/ExtLibs/ArduPilot/CurrentState.cs#L1483


        //DIR to Home

        var dstlon = TrackerLocation.Lng - lng; //OffSet_X

        var dstlat = (TrackerLocation.Lat - lat) * scaleLongUp; //OffSet Y

        var bearing = 90 + Math.Atan2(dstlat, -dstlon) * 57.295775; //absolut home direction

        if (bearing < 0) bearing += 360; //normalization

        //bearing = bearing - 180;//absolut return direction

        //if (bearing < 0) bearing += 360;//normalization



        var dist = DistToHome / multiplierdist;



        if (dist < 5)

            return 0;



        return (float)bearing;

    }

}



[DisplayText("Sonar Range (meters)")]

public float sonarrange

{

    get => (float)toDistDisplayUnit(_sonarrange);

github.com

ArduPilot/MissionPlanner/blob/master/ExtLibs/ArduPilot/CurrentState.cs#L1452


    }

}



[DisplayText("Elevation to Mav (deg)")]

public float ELToMAV

{

    get

    {

        var dist = DistToHome / multiplierdist;



        if (dist < 5)

            return 0;



        var altdiff = (float)(_altasl - TrackerLocation.Alt);



        var angle = (float)(Math.Atan(altdiff / dist) * MathHelper.rad2deg);



        return angle;

    }

}

Webillo · August 9, 2021, 6:57am

Thanks for the attention. Certainly, given the circuit dimensions, it happens 5m from tracker location (I supposed measured on the base).

So in essence:

public float ELToMAV
{
    get
    {
        var dist = DistToHome / multiplierdist;
        if (dist < 5)
            return 0;
...
        return angle;
...                
                    
public float AZToMAV
    {
        get
        {
...
    var dist = DistToHome / multiplierdist;

    if (dist < 5)
        return 0;
...
    return (float)bearing;
...

But if in return 0 that is interpreted as an angle there is certainly going to be a sudden camera movement from previous angles to 0, as can be seen in above videos (entering <5 and exiting
<5). The camera by itself often shows “Panning too fast” message.

I would prefer maintain angles (dead zone concept), or better simply rely on speed/acceleration servo movement limitations (suppress return 0), which works very well as can be seen in Pololu indications (dot slowly converging to arrow).

Webillo · August 11, 2021, 4:00pm

Lines commented in MP build 1.3.7893.12317. It works beautifully now:

No AZtoMAV/ELtoMAV discontinuities.