Setting takeoff altitude in FlightPlan

Hi!

It will be great if you’ll add the possibility to set takeoff altitude in FlightPlan [as standard mavlink cmd MAV_CMD_NAV_TAKEOFF works]

It is often more safe to make takeoff at altitude equal to first waypoint altitude. Currently if first WP alt if far away from takeoff position UAV will takeoff at 1,5 meters and then go directly towards first WP. There may be obstacles on its way. So controlling takeoff altitude allows GCS to avoid such situation.

Sure we can add “virtual” WP at takeoff location - but it is less reliable scenario.

Don’t get me wrong, I think this is a good feature request. It would be nice to have an argument for it in MAV_CMD_NAV_TAKEOFF.

But I don’t think that’s how the drone works today. Take-off altitude is hard coded.

What you are really asking for is exactly what you call “less reliable”. I’m not sure what would make a waypoint at the take off coordinates specifying the desired altitude to be less reliable.

Sure, it’s an additional step coding wise, but the net result would always be the same.

You may upload FP, then perform manual taking off, move drone away from taking off location and execute FP. In case of virtual WP, drone will return to take off location.

But the better behavior is when UAV performs taking off directly at current position. By “taking off” i mean not only taking off from the ground, but also ascending to specified altitude if drone already in the air). This is how Ardupilot and PX4 firmwares work, for example. But not DJI. :slight_smile:

I haven’t tried this myself but ardupilot interprets MAV_CMD_NAV_WAYPOINT with ZERO for lat / lon with a non-zero altitude value as an altitude only command.
https://ardupilot.org/copter/docs/common-mavlink-mission-command-messages-mav_cmd.html#mav-cmd-nav-waypoint

This is not covered in the mavlink spec itself though. https://mavlink.io/en/messages/common.html#MAV_CMD_NAV_WAYPOINT

It may be worthwhile trying this in sphynx. I would be hesitant to attempt it with a physical drone as a first time try. You may end up with a drone traveling to the Atlantic Ocean off the western coast of Africa, doh.

zero-zero

Intrigued by this topic I decided to do some tests today by inserting a 0/0 lat/lon waypoint command following take-off in sphinx using a simulated Anafi:

QGC WPL 120
0 0 3 178 1.000000 10.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1
1 0 3 22 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1
2 0 3 16 0.000000 5.000000 0.000000 264.997181 0.000000 0.000000 50.000000 1
3 0 3 50000 0.000000 -1.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1
4 0 3 16 0.000000 5.000000 0.000000 264.997181 40.205437 -75.639188 0.967348 1
5 0 3 16 0.000000 5.000000 0.000000 264.997181 40.205437 -75.639192 10.000000 1
6 0 3 16 0.000000 5.000000 0.000000 1.649133 40.205437 -75.639192 10.000000 1
7 0 3 16 0.000000 5.000000 0.000000 1.649133 40.205570 -75.639187 10.000000 1

This is what I got:

