Gimbal limits with SToRM32 backend (MAVLink) not applied correctly?

VDLJu · January 23, 2020, 7:28am

Hi,

I’m having trouble with a MAVLink controlled gimbal (basecamelectronics firmware) with AC4.0 where the applied angle commands from RC are limited to a wrong range. Same setup works well on AC3.6.12.

The parameters are following:
MTN_ANGMIN_TILT = -9000 (-90 degrees)
MTN_ANGMAX_TILT = 1000 (+10 degrees)
MNT_NEUTRAL_X = 0
MNT_NEUTRAL_Y = 0
MNT_NEUTRAL_Z = 0

Software limits on gimbal:
Tilt axis low: -90 degrees
Tilt axis high: +15degrees

On 3.6.X the gimbal works like expected with these settings mentioned above. On tilt axis, it can point -90 degrees down and +10 degrees up.

On 4.0 the gimbal range is limited to -90 deg to -40 deg with these same settings. Note that I don’t know if the autopilot is commanding angles lower than -90 deg as it is limited by the gimbal software and hardware.

Using MTN_ANGMAX_TILT = 11000 (+110 degrees) and MTN_ANGMIN_TILT = -9000 (-90 degrees) gives the wanted control range of -90 to +10 degrees.

VDLJu · January 23, 2020, 8:41am

I checked the MAVLink messages with Qgroundcontrol MAVLink inspector and found that MOUNT_STATUS message, which contains the commanded angles, has limited range.

It should have the range from -90 to +10 degrees, but the commanded range is only -90 to -40 degrees.

29963a266631344704af3d2ed9badc73f259634ffdc9c8a5e3eb1f96c91b0e99

This message is made here:

github.com

ArduPilot/ardupilot/blob/master/libraries/AP_Mount/AP_Mount_SToRM32.cpp#L107


    }


    // record the mode change
    _state._mode = mode;
}


// send_mount_status - called to allow mounts to send their status to GCS using the MOUNT_STATUS message
void AP_Mount_SToRM32::send_mount_status(mavlink_channel_t chan)
{
    // return target angles as gimbal's actual attitude.  To-Do: retrieve actual gimbal attitude and send these instead
    mavlink_msg_mount_status_send(chan, 0, 0, ToDeg(_angle_ef_target_rad.y)*100, ToDeg(_angle_ef_target_rad.x)*100, ToDeg(_angle_ef_target_rad.z)*100);
}


// search for gimbal in GCS_MAVLink routing table
void AP_Mount_SToRM32::find_gimbal()
{
    // return immediately if initialised
    if (_initialised) {
        return;
    }

And the following angle set here on RC control:

github.com

ArduPilot/ardupilot/blob/master/libraries/AP_Mount/AP_Mount_Backend.cpp#L130


        rate_input_rad(_angle_ef_target_rad.z,
                       pan_ch,
                       _state._pan_angle_min,
                       _state._pan_angle_max);
    } else {
        // allow pilot rate input to come directly from an RC_Channel
        if ((roll_ch != nullptr) && (roll_ch->get_radio_in() != 0)) {
            _angle_ef_target_rad.x = angle_input_rad(roll_ch, _state._roll_angle_min, _state._roll_angle_max);
        }
        if ((tilt_ch != nullptr) && (tilt_ch->get_radio_in() != 0)) {
            _angle_ef_target_rad.y = angle_input_rad(tilt_ch, _state._tilt_angle_min, _state._tilt_angle_max);
        }
        if ((pan_ch != nullptr) && (pan_ch->get_radio_in() != 0)) {
            _angle_ef_target_rad.z = angle_input_rad(pan_ch, _state._pan_angle_min, _state._pan_angle_max);
        }
    }
}


// returns the angle (radians) that the RC_Channel input is receiving
float AP_Mount_Backend::angle_input_rad(const RC_Channel* rc, int16_t angle_min, int16_t angle_max)
{

Can anyone spot where the cause for the problem is?

VDLJu · January 23, 2020, 9:39am

I have replicated the function on this line and got following results that match the Gimbal output I have got:

github.com

ArduPilot/ardupilot/blob/master/libraries/AP_Mount/AP_Mount_Backend.cpp#L141


        }
        if ((pan_ch != nullptr) && (pan_ch->get_radio_in() != 0)) {
            _angle_ef_target_rad.z = angle_input_rad(pan_ch, _state._pan_angle_min, _state._pan_angle_max);
        }
    }
}


