ArduPilot / ardupilot

ArduPlane, ArduCopter, ArduRover, ArduSub source
http://ardupilot.org/
GNU General Public License v3.0
10.98k stars 17.51k forks source link

Plane: Quadplane: `control_auto` in VTOL approach `should_run_motors` is always false #24878

Open prihex opened 1 year ago

prihex commented 1 year ago

IF YOU DON'T REMOVE THESE FOUR LINES, THEN YOUR REPORT WILL BE CLOSED AUTOMATICALLY Questions and user problems should be directed at the forum (http://discuss.ardupilot.org) Please be very sure you have found a bug when opening this issue If there was a previous discussion in the forum, link to it

Bug report

Issue details

In ArduPlane/quadplane.cpp:3105 in function void QuadPlane::control_auto(void), I noticed that should_run_motors variable is never set to true and remains false throughout its scope. Thus motors' desired spool state is never going to be THROTTLE_UNLIMITED when state is higher than QPOS_APPROACH, which leads to unexpected behavior according to previous codebase.

    if (poscontrol.get_state() > QPOS_APPROACH) {
        bool should_run_motors = false;

        // don't run the motors if in an arming delay
        if (plane.arming.get_delay_arming()) {
            should_run_motors = false;
        }

        // don't run motors if we are in the wait state for payload place
        if (motors->get_desired_spool_state() == AP_Motors::DesiredSpoolState::SHUT_DOWN &&
            plane.in_auto_mission_id(MAV_CMD_NAV_PAYLOAD_PLACE) &&
            poscontrol.get_state() == QPOS_LAND_COMPLETE) {
            should_run_motors = false;
        }

        if (should_run_motors) {
            set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
        }
    }

I think, when first defining should_run_motors, we should set it as true.

I'm curious about your opinions.

Version Plane-4.4.0, commit 710b94a8851d723a3293cc459b2bfa7d74e1759d

Platform [ ] All [ ] AntennaTracker [ ] Copter [x] Plane [ ] Rover [ ] Submarine

Airframe type Quadplane

Hardware type

Logs

IamPete1 commented 1 year ago

Well found, it looks like this regression came in with https://github.com/ArduPilot/ardupilot/pull/22673

IamPete1 commented 1 year ago

Did you find any flight issue because of this or just from code inspection?

In some code paths we set the spool state again, others we check the state for actions. Its not immediately clear that just reinstating the old behavior is the correct thing to do.

prihex commented 1 year ago

Did you find any flight issue because of this or just from code inspection?

My quadcopter crashed because one motor stopped during the landing phase in a previous beta release of Plane-4.4 when in POS1 state. I believe this issue was not the primary reason for the crash; instead, I suspect it was caused by an ESC desync problem. I discovered this issue during a random code inspection.

In some code paths we set the spool state again, others we check the state for actions. Its not immediately clear that just reinstating the old behavior is the correct thing to do.

I believe so. It wouldn't be appropriate to revert all the code changes. What I meant is that when encountering the old behavior, we can simply set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);. However, we can also address this issue by setting should_run_motors to true when first declared, which should resolve the problem. Just like the diff below.

    if (poscontrol.get_state() > QPOS_APPROACH) {
-       bool should_run_motors = false;
+       bool should_run_motors = true;

        // don't run the motors if in an arming delay
        if (plane.arming.get_delay_arming()) {
            should_run_motors = false;
        }

        // don't run motors if we are in the wait state for payload place
        if (motors->get_desired_spool_state() == AP_Motors::DesiredSpoolState::SHUT_DOWN &&
            plane.in_auto_mission_id(MAV_CMD_NAV_PAYLOAD_PLACE) &&
            poscontrol.get_state() == QPOS_LAND_COMPLETE) {
            should_run_motors = false;
        }

        if (should_run_motors) {
            set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
        }
    }