mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-02-20 10:54:14 +01:00
OP-1900 Put Sequencing state machine for vixed wing autotakeoff out of plan.c and into vtolautotakeoffcontroller, removed all special casing for autotakeoff and land from outside of pathfollower itself and code in plans.c - especially cleaned up pathplanner.c and made mode agnostic (which it should be) - added optional rewinding for pathplans
This commit is contained in:
parent
4b7edee534
commit
ccfe39633e
@ -169,172 +169,33 @@ void plan_setup_returnToBase()
|
||||
PathDesiredSet(&pathDesired);
|
||||
}
|
||||
|
||||
|
||||
// Vtol AutoTakeoff invocation from flight mode requires the following sequence:
|
||||
// 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.
|
||||
|
||||
static StatusVtolAutoTakeoffControlStateOptions autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORARMED;
|
||||
|
||||
#define AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MIN 2.0f
|
||||
#define AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MAX 50.0f
|
||||
static void plan_setup_AutoTakeoff_helper(PathDesiredData *pathDesired)
|
||||
void plan_setup_AutoTakeoff()
|
||||
{
|
||||
PathDesiredData pathDesired;
|
||||
PositionStateData positionState;
|
||||
|
||||
PositionStateGet(&positionState);
|
||||
float velocity_down;
|
||||
float autotakeoff_height;
|
||||
|
||||
FlightModeSettingsAutoTakeOffVelocityGet(&velocity_down);
|
||||
FlightModeSettingsAutoTakeOffHeightGet(&autotakeoff_height);
|
||||
autotakeoff_height = fabsf(autotakeoff_height);
|
||||
if (autotakeoff_height < AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MIN) {
|
||||
autotakeoff_height = AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MIN;
|
||||
} else if (autotakeoff_height > AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MAX) {
|
||||
autotakeoff_height = AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MAX;
|
||||
}
|
||||
autotakeoff_height = fabsf(autotakeoff_height);
|
||||
|
||||
pathDesired.Start.North = positionState.North;
|
||||
pathDesired.Start.East = positionState.East;
|
||||
pathDesired.Start.Down = positionState.Down;
|
||||
pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_NORTH] = 0.0f;
|
||||
pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_EAST] = 0.0f;
|
||||
pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_DOWN] = 0.0f;
|
||||
pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_CONTROLSTATE] = 0.0f;
|
||||
|
||||
pathDesired->Start.North = positionState.North;
|
||||
pathDesired->Start.East = positionState.East;
|
||||
pathDesired->Start.Down = positionState.Down;
|
||||
pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_NORTH] = 0.0f;
|
||||
pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_EAST] = 0.0f;
|
||||
pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_DOWN] = -velocity_down;
|
||||
pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_CONTROLSTATE] = (float)autotakeoffState;
|
||||
pathDesired.End.North = positionState.North;
|
||||
pathDesired.End.East = positionState.East;
|
||||
pathDesired.End.Down = positionState.Down - autotakeoff_height;
|
||||
|
||||
pathDesired->End.North = positionState.North;
|
||||
pathDesired->End.East = positionState.East;
|
||||
pathDesired->End.Down = positionState.Down - autotakeoff_height;
|
||||
|
||||
pathDesired->StartingVelocity = 0.0f;
|
||||
pathDesired->EndingVelocity = 0.0f;
|
||||
pathDesired->Mode = PATHDESIRED_MODE_AUTOTAKEOFF;
|
||||
}
|
||||
|
||||
#define AUTOTAKEOFF_INFLIGHT_THROTTLE_CHECK_LIMIT 0.2f
|
||||
void plan_setup_AutoTakeoff()
|
||||
{
|
||||
FrameType_t frame = GetCurrentFrameType();
|
||||
VtolPathFollowerSettingsTreatCustomCraftAsOptions TreatCustomCraftAs;
|
||||
|
||||
VtolPathFollowerSettingsTreatCustomCraftAsGet(&TreatCustomCraftAs);
|
||||
if (frame == FRAME_TYPE_CUSTOM && TreatCustomCraftAs == VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_FIXEDWING) {
|
||||
frame = FRAME_TYPE_FIXED_WING;
|
||||
}
|
||||
|
||||
autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORARMED;
|
||||
// We only allow takeoff if the state transition of disarmed to armed occurs
|
||||
// whilst in the autotake flight mode
|
||||
FlightStatusData flightStatus;
|
||||
FlightStatusGet(&flightStatus);
|
||||
StabilizationDesiredData stabiDesired;
|
||||
StabilizationDesiredGet(&stabiDesired);
|
||||
|
||||
if (frame != FRAME_TYPE_FIXED_WING) {
|
||||
// Are we inflight?
|
||||
if (flightStatus.Armed && stabiDesired.Thrust > AUTOTAKEOFF_INFLIGHT_THROTTLE_CHECK_LIMIT) {
|
||||
// ok assume already in flight and just enter position hold
|
||||
// if we are not actually inflight this will just be a violent autotakeoff
|
||||
autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_POSITIONHOLD;
|
||||
plan_setup_positionHold();
|
||||
} else {
|
||||
if (flightStatus.Armed) {
|
||||
autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_REQUIREUNARMEDFIRST;
|
||||
// Note that if this mode was invoked unintentionally whilst in flight, effectively
|
||||
// all inputs get ignored and the vtol continues to fly to its previous
|
||||
// stabi command.
|
||||
}
|
||||
PathDesiredData pathDesired;
|
||||
plan_setup_AutoTakeoff_helper(&pathDesired);
|
||||
PathDesiredSet(&pathDesired);
|
||||
}
|
||||
} else {
|
||||
// fixed wings do not require arming - or sequencing, as the takeoffcontroller has its own independent state machine
|
||||
if (flightStatus.Armed) {
|
||||
autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORMIDTHROTTLE;
|
||||
}
|
||||
PathDesiredData pathDesired;
|
||||
plan_setup_AutoTakeoff_helper(&pathDesired);
|
||||
PathDesiredSet(&pathDesired);
|
||||
}
|
||||
}
|
||||
|
||||
#define AUTOTAKEOFF_THROTTLE_LIMIT_TO_ALLOW_TAKEOFF_START 0.3f
|
||||
#define AUTOTAKEOFF_THROTTLE_ABORT_LIMIT 0.1f
|
||||
void plan_run_AutoTakeoff()
|
||||
{
|
||||
StatusVtolAutoTakeoffControlStateOptions priorState = autotakeoffState;
|
||||
|
||||
switch (autotakeoffState) {
|
||||
case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_REQUIREUNARMEDFIRST:
|
||||
{
|
||||
FlightStatusData flightStatus;
|
||||
FlightStatusGet(&flightStatus);
|
||||
if (!flightStatus.Armed) {
|
||||
autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORMIDTHROTTLE;
|
||||
}
|
||||
}
|
||||
break;
|
||||
case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORARMED:
|
||||
{
|
||||
FlightStatusData flightStatus;
|
||||
FlightStatusGet(&flightStatus);
|
||||
if (flightStatus.Armed) {
|
||||
autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORMIDTHROTTLE;
|
||||
}
|
||||
}
|
||||
break;
|
||||
case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORMIDTHROTTLE:
|
||||
{
|
||||
ManualControlCommandData cmd;
|
||||
ManualControlCommandGet(&cmd);
|
||||
|
||||
if (cmd.Throttle > AUTOTAKEOFF_THROTTLE_LIMIT_TO_ALLOW_TAKEOFF_START) {
|
||||
autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_INITIATE;
|
||||
}
|
||||
}
|
||||
break;
|
||||
case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_INITIATE:
|
||||
{
|
||||
ManualControlCommandData cmd;
|
||||
ManualControlCommandGet(&cmd);
|
||||
|
||||
if (cmd.Throttle < AUTOTAKEOFF_THROTTLE_ABORT_LIMIT) {
|
||||
autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_ABORT;
|
||||
plan_setup_land();
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_ABORT:
|
||||
{
|
||||
FlightStatusData flightStatus;
|
||||
FlightStatusGet(&flightStatus);
|
||||
if (!flightStatus.Armed) {
|
||||
autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORARMED;
|
||||
}
|
||||
}
|
||||
break;
|
||||
case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_POSITIONHOLD:
|
||||
// nothing to do. land has been requested. stay here for forever until mode change.
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
if (autotakeoffState != STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_ABORT &&
|
||||
autotakeoffState != STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_POSITIONHOLD) {
|
||||
if (priorState != autotakeoffState) {
|
||||
PathDesiredData pathDesired;
|
||||
plan_setup_AutoTakeoff_helper(&pathDesired);
|
||||
PathDesiredSet(&pathDesired);
|
||||
}
|
||||
}
|
||||
pathDesired.StartingVelocity = 0.0f;
|
||||
pathDesired.EndingVelocity = 0.0f;
|
||||
pathDesired.Mode = PATHDESIRED_MODE_AUTOTAKEOFF;
|
||||
PathDesiredSet(&pathDesired);
|
||||
}
|
||||
|
||||
static void plan_setup_land_helper(PathDesiredData *pathDesired)
|
||||
|
@ -140,9 +140,6 @@ void pathFollowerHandler(bool newinit)
|
||||
plan_run_VelocityRoam();
|
||||
}
|
||||
break;
|
||||
case FLIGHTSTATUS_FLIGHTMODE_AUTOTAKEOFF:
|
||||
plan_run_AutoTakeoff();
|
||||
break;
|
||||
case FLIGHTSTATUS_FLIGHTMODE_AUTOCRUISE:
|
||||
plan_run_AutoCruise();
|
||||
break;
|
||||
|
@ -70,6 +70,8 @@ private:
|
||||
PIDControlDown controlDown;
|
||||
PIDControlNE controlNE;
|
||||
uint8_t mActive;
|
||||
uint8_t mOverride;
|
||||
StatusVtolAutoTakeoffControlStateOptions autotakeoffState;
|
||||
};
|
||||
|
||||
#endif // VTOLAUTOTAKEOFFCONTROLLER_H
|
||||
|
@ -47,7 +47,7 @@ typedef enum {
|
||||
AUTOTAKEOFF_STATE_THRUSTDOWN, // Thrust down sequence
|
||||
AUTOTAKEOFF_STATE_THRUSTOFF, // Thrust is now off
|
||||
AUTOTAKEOFF_STATE_DISARMED, // Disarmed
|
||||
AUTOTAKEOFF_STATE_ABORT, // Abort on error triggerig fallback to hold
|
||||
AUTOTAKEOFF_STATE_ABORT, // Abort on error triggerig fallback to land
|
||||
AUTOTAKEOFF_STATE_SIZE
|
||||
} PathFollowerFSM_AutoTakeoffState_T;
|
||||
|
||||
|
@ -71,6 +71,7 @@ private:
|
||||
PIDControlDown controlDown;
|
||||
PIDControlNE controlNE;
|
||||
uint8_t mActive;
|
||||
uint8_t mOverride;
|
||||
};
|
||||
|
||||
#endif // VTOLLANDCONTROLLER_H
|
||||
|
@ -57,6 +57,7 @@ extern "C" {
|
||||
#include <stabilizationbank.h>
|
||||
#include <stabilizationdesired.h>
|
||||
#include <vtolselftuningstats.h>
|
||||
#include <statusvtolautotakeoff.h>
|
||||
#include <pathsummary.h>
|
||||
}
|
||||
|
||||
@ -67,6 +68,11 @@ extern "C" {
|
||||
#include "pidcontroldown.h"
|
||||
|
||||
// Private constants
|
||||
#define AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MIN 2.0f
|
||||
#define AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MAX 50.0f
|
||||
#define AUTOTAKEOFF_INFLIGHT_THROTTLE_CHECK_LIMIT 0.2f
|
||||
#define AUTOTAKEOFF_THROTTLE_LIMIT_TO_ALLOW_TAKEOFF_START 0.3f
|
||||
#define AUTOTAKEOFF_THROTTLE_ABORT_LIMIT 0.1f
|
||||
|
||||
// pointer to a singleton instance
|
||||
VtolAutoTakeoffController *VtolAutoTakeoffController::p_inst = 0;
|
||||
@ -79,11 +85,34 @@ VtolAutoTakeoffController::VtolAutoTakeoffController()
|
||||
void VtolAutoTakeoffController::Activate(void)
|
||||
{
|
||||
if (!mActive) {
|
||||
mActive = true;
|
||||
mActive = true;
|
||||
mOverride = true;
|
||||
SettingsUpdated();
|
||||
fsm->Activate();
|
||||
controlDown.Activate();
|
||||
controlNE.Activate();
|
||||
autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORARMED;
|
||||
// We only allow takeoff if the state transition of disarmed to armed occurs
|
||||
// whilst in the autotake flight mode
|
||||
FlightStatusData flightStatus;
|
||||
FlightStatusGet(&flightStatus);
|
||||
StabilizationDesiredData stabiDesired;
|
||||
StabilizationDesiredGet(&stabiDesired);
|
||||
|
||||
if (flightStatus.Armed) {
|
||||
// Are we inflight?
|
||||
if (stabiDesired.Thrust > AUTOTAKEOFF_INFLIGHT_THROTTLE_CHECK_LIMIT || flightStatus.ControlChain.PathPlanner == FLIGHTSTATUS_CONTROLCHAIN_TRUE) {
|
||||
// ok assume already in flight and just enter position hold
|
||||
// if we are not actually inflight this will just be a violent autotakeoff
|
||||
autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_POSITIONHOLD;
|
||||
} else {
|
||||
autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_REQUIREUNARMEDFIRST;
|
||||
// Note that if this mode was invoked unintentionally whilst in flight, effectively
|
||||
// all inputs get ignored and the vtol continues to fly to its previous
|
||||
// stabi command.
|
||||
}
|
||||
}
|
||||
fsm->setControlState(autotakeoffState);
|
||||
}
|
||||
}
|
||||
|
||||
@ -100,14 +129,37 @@ uint8_t VtolAutoTakeoffController::Mode(void)
|
||||
// Objective updated in pathdesired, e.g. same flight mode but new target velocity
|
||||
void VtolAutoTakeoffController::ObjectiveUpdated(void)
|
||||
{
|
||||
// Set the objective's target velocity
|
||||
if (mOverride) {
|
||||
// override pathDesired from PathPLanner with current position,
|
||||
// as we deliberately don' not care about the location of the waypoints on the map
|
||||
float velocity_down;
|
||||
float autotakeoff_height;
|
||||
PositionStateData positionState;
|
||||
PositionStateGet(&positionState);
|
||||
FlightModeSettingsAutoTakeOffVelocityGet(&velocity_down);
|
||||
FlightModeSettingsAutoTakeOffHeightGet(&autotakeoff_height);
|
||||
autotakeoff_height = fabsf(autotakeoff_height);
|
||||
if (autotakeoff_height < AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MIN) {
|
||||
autotakeoff_height = AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MIN;
|
||||
} else if (autotakeoff_height > AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MAX) {
|
||||
autotakeoff_height = AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MAX;
|
||||
}
|
||||
controlDown.UpdateVelocitySetpoint(velocity_down);
|
||||
controlNE.UpdateVelocitySetpoint(0.0f, 0.0f);
|
||||
controlNE.UpdatePositionSetpoint(positionState.North, positionState.East);
|
||||
|
||||
controlDown.UpdateVelocitySetpoint(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_DOWN]);
|
||||
controlNE.UpdateVelocitySetpoint(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_NORTH],
|
||||
pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_EAST]);
|
||||
controlNE.UpdatePositionSetpoint(pathDesired->End.North, pathDesired->End.East);
|
||||
controlDown.UpdatePositionSetpoint(pathDesired->End.Down);
|
||||
fsm->setControlState((StatusVtolAutoTakeoffControlStateOptions)pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_CONTROLSTATE]);
|
||||
|
||||
controlDown.UpdatePositionSetpoint(positionState.Down - autotakeoff_height);
|
||||
mOverride = false; // further updates always come from ManualControl and will control horizontal position
|
||||
} else {
|
||||
// Set the objective's target velocity
|
||||
|
||||
controlDown.UpdateVelocitySetpoint(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_DOWN]);
|
||||
controlNE.UpdateVelocitySetpoint(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_NORTH],
|
||||
pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_EAST]);
|
||||
controlNE.UpdatePositionSetpoint(pathDesired->End.North, pathDesired->End.East);
|
||||
controlDown.UpdatePositionSetpoint(pathDesired->End.Down);
|
||||
}
|
||||
}
|
||||
void VtolAutoTakeoffController::Deactivate(void)
|
||||
{
|
||||
@ -224,6 +276,16 @@ int8_t VtolAutoTakeoffController::UpdateStabilizationDesired()
|
||||
|
||||
controlNE.GetNECommand(&northCommand, &eastCommand);
|
||||
stabDesired.Thrust = controlDown.GetDownCommand();
|
||||
switch (autotakeoffState) {
|
||||
case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORARMED:
|
||||
case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORMIDTHROTTLE:
|
||||
case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_REQUIREUNARMEDFIRST:
|
||||
case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_ABORT:
|
||||
stabDesired.Thrust = 0.0f;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
float angle_radians = DEG2RAD(attitudeState.Yaw);
|
||||
float cos_angle = cosf(angle_radians);
|
||||
@ -251,6 +313,79 @@ int8_t VtolAutoTakeoffController::UpdateStabilizationDesired()
|
||||
|
||||
void VtolAutoTakeoffController::UpdateAutoPilot()
|
||||
{
|
||||
// state machine updates:
|
||||
// Vtol AutoTakeoff invocation from flight mode requires the following sequence:
|
||||
// 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.
|
||||
|
||||
switch (autotakeoffState) {
|
||||
case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_REQUIREUNARMEDFIRST:
|
||||
{
|
||||
FlightStatusData flightStatus;
|
||||
FlightStatusGet(&flightStatus);
|
||||
if (!flightStatus.Armed) {
|
||||
autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORARMED;
|
||||
fsm->setControlState(autotakeoffState);
|
||||
}
|
||||
}
|
||||
break;
|
||||
case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORARMED:
|
||||
{
|
||||
FlightStatusData flightStatus;
|
||||
FlightStatusGet(&flightStatus);
|
||||
if (flightStatus.Armed) {
|
||||
autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORMIDTHROTTLE;
|
||||
fsm->setControlState(autotakeoffState);
|
||||
}
|
||||
}
|
||||
break;
|
||||
case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORMIDTHROTTLE:
|
||||
{
|
||||
ManualControlCommandData cmd;
|
||||
ManualControlCommandGet(&cmd);
|
||||
|
||||
if (cmd.Throttle > AUTOTAKEOFF_THROTTLE_LIMIT_TO_ALLOW_TAKEOFF_START) {
|
||||
autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_INITIATE;
|
||||
fsm->setControlState(autotakeoffState);
|
||||
}
|
||||
}
|
||||
break;
|
||||
case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_INITIATE:
|
||||
{
|
||||
ManualControlCommandData cmd;
|
||||
ManualControlCommandGet(&cmd);
|
||||
FlightStatusData flightStatus;
|
||||
FlightStatusGet(&flightStatus);
|
||||
|
||||
// we do not do a takeoff abort in pathplanner mode
|
||||
if (flightStatus.ControlChain.PathPlanner != FLIGHTSTATUS_CONTROLCHAIN_TRUE &&
|
||||
cmd.Throttle < AUTOTAKEOFF_THROTTLE_ABORT_LIMIT) {
|
||||
autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_ABORT;
|
||||
fsm->setControlState(autotakeoffState);
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_ABORT:
|
||||
{
|
||||
FlightStatusData flightStatus;
|
||||
FlightStatusGet(&flightStatus);
|
||||
if (!flightStatus.Armed) {
|
||||
autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORARMED;
|
||||
fsm->setControlState(autotakeoffState);
|
||||
}
|
||||
}
|
||||
break;
|
||||
case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_POSITIONHOLD:
|
||||
// nothing to do. land has been requested. stay here for forever until mode change.
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
fsm->Update();
|
||||
|
||||
UpdateVelocityDesired();
|
||||
|
@ -76,7 +76,8 @@ VtolLandController::VtolLandController()
|
||||
void VtolLandController::Activate(void)
|
||||
{
|
||||
if (!mActive) {
|
||||
mActive = true;
|
||||
mActive = true;
|
||||
mOverride = true;
|
||||
SettingsUpdated();
|
||||
fsm->Activate();
|
||||
controlDown.Activate();
|
||||
@ -97,11 +98,24 @@ uint8_t VtolLandController::Mode(void)
|
||||
// Objective updated in pathdesired, e.g. same flight mode but new target velocity
|
||||
void VtolLandController::ObjectiveUpdated(void)
|
||||
{
|
||||
// Set the objective's target velocity
|
||||
controlDown.UpdateVelocitySetpoint(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_DOWN]);
|
||||
controlNE.UpdateVelocitySetpoint(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_NORTH],
|
||||
pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_EAST]);
|
||||
controlNE.UpdatePositionSetpoint(pathDesired->End.North, pathDesired->End.East);
|
||||
if (mOverride) {
|
||||
// override pathDesired from PathPLanner with current position,
|
||||
// as we deliberately don' not care about the location of the waypoints on the map
|
||||
float velocity_down;
|
||||
PositionStateData positionState;
|
||||
PositionStateGet(&positionState);
|
||||
FlightModeSettingsLandingVelocityGet(&velocity_down);
|
||||
controlDown.UpdateVelocitySetpoint(velocity_down);
|
||||
controlNE.UpdateVelocitySetpoint(0.0f, 0.0f);
|
||||
controlNE.UpdatePositionSetpoint(positionState.North, positionState.East);
|
||||
mOverride = false; // further updates always come from ManualControl and will control horizontal position
|
||||
} else {
|
||||
// Set the objective's target velocity
|
||||
controlDown.UpdateVelocitySetpoint(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_DOWN]);
|
||||
controlNE.UpdateVelocitySetpoint(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_NORTH],
|
||||
pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_EAST]);
|
||||
controlNE.UpdatePositionSetpoint(pathDesired->End.North, pathDesired->End.East);
|
||||
}
|
||||
}
|
||||
void VtolLandController::Deactivate(void)
|
||||
{
|
||||
|
@ -48,8 +48,6 @@
|
||||
#include "plans.h"
|
||||
#include <sanitycheck.h>
|
||||
#include <vtolpathfollowersettings.h>
|
||||
#include <statusvtolautotakeoff.h>
|
||||
#include <statusvtolland.h>
|
||||
#include <manualcontrolcommand.h>
|
||||
|
||||
// Private constants
|
||||
@ -80,9 +78,6 @@ static uint8_t conditionPointingTowardsNext();
|
||||
static uint8_t conditionPythonScript();
|
||||
static uint8_t conditionImmediate();
|
||||
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev);
|
||||
static void planner_setup_pathdesired_land(PathDesiredData *pathDesired);
|
||||
static void planner_setup_pathdesired_takeoff(PathDesiredData *pathDesired);
|
||||
static void planner_setup_pathdesired(PathDesiredData *pathDesired, bool overwrite_start_position);
|
||||
|
||||
|
||||
// Private variables
|
||||
@ -132,8 +127,6 @@ int32_t PathPlannerInitialize()
|
||||
VelocityStateInitialize();
|
||||
WaypointInitialize();
|
||||
WaypointActiveInitialize();
|
||||
StatusVtolAutoTakeoffInitialize();
|
||||
StatusVtolLandInitialize();
|
||||
|
||||
pathPlannerHandle = PIOS_CALLBACKSCHEDULER_Create(&pathPlannerTask, CALLBACK_PRIORITY_REGULAR, TASK_PRIORITY, CALLBACKINFO_RUNNING_PATHPLANNER0, STACK_SIZE_BYTES);
|
||||
pathDesiredUpdaterHandle = PIOS_CALLBACKSCHEDULER_Create(&updatePathDesired, CALLBACK_PRIORITY_CRITICAL, TASK_PRIORITY, CALLBACKINFO_RUNNING_PATHPLANNER1, STACK_SIZE_BYTES);
|
||||
@ -236,17 +229,17 @@ static void pathPlannerTask()
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
// the transition from pathplanner to another flightmode back to pathplanner
|
||||
// triggers a reset back to 0 index in the waypoint list
|
||||
if (pathplanner_active == false) {
|
||||
pathplanner_active = true;
|
||||
pathplanner_active = true;
|
||||
|
||||
// This triggers callback to update variable
|
||||
waypointActive.Index = 0;
|
||||
WaypointActiveSet(&waypointActive);
|
||||
|
||||
return;
|
||||
FlightModeSettingsFlightModeChangeRestartsPathPlanOptions restart;
|
||||
FlightModeSettingsFlightModeChangeRestartsPathPlanGet(&restart);
|
||||
if (restart == FLIGHTMODESETTINGS_FLIGHTMODECHANGERESTARTSPATHPLAN_TRUE) {
|
||||
setWaypoint(0);
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
WaypointInstGet(waypointActive.Index, &waypoint);
|
||||
@ -265,21 +258,6 @@ static void pathPlannerTask()
|
||||
return;
|
||||
}
|
||||
|
||||
// check start conditions
|
||||
// autotakeoff requires midpoint thrust if we are in a pending takeoff situation
|
||||
if (pathAction.Mode == PATHACTION_MODE_AUTOTAKEOFF) {
|
||||
pathAction.EndCondition = PATHACTION_ENDCONDITION_LEGREMAINING;
|
||||
if ((uint8_t)pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_CONTROLSTATE] == STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORMIDTHROTTLE) {
|
||||
ManualControlCommandData cmd;
|
||||
ManualControlCommandGet(&cmd);
|
||||
if (cmd.Throttle > AUTOTAKEOFF_THROTTLE_LIMIT_TO_ALLOW_TAKEOFF_START) {
|
||||
pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_CONTROLSTATE] = (float)STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_INITIATE;
|
||||
PathDesiredSet(&pathDesired);
|
||||
}
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// check if condition has been met
|
||||
endCondition = pathConditionCheck();
|
||||
@ -331,22 +309,45 @@ void updatePathDesired()
|
||||
WaypointActiveGet(&waypointActive);
|
||||
WaypointInstGet(waypointActive.Index, &waypoint);
|
||||
|
||||
// Capture if current mode is takeoff
|
||||
bool autotakeoff = (pathAction.Mode == PATHACTION_MODE_AUTOTAKEOFF);
|
||||
|
||||
PathActionInstGet(waypoint.Action, &pathAction);
|
||||
|
||||
PathDesiredData pathDesired;
|
||||
switch (pathAction.Mode) {
|
||||
case PATHACTION_MODE_AUTOTAKEOFF:
|
||||
planner_setup_pathdesired_takeoff(&pathDesired);
|
||||
break;
|
||||
case PATHACTION_MODE_LAND:
|
||||
planner_setup_pathdesired_land(&pathDesired);
|
||||
break;
|
||||
default:
|
||||
planner_setup_pathdesired(&pathDesired, autotakeoff);
|
||||
break;
|
||||
|
||||
pathDesired.End.North = waypoint.Position.North;
|
||||
pathDesired.End.East = waypoint.Position.East;
|
||||
pathDesired.End.Down = waypoint.Position.Down;
|
||||
pathDesired.EndingVelocity = waypoint.Velocity;
|
||||
pathDesired.Mode = pathAction.Mode;
|
||||
pathDesired.ModeParameters[0] = pathAction.ModeParameters[0];
|
||||
pathDesired.ModeParameters[1] = pathAction.ModeParameters[1];
|
||||
pathDesired.ModeParameters[2] = pathAction.ModeParameters[2];
|
||||
pathDesired.ModeParameters[3] = pathAction.ModeParameters[3];
|
||||
pathDesired.UID = waypointActive.Index;
|
||||
|
||||
|
||||
if (waypointActive.Index == 0) {
|
||||
PositionStateData positionState;
|
||||
PositionStateGet(&positionState);
|
||||
// First waypoint has itself as start point (used to be home position but that proved dangerous when looping)
|
||||
|
||||
/*pathDesired.Start[PATHDESIRED_START_NORTH] = waypoint.Position[WAYPOINT_POSITION_NORTH];
|
||||
pathDesired.Start[PATHDESIRED_START_EAST] = waypoint.Position[WAYPOINT_POSITION_EAST];
|
||||
pathDesired.Start[PATHDESIRED_START_DOWN] = waypoint.Position[WAYPOINT_POSITION_DOWN];*/
|
||||
// note: if certain flightmodes need to override Start, End or mode parameters, that should happen within
|
||||
// the pathfollower as needed. This also holds if Start is replaced by current position for takeoff and landing
|
||||
pathDesired.Start.North = positionState.North;
|
||||
pathDesired.Start.East = positionState.East;
|
||||
pathDesired.Start.Down = positionState.Down;
|
||||
pathDesired.StartingVelocity = pathDesired.EndingVelocity;
|
||||
} else {
|
||||
// Get previous waypoint as start point
|
||||
WaypointData waypointPrev;
|
||||
WaypointInstGet(waypointActive.Index - 1, &waypointPrev);
|
||||
|
||||
pathDesired.Start.North = waypointPrev.Position.North;
|
||||
pathDesired.Start.East = waypointPrev.Position.East;
|
||||
pathDesired.Start.Down = waypointPrev.Position.Down;
|
||||
pathDesired.StartingVelocity = waypointPrev.Velocity;
|
||||
}
|
||||
|
||||
PathDesiredSet(&pathDesired);
|
||||
@ -437,112 +438,6 @@ void statusUpdated(__attribute__((unused)) UAVObjEvent *ev)
|
||||
PIOS_CALLBACKSCHEDULER_Dispatch(pathPlannerHandle);
|
||||
}
|
||||
|
||||
// Standard setup of a pathDesired command from the waypoint path plan
|
||||
static void planner_setup_pathdesired(PathDesiredData *pathDesired, bool overwrite_start_position)
|
||||
{
|
||||
pathDesired->End.North = waypoint.Position.North;
|
||||
pathDesired->End.East = waypoint.Position.East;
|
||||
pathDesired->End.Down = waypoint.Position.Down;
|
||||
pathDesired->EndingVelocity = waypoint.Velocity;
|
||||
pathDesired->Mode = pathAction.Mode;
|
||||
pathDesired->ModeParameters[0] = pathAction.ModeParameters[0];
|
||||
pathDesired->ModeParameters[1] = pathAction.ModeParameters[1];
|
||||
pathDesired->ModeParameters[2] = pathAction.ModeParameters[2];
|
||||
pathDesired->ModeParameters[3] = pathAction.ModeParameters[3];
|
||||
pathDesired->UID = waypointActive.Index;
|
||||
|
||||
|
||||
if (waypointActive.Index == 0 || overwrite_start_position) {
|
||||
PositionStateData positionState;
|
||||
PositionStateGet(&positionState);
|
||||
// First waypoint has itself as start point (used to be home position but that proved dangerous when looping)
|
||||
|
||||
/*pathDesired.Start[PATHDESIRED_START_NORTH] = waypoint.Position[WAYPOINT_POSITION_NORTH];
|
||||
pathDesired.Start[PATHDESIRED_START_EAST] = waypoint.Position[WAYPOINT_POSITION_EAST];
|
||||
pathDesired.Start[PATHDESIRED_START_DOWN] = waypoint.Position[WAYPOINT_POSITION_DOWN];*/
|
||||
// note takeoff relies on the start being the current location as it merely ascends and using
|
||||
// the start as assumption current NE location
|
||||
pathDesired->Start.North = positionState.North;
|
||||
pathDesired->Start.East = positionState.East;
|
||||
pathDesired->Start.Down = positionState.Down;
|
||||
pathDesired->StartingVelocity = pathDesired->EndingVelocity;
|
||||
} else {
|
||||
// Get previous waypoint as start point
|
||||
WaypointData waypointPrev;
|
||||
WaypointInstGet(waypointActive.Index - 1, &waypointPrev);
|
||||
|
||||
pathDesired->Start.North = waypointPrev.Position.North;
|
||||
pathDesired->Start.East = waypointPrev.Position.East;
|
||||
pathDesired->Start.Down = waypointPrev.Position.Down;
|
||||
pathDesired->StartingVelocity = waypointPrev.Velocity;
|
||||
}
|
||||
}
|
||||
|
||||
#define AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MIN 2.0f
|
||||
#define AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MAX 50.0f
|
||||
static void planner_setup_pathdesired_takeoff(PathDesiredData *pathDesired)
|
||||
{
|
||||
PositionStateData positionState;
|
||||
|
||||
PositionStateGet(&positionState);
|
||||
float velocity_down;
|
||||
float autotakeoff_height;
|
||||
FlightModeSettingsAutoTakeOffVelocityGet(&velocity_down);
|
||||
FlightModeSettingsAutoTakeOffHeightGet(&autotakeoff_height);
|
||||
autotakeoff_height = fabsf(autotakeoff_height);
|
||||
if (autotakeoff_height < AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MIN) {
|
||||
autotakeoff_height = AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MIN;
|
||||
} else if (autotakeoff_height > AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MAX) {
|
||||
autotakeoff_height = AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MAX;
|
||||
}
|
||||
|
||||
|
||||
pathDesired->Start.North = positionState.North;
|
||||
pathDesired->Start.East = positionState.East;
|
||||
pathDesired->Start.Down = positionState.Down;
|
||||
pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_NORTH] = 0.0f;
|
||||
pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_EAST] = 0.0f;
|
||||
pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_DOWN] = -velocity_down;
|
||||
// initially halt takeoff.
|
||||
pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_CONTROLSTATE] = (float)STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORMIDTHROTTLE;
|
||||
|
||||
pathDesired->End.North = positionState.North;
|
||||
pathDesired->End.East = positionState.East;
|
||||
pathDesired->End.Down = positionState.Down - autotakeoff_height;
|
||||
|
||||
pathDesired->StartingVelocity = 0.0f;
|
||||
pathDesired->EndingVelocity = 0.0f;
|
||||
pathDesired->Mode = PATHDESIRED_MODE_AUTOTAKEOFF;
|
||||
|
||||
pathDesired->UID = waypointActive.Index;
|
||||
}
|
||||
|
||||
static void planner_setup_pathdesired_land(PathDesiredData *pathDesired)
|
||||
{
|
||||
PositionStateData positionState;
|
||||
|
||||
PositionStateGet(&positionState);
|
||||
float velocity_down;
|
||||
|
||||
FlightModeSettingsLandingVelocityGet(&velocity_down);
|
||||
|
||||
pathDesired->Start.North = positionState.North;
|
||||
pathDesired->Start.East = positionState.East;
|
||||
pathDesired->Start.Down = positionState.Down;
|
||||
pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_VELOCITYVECTOR_NORTH] = 0.0f;
|
||||
pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_VELOCITYVECTOR_EAST] = 0.0f;
|
||||
pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_VELOCITYVECTOR_DOWN] = velocity_down;
|
||||
|
||||
pathDesired->End.North = positionState.North;
|
||||
pathDesired->End.East = positionState.East;
|
||||
pathDesired->End.Down = positionState.Down;
|
||||
|
||||
pathDesired->StartingVelocity = 0.0f;
|
||||
pathDesired->EndingVelocity = 0.0f;
|
||||
pathDesired->Mode = PATHDESIRED_MODE_LAND;
|
||||
pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_OPTIONS] = (float)PATHDESIRED_MODEPARAMETER_LAND_OPTION_HORIZONTAL_PH;
|
||||
}
|
||||
|
||||
|
||||
// helper function to go to a specific waypoint
|
||||
static void setWaypoint(uint16_t num)
|
||||
|
@ -87,6 +87,7 @@
|
||||
<field name="AutoTakeOffHeight" units="m" type="float" elements="1" defaultvalue="2.5" description="height in meters above arming altitude to climb to during autotakeoff"/>
|
||||
<field name="PositionHoldOffset" units="m" type="float" elementnames="Horizontal,Vertical" defaultvalue="30,15" description="stick sensitivity for position roam modes"/>
|
||||
<field name="VarioControlLowPassAlpha" units="" type="float" elements="1" defaultvalue="0.98" description="stick low pass filter for position roam modes"/>
|
||||
<field name="FlightModeChangeRestartsPathPlan" units="bool" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="TRUE"/>
|
||||
<access gcs="readwrite" flight="readwrite"/>
|
||||
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
|
||||
<telemetryflight acked="true" updatemode="onchange" period="0"/>
|
||||
|
Loading…
x
Reference in New Issue
Block a user