// returns the angle (radians) that the RC_Channel input is receiving
float AP_Mount_Backend::angle_input_rad(const RC_Channel* rc, int16_t angle_min, int16_t angle_max)
{
    return radians(((rc->norm_input() + 1.0f) * 0.5f * (angle_max - angle_min) + angle_min)*0.01f);
}


bool AP_Mount_Backend::calc_angle_to_roi_target(Vector3f& angles_to_target_rad,
                                                bool calc_tilt,
                                                bool calc_pan,
                                                bool relative_pan) const
{
    if (!_state._roi_target_set) {
        return false;
    }

Here the angle_max=10 and angle_min=-90 which gives us a range of -0.9 to -0.4 -> -90 to -40deg

Here the angle_max=110 and angle_min=-90 which gives us a range of -0.9 to 0.1 -> -90 to 10deg

So the problem is in that function.

rmackay9 · January 24, 2020, 9:20am

Great, thanks for the report and investigation. I hope to investigate this next week.

peterbarker · January 30, 2020, 2:06am

@VDLJu Thanks for the analysis.

Just starting to look at this.

First thing I’d note is that that range on f appears to be incorrect. You need to remove the *0.5 or change the range from 0 to 2. f here is (rc->norm_input() + 1.0f) and given norm_input returns -1 to 1, the range is 0-2.

Some parenthesis in the original code might have made it a bit clearer what was going on (changing a range of -1 to 1 to a range of 0-1).

It would be nice to see a log with the problem happening - particularly it would be nice to see that your RC inputs are meeting the RCn_MIN and RCn_MAX values specified in the parameters.

peterbarker · January 30, 2020, 3:12am

I’ve added a test for your specific case here: https://github.com/ArduPilot/ardupilot/pull/13424/files#diff-205ba7eeea0bd19f1af46b8419add13eR3385

Current code does seem to give the correct numbers there - note that this is a servo mount rather than a mavlink backend, 'though!

I’m leaning towards the problem being on the output side rather than the input side at the moment.

Those logs would be handy

VDLJu · January 31, 2020, 8:35am

@peterbarker Thanks for looking into this.

I’m quite busy right now and not able to get you the needed logs.
I’ll get back to this as soon as possible, probably at beginning of next week.

VDLJu · February 3, 2020, 9:04am

Hi @peterbarker,

Here are logs with both ANGXXX_TILT settings:

MTN_ANGMIN_TILT = -9000 gives -90 degrees
MTN_ANGMAX_TILT = 11000 gives +10 degrees
2020-02-03 10-23-02_wrong_limits_work.bin (298.4 KB)

MTN_ANGMIN_TILT = -9000 gives -90 degrees
MTN_ANGMAX_TILT = 1000 gives -40 degrees
2020-02-03 10-25-08_correct_limits_not_working.bin (439.5 KB)

I’m not aware of a way to see the mavlink output from the logs. Let me know if there is a way.

VDLJu · February 3, 2020, 9:10am

Just making it clear that Qgroundcontrol MAVLink inspector shows that MAVLink output is not correct.

rmackay9 · February 22, 2020, 1:00am

@VDLJu,

I had a quick look at reproducing this and I think the issue exists in Copter-3.6.12 as well. So not saying that it’s not an issue but just that it’s perhaps not a regression.

In the screen shot below you can see that I’ve got Copter-3.6.12 on the vehicle (a CubeBlack not that it matters) and the MNT_ANGMAX_TIL is set to 45deg but I’ve managed to set the target to 80deg using a mission command

Here’s a screen shot of the command just FYI:

I’m still investigating though…

rmackay9 · February 22, 2020, 1:33am

@VDLJu,

I think I’ve discovered the issue that you’ve hit. It is caused by this PR which had the side effect of adding a requirement that the RCx_TRIM be set correctly. Looking at your logs it seems like the RC7_TRIM is set to the top of the range.

I will likely revert the portion of the above change that added the dependency on the RCx_TRIM value.

Thanks for the report!

rmackay9 · February 22, 2020, 5:18am

@VDLJu,

I’ve reproduced the issue you were seeing and I’ve created a PR here which removes the reliance on the RCx_TRIM value. This will be peer reviewed and then I expect we can release it with Copter-4.0.4 that should start beta testing in about a month.