goto control class handles smoothing of goto setpoints, outputs trajectory setpoint for mc pos control
some minor encapsulation done in mc pos control for readability
new param MPC_YAWAAUTO_MAX limiting heading accelerations in heading smoother
- force initialize takeoff airspeed setpoint at start of takeoff modes
- force set airspeed constraints if slewed value is out of bounds
- always slew airspeed setpoints as long as inside constraints
- move target airspeed setpoint calculation into mode specific logic regions (hand vs runway)
landing airspeed was previously defined by a scale factor multiplied by minimum airspeed. this commit changes this parameter to an explicit speed, and when unspecified, defaults to the minimum airspeed
previously, the next position setpoint in the triplet was left unchanged during an abort, which meant that the abort loiter current point combined with land next point triggered the early landing config logic. this commit is only a hack to make things work temporarily.. this needs to be handled better.
- explicitly defined takeoff airspeed setpoint
- dont use climbout mode
- allow max climb on takeoff
- dont handle post clearance altitude case (navigator will switch anyway)
- also put flaring internal states into a struct to organize a bit
- one concern with blending the throttle setpoint like this with the flare time param is that folding prop belly landing airframes may want to have a separate param for shorter throttle kill and still use the flare time ramps for everything else
abruptly changing to a heading setpoint on flare can cause the aircraft to roll and deviate from the runway, this commit
- maintains path following control during the flare not to disrupt the tracking just before touchdown
- (unfortunately for crosswind landing) removes the body axis alignment for runway bearing - this is a compromise
to achieve both runway bearing body axis alignment AND a specific touchdown point, either
1. the wind would need to be considered, and an appropriate diagonal approach (obstructions allowing) defined to the runway
2. slip control added, keeping path following outputs only commanding roll (controlling airspeed vector) and using yaw-rate command (only actuated by e.g. rudder) to align body axis with the runway
- consolidate takeoff rotation transition times for pitch constraints and throttle setpoint with a single param
- consolidate pitch takeoff constraint parameters (remove rwto_max_pitch, use nominal max)
- input correct units to rwto pitch constraint getters
- encapsulate absolute time interpolator method for transitions
- start runway ops from idle throttle
TECS climbout mode was used for takeoff climbout, which puts throttle to full and does not regulate a specific airspeed.
This commit sets the desired takeoff airspeed explicitly and allows max climb rate to track the ascent.
previously a scale factor param on min airspeed was used to define the climbout airspeed for runway takeoff
additionally, the rotation speed was defined by another hardcoded scale on top of the previously scaled min airspeed
this commit explicitly defines a takeoff speed and rotation speed for runway takeoff in params, with option to disable
previous change in logic to hold after mission clear also broke rtl, as non-mission takeoff still published a mission result which allowed entering the mission finished condition and always changing state to loiter (ignoring rtl). new logic only switches navigation states if mission is finish and the nav state is explicitly in takeoff state, or in mission state
before there was a corner case where if in an auto mode that sets a reset command on the attitude setpoint message (e.g. auto takeoff), if the mode was then switched stabilized, this reset bool would never be changed back to false and the integrators would reset every cycle
rtl state was getting reset on inactive, which meant that the state which triggered resuming e.g. mission landing would be overwritten, and the navigator mode would switch back and forth between rtl and mission. this commit:
1. moves the reset of rtl state to the on activation function (removing it from the on inactive function)
2. functionalizes the rtl state input to the rtl time estimator so that rtl time can still be calculated from state=none while inactive
FW_LND_USETER defaulted to 1 and FW_LND_ABORT terrain based bits all enabled. why? because using a distance sensor is critical to detecting when to flare, and we want to force the user to actively disable these safety settings if they so choose, so that they understand the implications.
- new param, FW_LND_TER_REL
- fixing the glide slope helps keep the landing glide behavior steady (avoiding bumps in the altitude setpoint from e.g. trees)
- flare is still triggered via the distance sensor, if enabled by FW_LND_USETERR
when the vehicle did not track the slope well (e.g. at an offset above the track) and the altitude setpoint flattening on intersection with terrain, the throttle would spool up to smoothly intersect the newly flattened altitude setpoint, this could happen before the flare altitude was reached, which is bad. now the steady state glide behavior will be maintained, and flare can trigger at the appropriate time
- the target_sink_rate param could possibly constrain the maximum commanded sink rate to something less than that of the landing glide slope, which would make it impossible to track. this commit allows opening up the desired max sink rate up to the performance limits of the aircraft, if necessary, for the landing case
- landing slope/curve library removed
- flare curve removed (the position setpoints will not be tracked during a flare, and were being ignored by open-loop maneuvers anyway)
- flare curve replaced by simply commanding a constant glide slope to the ground from the approach entrance, and commanding a sink rate once below flaring alt
- flare is now time-to-touchdown -based to account for differing descent rates (e.g. due to wind)
- flare pitch limits and height rate commands are ramped in from the previous iteration's values at flare onset to avoid jumpy commands
- TECS controls all aspects of the auto landing airspeed and altitude/height rate, and is only constrained by pitch and throttle limits (lessening unintuitive open loop manuever overrides)
- throttle is killed on flare
- flare is the singular point of no return during landing
- lateral manual nudging of the touchdown point is configurable via parameter, allowing the operator to nudge (via remote) either the touchdown point itself (adjusting approach vector) or shifting the entire approach path to the left or right. this helps when GCS map or GNSS uncertainties set the aircraft on a slightly offset approach"
- post takeoff, the aircraft follows the infinite line sourced from the launch point in the direction of the takeoff waypoint
- takeoff waypoint altitude is used as a clearance altitude, set such that once above, the aircraft has cleared all ground occlusions and may proceed with the mission
- runway takeoff state machine simplified to throttle ramp, clamped to runway, climbout, and fly
- throttle ramp must complete before switching to next state to avoid a jump in throttle setpoint just after takeoff if the takeoff airspeed is reached before the ramp is complete
- roll constraints near ground post takeoff removed from runway takeoff class (handled externally now)
- minimum airspeed in TECS is reduced to takeoff speed (if necessary) to lower the underspeed detection bound
- lateral-directional guidance uses a different period parameter during ground roll
- apply constraint only for takeoff and landing modes
- add two params, wing span and wing height, to calculate a reasonable height at which roll limits can be opened
- takeoff situational knowledge removed from all other modes except manual (or actual takeoff mode)
- manual takeoff is marked complete if at a controllable airspeed
- minimum pitch bounds TECS until manual takeoff complete
- remove individual roll constraints during manual takeoff (ground proximity constraints coming in subsequent commit)
- underspeed condition only determined by true airspeed undershoot
- change binary underspeed condition to a continuous percent undersped
- ramp-in max throttle, pitch speed weight, and TAS setpoint reduction during underspeed to avoid jumpy commands at the true airspeed error boundary
- let true airspeed filter reach zero airspeed
- create integral and trajectory generator reset methods
- always run TECS unless in rotary-wing mode (or in transition)
- constantly reset TECS integrals and trajectory generators when landed
- simplify takeoff reset method
- removes _last_manual variable in favor of _skipping_takeoff_detection, which is handled in the control mode setter
- takeoff detection (both launch and runway) is skipped if entering takeoff mode from any other mode while having already been in the air
- added method to runway takeoff class for force setting the fly state when we want to skip the takeoff detection
- documentation of units, params, returns, and descriptions for variables and methods
- rename ambiguous or erroneous state variables
- remove unused or unecessary input arguments to functions
- remove ugly header white space
Thomas Stastny