rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-01 09:24:10 +01:00
Code Issues Releases Activity

Merge branch 'abeck/OP-1858-autotakeoff' into next

Browse Source
This commit is contained in:
abeck70 2015-04-30 17:35:33 +10:00
commit 13375ff0f5
3 changed files with 128 additions and 61 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
flight/modules/ManualControl/armhandler.c
View File

@ -317,6 +317,7 @@ static bool okToArm(void)
return false;
case FLIGHTSTATUS_FLIGHTMODE_AUTOTAKEOFF:
case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER:
return true;
default:

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

@ -101,33 +101,12 @@ void VtolAutoTakeoffController::ObjectiveUpdated(void)
{
// Set the objective's target velocity
if (flightStatus->ControlChain.PathPlanner != FLIGHTSTATUS_CONTROLCHAIN_TRUE) {
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]);
} else {
float velocity_down;
FlightModeSettingsAutoTakeOffVelocityGet(&velocity_down);
controlDown.UpdateVelocitySetpoint(-velocity_down);
controlNE.UpdateVelocitySetpoint(0.0f, 0.0f);
// pathplanner mode the waypoint provides the final destination including altitude. Takeoff would
// often be waypoint 0, in which case the starting location is the current location, and the end location
// is the waypoint location which really needs to be the same as the end in north and east. If it is not,
// it will fly to that location the during ascent.
// Note in pathplanner mode we use the start location which is the current initial location as the
// target NE to control. Takeoff only ascends vertically.
controlNE.UpdatePositionSetpoint(pathDesired->Start.North, pathDesired->Start.East);
// Sanity check that the end location is at least a reasonable height above the start location in the down direction
float autotakeoff_height = pathDesired->Start.Down - pathDesired->End.Down;
if (autotakeoff_height < 2.0f) {
pathDesired->End.Down = pathDesired->Start.Down - 2.0f;
}
controlDown.UpdatePositionSetpoint(pathDesired->End.Down); // the altitude is set by the end location.
fsm->setControlState(STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_INITIATE);
}
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]);
}
void VtolAutoTakeoffController::Deactivate(void)
{

155
flight/modules/PathPlanner/pathplanner.c
View File

@ -48,6 +48,8 @@
#include "plans.h"
#include <sanitycheck.h>
#include <vtolpathfollowersettings.h>
#include <statusvtolautotakeoff.h>
#include <manualcontrolcommand.h>
// Private constants
#define STACK_SIZE_BYTES 1024
@ -77,6 +79,8 @@ static uint8_t conditionPointingTowardsNext();
static uint8_t conditionPythonScript();
static uint8_t conditionImmediate();
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev);
static void planner_setup_pathdesired_takeoff(PathDesiredData *pathDesired);
static void planner_setup_pathdesired(PathDesiredData *pathDesired);
// Private variables
@ -167,6 +171,7 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
}
}
#define AUTOTAKEOFF_THROTTLE_LIMIT_TO_ALLOW_TAKEOFF_START 0.3f
/**
* Module task
@ -219,6 +224,17 @@ static void pathPlannerTask()
WaypointActiveGet(&waypointActive);
// with the introduction of takeoff, we allow for arming
// whilst in pathplanner mode. Previously it was just an assumption that
// a user never armed in pathplanner mode. This check allows a user to select
// pathplanner, to upload waypoints, and then arm in pathplanner.
if (!flightStatus.Armed) {
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;
@ -245,6 +261,22 @@ 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();
// decide what to do
@ -283,6 +315,35 @@ static void pathPlannerTask()
}
}
// callback function when waypoints changed in any way, update pathDesired
void updatePathDesired()
{
// only ever touch pathDesired if pathplanner is enabled
if (!pathplanner_active) {
return;
}
PathDesiredData pathDesired;
// find out current waypoint
WaypointActiveGet(&waypointActive);
WaypointInstGet(waypointActive.Index, &waypoint);
PathActionInstGet(waypoint.Action, &pathAction);
switch (pathAction.Mode) {
case PATHACTION_MODE_AUTOTAKEOFF:
planner_setup_pathdesired_takeoff(&pathDesired);
break;
default:
planner_setup_pathdesired(&pathDesired);
break;
}
PathDesiredSet(&pathDesired);
}
// safety checks for path plan integrity
static uint8_t checkPathPlan()
{
@ -367,33 +428,20 @@ void statusUpdated(__attribute__((unused)) UAVObjEvent *ev)
PIOS_CALLBACKSCHEDULER_Dispatch(pathPlannerHandle);
}
// callback function when waypoints changed in any way, update pathDesired
void updatePathDesired()
// Standard setup of a pathDesired command from the waypoint path plan
static void planner_setup_pathdesired(PathDesiredData *pathDesired)
{
// only ever touch pathDesired if pathplanner is enabled
if (!pathplanner_active) {
return;
}
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;
PathDesiredData pathDesired;
// find out current waypoint
WaypointActiveGet(&waypointActive);
WaypointInstGet(waypointActive.Index, &waypoint);
PathActionInstGet(waypoint.Action, &pathAction);
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;
@ -405,23 +453,62 @@ void updatePathDesired()
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;
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;
pathDesired->Start.North = waypointPrev.Position.North;
pathDesired->Start.East = waypointPrev.Position.East;
pathDesired->Start.Down = waypointPrev.Position.Down;
pathDesired->StartingVelocity = waypointPrev.Velocity;
}
PathDesiredSet(&pathDesired);
}
#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;
}
// helper function to go to a specific waypoint
static void setWaypoint(uint16_t num)
{