2020-08-22 13:14:09.400 D/arsdkcore_command: >> Common.Mavlink.Start | Filepath='abca3aaee78777b56f00f50ced8b35aa' | Type=FLIGHTPLAN
2020-08-22 13:14:09.505 D/arsdkcore_command: << Common.FlightPlanState.ComponentStateListChanged | Component=GPS | State=1
2020-08-22 13:14:09.834 D/arsdkcore_command: << Common.FlightPlanState.ComponentStateListChanged | Component=CALIBRATION | State=1
2020-08-22 13:14:09.834 D/arsdkcore_command: << Common.FlightPlanState.ComponentStateListChanged | Component=TAKEOFF | State=1
2020-08-22 13:14:09.834 D/arsdkcore_command: << Common.FlightPlanState.AvailabilityStateChanged | AvailabilityState=1
2020-08-22 13:14:09.834 D/arsdkcore_command: << Common.MavlinkState.MavlinkFilePlayingStateChanged | State=PLAYING | Filepath='abca3aaee78777b56f00f50ced8b35aa' | Type=FLIGHTPLAN
2020-08-22 13:14:09.834 D/arsdkcore_command: << Common.FlightPlanState.ComponentStateListChanged | Component=GPS | State=1
2020-08-22 13:14:09.834 D/arsdkcore_command: << Common.FlightPlanState.ComponentStateListChanged | Component=CALIBRATION | State=1
2020-08-22 13:14:09.834 D/arsdkcore_command: << Common.FlightPlanState.ComponentStateListChanged | Component=TAKEOFF | State=1
2020-08-22 13:14:09.834 D/arsdkcore_command: << Common.FlightPlanState.AvailabilityStateChanged | AvailabilityState=1
2020-08-22 13:14:09.834 D/arsdkcore_command: << Common.MavlinkState.MissionItemExecuted | Idx=0
2020-08-22 13:14:09.834 D/arsdkcore_command: << Common.MavlinkState.MissionItemExecuted | Idx=1
2020-08-22 13:14:09.834 D/arsdkcore_command: << Common.FlightPlanState.ComponentStateListChanged | Component=MAVLINK_FILE | State=1
2020-08-22 13:14:09.834 D/arsdkcore_command: << Common.FlightPlanState.ComponentStateListChanged | Component=WAYPOINTSBEYONDGEOFENCE | State=1
2020-08-22 13:14:09.834 D/arsdkcore_command: << Common.MavlinkState.MavlinkFilePlayingStateChanged | State=PLAYING | Filepath='abca3aaee78777b56f00f50ced8b35aa' | Type=FLIGHTPLAN
2020-08-22 13:14:09.834 D/arsdkcore_command: << Common.FlightPlanState.ComponentStateListChanged | Component=GPS | State=1
2020-08-22 13:14:09.834 D/arsdkcore_command: << Common.FlightPlanState.ComponentStateListChanged | Component=CALIBRATION | State=1
2020-08-22 13:14:09.834 D/arsdkcore_command: << Common.FlightPlanState.ComponentStateListChanged | Component=TAKEOFF | State=1
2020-08-22 13:14:09.835 D/arsdkcore_command: << Common.FlightPlanState.AvailabilityStateChanged | AvailabilityState=1
2020-08-22 13:14:09.835 D/arsdkcore_command: << Common.MavlinkState.MavlinkFilePlayingStateChanged | State=PLAYING | Filepath='abca3aaee78777b56f00f50ced8b35aa' | Type=FLIGHTPLAN
2020-08-22 13:14:09.835 D/arsdkcore_command: << Common.FlightPlanState.ComponentStateListChanged | Component=GPS | State=1
2020-08-22 13:14:09.835 D/arsdkcore_command: << Common.FlightPlanState.ComponentStateListChanged | Component=CALIBRATION | State=1
2020-08-22 13:14:09.835 D/arsdkcore_command: << Common.FlightPlanState.ComponentStateListChanged | Component=TAKEOFF | State=1
2020-08-22 13:14:09.835 D/arsdkcore_command: << Common.FlightPlanState.AvailabilityStateChanged | AvailabilityState=1
2020-08-22 13:14:09.835 D/arsdkcore_command: << Ardrone3.PilotingState.FlyingStateChanged | State=FLYING
2020-08-22 13:14:09.835 D/arsdkcore_command: << Common.MavlinkState.MavlinkFilePlayingStateChanged | State=STOPPED | Filepath='abca3aaee78777b56f00f50ced8b35aa' | Type=FLIGHTPLAN
2020-08-22 13:14:09.835 D/arsdkcore_command: << Common.FlightPlanState.ComponentStateListChanged | Component=GPS | State=1
2020-08-22 13:14:09.835 D/arsdkcore_command: << Common.FlightPlanState.ComponentStateListChanged | Component=CALIBRATION | State=1
2020-08-22 13:14:09.835 D/arsdkcore_command: << Common.FlightPlanState.ComponentStateListChanged | Component=TAKEOFF | State=1
2020-08-22 13:14:09.835 D/arsdkcore_command: << Common.FlightPlanState.AvailabilityStateChanged | AvailabilityState=1
2020-08-22 13:14:09.835 D/arsdkcore_command: << Common.FlightPlanState.ComponentStateListChanged | Component=GPS | State=1
2020-08-22 13:14:09.835 D/arsdkcore_command: << Common.FlightPlanState.ComponentStateListChanged | Component=CALIBRATION | State=1
2020-08-22 13:14:09.835 D/arsdkcore_command: << Common.FlightPlanState.ComponentStateListChanged | Component=TAKEOFF | State=1
2020-08-22 13:14:09.835 D/arsdkcore_command: << Common.FlightPlanState.AvailabilityStateChanged | AvailabilityState=1
2020-08-22 13:14:09.835 D/arsdkcore_command: << Ardrone3.PilotingState.FlyingStateChanged | State=HOVERING

