This limits the change in desired turn rate to reduce impossible requests which should help avoid overshoot
Also add rotation rate limit to turn-rate controller
See discussion here:
https://github.com/ArduPilot/ardupilot/issues/7331
we were getting some uninitialised variables. While it only showed up in
AP_SbusOut, it means we can't be sure it won't happen on other objects,
so safest to remove the approach
Thanks to assistance from Lucas, Peter and Francisco
AFAIK there is still a bug in ArduPilot where subparams should not start at index 0. This is due to the way the index math and bit shifing works as it incorrectly offsets all 0 index params to 0. We allow 2 levels of sub params - 3 levels in total. So params, sub params and sub sub params. The 0 parameter in all those is unfortunately always references param[0]. So param[0] and subparam[0] and subsubparam[0] will reference the same parameter value. Its why we always say start the index from 1 as the math and bitshifting then works correctly.
Yeah this is worded badly - hard to explain.
this fix ensures the output throttle is never in the opposite direction from the desired-speed
there is a possibility that this could lead to rougher throttle response when the vehicle is transitioning from forward to backwards motion because the throttle response will immediately go to zero when the desired speed cross over zero
Throttle controller has these advantages over existing controller:
based on velocity in vehicle's forward-back axis rather than ground-speed
straight-forward PID controller using speed error as input
speed control acceleration limts
stop control slows vehicle smoothly
configurable filtering
Steer controller has these advantages over existing controller:
output scaled properly for skid-steering vehicles
layered P and PID controller for angular error and rate control
configurable filtering
this reduces elevator control when rolled over hard in fixed
wing. Using the elevator when on the side just caused earth frame yaw
and is counter productive. It can also prevent some aircraft from
recovering from inverted flight.
Rather then just using the standard z gyro by using the earth frame it
takes into account when a rover leans over in hard corners. My rover
leans 15 degrees no problem which is why this is needed.
The problem with using min() and max() is that they conflict with some
C++ headers. Name the macros in uppercase instead. We may go case by
case later converting them to be typesafe.
Changes generated with:
git ls-files '*.cpp' '*.h' -z | xargs -0 sed -i 's/\([^_[:alnum:]]\)max(/\1MAX(/g'
git ls-files '*.cpp' '*.h' -z | xargs -0 sed -i 's/\([^_[:alnum:]]\)min(/\1MIN(/g'
Most of AP_Progmem is already gone so we can stop including it in most
of the places. The only places that need it are the ones using
pgm_read_*() APIs.
In some cases the header needed to be added in the .cpp since it was
removed from the .h to reduce scope. In those cases the headers were
also reordered.
prog_char and prog_char_t are now the same as char on supported
platforms. So, just change all places that use them and prefer char
instead.
AVR-specific places were not changed.
Now variables don't have to be declared with PROGMEM anymore, so remove
them. This was automated with:
git grep -l -z PROGMEM | xargs -0 sed -i 's/ PROGMEM / /g'
git grep -l -z PROGMEM | xargs -0 sed -i 's/PROGMEM//g'
The 2 commands were done so we don't leave behind spurious spaces.
AVR-specific places were not changed.
The PSTR is already define as a NOP for all supported platforms. It's
only needed for AVR so here we remove all the uses throughout the
codebase.
This was automated with a simple python script so it also converts
places which spans to multiple lines, removing the matching parentheses.
AVR-specific places were not changed.
This commit changes the way libraries headers are included in source files:
- If the header is in the same directory the source belongs to, so the
notation '#include ""' is used with the path relative to the directory
containing the source.
- If the header is outside the directory containing the source, then we use
the notation '#include <>' with the path relative to libraries folder.
Some of the advantages of such approach:
- Only one search path for libraries headers.
- OSs like Windows may have a better lookup time.
this adds autotune to the roll/pitch controllers using a very simple
mechanism. The plan is that this provides a framework which Paul and
Jon will build upon.
this will make it possible to do rate based pitch control without
having a coordinated turn, for in ACRO mode
Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>
Gain definitions in roll and pitch controllers were updated previously
so that the old PID tuning values could be transferred across.
Updated tuning guide for revised gain definition.
auto-reverse pitch control when inverted. This is useful not just for
inverted flight mode, but also for recovering from poor manual flight
Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>
These changes reduce height variation in turns and improve
robustness. the specific changes are:
1) Linked roll and pitch integrator protection to the final output
value so that if final output is on upper limit, the integrator is
prevented from increasing and vice-versa. This improves wind-up
protection.
2) Modified rate feedback in roll and pitch controllers to use body
rates rather than Euler or earth rates.
3) Changed the roll to pitch compensation to use measured roll angle
and estimated airspeed to calculate the component of turn rate
(assuming a level coordinated turn) around the pitch axis. This a
mathematically correct calculation and will work over a range of bank
angles and aircraft with minimal (if any) tuning required.
4) The integrator in the roll and pitch loop is clamped when the
estimated speed is below the minimum FBW speed
5) The noise filter in the pitch and roll loop has been changed to use
a FOH discretisation. This gives improved noise rejection and less
phase loss when compared to the previous filter that used a ZOH or
equivalent discretisation.
This has been flown on the rascal in the SITL and on a X-8 with
limited flight testing. Initial results have been encouraging with
reduced height variation in turns. Compare to standard PIDS, the
revised pitch and roll controllers allow the use of rate feedback
(effectively the same as the old D term) without beating the servos to
death. The bank angle compensation in the pitch loop works
effectively over a much larger range of bank angles and requires
minimal tuning compared to the old calculation.
YAW CONTROLLER
Currently testing the a 3-loop acceleration autopilot topology for the
yaw loop with feed forward yaw rate for turn compensation. This 3-loop
topology is commonly used in tactical skid to to turn missiles and is
easy to tune. The following block diagram shows the general signal
flow
Note that the acceleration measurement has to pass through an
integrator before it gets to the actuator. This is a important feature
as it eliminates problems of high frequency noise and potential
coupling with structural modes associated with direct feedback of
measured acceleration to actuator.
The high pass filter has been inserted to compensate for airspeed and
bank angle measurement errors which will cause steady state errors in
the calculation of the turn yaw rate.
The yaw controller flies SITL well, but hasn't been flight tested
yet. It can be configured either as a simple yaw damper, or the
acceleration and integral term can be turned on to allow feedback
control of lateral acceleration/sideslip.
TO DO:
Need to reduce number of tuning parameters and provide consistent
naming Need to provide guidance on tuning these loops with definitions
for all the gain terms. Need to check signs and units into and out of
lateral loops.
DESIGN DECISIONS PENDING:
1) Can we remove the noise filters? Provided the mpu6k noise filter is
running they are of limited benefit given the 25Hz Nyquist frequency
2) If we do remove them and rely on the mpu6k noise filter, what is
the apprporiate default cutoff frequency for plane use. 20Hz is
probably OK for most setups, but some noisy/high vibration setups
would require as low as 10Hz
3) The inverted flight logic looks like a crash waiting to
happen. It's problematic to test and even if implemented correctly
would still crash a plane with poor inverted flight capability. We
should either implement it properly and fully tested or delete it.
The previous calculation constrained the speed used to calculate the
bank compensation rate offset between the min and max fbw speeds. This
would result in an unwanted climb if flown above the max fbw speed
(this could happen in fbw-a mode)
- Define float versions of math functions to the double versions
on AVR (eg. #define sinf sin).
- These macros appear to be missing in older versions of avr-libs.
- Include AP_Math.h rather than math.h to get these definitions.
- Allows use of hardware floating point on the Cortex-M4.
- Added "f" suffix to floating point literals.
- Call floating point versions of stdlib math functions.
Randy Mackay
Peter Barker
Andrew Tridgell
Michael Oborne
Grant Morphett
khancyr
Daniel Widmann
Lucas De Marchi
Michael du Breuil
Dr.-Ing. Amilcar Do Carmo Lucas
Mathieu OTHACEHE
Tom Pittenger
AndersonRayner
Ricardo de Almeida Gonzaga
Will Sackfield
Caio Marcelo de Oliveira Filho
Gustavo Jose de Sousa
Michael du Breuil
Andrew Tridgell
Peter Barker
Paul Riseborough
Don Gagne
James Bielman
Pat Hickey
Jonathan Challinger