Constrain the specific energy balance integrator input to prevent increasing saturation of pitch demand.
Decay the specific energy balance integrator state if the pitch demand is saturated to reduce saturation to zero and do so at the same tome constant as the control loop
Relax the clipping threshold on the specific energy balance integrator to allow the input constraint and decay functions to do more of the work
Improve variable naming and commenting

- when limiting the pitch integrator input the value was related to
a quantity with different units (specific energy error rate vs delta pitch)
- once the unconstrained pitch demand is larger / smaller than the max/min
allowed pitch angle the integrator input should only be allowed to drag
the integrator into the direction leading to less pitch demand violation

Signed-off-by: Roman <bapstroman@gmail.com>

- if the specific energy balance correction term produced a demanded
pitch value which exceeded the aircraft pitch limits then the pitch
integrator was shifted such that the pitch demand violation was prevented.
However, this meant that the exceeding pitch was just unloaded into the
integrator and caused unexpected behavior of the pitch loop.
In an underspeed condition e.g. this has lead to the plane pulling up it's
nose very quickly shorty after the underspeed condition kicked in.

Signed-off-by: Roman <bapstroman@gmail.com>

- the method in TECS for detecting an underspeed condition was run after
the method which calculated the airspeed demand. As a result the specific
engergy balance error signal showed a spike when TECS detected an underspeed
condition.

Signed-off-by: Roman <bapstroman@gmail.com>

In most cases, really only 1 element is needed. The dynamic allocation
handles cases where more are necessary. This is all done within a locked
state, so no races can occur.

Frees roughly 2.3KB RAM.

We expect a short round-trip time, so lowering the retry timeout will
increase throughput on links with high drop rate.

Avoid sending data before we have an ack from the logger.

Running this script will parse the top of all source files
that are not submodules, examples or test cases, to find
all #include's and then do basically two things:

1) Reorder and group the headers so that px4_* headers are
   included first, then local headers (headers of the project
   that aren't submodules) then C++ headers if any, then C
   headers if any, then system headers (which includes submodules)
   and finally any #includes that are inside #if*...#endif
   constructs.
2) Fix the use of "" or <> in a consistent manner.

Afterwards few fixes might be necessary for compile errors that
pop up. Most of those are already fixed in my previous commits.
However, I was not able to test a compilation for ros
(with __PX4_ROS defined) -- so some more fixes might be necessary
because of the header reordering.

The script comes with a progress counter and an error summary
(if any) at the end (so no scrolling back is necessary).
It is highly recommended to only run this script in a clean
project with no outstanding changes that need to be committed.
In fact, the script enforces this (unless you pass --force).
Reverting a run of the script is then easy with 'git checkout .'.

It is also possible to run the script on a single file
by passing that on the command line. In that case it
might make sense to pass --debug too, though that was really
meant for just me, while developing the script. This will write
debug output into the file that is passed, so you don't
want to commit that! ;)

<systemlib/err.h> --> "systemlib/err.h" that fix_headers.sh
would miss because it comes after code it doesn't understand.

Effectively remove the '__EXPORT extern perf_counter_t perf_alloc('
line, because currently perf_alloc is defined to be NULL, and
after running fix_headers.sh that happens *before* this header
is included (the order of headers will be changed).

Do not define NULL to be (void*)0: that only works for C.
In fact, the conversions needed for NULL are so full of exceptions
that standard C++ introduced a new *keyword*: nullptr.
That's what we really should be using for C++ code.
In this case I just include the correct header to define NULL
the correct way.

Not really related to the header line:
Removed an #include <time.h> because I noted that px4_time.h
was already included... and moved a #include <sys/time.h>
to the top of the file (not really a fan of including headers
in the middle unless absolutely necessary).
Removed a include of queue.h because I noted it wasn't used.

In this particular case it does no harm,
but since in other cases it can lead to
problems I didn't want to add an exception
for this case to fix_headers.sh, that currently
chokes on this because it doesn't know better
than that it's a bad thing.

Note on how #pragma once works: when encountered
(aka the #ifdef that it is inside has to
be true), the compiler marks the whole
file as "seen" (this is implementation
defined, but most implementations store
the inode of the file). Subsequent #include's
of that file/inode are then completely skipped.
Hence it doesn't matter if the #pragma is at
the beginning, at the end or in the middle,
but it should be encountered every #include,
usually, and thus not be inside an #if... #endif
construct.

These headers files were missing from the header files that
I added them to; the fact that they were missing didn't
lead to compile errors because by coincidence the missing
headers are included in the source files before including
these headers. But, after the reordering of header inclusions
by Tools/fix_headers.sh, these cases will give rise to compiler
errors.

- allowing decoding of SUMD frame with failsafe bit set
- updating stack failsafe state with the sumd failsafe info

Refer to #5817 issue

* Pixhawk2: Check for mpu9250 during boot

* Fix indentation style

Displaying the RMS innovation values at the end of each replay assets with rapid iteration for time delay parameter tuning without having to plot or post process using another tool.

Signed-off-by: Mark Charlebois <charlebm@gmail.com>

This updates cmake_hexagon and cmake_hexagon inside DriverFramework
which fixes somethine about rpcmem.a and the double linking issue.

Flight tested and is working.

Minor message clarity fix.

Have confirmed this works with gazebo sitl.

This adds the message MOUNT_STATUS to report about the attitude of a
gimbal.

We need feedback in mavlink about the attitude of the gimbal. Therefore
the gimbal output angles are published in vmount.

With mavlink2 new messages are added with msg IDs greated than 255.
Therefore the msg ID types needed to be raised everywhere.

There was no feedback if a CMD_SET_MESSAGE_INTERVAL went through or not.