51 Commits

Author SHA1 Message Date
Matthias Grob
e7a90bf367 PositionControl: correct horizontal margin calculation
It was using the already reduced vertical thrust to do
the horizontal limitation resulting in no margin.
2021-09-14 10:57:38 +02:00
Matthias Grob
58ea97a699 MulticopterPositionControl: add horizontal margin for saturation cases 2021-09-08 19:55:27 +02:00
Daniel Agar
883624d915 lib/geo: move from ecl 2021-07-15 10:38:24 -04:00
David Jablonski
5233737a86 adjust some limits to prevent divide-by-zero 2021-03-05 10:42:19 +01:00
Daniel Agar
7a7d316a32 Update src/modules/mc_pos_control/PositionControl/CMakeLists.txt
Co-authored-by: Matthias Grob <maetugr@gmail.com>
2021-03-03 18:53:56 +01:00
Daniel Agar
266ea377da move takeoff state machine flight_mode_manager -> mc_pos_control 2021-03-03 18:53:56 +01:00
Nicolas Martin
171bd6d784 positionControl: add check on sign before sqrtf 2020-10-27 18:52:23 -04:00
Daniel Agar
97fc1db768 vehicle_local_position: rename yaw -> heading and add reset logic
- vehicle_global_position yaw removed (redundant)
2020-08-10 11:42:03 +02:00
Matthias Grob
cdf37ca557 PositionControl: deconflict hover thrust estimator, acceleration control
- Avoid constantly adjusting the velocity gains with the HTE
- Make sure the hover thrust integral update doesn't break
  even though its unit is acceleration and not unit thrust anymore

We need to convert the velocity gains to not contain/depend on the
hover thrust. In horizontal direction it doesn't make sense to scale
them with the hover thrust and in vertical direction the adjustments are
already done in the acceleration to collective thrust conversion.
2020-05-04 22:17:56 +02:00
Matthias Grob
809b45eac8 FlightTasks: do not adjust tilt limit of the position control
Adjusting the tilt limit can lead to diverging position control
and should only be used by setting a sanity limit for the controller
and not to adjust during the descent phase of a Land or RTL.
Otherwise it leads to flyaways in important failsafe modes when
there's stronger disturbance e.g. wind.
2020-04-21 09:35:52 +02:00
Matthias Grob
38093e4887 mc_pos_control: correct sign of acceleration state
Non-functional change, just change the sign in the correct place to
avoid further confusion.
2020-04-09 17:15:46 +02:00
Matthias Grob
58fd3e2ddc PositionControl: fix horizontal integral anti-windup 2020-03-30 09:58:39 +02:00
Matthias Grob
b3d7445059 PositionControl: acceleration based control strategy 2020-03-30 09:58:39 +02:00
Matthias Grob
1d2ac41edc ControlMath: adding limitTilt() helper function
which takes care of limiting the lilt angle of a "body" vector with
respect to a "world" vector. Both vectors have to be unit length!
2020-03-30 09:58:39 +02:00
Matthias Grob
aa53cabaa4 PositionControl: remove confusing uMin uMax alias 2020-03-30 09:58:39 +02:00
Matthias Grob
eb3b0f6b55 mathlib: get rid of now duplicate sign() function
Instead use the one that was copied to the matrix library in
https://github.com/PX4/Matrix/pull/116/
2020-03-17 10:01:42 +01:00
bresch
6ccf55b6fd MPC: add updateHoverThrust function
This function updates the vertical velocity integrator with the change
in hover thrust to avoid propagating discontinuities through the
controller when changing the hover thrust.
This is also important when using the hover thrust estimator as its
estimate has unconstrained dynamics and can cause drops or kicks when
the estimate updates faster than the velocity integrator.
2020-03-15 13:57:02 -04:00
Matthias Grob
f90d3671c0 vehicle_attitude_setpoint: get rid of unused q_d_valid flag 2020-02-27 09:17:52 +01:00
Matthias Grob
b75c1308f9 PositionControl: replace interfacemapping checks
Removing the skip_controller and interfaceMapping
concept and replacing it with a single method checking
if the position control update was successful and
return the result in the PositionControl.update().
2020-01-30 07:28:18 -05:00
Matthias Grob
e06fff94bb PositionControl: handle yaw NAN inputs 2020-01-30 07:28:18 -05:00
Matthias Grob
dcc0339773 PositionControl: calculate velocity integral once using Vector3f 2020-01-30 07:28:18 -05:00
Matthias Grob
9109d22c28 PositionControl: simplify horizontal thrust limiting 2020-01-30 07:28:18 -05:00
Matthias Grob
83653d6581 PositionControl: readd direct thrust setpoint support
This restores original horizontal thrust setpoint execution
that I intentionally broke three commits ago. It's necessary
for correct stick to tilt mapping of FlightTaskManualAltitude.
2020-01-30 07:28:18 -05:00
Matthias Grob
282d46efcb PositionControl: add feed-forwards with addIfNotNan() 2020-01-30 07:28:18 -05:00
Matthias Grob
62fb06bd58 PositionControl: calculate velocity PID once using Vector3f 2020-01-30 07:28:18 -05:00
Matthias Grob
5bd8da2286 PositionControl: temporarily remove direct thrust setpoint support
This commit temporarily breaks direct horizontal thrust setpoint execution
which is used by FlightTaskManualAltitude. This is necessary to allow for
PositionControl cleanup namely calculating the whole velocity PID in one
Vector3f formula.

