What is limiting the maximum lean angle for position controller in non-GPS modes?

ArduCopter Copter 4.5
1

I am testing with optical flow (as well as with external velocity / position data fed through ODOMETRY messages). In SITL simulation, you can reproduce this problem when you add the virtual optical flow as described here: Adding Simulated Peripherals to sim_vehicle — Dev documentation and virtual rangefinder (on the same page; to allow altitudes above 50m, you can adjust SIM_SONAR_SCALE and RNGFND1_SCALING e.g. to 30 and RNGFND1_MAX_CM to 15000), EKF-Sources are configured for optical flow like:

  • EK3_SRC1_POSXY = 0 (None)
  • EK3_SRC1_VELXY = 5 (Optical Flow)
  • EK3_SRC1_POSZ = 1 (Baro)
  • EK3_SRC1_VELZ = 0 (None)
  • EK3_SRC1_YAW = 1 (Compass)

Now, when I set SIM_WIND_SPD = 5 (m/s) and SIM_WIND_TURB = 3 (m/s) and climb up to about 50m (or higher), the copter cannot hold it’s position in Loiter mode anymore and also drifts away from a desired path in AUTO mode.

As far as I could see, the maximum lean angle on pitch and roll seem to be limited to 10 degrees here. I checked a lot of params which may influence that behaviour but could not find the responsible one. ANGLE_MAX is set to 3000 (30°), PSC_ANGLE_MAX and LOIT_ANGLE_MAX are set to 0 (which should allow 2/3 of ANGLE_MAX, so 20° should be possible). WPNav speed is set to 1000 (10 m/s) and Loiter speed is set to 1250 cm. All these settings should allow to hold a position at the given simulated wind speed and turbulences.

In AltHold flight mode, the drone can be flown fast enough to counteract this wind and when EKF3 Sources are configured for GPS operation, it also can hold it’s position without limiting the lean angles to 10°.

2

Sorry for nudging you so boldly, @rmackay9, @Leonardthall and @tridge but this problem prevented me from achieving a better ranking than third place at one of the largest European competitions for GNSS-denied autonomous flights last week.

Like seen already in SITL simulation, also the real drone could not hold it’s position or continue following the planned track when the wind got stronger and lean angles larger 10° would have been required. This limit happens only, when optical flow or my own visual odometry (sending ODOMETRY messages for velocities and position) is used as EKF3 source. With manual stick commands or when switching to GPS as EKF3 source for velocities and position, larger lean angles (up to the configured limit) are usable. As written before, I could not find any parameter responsible for that limitation.

Since an increasing number of projects are now working on optical flow or visual odometry based solutions for flying outdoors without GNSS, this will certainly hinder others sooner or later.

1 Like
3

Can you share a log?

4

Here you find a log from a SITL flight, that demonstrates that behaviour:

This graph illustrates what has happened:

grafik
grafik1920×1141 153 KB

Explanation:
SITL quad was configured to use optical flow as EKF3 velocity source (using simulated optical flow and simulated rangefinder as explained already in first post), GPS was deactivated (GPS_TYPE = 0)
SIM_WIND_SPD was set to 5 m/s from 180 degree and the yaw of the drone is also set to 180 degrees.
A position was set using “Set EKF Origin here” on the map.

Now the drone was armed in ALTHOLD and after climbing manually controlled to ca. 7 meters, switched to Loiter. Here it could hold it’s position. Now, an AUTO mission was started where the drone climbs up to 50m on the same position. At around 30m the drone starts drifting away as the simulated wind gets already too strong at that height. The pitch angle is around -6 degree here (6 degree nose down against the wind). With the drone still climbing and drifting faster and faster, the pitch angle is not increased further, it stays fixed at 6 degrees (so I have to correct myself as I earlier assumed it was about 10 degrees).
Now, I took over manually by switching to ALTHOLD and back to LOITER and push the “forward” stick to manually fly back to the starting position. Here you can see, that the pitch angle increases up to 25° degrees, so there is no reason, why the drone leans to not more than 6° (or -6°) only when in Loiter or in Auto Mode. Then I switch to LAND Mode and interestingly, in this mode it is also able to use lean angles without the 6° limitation.

5

@Hacky,

Really sorry for the slow reply and the impact on your competition.

I think the issue is probably these lines in the position controller.

    float ahrsGndSpdLimit, ahrsControlScaleXY;
    AP::ahrs().getControlLimits(ahrsGndSpdLimit, ahrsControlScaleXY);

These were added many years ago to limit the maximum horizontal speed of the vehicle so that it doesn’t go beyond the limits of the optical flow sensor. I suspect there are better ways to handle the limitations of the flow sensor including warnings to the user if the sensors are being saturated. I think this saturation happens due to high rotation speeds of the vehicle and may not necessarily be directly related to the horizontal speed of the vehicle.

Maybe we could remove the limit or make them optional.

FYI @priseborough

6

Thank you Randy for looking inside.
The behaviour is the same, when I use “ExternalNav” (6) as EKF3 velocity source (using the ODOMETRY message), so it is not only limited to optical flow sensors (as the ODOMETRY message can also be generated by systems using completely different technologies for velocity measurement). My solution uses these ODOMETRY messages but I demonstrated it using the optical flow configuration here in this example as it shows the same behaviour and is easier to reproduce for anyone else.

