Commit Graph
100 Commits
Author SHA1 Message Date
Roman Bapst d6413a6a90 Refactor uncommanded descend Quad-Chute (#21598)
* refactored uncommanded descend quadchute
- use fixed altitude error threshold
- compute error relative to higest altitude the vehicle has achieved
since it has flown below the altitude reference (setpoint)

* disabled altitude loss quadchute by default

* altitude loss quadchute: added protection against invalid local z


---------

Signed-off-by: RomanBapst <bapstroman@gmail.com>
2023-05-31 17:57:50 +02:00
Roman Bapst 33b54f7c57 vtol_att_control: Consolidate logic for front transiton completion (#21107)
* vtol_att_control: consolidate logic for front transiton completion

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

* addressed review comments

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

---------

Signed-off-by: RomanBapst <bapstroman@gmail.com>
2023-02-24 15:52:27 +03:00
RomanBapst b00efcd966 cleanup
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2023-02-16 11:28:41 +01:00
RomanBapst 7ef2bff0a2 FeasibilityChecker: Fixed bug and added unit test for it
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2023-02-16 11:28:41 +01:00
RomanBapst 2e50277695 improved function naming
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2023-02-16 11:28:41 +01:00
RomanBapst 8ecb550331 cleanup
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2023-02-16 11:28:41 +01:00
RomanBapst 00b1968a5c more clang tidy stuff
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2023-02-16 11:28:41 +01:00
RomanBapst b8d0a8821a fixed clang tidy
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2023-02-16 11:28:41 +01:00
RomanBapst c09263d53c use correct type
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2023-02-16 11:28:41 +01:00
RomanBapst 11fd3ef71a use legacy parameter system and cleaned up vehicle type
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2023-02-16 11:28:41 +01:00
RomanBapst 741fbb931d fixed land requirement for VTOL
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2023-02-16 11:28:41 +01:00
RomanBapst 6e07af959f fixed bug in Matrix library
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2023-02-16 11:28:41 +01:00
RomanBapst 925ad97ff3 added unit tests
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2023-02-16 11:28:41 +01:00
RomanBapst 11143def82 tried to add functional unit test
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2023-02-16 11:28:41 +01:00
RomanBapst ceb8f6e1d5 started with feasibility checks
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2023-02-16 11:28:41 +01:00
Roman Bapst 4646762f9d Use multiplication instead of division 2023-01-28 10:31:17 -05:00
RomanBapst ffdf186598 vtol_att_control: reset transition timer states when starting a transition
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2022-12-16 11:36:00 +03:00
RomanBapst 48e8477799 vtol_att_control: disable maximum height for quadchute by default
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2022-12-15 15:40:53 +03:00
RomanBapst 9ed51ba8ed quadchute: introduced parameter to specify maximum height
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2022-12-15 15:40:53 +03:00
Roman Bapst 257d4e473b fixed tailsitter transitions (#20756)
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2022-12-13 10:38:38 +01:00
Roman BapstandSilvan Fuhrer cfb670fbb3 Refactor quadchute logic (#20704)
* moved computation of _time_since_trans_start into base class

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

* refactor quadchute logic
- move entired logic into VtolType class
- split into smaller functions

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

* Update src/modules/vtol_att_control/vtol_type.h

Co-authored-by: Silvan Fuhrer <silvan@auterion.com>

Signed-off-by: RomanBapst <bapstroman@gmail.com>
Co-authored-by: Silvan Fuhrer <silvan@auterion.com>
2022-12-08 11:56:20 +03:00
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