Having this in a separate commit is useful since it reduces indentation
of a bigger code block.
2020-01-30 07:28:18 -05:00
Matthias Grob
e53ae45188 ControlMath: add addIfNotNan helper functions 2020-01-30 07:28:18 -05:00
Matthias Grob
ffff35a597 Unit tests: use EXPECT_FLOAT_EQ for float comparisons
as precaution to prevent failing tests because of small potential
rounding errors on certain platforms or compilares.
2019-12-26 12:53:07 +01:00
Matthias Grob
ee62d0296f ControlMathTest: consistent short float literal notation 2019-12-26 12:53:07 +01:00
Matthias Grob
ad148fdfad ControlMathTest: switch back to direct EXPECT_EQ calls for Vectors
Since the google test calls matrix::operator==()
which calls matrix::isEqual().
2019-12-26 12:53:07 +01:00
Lorenz Meier
9ac68abd5c Fix float accuracy in ControlMathTest
The test was testing the result of 3D float vector operations with binary equality of the floating point numbers, which is not a valid assumption to make for floating point math. This change switches to proper comparisons with float accuracy and compares vectors using the norm of their difference.
2019-12-23 22:29:09 +01:00
Matthias Grob
a46add9a93 PositionControl: isolate skip controller, leave away unnecessary include 2019-12-18 15:57:48 +01:00
Matthias Grob
8441bdb9b5 PositionControl: remove complicated internal control flags 2019-12-18 15:57:48 +01:00
Matthias Grob
83e51ad192 PositionControl: remove speed_xy constraint since it's anyways unused 2019-12-18 15:57:48 +01:00
Matthias Grob
a4a9d50a97 ControlMath: refactor thrustToAttitude calculation 2019-12-18 15:57:48 +01:00
Matthias Grob
42dbddb156 mc_pos_control: adopt to module internal include convention 2019-12-18 15:57:48 +01:00
Matthias Grob
2303071df4 PositionControl: simplify constraint conditions 2019-12-18 15:57:48 +01:00
Matthias Grob
121d743049 PositionControl: clear setter interface naming, order, description 2019-12-18 15:57:48 +01:00
Matthias Grob
b63044393e PositionControl: remove unwanted debug printfs 2019-11-21 00:52:09 +01:00
Matthias Grob
1bef35d843 PositionControl: correct float initialization literals
according to @bresch's review
2019-11-20 11:42:09 +01:00
Matthias Grob
bd491c8511 PositionControl: add unit tests for directions and limits 2019-11-20 11:42:09 +01:00
Matthias Grob
25a445d5f8 PositionControl: remove parameter dependency
to make the class easier to understand,
more modular and easily unit testable
2019-11-20 11:42:09 +01:00
Matthias Grob
b6de83117e PositionControl: fix attitude setpoint timestamp
The plot of the attitude setpoint in the log did not show any values
because the message timestamp that the position control module sets
was overwritten by the PositionControl attitude generation.
2019-11-05 13:05:16 +01:00
Beat Küng
3198610f85 src/platforms: move all headers to platforms/common/include/px4_platform_common
and remove the px4_ prefix, except for px4_config.h.

command to update includes:
for k in app.h atomic.h cli.h console_buffer.h defines.h getopt.h i2c.h init.h log.h micro_hal.h module.h module_params.h param.h param_macros.h posix.h sem.h sem.hpp shmem.h shutdown.h tasks.h time.h workqueue.h; do for i in $(grep -rl 'include <px4_'$k src platforms boards); do sed -i 's/#include <px4_'$k'/#include <px4_platform_common\/'$k/ $i; done; done
for in $(grep -rl 'include <px4_config.h' src platforms boards); do sed -i 's/#include <px4_config.h/#include <px4_platform_common\/px4_config.h'/ $i; done

Transitional headers for submodules are added (px4_{defines,log,time}.h)
2019-10-30 11:48:47 +01:00
Matthias Grob
d0084766ff PositionControl: make getters const functions 2019-10-26 06:11:18 +02:00
Matthias Grob
f70d4d21a1 PositionControl: add getter for output attitude setpoint 2019-10-26 06:11:18 +02:00
Matthias Grob
198605d258 PositionControl: add getter for output position setpoint 2019-10-26 06:11:18 +02:00
Matthias Grob
9c25b987bc vehicle_local_position_setpoint: make acceleration and jerk arrays 2019-10-24 14:58:57 +02:00
Matthias Grob
38651ceee1 mc_pos_control: address @bresch's review comments 2019-10-23 17:33:47 +02:00
Matthias Grob
d60e1e2774 ControlMath: switch to gtest for unit tessting 2019-10-23 17:33:47 +02:00