Set altitude target functionality

[xznhj8129]

This is an experimental addition to be able to set the navigation altitude target without RC. It's implemented as an MSP command, Programming operator and Mavlink (MAV_CMD_DO_CHANGE_ALTITUDE).

This isn't final, i'm showing the general concept, specifics like exact way to check for altitude control state and how to actually set the altitude target may be wrong; this is a first draft and no time to test immediately

How it works is a dual-purpose get/set MSP message MSP2_INAV_ALT_TARGET; empty request to get current target, data in request to set the new target. It only works if:

• Baro enabled
• Altitude is controlled by Nav (Alt hold, Poshold, WP, RTH)
• Not landing
• Valid altitude estimate

Dual purpose get/set MSP is a thing i'm not sure about, if we want to maintain seperate GET/SET messages or combine them as general rule.

We use geoAltitudeDatumFlag_e as a reference frame flag, so default is NAV_WP_TAKEOFF_DATUM (0).

bool navigationSetAltitudeTargetWithDatum(geoAltitudeDatumFlag_e datumFlag, int32_t targetAltitudeCm)
{
    const navigationFSMStateFlags_t stateFlags = navGetCurrentStateFlags();
    if (!(stateFlags & NAV_CTL_ALT) || (stateFlags & NAV_CTL_LAND) || navigationIsExecutingAnEmergencyLanding() || posControl.flags.estAltStatus == EST_NONE) {
        return false;
    }

    float targetAltitudeLocalCm;
    switch (datumFlag) {
    case NAV_WP_TAKEOFF_DATUM:
        targetAltitudeLocalCm = (float)targetAltitudeCm;
        break;
    case NAV_WP_MSL_DATUM:
        if (!posControl.gpsOrigin.valid) {
            return false;
        }
        targetAltitudeLocalCm = (float)(targetAltitudeCm - posControl.gpsOrigin.alt);
        break;
    case NAV_WP_TERRAIN_DATUM:
    default:
        return false;
    }

    updateClimbRateToAltitudeController(0.0f, targetAltitudeLocalCm, ROC_TO_ALT_TARGET);
    return true;
}

With the enum, we can set different altitude frames: ASL, Takeoff (default), or, in the future, AGL.

Programming Framework supports new logic operator LOGIC_CONDITION_SET_ALTITUDE_TARGET through the same navigation function, allowing you to do that too.

MAVLink also handles it with MAV_CMD_DO_CHANGE_ALTITUDE via COMMAND_INT with frame GLOBAL and GLOBAL_RELATIVE

Comments encouraged