// because MISSION_SET_CURRENT is a message not a command,
// there is not ACK associated with us successfully changing
// our waypoint. Some GCSs use the fact we return exactly the
// same mission sequence number in this packet as an ACK - so
// if they send a MISSION_SET_CURRENT with seq number of 4
// then they expect to receive a MISSION_CURRENT message with
// exactly that sequence number in it, even if ArduPilot never
// actually holds that as a sequence number (e.g. packet.seq==0).
In the case you only have a forward-pointing LIDAR we'd send messages
for each of the other orientations from proximty's horizontal-distances
array, chewing up bandwidth and processing time.
Update libraries/GCS_MAVLink/MissionItemProtocol.cpp
Co-authored-by: WickedShell <Wicked.Shell.Scripts@gmail.com>
GCS_MAVLink: Notify the maximum number of missions
Was "UNSUPPORTED", which is supposed to mean, "Command is not supported"
Now is either "DENIED" (Command is invalid (is supported but has invalid
parameters)) or "FAILED" (Command is valid, but execution has failed.)
We should probably returned DENIED if we try to change to a mode which
doesn't exist, but that would require another callback on AP_Vehicle.
It would also lead to questions around what a valid mode is - so
QSTABILIZE if quadplane is disabled in Plane, for example.
Using a component ID of 0 for your source is invalid according to
common.xml
However, some clients do use it.
This stops us learning a route to that client for the broadcast client
for our own system ID.
The code here was never meant to maintain an "average" streamrate. It
was designed so that we would maintain a consistent clock in the face of
minor scheduling anomalies (like an EKF fusion step).
The way this is written, however, makes us spit out a message for each
of the intervals we missed - clearly not intended behaviour.
This was tested by inserting the following code:
void GCS_MAVLINK::update_send()
{
+ const uint32_t xnow = AP_HAL::millis();
+ if (xnow > 10000 &&
+ xnow < 20000) {
+ return;
+ }
+