It also does not depend on the ground speed limit (at least not on the WPNav Speed and the Loiter Speed limits) as it makes no difference, when I increase them. It really seems to be an angle limitation.

When you look at the log, there is also no angular rotation speed which could saturate. The vehicle just constantly leans to about 6° and not more.

I marked it with an orange circle in this graph:

graph
graph1920×1141 154 KB

You can see that the pitch angle increases during the climb as the simulated wind also increases depending on the altitude. Then, when the vehicle gets higher than 30m the (absolute) pitch angle does not increase anymore and stays constant at 6° (in that case -6°) and the vehicle starts drifting until I intercept manually.

7

Thanks for the extra info. I’m pretty sure the Control Limits change as the vehicle gets higher above the ground so I definitely think we should try removing those lines from the position controller and see if it resolves the issue.

8

Just to make sure, that we do not misunderstand each other:
In my opinion this has nothing to do directly with the altitude. I am just using the climb for demonstration because the wind simulation in SITL scales the wind with the altitude (up to 60m or whatever is configured as SIM_WIND_T_ALT), so I can see, at which altitude (and the wind speed on that altitude) the drone starts drifting.

9

Hi @Hacky,

Sorry I can’t help more directly, I’ve got an upcoming search and rescue competition and the developer conference that are taking up a lot of my time.

It would be good to try and determine if it is a estimation issue or a control issue. it is possible to see the simulated vehicle’s actual position by setting, “map setsimpos 1”. Two vehicles should appear on the map, one is the estimated position and the other is the actual position. If the two vehicles drift apart then it is an estimation issue (e.g. EKF) and not a control issue.

1 Like
10

I am pretty sure, it is a control issue, but in order to follow your advice, I would like to try the “map setsimpos 1” option. The problem is, that I have no idea, where to set this. It is no parameter in the SITL parameters configuration, no option I can find in the Mission Planner config screen, nothing that I can find as option, when I “right click” on the map, no command that the mavproxy map module seems to accept and also Google returns nothing usable when I search for that.

So, could you please give a little more information, where this option can be set?

11

Hi @Hacky,

Ah, “map setsimpos 1” can be entered into the MAVProxy terminal when mavproxy is started from the command line. I don’t think that terminal is available when using MP’s built-in simulator although it is the same code.

12

Looks like you meant “map set showsimpos 1” in mavproxy.

2 Likes
13

OK, by using “map set showsimpos 1” in mavproxy console, I can see two vehicles at the same position. They stick together all the time, also when the vehicle starts drifting due to the lean angle limitation.

However, I don’t see this as proof that it is an “estimation issue”. Rather, it shows that the estimation is correct, but the control is not able to prevent the drone from drifting - precisely because the maximum lean angle is not sufficient for this.

I think, it has something to do with the code some lines below where you already pointed to.
I mean at line 675ff ardupilot/libraries/AC_AttitudeControl/AC_PosControl.cpp at master · ArduPilot/ardupilot · GitHub

    // Acceleration Controller

    // limit acceleration using maximum lean angles
    float angle_max = MIN(_attitude_control.get_althold_lean_angle_max_cd(), get_lean_angle_max_cd());
    float accel_max = angle_to_accel(angle_max * 0.01) * 100;
    // Define the limit vector before we constrain _accel_target 
    _limit_vector.xy() = _accel_target.xy();
    if (!limit_accel_xy(_vel_desired.xy(), _accel_target.xy(), accel_max)) {
        // _accel_target was not limited so we can zero the xy limit vector
        _limit_vector.xy().zero();
    } else {
        // Check for pitch limiting in the forward direction
        const float accel_fwd_unlimited = _limit_vector.x * _ahrs.cos_yaw() + _limit_vector.y * _ahrs.sin_yaw();
        const float pitch_target_unlimited = accel_to_angle(- MIN(accel_fwd_unlimited, accel_max) * 0.01f) * 100;
        const float accel_fwd_limited = _accel_target.x * _ahrs.cos_yaw() + _accel_target.y * _ahrs.sin_yaw();
        const float pitch_target_limited = accel_to_angle(- accel_fwd_limited * 0.01f) * 100;
        _fwd_pitch_is_limited = is_negative(pitch_target_unlimited) && pitch_target_unlimited < pitch_target_limited;
    }

    // update angle targets that will be passed to stabilize controller
    accel_to_lean_angles(_accel_target.x, _accel_target.y, _roll_target, _pitch_target);
    calculate_yaw_and_rate_yaw();
2 Likes
14

Hi @Hacky,

Yes, I agree that if the two vehicles stay together then it means that it is NOT an estimation issue but instead a control issue.

I can create a PR to allow disabling the limit (although I’m tempted to just remove it) and then maybe you could test it somehow? If you have a real vehicle you’d like to test on I can produce a binary for the autopilot (just tell me which one). Testing first in the simulator is easiest although that would require you loading my branch which takes some “git” skills. Of course I can likely reproduce your test fairly easily…

I’ve created an issue here

15

Hi Randy,
thanks for diving deeper into that issue.

As wheather is getting worse here and time is limited, I would prefer to test that in simulation. The good thing is, that it can be reproduced in simulation quite easily and approaches to solve it should show in simulation as well, if they work and what other impacts may be.

I am not an expert in git but for my daily development work, it’s enough :wink:
So let me know when and how I can test something.