- the hover thrust estimate will often invalidate upon ground contact, but before the land detector ground_contact triggers
- use same time throughout call to avoid subtle timing surprises
- track and publish validity based on hover thrust variance, innovation test ratio, and hysteresis
- only publish on actual updates or becoming inactive
- fix dt (previous timestamp wasn't being saved)
- use local position timestamp (corresponding) to accel data rather than current time to avoid unnecessary timing jitter
- check local position validity before using
- mc_hover_thrust_estimator: move from wq:lp_default -> wq:nav_and_controllers to ensure the hover thrust estimator runs after the position controller and uses the same vehicle_local_position data
- land_detector: check hover thrust estimate validity and adjust low throttle thresholds if hover thrust is available
- mc_pos_control: only use hover thrust estimate if valid
- if "landed" and "maybe_landed" states are false then both the "hit_ground" and the "low_thrust" condition need to be true in order to detect landing
- ground contact MC NAN setpoint workaround
- ground contact additionally check acceleration setpoint
- schedule with vehicle_local_position updates (most updates require valid local position)
- don't allow LNDMC_Z_VEL_MAX to exceed MPC_LAND_SPEED
- ground contact horizontal movement checks default to failed if estimates aren't available
HTE runs based on the position controller so, even if we whish to use
the estimate, it is only available in altitude and position modes.
Therefore, we need to always initialize the hoverThrottle using the hover
thrust parameter in case we fly in stabilized
* MC_HTE: unitialize with hover_thrust parameter
* MC_HTE: constrain hover thrust setter between 0.1 and 0.9
* MC_HTE: integrate with land detector and velocity controller
* MCHoverThrustEstimator: Always publish an estimate even when not fusing measurements. This is required as the land detector and the position controller need to receive a hover thrust value.
* MC_HTE: use altitude agl threshold to start the estimator
local_position.z is relative to the origin of the EKF while dist_bottom
is above ground
Co-authored-by: bresch <brescianimathieu@gmail.com>
Consolidate _update_params() methods for improved inheritance from the LandDetector base class.
Move common uORB::Subscriptions to the base class for inheritance.
Deprecate redundant override methods.
PR #12681 added a check to the MulticopterLandDetector::_get_maybe_landed_state() method for a valid vehicle_attitude.timstamp value to finish work in PR #9756. It was discovered that the addition of that check leaves it possible to fly in acro mode without a valid attitude and auto-disarm can engage, allowing the multicopter to fall out of the sky.
instead of the derivative of the vertical position which is
not the same value when using the ekf2 estimation.
Using the position derivative resulted in delayed landing because
the value for some reason often bumped over the theshold before
slowly converging towards zero while the velocity was within expected
accuary in all these cases.
PositionControl: update comment about the order of thrust and position
mc_pos_control: reset setpoints to NAN if required
MulticopterLandDetector: consider to be landed if vehicle is not armed
mc_pos_control: initialize landing struct with landed
mc-pos-ctrl: adjust thrust-setpoint and yaw during ground-contact/maybe-landed without capturing
that information within vehicle-local-position-setpoint topic because that information
does not belong to user intention
PositionControl: set local position/velocity setpoint to NAN if not used in the control pipeline
mc-pos-ctrl: Fill vehicle_local_position_setpoint_s structure as follow:
The message contains setpoints where each type of setpoint is either the input to the PositionController
or was generated by the PositionController and therefore corresponds to the PositioControl internal states (states that were generated by P-PID).
Example:
If the desired setpoint is position-setpoint, _local_pos_sp will contain
position-, velocity- and thrust-setpoint where the velocity- and thrust-setpoint were generated by the PositionControlller.
If the desired setpoint has a velocity-setpoint only, then _local_pos_sp will contain valid velocity- and thrust-setpoint, but the position-setpoint
will remain NAN. Given that the PositionController cannot generate a position-setpoint, this type of setpoint is always equal to the input to the
PositionController.
mc_pos_control: switch to designated initializer for landed
It's less error prone because it produces an error on every discrepancy.
- no airframe changes the default
- it does not make much sense to be able to configure the 0.1 threshold
but the 0.3 threshold for ground contact detection cannot be configured.
Using the vertical derivative estimate prevents vertical velocity offsets caused by estimation vertical velocity errors preventing the vehicle disarming.
Before that, different modules (ekf2, commander & land detector) changed
params upon different events:
- ekf2 & commander set params after disarm
- land detector set params on land detected
If the 2 events were several 100ms appart, it led to 2 param saves, and
the latter param set could have been blocked by an ongoing save. And if
the land detector was blocked, it could lead to mag timeouts.
This patch makes all modules use the same event, thus only a single param
save will happen.
If we want to have guarantees to never block, we should introduce a
param_try_set() API method.
This commit is an attempt to fix a race condition happening on takeoff
between the land detector and the multicopter position controller.
Previously, an auto-takeoff leads to the following events:
1. A takeoff setpoint is given.
2. The thrust setpoint spikes because we don't enter smooth takeoff yet.
3. The land detector detects a takeoff because of the high thrust.
4. The position controller sees the landed state transition and
initiates the smooth takeoff. Thrust goes back down.
5. Depending on control gains the takeoff is successful or fails
if the smoothing takes too long which causes thrust to be too low, so
the land detector detects land again.
The two obvious problems with this are:
- The intermittent spike.
- The failed takeoff because of the smoothing leads to a delay..
With this change, the logic for a takeoff detection is moved from the
land detector to the position controller.
The events are now:
1. A takeoff setpoint is given.
2. The position controller detects the takeoff setpoint and initiates
the smooth takeoff.
3. As thrust ramps up, the land detector detects the take off.
In the same way, we now detect the intent to takeoff in manual,
altitude, control, position control in the position controller instead
of in the land detector.
Daniel Agar