rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-20 10:54:14 +01:00
Code Issues Releases Activity

Merged in Oblivium/librepilot/LP-598_Autotakeoff_Corrections (pull request #512)

Browse Source 
LP-598 Autotakeoff Corrections

Approved-by: Lalanne Laurent <f5soh@free.fr>
Approved-by: Jan NIJS <dr.oblivium@gmail.com>
This commit is contained in:
Jan NIJS 2018-09-29 09:54:27 +00:00 committed by Lalanne Laurent
commit 3bef6b29d0
5 changed files with 42 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
flight/modules/PathFollower/inc/vtolautotakeofffsm.h
View File

@ -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);

5
flight/modules/PathFollower/inc/vtollandfsm.h
View File

@ -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);

18
flight/modules/PathFollower/vtolautotakeoffcontroller.cpp
View File

@ -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:

39
flight/modules/PathFollower/vtolautotakeofffsm.cpp
View File

@ -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;

17
flight/modules/PathFollower/vtollandfsm.cpp
View File

@ -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;
}