Commit Graph
100 Commits
Author SHA1 Message Date
Roman Bapst 20457c5e2e position_setpoint: replaced loiter_direction integer by boolean (#20317)
* position_setpoint: replaced loiter_direction integer by boolean (loiter_direction_counter_clockwise)

Signed-off-by: RomanBapst <bapstroman@gmail.com>
2022-09-29 11:06:10 +02:00
RomanBapst e41913a699 vtol_type: removed unused "was_in_trans_mode" function
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2022-09-20 16:50:07 +02:00
RomanBapst 2a6cd4a409 vtol_att_control: avoid using non-recent attitude setpoint during beginning
of transition

Signed-off-by: RomanBapst <bapstroman@gmail.com>
2022-09-20 16:50:07 +02:00
RomanBapst 0c860fa227 airspeed_selector: don't declare wind estimate valid if estmator is not initialised
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2022-09-05 10:21:38 -04:00
Roman Bapst dbf7d32e07 Skip VTOL_TAKEOFF mission item when in fixed wing mode (#19985)
* mission: skip VTOL_TAKEOFF mission item when in fixed wing mode

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

* mission: added better comment regarding skipping VTOL Takeoff in fw mode

Signed-off-by: RomanBapst <bapstroman@gmail.com>
2022-08-02 15:34:42 +02:00
RomanBapst f11f2e9797 addressed review comments
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2022-08-02 15:29:25 +02:00
RomanBapst a425bc4c92 vehicle_local_position: fixed comment
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2022-08-02 15:29:25 +02:00
RomanBapst fbd4534edc WindEstimator: reworked filter initialisation
- separate initialisation with and without airspeed

Signed-off-by: RomanBapst <bapstroman@gmail.com>
2022-08-02 15:29:25 +02:00
RomanBapst a63f1b71fe wind_estimator: added simple check for validity of synthetic airspeed
- synthetic airspeed will only be declared valid as soon as the wind variance
has dropped below a parameterized threshold. This is useful for vehicles without
an airspeed sensor which rely on synthetic airspeed but only once the vehicle
has turned sufficiently for the wind estimates to be reliable.

Signed-off-by: RomanBapst <bapstroman@gmail.com>
2022-08-02 15:29:25 +02:00
RomanBapst 97f632a408 vtol_takeoff: reset reposition triplet before handing over to loiter mode
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2022-07-25 14:48:18 +02:00
RomanBapst 9886660862 gps_inject_data: fixed integer overflow
- array length of data was increased without changing the data type of
the variable holding the length

Signed-off-by: RomanBapst <bapstroman@gmail.com>
2022-07-13 07:49:47 +02:00
RomanBapst a23cb111b7 added comment for cruising throttle only affecting rover
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2022-07-12 09:07:20 +02:00
RomanBapst b7d6e646cc FixedWingPositionControl: better naming for manual airspeed setpoint
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2022-07-12 09:07:20 +02:00
RomanBapst c2f5ffdfcd log tecs trim throttle
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2022-07-12 09:07:20 +02:00
RomanBapst 92cdff6798 TECS: rename airspeed/throttle "cruise" to "trim"
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2022-07-12 09:07:20 +02:00
RomanBapst b57d3c6d74 TECS: set true airspeed rate setpoint to zero if airspeed is not enabled
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2022-07-12 09:07:20 +02:00
RomanBapst 6dbea21ef5 TECS: renamed airspeed cruse to airspeed trim & general cleanup
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2022-07-12 09:07:20 +02:00
RomanBapst c05e0f076b FixedWingPositionControl: do not pass trim throttle via position setpoint triplet
- trim throttle is handled entirely by the position controller
- navigator should use speed setpoints
- added flag gliding_enabled in position setpoint to stills support gliding

Signed-off-by: RomanBapst <bapstroman@gmail.com>
2022-07-12 09:07:20 +02:00
RomanBapst cae7e1b88b improved trim throttle compensation
- allow compensation based on vehicle weight (parameterized)
- use density for calculating trim throttle compensation instead of pressure
- removed parameter FW_THR_ALT_SCL

Signed-off-by: RomanBapst <bapstroman@gmail.com>
2022-07-12 09:07:20 +02:00
RomanBapst 461d0561b8 replace FW_THR_CRUISE with FW_THR_TRIM
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2022-07-12 09:07:20 +02:00
RomanBapst 44f98ac355 standard: fixed pusher assist in hover
- in hover mode the pusher assist is already set in update_mc_state()

Signed-off-by: RomanBapst <bapstroman@gmail.com>
2022-07-06 16:07:55 +02:00
RomanBapst 42cd0b4ce0 FlightTaskAuto: don't override landing gear state for takeoff
- this allows landing gear to retract automatically when doing a takeoff
and the vehicle is considered high enough

Signed-off-by: RomanBapst <bapstroman@gmail.com>
2022-07-06 11:11:14 +02:00
RomanBapst a7683eea07 mission_block: fix vehicle not exiting loiter after reaching exit heading
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2022-04-19 13:32:18 +02:00
RomanBapst 3ed929c7b6 addressed review comments
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2022-04-13 12:23:27 +03:00
RomanBapst b335710655 vehicle_command: added enum for speed types
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2022-04-13 12:23:27 +03:00
RomanBapst d41de33a85 FlightModeManager: handle MAV_CMD_DO_CHANGE_SPEED
- support setting the cruise speed of the auto flight task via command

Signed-off-by: RomanBapst <bapstroman@gmail.com>
2022-04-13 12:23:27 +03:00
RomanBapst ca657f36ef FixedWingPositionControl: handle VEHICLE_CMD_DO_CHANGE_SPEED
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2022-04-13 12:23:27 +03:00
RomanBapst 36e32ecd7b navigator: stop handling speed changes via reposition triplet
- the mc and fw controllers are handling the speed changes directly

Signed-off-by: RomanBapst <bapstroman@gmail.com>
2022-04-13 12:23:27 +03:00
RomanBapst 18074dec5a simulator: fix conversion from hPa to Pa
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2022-03-29 17:41:54 +03:00
RomanBapst 60231bbcb6 navigator: don't reset cruise speed and throttle during a transition as part
of the VTOL takeoff navigation state

- the reset causes the loiter circle to be reset to home

Signed-off-by: RomanBapst <bapstroman@gmail.com>
2022-03-22 15:14:02 +01:00
RomanBapst 58a4c38519 rtl: don't fly mission landing if we trigger rtl in hover
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2022-03-11 15:23:31 +03:00
Roman Bapst 69560bd4f4 Compensate VTOL transition time for air density (#19293)
* vtol: compensate front transition minimum time and front transiton openloop time
for air density

Signed-off-by: RomanBapst <bapstroman@gmail.com>
2022-03-09 08:41:30 -05:00
Roman Bapst 6de5d24e00 Added VTOL Takeoff navigation mode (#19027)
* Commander: added support for MAIN_STATE_AUTO_VTOL_TAKEOFF
* navigator: added support for vtol_takeoff navigation mode
2022-02-15 14:56:57 +01:00
Roman BapstandSilvan Fuhrer dc6067ed12 Update src/modules/fw_att_control/fw_att_control_params.c
Co-authored-by: Silvan Fuhrer <silvan@auterion.com>
2022-01-14 16:34:44 +03:00
RomanBapst 34bb671a73 improve parameter descriptions for fw maximum/minimum roll/pitch angles
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2022-01-14 16:34:44 +03:00
RomanBapst 555ee371e8 FixedWingPositionControl: consistently use the same roll and pitch angle
limits for autonomous and semi-autonomous modes (altitude & position control)

Signed-off-by: RomanBapst <bapstroman@gmail.com>
2022-01-14 16:34:44 +03:00
RomanBapst 439400f76e commander: don't ack gimbal commands
- this leads to an unnecessary amount of acks which can in turn overflow
the orb queue

Signed-off-by: RomanBapst <bapstroman@gmail.com>
2021-11-13 11:59:28 -05:00
RomanBapst 56cf77a74c addressed review comments
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2021-11-05 18:04:33 +01:00
RomanBapst e36e527488 fixed wrong path
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2021-11-05 18:04:33 +01:00
Roman Bapst d58d13b2e8 APX4-1617: Fix manual climb rate in TECS (#1463)
* tecs: workaround for the fact that vel smoothing lib uses a DOWN coordinate system

Signed-off-by: RomanBapst <bapstroman@gmail.com>
2021-11-05 18:04:33 +01:00
RomanBapst 7cb99f521c moved ManualVelocitySmoothing to motion_planning lib
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2021-11-05 18:04:33 +01:00
RomanBapst 924298e9c3 addressed review comments
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2021-11-05 18:04:33 +01:00
RomanBapst 0ac3077bdc tecs: use trajectory generation library to compute height rate setpoint
- added ability to specify maximum acceleration constraint for height rate setpoint
- added support for locking altitude setpoint when in height rate control
mode and height rate input is zero

Signed-off-by: RomanBapst <bapstroman@gmail.com>
2021-11-05 18:04:33 +01:00
RomanBapst 6e75b7cffd FixedWingPositionControl: push altitude setpoint locking into TECS
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2021-11-05 18:04:33 +01:00
RomanBapst 514cf12c57 addressed review comments
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2021-11-05 17:58:21 +03:00
RomanBapst eee5f501cd navigator: fix flyaway when altitude change is commanded without a valid
triplet

Signed-off-by: RomanBapst <bapstroman@gmail.com>
2021-10-27 11:01:13 +03:00
RomanBapst bf6a47ba6a navigator: cleanup of set_loiter_item
Unwraps the set_loiter_item() to solve the issue where the altitdue setpoint
in a MC takeoff wasn't correctly used.

Signed-off-by: RomanBapst <bapstroman@gmail.com>
2021-10-27 11:01:13 +03:00
RomanBapst d678e792cc mission_block: don't require an exiting heading when loitering if the next
waypoint is within the loiter radius of the current waypoint

Signed-off-by: RomanBapst <bapstroman@gmail.com>
2021-10-27 11:01:13 +03:00
RomanBapst d84b0296d2 support orbit command in fixed wing mode
Signed-off-by: RomanBapst <bapstroman@gmail.com>

commander: support orbit mode for fixed wings

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

FwPositionControl: publish orbit status

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

commander:reject orbit mode while doing a vtol transition

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

FixedWingPositionControl: explicitly cast waypoint for Orbit status

FixedwingPositionControl: fill missing orbit_status fields

navigator_main: handle reposition/orbit corner cases

- set orbit rotation direction correctly
- send mavlink message when orbit is rejected

FixedWingPositionControl: correctly report rotation direction in orbit_status

navigator: hack to not break orbit while doing altitude changes

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

navigator: set cruise throttle for orbit command

Signed-off-by: RomanBapst <bapstroman@gmail.com>
2021-10-25 09:48:27 +02:00
RomanBapst 8e8c6efd66 Mission block: do not care for altitdue acceptance when approaching backtransition point
Not accepting the waypoint causes the vehicle to perform a sharp turn after passing
the land waypoint and this causes worse unexected behavior.

Signed-off-by: RomanBapst <bapstroman@gmail.com>
2021-10-22 15:37:10 +02:00
RomanBapst af291e2040 FlightTaskTransition: Transition improvements
- use fw pitch setpoint offset during transition
- take over previous vertical velocity and smooth out over transition

Signed-off-by: RomanBapst <bapstroman@gmail.com>
2021-10-15 09:58:48 +02:00
RomanBapst f61853d428 vtol: implement throttle blending out and into transition
- blend into TECS throttle after front transition
- blend out of TECS throttle during backtransition

Signed-off-by: RomanBapst <bapstroman@gmail.com>
2021-10-15 09:58:48 +02:00
RomanBapst 8dd76050e0 vtol: take fixed wing attitude setpoint during transition if altitude is
not controlled

- required as there is no flightask running if altitude is not controlled

Signed-off-by: RomanBapst <bapstroman@gmail.com>
2021-10-15 09:58:48 +02:00
RomanBapst 8f8304f31e FixedWingPositionController: Set l1 variables to NAN if no l1 guidance
took place

Signed-off-by: RomanBapst <bapstroman@gmail.com>
2021-10-11 16:41:42 +02:00
RomanBapst ae5d3103f4 fw l1 controller: added a flag to indicate if navigation has updated
- can be used to check if l1 controller ran during a cycle

Signed-off-by: RomanBapst <bapstroman@gmail.com>
2021-10-11 16:41:42 +02:00
RomanBapst 93075ad5e8 rtl: fixed computation of return altitude if destination is not home
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2021-09-05 10:26:24 +02:00
RomanBapst 92abf0545a GeofenceBreachAvoidance: fixed bug which caused min vertical distance to fence
not to be calculated

Signed-off-by: RomanBapst <bapstroman@gmail.com>
2021-09-02 15:45:19 +03:00
RomanBapst ad9288536b TECS: enable direct height-rate control
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2021-06-04 14:51:27 +02:00
RomanBapst f6de99d42e FixedWingPositionControl: control only height rate when using pitch stick
in manual altitude controlled modes

Signed-off-by: RomanBapst <bapstroman@gmail.com>
2021-06-04 14:51:27 +02:00
RomanBapst 19449a4e75 FixedWingPositionControl: use target climb/sink rate parameters as maximum
rates in manual altitude controlled modes

Signed-off-by: RomanBapst <bapstroman@gmail.com>
2021-06-04 14:51:27 +02:00
RomanBapst a5cc449e69 tecs: propagate altitude setpoint based on target climb/sink rate
- avoids tecs always climbing and sinking and max rates and allows to fine tune these rates
- avoid numerical calculation of feedforward velocity using derivative, this
 was prone to jitter in dt

Signed-off-by: RomanBapst <bapstroman@gmail.com>
2021-06-04 14:51:27 +02:00
RomanBapst 4b7416c05b FixedWingPosControlL1: added target climb and sink rate parameters
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2021-06-04 14:51:27 +02:00
RomanBapst b06b46b224 log raw airspeed derivative, pitch setpoint and airspeed innovation for TECS
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2021-05-31 23:56:23 +02:00
RomanBapst 3b27864e53 vehicle_status: added field for geofence violation
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2021-05-10 10:46:49 +03:00
RomanBapst 3ac8c23dd0 commander: added prearm check for geofence violation
- if geofence action is not none, then don't allow arming outside of geofence

Signed-off-by: RomanBapst <bapstroman@gmail.com>
2021-05-10 10:46:49 +03:00
RomanBapst 6215e6c7ec navigator: do not emit geofence warnings if system is not armed
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2021-05-10 10:46:49 +03:00
RomanBapst 269ce07cb5 land detector: log more states in order to facilitate debugging ground contact state
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2021-05-04 16:43:33 +02:00
RomanBapst cd4378b8c6 vtol: init boolean consistently
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2021-05-03 18:58:26 -04:00
RomanBapst 171c26373b land_detector: robustify land detection by using distance to ground info
- if distance to the ground is available then hysteresis times will be increased
by a factor of 3 if vehicle is higher than 1m above ground

Signed-off-by: RomanBapst <bapstroman@gmail.com>
2021-04-27 08:19:19 +02:00
RomanBapst f6a34e1e80 vtol_type: reset accel to pitch integrator
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2021-04-27 07:43:38 +03:00
RomanBapst b66ae5f2d4 tecs: use raw inertial acceleration for true airspeed complementary filter
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2021-03-31 20:12:40 -04:00
RomanBapst 5b08362ba3 position_controller_status message: added comments regarding NAN
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2021-03-31 11:23:33 +02:00
RomanBapst 82a2126f97 TECS: add feedforward gain for total energy balance rate
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2021-03-15 11:47:07 -04:00
RomanBapst b1e442b830 vehicle_local_position: added bitfield for terrain estimate sensor info
- indicate how the distance to the bottom is estimated as this is important
to know in the context

Signed-off-by: RomanBapst <bapstroman@gmail.com>
2021-01-17 12:06:59 +01:00
RomanBapst 96dfa7fc71 ROMFS: set cruise throttle correctly for all gazebo vtol configs
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2021-01-14 09:58:37 +03:00
RomanBapst 47364ce8d6 updated tuning for gazebo plane airframe config
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2021-01-13 21:21:07 +03:00
RomanBapst f05599caff tecs: use speed derivative provided by local position
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2021-01-13 21:21:07 +03:00
RomanBapst e546584555 tecs: moved trivial getter methods to header file
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2021-01-13 21:21:07 +03:00
Roman BapstandMathieu Bresciani 701c105ab6 Update src/lib/tecs/TECS.cpp
Co-authored-by: Mathieu Bresciani <brescianimathieu@gmail.com>
2021-01-13 21:21:07 +03:00
RomanBapst fa559aef43 TECS: stop learing integrator if airspeed is not available, but don't zero it
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2021-01-13 21:21:07 +03:00
RomanBapst d4d1c0fe01 TECS: some cosmetics
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2021-01-13 21:21:07 +03:00
RomanBapst 97cc1e25dd TECS: hande pitch and throttle integrator saturation consistently
- don't decay integrator during saturation but prevent it from being learned
into the direction which causes more actuator saturation

Signed-off-by: RomanBapst <bapstroman@gmail.com>
2021-01-13 21:21:07 +03:00
RomanBapst 77627a08ca TECS: don't allow speed height weights to be larger than one
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2021-01-13 21:21:07 +03:00
RomanBapst 987c320825 TECS: removed unused parameter
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2021-01-13 21:21:07 +03:00
RomanBapst 5c4c2c240c tecs: dot not clip throttle integrator during saturation but only allow
it to propagate into a direction which does not saturate

Signed-off-by: RomanBapst <bapstroman@gmail.com>
2021-01-13 21:21:07 +03:00
RomanBapst 01f891618b TECS: move to new control loop architecture
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2021-01-13 21:21:07 +03:00
RomanBapst 447e14906c TECS: log more TECS states to enable better analysis
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2021-01-13 21:21:07 +03:00
RomanBapst 8823f5e1ec TECS: added method to get SKE weighting
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2021-01-13 21:21:07 +03:00
RomanBapst d0e8b882a2 TECS: added alpha filter classes to energy rate and tas derivative
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2021-01-13 21:21:07 +03:00
RomanBapst f97dcde4e2 GeofenceBreachAvoidanceTest: added tests for max dist to home
Signed-off-by: Julian Kent <julian@auterion.com>
2021-01-13 10:26:10 +01:00
RomanBapst 3fa15cec91 GeofenceBreachAvoidance: improve behavior when using max dist to home option
- ensure that the vehicle is loitering with sufficient clearance to the boundary

Signed-off-by: Julian Kent <julian@auterion.com>
2021-01-13 10:26:10 +01:00
RomanBapst 0ac597fba8 Use new GeofenceBreachAvoidance from Navigator
Signed-off-by: Julian Kent <julian@auterion.com>
2021-01-13 10:26:10 +01:00
RomanBapst e536868104 Add GeofenceBreachAvoidance class
Signed-off-by: Julian Kent <julian@auterion.com>
2021-01-13 10:26:10 +01:00
RomanBapst 0871f0f52d vtol: refactor of maximum pwm value settings for main and alternate motors
- main motors are the ones which are meant to be used as propulsion in fw mode
- alternate motors are the ones usually meant to be switched off in fw mode

Signed-off-by: RomanBapst <bapstroman@gmail.com>
2021-01-04 15:32:00 +01:00
RomanBapst 68f27ba7b7 navigator: deactivate line following after backt-ransition and before landing
- especially when there are strong winds it's better to just go straight
to the landing point instead of trying to follow the planned path

Signed-off-by: RomanBapst <bapstroman@gmail.com>
2021-01-04 11:58:16 +01:00
RomanBapst b63f756745 navigator: don't execute land pattern if vtol in rotary wing mode
- if vtol and in rotary wing mode then don't execute the mission landing
because it's designed to be flow as a fixed wing
- if vtol and in rotary wing mode and mission land is available then fly directly
to landing point and don't go home!

Signed-off-by: RomanBapst <bapstroman@gmail.com>
2021-01-04 11:58:16 +01:00
RomanBapst 6c3f413379 mission: reset work_item_type during mission inactivation
- this fixes a race condition which happens when an RTL is triggered
during the final approach of a mission landing. In that case the mission inactive
method is never called.

Signed-off-by: RomanBapst <bapstroman@gmail.com>
2021-01-04 11:58:16 +01:00
RomanBapst b63f202b3c various fixed for bugs encountered during testing
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2021-01-04 11:58:16 +01:00
RomanBapst cfb3cdc82f rtl: use math namespace
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2021-01-04 11:58:16 +01:00
RomanBapst 94d3ab28ee rtl: fixed computation of return altitude when mission landing is available
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2021-01-04 11:58:16 +01:00