This entry got my attention so I verified geofence is disabled (which it was):

`2020-08-22 13:14:09.834 D/arsdkcore_command: << Common.FlightPlanState.ComponentStateListChanged | Component=WAYPOINTSBEYONDGEOFENCE | State=1`

Upon removing the 0/0 lan/lon waypoint, I re-executed the plan and still got the WAYPOINTSBEYONDGEOFENCE component state so I’m guessing its anemic.

QGC WPL 120
0 0 3 178 1.000000 10.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1
1 0 3 50000 0.000000 -1.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1
2 0 3 16 0.000000 5.000000 0.000000 179.952404 40.205570 -75.639192 10.001607 1
3 0 3 16 0.000000 5.000000 0.000000 179.952404 40.205437 -75.639192 10.000000 1
4 0 3 16 0.000000 5.000000 0.000000 1.649133 40.205437 -75.639192 10.000000 1
5 0 3 16 0.000000 5.000000 0.000000 1.649133 40.205570 -75.639187 10.000000 1

resulted in:

2020-08-22 13:33:32.957 D/arsdkcore_command: >> Common.Mavlink.Start | Filepath='52180845d9dfa813465c85d413857248' | Type=FLIGHTPLAN
2020-08-22 13:33:32.959 D/arsdkcore_command: << Common.FlightPlanState.ComponentStateListChanged | Component=GPS | State=1
2020-08-22 13:33:33.311 D/arsdkcore_command: << Common.FlightPlanState.ComponentStateListChanged | Component=CALIBRATION | State=1
2020-08-22 13:33:33.311 D/arsdkcore_command: << Common.FlightPlanState.ComponentStateListChanged | Component=TAKEOFF | State=1
2020-08-22 13:33:33.311 D/arsdkcore_command: << Common.FlightPlanState.AvailabilityStateChanged | AvailabilityState=1
2020-08-22 13:33:33.311 D/arsdkcore_command: << Common.MavlinkState.MavlinkFilePlayingStateChanged | State=PLAYING | Filepath='52180845d9dfa813465c85d413857248' | Type=FLIGHTPLAN
2020-08-22 13:33:33.311 D/arsdkcore_command: << Common.FlightPlanState.ComponentStateListChanged | Component=GPS | State=1
2020-08-22 13:33:33.311 D/arsdkcore_command: << Common.FlightPlanState.ComponentStateListChanged | Component=CALIBRATION | State=1
2020-08-22 13:33:33.311 D/arsdkcore_command: << Common.FlightPlanState.ComponentStateListChanged | Component=TAKEOFF | State=1
2020-08-22 13:33:33.311 D/arsdkcore_command: << Common.FlightPlanState.AvailabilityStateChanged | AvailabilityState=1
2020-08-22 13:33:33.311 D/arsdkcore_command: << Common.MavlinkState.MissionItemExecuted | Idx=0
2020-08-22 13:33:33.311 D/arsdkcore_command: << Common.MavlinkState.MissionItemExecuted | Idx=1
2020-08-22 13:33:33.311 D/arsdkcore_command: << Common.FlightPlanState.ComponentStateListChanged | Component=MAVLINK_FILE | State=1
2020-08-22 13:33:33.311 D/arsdkcore_command: << Common.FlightPlanState.ComponentStateListChanged | Component=WAYPOINTSBEYONDGEOFENCE | State=1
2020-08-22 13:33:33.311 D/arsdkcore_command: << Common.MavlinkState.MavlinkFilePlayingStateChanged | State=PLAYING | Filepath='52180845d9dfa813465c85d413857248' | Type=FLIGHTPLAN
2020-08-22 13:33:33.311 D/arsdkcore_command: << Common.FlightPlanState.ComponentStateListChanged | Component=GPS | State=1
2020-08-22 13:33:33.311 D/arsdkcore_command: << Common.FlightPlanState.ComponentStateListChanged | Component=CALIBRATION | State=1
2020-08-22 13:33:33.311 D/arsdkcore_command: << Common.FlightPlanState.ComponentStateListChanged | Component=TAKEOFF | State=1
2020-08-22 13:33:33.311 D/arsdkcore_command: << Common.FlightPlanState.AvailabilityStateChanged | AvailabilityState=1
2020-08-22 13:33:33.311 D/arsdkcore_command: << Common.MavlinkState.MavlinkFilePlayingStateChanged | State=PLAYING | Filepath='52180845d9dfa813465c85d413857248' | Type=FLIGHTPLAN
2020-08-22 13:33:33.311 D/arsdkcore_command: << Common.FlightPlanState.ComponentStateListChanged | Component=GPS | State=1
2020-08-22 13:33:33.311 D/arsdkcore_command: << Common.FlightPlanState.ComponentStateListChanged | Component=CALIBRATION | State=1
2020-08-22 13:33:33.311 D/arsdkcore_command: << Common.FlightPlanState.ComponentStateListChanged | Component=TAKEOFF | State=1
2020-08-22 13:33:33.311 D/arsdkcore_command: << Common.FlightPlanState.AvailabilityStateChanged | AvailabilityState=1
2020-08-22 13:33:33.311 D/arsdkcore_command: << Ardrone3.PilotingState.FlyingStateChanged | State=FLYING
2020-08-22 13:33:33.768 D/arsdkcore_command: << Common.MavlinkState.MissionItemExecuted | Idx=2
2020-08-22 13:33:41.096 D/arsdkcore_command: << Rth.Home_reachability | Status=UNKNOWN
2020-08-22 13:33:53.323 D/arsdkcore_command: << Common.MavlinkState.MissionItemExecuted | Idx=3
2020-08-22 13:33:53.643 D/arsdkcore_command: << Common.MavlinkState.MissionItemExecuted | Idx=4
2020-08-22 13:34:13.475 D/arsdkcore_command: << Rth.Home_reachability | Status=REACHABLE
2020-08-22 13:34:15.685 D/arsdkcore_command: << Common.MavlinkState.MissionItemExecuted | Idx=5
2020-08-22 13:34:16.011 D/arsdkcore_command: << Common.MavlinkState.MavlinkFilePlayingStateChanged | State=STOPPED | Filepath='52180845d9dfa813465c85d413857248' | Type=FLIGHTPLAN
2020-08-22 13:34:16.011 D/arsdkcore_command: << Common.FlightPlanState.ComponentStateListChanged | Component=GPS | State=1
2020-08-22 13:34:16.011 D/arsdkcore_command: << Common.FlightPlanState.ComponentStateListChanged | Component=CALIBRATION | State=1
2020-08-22 13:34:16.011 D/arsdkcore_command: << Common.FlightPlanState.ComponentStateListChanged | Component=TAKEOFF | State=1
2020-08-22 13:34:16.011 D/arsdkcore_command: << Common.FlightPlanState.AvailabilityStateChanged | AvailabilityState=1
2020-08-22 13:34:16.011 D/arsdkcore_command: << Common.FlightPlanState.ComponentStateListChanged | Component=GPS | State=1
2020-08-22 13:34:16.011 D/arsdkcore_command: << Common.FlightPlanState.ComponentStateListChanged | Component=CALIBRATION | State=1
2020-08-22 13:34:16.011 D/arsdkcore_command: << Common.FlightPlanState.ComponentStateListChanged | Component=TAKEOFF | State=1
2020-08-22 13:34:16.011 D/arsdkcore_command: << Common.FlightPlanState.AvailabilityStateChanged | AvailabilityState=1
2020-08-22 13:34:16.323 D/arsdkcore_command: << Ardrone3.PilotingState.FlyingStateChanged | State=HOVERING

