mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-02-20 10:54:14 +01:00
Merged in Oblivium/librepilot/LP-598_Autotakeoff_Corrections (pull request #512)
LP-598 Autotakeoff Corrections Approved-by: Lalanne Laurent <f5soh@free.fr> Approved-by: Jan NIJS <dr.oblivium@gmail.com>
This commit is contained in:
commit
3bef6b29d0
@ -3,13 +3,13 @@
|
||||
* @addtogroup LibrePilotModules LibrePilot Modules
|
||||
* @{
|
||||
* @addtogroup PathFollower FSM
|
||||
* @brief Executes landing sequence via an FSM
|
||||
* @brief Executes auto takeoff sequence via an FSM
|
||||
* @{
|
||||
*
|
||||
* @file vtollandfsm.h
|
||||
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
|
||||
* @file vtolautotakeofffsm.h
|
||||
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016-2018
|
||||
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
|
||||
* @brief Executes FSM for landing sequence
|
||||
* @brief Executes FSM for auto takeoff sequence
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
@ -144,7 +144,7 @@ protected:
|
||||
int32_t runAlways();
|
||||
|
||||
void updateVtolAutoTakeoffFSMStatus();
|
||||
void assessAltitude(void);
|
||||
float assessAltitude(void);
|
||||
|
||||
void setStateTimeout(int32_t count);
|
||||
|
||||
|
@ -7,7 +7,8 @@
|
||||
* @{
|
||||
*
|
||||
* @file vtollandfsm.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
|
||||
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2018
|
||||
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
|
||||
* @brief Executes FSM for landing sequence
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
@ -133,7 +134,7 @@ protected:
|
||||
int32_t runState();
|
||||
int32_t runAlways();
|
||||
void updateVtolLandFSMStatus();
|
||||
void assessAltitude(void);
|
||||
float assessAltitude(void);
|
||||
|
||||
void setStateTimeout(int32_t count);
|
||||
|
||||
|
@ -1,10 +1,10 @@
|
||||
/*
|
||||
******************************************************************************
|
||||
*
|
||||
* @file vtollandcontroller.cpp
|
||||
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
|
||||
* @file vtolautotakeoffcontroller.cpp
|
||||
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016-2018
|
||||
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
|
||||
* @brief Vtol landing controller loop
|
||||
* @brief Vtol auto takeoff controller loop
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
* @addtogroup LibrePilot LibrePilotModules Modules PathFollower Navigation
|
||||
*
|
||||
@ -132,11 +132,11 @@ void VtolAutoTakeoffController::ObjectiveUpdated(void)
|
||||
if (mOverride) {
|
||||
// override pathDesired from PathPlanner with current position,
|
||||
// as we deliberately don't care about the location of the waypoints on the map
|
||||
float velocity_down;
|
||||
float autotakeoff_velocity;
|
||||
float autotakeoff_height;
|
||||
PositionStateData positionState;
|
||||
PositionStateGet(&positionState);
|
||||
FlightModeSettingsAutoTakeOffVelocityGet(&velocity_down);
|
||||
FlightModeSettingsAutoTakeOffVelocityGet(&autotakeoff_velocity);
|
||||
FlightModeSettingsAutoTakeOffHeightGet(&autotakeoff_height);
|
||||
autotakeoff_height = fabsf(autotakeoff_height);
|
||||
if (autotakeoff_height < AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MIN) {
|
||||
@ -144,7 +144,7 @@ void VtolAutoTakeoffController::ObjectiveUpdated(void)
|
||||
} else if (autotakeoff_height > AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MAX) {
|
||||
autotakeoff_height = AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MAX;
|
||||
}
|
||||
controlDown.UpdateVelocitySetpoint(velocity_down);
|
||||
controlDown.UpdateVelocitySetpoint(-autotakeoff_velocity);
|
||||
controlNE.UpdateVelocitySetpoint(0.0f, 0.0f);
|
||||
controlNE.UpdatePositionSetpoint(positionState.North, positionState.East);
|
||||
|
||||
@ -320,9 +320,9 @@ void VtolAutoTakeoffController::UpdateAutoPilot()
|
||||
// 1. Arming must be done whilst in the AutoTakeOff flight mode
|
||||
// 2. If the AutoTakeoff flight mode is selected and already armed, requires disarming first
|
||||
// 3. Wait for armed state
|
||||
// 4. Once the user increases the throttle position to above 50%, then and only then initiate auto-takeoff.
|
||||
// 5. Whilst the throttle is < 50% before takeoff, all stick inputs are being ignored.
|
||||
// 6. If during the autotakeoff sequence, at any stage, if the throttle stick position reduces to less than 10%, landing is initiated.
|
||||
// 4. Once the user increases the throttle position to above 30%, then and only then initiate auto-takeoff.
|
||||
// 5. Whilst the throttle is < 30% before takeoff, all stick inputs are being ignored.
|
||||
// 6. If during the autotakeoff sequence, at any stage, the throttle stick position reduces to less than 10%, landing is initiated.
|
||||
|
||||
switch (autotakeoffState) {
|
||||
case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_REQUIREUNARMEDFIRST:
|
||||
|
@ -2,7 +2,8 @@
|
||||
******************************************************************************
|
||||
*
|
||||
* @file vtolautotakeofffsm.cpp
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
|
||||
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2018
|
||||
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
|
||||
* @brief This autotakeoff state machine is a helper state machine to the
|
||||
* VtolAutoTakeoffController.
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
@ -60,11 +61,11 @@ extern "C" {
|
||||
|
||||
|
||||
// Private constants
|
||||
#define TIMER_COUNT_PER_SECOND (1000 / vtolPathFollowerSettings->UpdatePeriod)
|
||||
#define TIMEOUT_SLOWSTART (2 * TIMER_COUNT_PER_SECOND)
|
||||
#define TIMEOUT_THRUSTUP (1 * TIMER_COUNT_PER_SECOND)
|
||||
#define TIMEOUT_THRUSTDOWN (5 * TIMER_COUNT_PER_SECOND)
|
||||
#define AUTOTAKEOFFING_SLOWDOWN_HEIGHT -5.0f
|
||||
#define TIMER_COUNT_PER_SECOND (1000 / vtolPathFollowerSettings->UpdatePeriod)
|
||||
#define TIMEOUT_SLOWSTART (2 * TIMER_COUNT_PER_SECOND)
|
||||
#define TIMEOUT_THRUSTUP (1 * TIMER_COUNT_PER_SECOND)
|
||||
#define TIMEOUT_THRUSTDOWN (5 * TIMER_COUNT_PER_SECOND)
|
||||
#define AUTOTAKEOFF_SLOWDOWN_HEIGHT -5.0f
|
||||
|
||||
VtolAutoTakeoffFSM::PathFollowerFSM_AutoTakeoffStateHandler_T VtolAutoTakeoffFSM::sAutoTakeoffStateTable[AUTOTAKEOFF_STATE_SIZE] = {
|
||||
[AUTOTAKEOFF_STATE_INACTIVE] = { .setup = &VtolAutoTakeoffFSM::setup_inactive, .run = 0 },
|
||||
@ -246,14 +247,8 @@ void VtolAutoTakeoffFSM::setState(StatusVtolAutoTakeoffStateOptions newState, St
|
||||
mAutoTakeoffData->currentState = newState;
|
||||
|
||||
if (newState != STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE) {
|
||||
PositionStateData positionState;
|
||||
PositionStateGet(&positionState);
|
||||
float takeOffDown = 0.0f;
|
||||
if (mAutoTakeoffData->takeOffLocation.Status == TAKEOFFLOCATION_STATUS_VALID) {
|
||||
takeOffDown = mAutoTakeoffData->takeOffLocation.Down;
|
||||
}
|
||||
mAutoTakeoffData->fsmAutoTakeoffStatus.AltitudeAtState[newState] = positionState.Down - takeOffDown;
|
||||
assessAltitude();
|
||||
float altitudeAboveTakeoff = assessAltitude();
|
||||
mAutoTakeoffData->fsmAutoTakeoffStatus.AltitudeAtState[newState] = altitudeAboveTakeoff;
|
||||
}
|
||||
|
||||
// Restart state timer counter
|
||||
@ -266,6 +261,7 @@ void VtolAutoTakeoffFSM::setState(StatusVtolAutoTakeoffStateOptions newState, St
|
||||
(this->*sAutoTakeoffStateTable[mAutoTakeoffData->currentState].setup)();
|
||||
}
|
||||
|
||||
// Update StatusVtolAutoTakeoff UAVObject with new status
|
||||
updateVtolAutoTakeoffFSMStatus();
|
||||
}
|
||||
|
||||
@ -276,6 +272,7 @@ void VtolAutoTakeoffFSM::setStateTimeout(int32_t count)
|
||||
mAutoTakeoffData->stateTimeoutCount = count;
|
||||
}
|
||||
|
||||
// Update StatusVtolAutoTakeoff UAVObject
|
||||
void VtolAutoTakeoffFSM::updateVtolAutoTakeoffFSMStatus()
|
||||
{
|
||||
mAutoTakeoffData->fsmAutoTakeoffStatus.State = mAutoTakeoffData->currentState;
|
||||
@ -288,7 +285,7 @@ void VtolAutoTakeoffFSM::updateVtolAutoTakeoffFSMStatus()
|
||||
}
|
||||
|
||||
|
||||
void VtolAutoTakeoffFSM::assessAltitude(void)
|
||||
float VtolAutoTakeoffFSM::assessAltitude(void)
|
||||
{
|
||||
float positionDown;
|
||||
|
||||
@ -298,11 +295,13 @@ void VtolAutoTakeoffFSM::assessAltitude(void)
|
||||
takeOffDown = mAutoTakeoffData->takeOffLocation.Down;
|
||||
}
|
||||
float positionDownRelativeToTakeoff = positionDown - takeOffDown;
|
||||
if (positionDownRelativeToTakeoff < AUTOTAKEOFFING_SLOWDOWN_HEIGHT) {
|
||||
if (positionDownRelativeToTakeoff < AUTOTAKEOFF_SLOWDOWN_HEIGHT) {
|
||||
mAutoTakeoffData->flLowAltitude = false;
|
||||
} else {
|
||||
mAutoTakeoffData->flLowAltitude = true;
|
||||
}
|
||||
// Return the altitude above takeoff, which is the negation of positionDownRelativeToTakeoff
|
||||
return -positionDownRelativeToTakeoff;
|
||||
}
|
||||
|
||||
// Action the required state from plans.c
|
||||
@ -347,10 +346,10 @@ void VtolAutoTakeoffFSM::setup_checkstate(void)
|
||||
// Assumptions that do not need to be checked if flight mode is AUTOTAKEOFF
|
||||
// 1. Already armed
|
||||
// 2. Not in flight. This was checked in plans.c
|
||||
// 3. User has placed throttle position to more than 50% to allow autotakeoff
|
||||
// 3. User has placed throttle position to more than 30% to allow autotakeoff
|
||||
|
||||
// If pathplanner, we need additional checks
|
||||
// E.g. if inflight, this mode is just positon hol
|
||||
// E.g. if inflight, this mode is just position hold
|
||||
StabilizationDesiredData stabDesired;
|
||||
|
||||
StabilizationDesiredGet(&stabDesired);
|
||||
@ -407,15 +406,13 @@ void VtolAutoTakeoffFSM::run_slowstart(__attribute__((unused)) uint8_t flTimeout
|
||||
}
|
||||
}
|
||||
|
||||
// STATE: THRUSTUP spools up motors to vtol min over 5 seconds for effect.
|
||||
// STATE: THRUSTUP spools up motors to vtol min over 1 second for effect.
|
||||
// PID loops may be cumulating I terms but that problem needs to be solved
|
||||
#define THRUSTUP_FINAL_THRUST_AS_RATIO_OF_VTOLMAX 0.8f
|
||||
void VtolAutoTakeoffFSM::setup_thrustup(void)
|
||||
{
|
||||
setStateTimeout(TIMEOUT_THRUSTUP);
|
||||
mAutoTakeoffData->flZeroStabiHorizontal = false;
|
||||
StabilizationDesiredData stabDesired;
|
||||
StabilizationDesiredGet(&stabDesired);
|
||||
mAutoTakeoffData->sum2 = THRUSTUP_FINAL_THRUST_AS_RATIO_OF_VTOLMAX * vtolPathFollowerSettings->ThrustLimits.Max;
|
||||
mAutoTakeoffData->sum1 = (mAutoTakeoffData->sum2 - mAutoTakeoffData->boundThrustMax) / (float)TIMEOUT_THRUSTUP;
|
||||
mAutoTakeoffData->boundThrustMin = vtolPathFollowerSettings->ThrustLimits.Min;
|
||||
|
@ -2,7 +2,8 @@
|
||||
******************************************************************************
|
||||
*
|
||||
* @file vtollandfsm.cpp
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
|
||||
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2018
|
||||
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
|
||||
* @brief This landing state machine is a helper state machine to the
|
||||
* VtolLandController.
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
@ -275,14 +276,8 @@ void VtolLandFSM::setState(StatusVtolLandStateOptions newState, StatusVtolLandSt
|
||||
mLandData->currentState = newState;
|
||||
|
||||
if (newState != STATUSVTOLLAND_STATE_INACTIVE) {
|
||||
PositionStateData positionState;
|
||||
PositionStateGet(&positionState);
|
||||
float takeOffDown = 0.0f;
|
||||
if (mLandData->takeOffLocation.Status == TAKEOFFLOCATION_STATUS_VALID) {
|
||||
takeOffDown = mLandData->takeOffLocation.Down;
|
||||
}
|
||||
mLandData->fsmLandStatus.AltitudeAtState[newState] = positionState.Down - takeOffDown;
|
||||
assessAltitude();
|
||||
float altitudeAboveTakeoff = assessAltitude();
|
||||
mLandData->fsmLandStatus.AltitudeAtState[newState] = altitudeAboveTakeoff;
|
||||
}
|
||||
|
||||
// Restart state timer counter
|
||||
@ -332,7 +327,7 @@ float VtolLandFSM::BoundVelocityDown(float velocity_down)
|
||||
}
|
||||
}
|
||||
|
||||
void VtolLandFSM::assessAltitude(void)
|
||||
float VtolLandFSM::assessAltitude(void)
|
||||
{
|
||||
float positionDown;
|
||||
|
||||
@ -347,6 +342,8 @@ void VtolLandFSM::assessAltitude(void)
|
||||
} else {
|
||||
mLandData->flLowAltitude = true;
|
||||
}
|
||||
// Return the altitude above takeoff, which is the negation of positionDownRelativeToTakeoff
|
||||
return -positionDownRelativeToTakeoff;
|
||||
}
|
||||
|
||||
|
||||
|
Loading…
x
Reference in New Issue
Block a user