Commit Graph
100 Commits
Author SHA1 Message Date
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
RomanBapst 12875f7869 mission: when land start marker present, look for loiter to altitude item
instead of using the previous waypoint as landing target
- the previous waypoint could be miles away

Signed-off-by: RomanBapst <bapstroman@gmail.com>
2021-01-04 11:58:16 +01:00
RomanBapst e0c394a00b mission: fixed bug in computation of landing altitude
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2021-01-04 11:58:16 +01:00
RomanBapst a4a03e86da mission: be more intelligent about saying that we are on a mission landing
- previously the decision of being on a landing pattern was taken by just
looking at the current mission index. However, even if the current mission
index indicates a landing pattern the vehicle could be at an arbitrary location, far
from being established on the pattern.

Signed-off-by: RomanBapst <bapstroman@gmail.com>
2021-01-04 11:58:16 +01:00
RomanBapst 29c3ce6d5d reworked RTL cone implementation
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2021-01-04 11:58:16 +01:00
RomanBapst dad5ce1e41 rtl: set proper acceptance radius for move_to_land item
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2021-01-04 11:58:16 +01:00
RomanBapst 6ff95ae645 RTL: implement proper RTL sequence for VTOL
- descend to RTL descend altitude
- transition
- move to land waypoint
- loiter and then land

Signed-off-by: RomanBapst <bapstroman@gmail.com>
2021-01-04 11:58:16 +01:00
RomanBapst 197a21a22e rtl: for fixed wing (also vtol) make the descend state of RTL use type
NAV_CMD_LOITER_TO_ALT

- this allows vtol to track the loiter circle during the transition instead
of trying to fly to the landing position

Signed-off-by: RomanBapst <bapstroman@gmail.com>
2021-01-04 11:58:16 +01:00
RomanBapst bb2826ad27 rtl: for vtol first desend down to RTL descend altitude before doing a transition
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2021-01-04 11:58:16 +01:00
RomanBapst e81d5daa66 rtl: if very close to home then do not descend if higher than RTL_DESCEND_ALT
before reaching home

Signed-off-by: RomanBapst <bapstroman@gmail.com>
2021-01-04 11:58:16 +01:00