Anyway, not much help here in the long run but I can confirm 0/0 lat/lon in a waypoint is a no go. Now I’m off to read the documentation related to component states to understand WAYPOINTSBEYONDGEOFENCE better.

Well that was easy:

WaypointsBeyondGeofence:
 	Component for waypoints beyond the geofence. State is 0 when one or more waypoints are beyond the geofence. (4)

I can also confirm that overriding write() in GSdk’s TakeOffCommand to force a non zero value for altitude has no effect. Unfortunately I think this takes us back to the original callout that Parrot’s mavlink implementation has no provision whatsoever for take off altitude and/or an altitude only command.

/*
 *     Copyright (C) 2019 Parrot Drones SAS
 *
 *     Redistribution and use in source and binary forms, with or without
 *     modification, are permitted provided that the following conditions
 *     are met:
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in
 *       the documentation and/or other materials provided with the
 *       distribution.
 *     * Neither the name of the Parrot Company nor the names
 *       of its contributors may be used to endorse or promote products
 *       derived from this software without specific prior written
 *       permission.
 *
 *     THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *     "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *     LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *     FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *     PARROT COMPANY BE LIABLE FOR ANY DIRECT, INDIRECT,
 *     INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *     BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 *     OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *     AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 *     OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
 *     OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
 *     SUCH DAMAGE.
 *
 */

package com.parrot.drone.groundsdk.mavlink;

import java.io.IOException;
import java.io.Writer;

import androidx.annotation.NonNull;

/**
 * MAVLink command which allows to take off.
 */
public final class TakeOffCommand extends MavlinkCommand {

    /**
     * Constructor.
     */
    public TakeOffCommand() {
        super(Type.TAKE_OFF);
    }

    @Override
    public boolean equals(Object o) {
        return this == o || (o != null && getClass() == o.getClass());
    }

    @Override
    public int hashCode() {
        return mType.code();
    }

    @Override
    void write(@NonNull Writer writer, int index) throws IOException {
        write(writer, index, 0, 0, 0, 0, 0, 0, 50);
    }
}

.

QGC WPL 120

0 0 3 178 1.000000 10.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1
1 0 3 22 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 50.000000 1
2 0 3 50000 0.000000 -1.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1
3 0 3 16 0.000000 5.000000 0.000000 290.215592 40.205400 -75.639100 0.000000 1
4 0 3 16 0.000000 5.000000 0.000000 290.215592 40.205457 -75.639302 10.000000 1
5 0 3 16 0.000000 5.000000 0.000000 299.239697 40.205457 -75.639302 10.000000 1
6 0 3 16 0.000000 5.000000 0.000000 299.239697 40.205592 -75.639619 10.000000 1

I know I’m being extremely verbose today. My apologies, but I did want to circle back to my original comment, and your feedback that it is not always reliable.

Again, I completely acknowledge this puts the burden entirely on us as the developer, but we can work around this limitation by always capturing the drone’s current coordinates upon activating a flight plan.

This does have some caveats. Namely, you must always generate a new mavlink file and upload it as part of flight plan activation.

And as part of mavlink file generation, the very first thing you should do is capture the drone’s current location:

final LatLng origin = new LatLng(drone.getGps().lastKnownLocation().getLatitude(), drone.getGps().lastKnownLocation().getLongitude());
final double originAltitude = drone.getAltimeter().getTakeOffRelativeAltitude();

Subsequently you need to use this “origin” reference when seeding your loop for generating a mavlink file and use these reference points for your very first waypoint mission item.

I have an existing use case where I do this specifically to set an initial yaw angle, forcing the drone to face its first real waypoint as it travels to it. You could easily adapt logic of this nature to also force an initial altitude.

Given our discussion I am considering extending my current logic to also include the altitude of my first “real” waypoint as, to your point, it ensures the drone is at the target altitude of your first real waypoint prior to traveling to it.

Here’s a short video of this behavior in action (ala sphinx): https://photos.app.goo.gl/STcMSgffeMyq3DSP8

Hope this helps,
Shell

Hi @synman !

Thanks for tests!

Regarding geofences - looks like it works only if we have zero lat\lon waypoint in list. :slight_smile: All other cases are ignored for me (i even made test flight on real drone for that)

This does have some caveats. Namely, you must always generate a new mavlink file and upload it as part of flight plan activation.

Exactly. It is very easy to implement, but we decided not to implement this logic now.