rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-18 03:52:11 +01:00
Code Issues Releases Activity

Merged in corvusvcorax/librepilot/LP-295_OP-1900_fixedwingautotakeoff_rebasenext (pull request #234)

Browse Source 
LP-295 fixedwingautotakeoff
This commit is contained in:
Lalanne Laurent 2016-05-15 18:42:53 +02:00
commit c6cf612bf4
21 changed files with 1104 additions and 504 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
flight/libraries/paths.c
View File

@ -2,11 +2,14 @@
******************************************************************************
*
* @file paths.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
*
* @brief Library path manipulation
*
* @see The GNU Public License (GPL) Version 3
*
* @addtogroup LibrePilotLibraries LibrePilot Libraries Navigation
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
@ -61,6 +64,7 @@ void path_progress(PathDesiredData *path, float *cur_point, struct path_status *
break;
case PATHDESIRED_MODE_GOTOENDPOINT:
case PATHDESIRED_MODE_AUTOTAKEOFF: // needed for pos hold at end of takeoff
return path_endpoint(path, cur_point, status, mode3D);
break;

173
flight/libraries/plans.c
View File

@ -1,16 +1,15 @@
/**
******************************************************************************
* @addtogroup OpenPilotLibraries OpenPilot Libraries
* @{
* @addtogroup Navigation
* @brief setups RTH/PH and other pathfollower/pathplanner status
* @{
*
* @file plan.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
*
* @brief setups RTH/PH and other pathfollower/pathplanner status
*
* @see The GNU Public License (GPL) Version 3
*
* @addtogroup LibrePilotLibraries LibrePilot Libraries Navigation
******************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
@ -43,6 +42,7 @@
#include <stabilizationbank.h>
#include <stabilizationdesired.h>
#include <sin_lookup.h>
#include <sanitycheck.h>
#include <statusvtolautotakeoff.h>
#define UPDATE_EXPECTED 0.02f
@ -170,158 +170,35 @@ 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;
memset(&pathDesired, 0, sizeof(PathDesiredData));
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()
{
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);
// 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;
// re-initialise in setup stage
memset(&pathDesired, 0, sizeof(PathDesiredData));
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;
// re-initialise in setup stage
memset(&pathDesired, 0, sizeof(PathDesiredData));
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)

15
flight/modules/ManualControl/pathfollowerhandler.c
View File

@ -1,16 +1,14 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup ManualControl
* @brief Interpretes the control input in ManualControlCommand
* @{
*
* @file pathfollowerhandler.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
*
* @brief Interpretes the control input in ManualControlCommand
*
* @see The GNU Public License (GPL) Version 3
*
* @addtogroup LibrePilotModules LibrePilot Modules ManualControl
******************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
@ -140,9 +138,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;

280
flight/modules/PathFollower/fixedwingautotakeoffcontroller.cpp Normal file
View File

@ -0,0 +1,280 @@
/*
******************************************************************************
*
* @file FixedWingAutoTakeoffController.cpp
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Fixed wing fly controller implementation
* @see The GNU Public License (GPL) Version 3
*
* @addtogroup LibrePilot LibrePilotModules Modules PathFollower Navigation
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
extern "C" {
#include <openpilot.h>
#include <pid.h>
#include <sin_lookup.h>
#include <pathdesired.h>
#include <fixedwingpathfollowersettings.h>
#include <flightstatus.h>
#include <pathstatus.h>
#include <stabilizationdesired.h>
#include <velocitystate.h>
#include <positionstate.h>
#include <attitudestate.h>
}
// C++ includes
#include "fixedwingautotakeoffcontroller.h"
// Private constants
// pointer to a singleton instance
FixedWingAutoTakeoffController *FixedWingAutoTakeoffController::p_inst = 0;
// Called when mode first engaged
void FixedWingAutoTakeoffController::Activate(void)
{
if (!mActive) {
setState(FW_AUTOTAKEOFF_STATE_LAUNCH);
}
FixedWingFlyController::Activate();
}
/**
* fixed wing autopilot
* use fixed attitude heading towards destination waypoint
*/
void FixedWingAutoTakeoffController::UpdateAutoPilot()
{
if (state < FW_AUTOTAKEOFF_STATE_SIZE) {
(this->*runFunctionTable[state])();
} else {
setState(FW_AUTOTAKEOFF_STATE_LAUNCH);
}
}
/**
* getAirspeed helper function
*/
float FixedWingAutoTakeoffController::getAirspeed(void)
{
VelocityStateData v;
float yaw;
VelocityStateGet(&v);
AttitudeStateYawGet(&yaw);
// current ground speed projected in forward direction
float groundspeedProjection = v.North * cos_lookup_deg(yaw) + v.East * sin_lookup_deg(yaw);
// note that airspeedStateBias is ( calibratedAirspeed - groundspeedProjection ) at the time of measurement,
// but thanks to accelerometers, groundspeedProjection reacts faster to changes in direction
// than airspeed and gps sensors alone
return groundspeedProjection + indicatedAirspeedStateBias;
}
/**
* setState - state transition including initialization
*/
void FixedWingAutoTakeoffController::setState(FixedWingAutoTakeoffControllerState_T setstate)
{
if (state < FW_AUTOTAKEOFF_STATE_SIZE && setstate != state) {
state = setstate;
(this->*initFunctionTable[state])();
}
}
/**
* setAttitude - output function to steer plane
*/
void FixedWingAutoTakeoffController::setAttitude(bool unsafe)
{
StabilizationDesiredData stabDesired;
stabDesired.Roll = 0.0f;
stabDesired.Yaw = initYaw;
if (unsafe) {
stabDesired.Pitch = fixedWingSettings->LandingPitch;
stabDesired.Thrust = 0.0f;
} else {
stabDesired.Pitch = fixedWingSettings->TakeOffPitch;
stabDesired.Thrust = fixedWingSettings->ThrustLimit.Max;
}
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
StabilizationDesiredSet(&stabDesired);
if (unsafe) {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING);
pathStatus->Status = PATHSTATUS_STATUS_CRITICAL;
} else {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
}
// calculate fractional progress based on altitude
float downPos;
PositionStateDownGet(&downPos);
if (fabsf(pathDesired->End.Down - pathDesired->Start.Down) < 1e-3f) {
pathStatus->fractional_progress = 1.0f;
pathStatus->error = 0.0f;
} else {
pathStatus->fractional_progress = (downPos - pathDesired->Start.Down) / (pathDesired->End.Down - pathDesired->Start.Down);
}
pathStatus->error = fabsf(downPos - pathDesired->End.Down);
PathStatusSet(pathStatus);
}
/**
* check if situation is unsafe
*/
bool FixedWingAutoTakeoffController::isUnsafe(void)
{
bool abort = false;
float speed = getAirspeed();
if (speed > maxVelocity) {
maxVelocity = speed;
}
// too much total deceleration (crash, insufficient climbing power, ...)
if (speed < maxVelocity - fixedWingSettings->SafetyCutoffLimits.MaxDecelerationDeltaMPS) {
abort = true;
}
AttitudeStateData attitude;
AttitudeStateGet(&attitude);
// too much bank angle
if (fabsf(attitude.Roll) > fixedWingSettings->SafetyCutoffLimits.RollDeg) {
abort = true;
}
if (fabsf(attitude.Pitch - fixedWingSettings->TakeOffPitch) > fixedWingSettings->SafetyCutoffLimits.PitchDeg) {
abort = true;
}
float deltayaw = attitude.Yaw - initYaw;
if (deltayaw > 180.0f) {
deltayaw -= 360.0f;
}
if (deltayaw < -180.0f) {
deltayaw += 360.0f;
}
if (fabsf(deltayaw) > fixedWingSettings->SafetyCutoffLimits.YawDeg) {
abort = true;
}
return abort;
}
// init inactive does nothing
void FixedWingAutoTakeoffController::init_inactive(void) {}
// init launch resets private variables to start values
void FixedWingAutoTakeoffController::init_launch(void)
{
// find out vector direction of *runway* (if any)
// and align, otherwise just stay straight ahead
pathStatus->path_direction_north = 0.0f;
pathStatus->path_direction_east = 0.0f;
pathStatus->path_direction_down = 0.0f;
pathStatus->correction_direction_north = 0.0f;
pathStatus->correction_direction_east = 0.0f;
pathStatus->correction_direction_down = 0.0f;
if (fabsf(pathDesired->Start.North - pathDesired->End.North) < 1e-3f &&
fabsf(pathDesired->Start.East - pathDesired->End.East) < 1e-3f) {
AttitudeStateYawGet(&initYaw);
} else {
initYaw = RAD2DEG(atan2f(pathDesired->End.East - pathDesired->Start.East, pathDesired->End.North - pathDesired->Start.North));
if (initYaw < -180.0f) {
initYaw += 360.0f;
}
if (initYaw > 180.0f) {
initYaw -= 360.0f;
}
}
maxVelocity = getAirspeed();
}
// init climb does nothing
void FixedWingAutoTakeoffController::init_climb(void) {}
// init hold does nothing
void FixedWingAutoTakeoffController::init_hold(void) {}
// init abort does nothing
void FixedWingAutoTakeoffController::init_abort(void) {}
// run inactive does nothing
// no state transitions
void FixedWingAutoTakeoffController::run_inactive(void) {}
// run launch tries to takeoff - indicates safe situation with engine power (for hand launch)
// run launch checks for:
// 1. min velocity for climb
void FixedWingAutoTakeoffController::run_launch(void)
{
// state transition
if (maxVelocity > fixedWingSettings->SafetyCutoffLimits.MaxDecelerationDeltaMPS) {
setState(FW_AUTOTAKEOFF_STATE_CLIMB);
}
setAttitude(isUnsafe());
}
// run climb climbs with max power
// run climb checks for:
// 1. min altitude for hold
// 2. critical situation for abort (different than launch)
void FixedWingAutoTakeoffController::run_climb(void)
{
bool unsafe = isUnsafe();
float downPos;
PositionStateDownGet(&downPos);
if (unsafe) {
// state transition 2
setState(FW_AUTOTAKEOFF_STATE_ABORT);
} else if (downPos < pathDesired->End.Down) {
// state transition 1
setState(FW_AUTOTAKEOFF_STATE_HOLD);
}
setAttitude(unsafe);
}
// run hold loiters like in position hold
// no state transitions (FlyController does exception handling)
void FixedWingAutoTakeoffController::run_hold(void)
{
// parent controller will do perfect position hold in autotakeoff mode
FixedWingFlyController::UpdateAutoPilot();
}
// run abort descends with wings level, engine off (like land)
// no state transitions
void FixedWingAutoTakeoffController::run_abort(void)
{
setAttitude(true);
}

290
flight/modules/PathFollower/fixedwingflycontroller.cpp
View File

@ -2,10 +2,13 @@
******************************************************************************
*
* @file FixedWingFlyController.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Fixed wing fly controller implementation
* @see The GNU Public License (GPL) Version 3
*
* @addtogroup LibrePilot LibrePilotModules Modules PathFollower Navigation
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
@ -26,23 +29,13 @@
extern "C" {
#include <openpilot.h>
#include <callbackinfo.h>
#include <math.h>
#include <pid.h>
#include <CoordinateConversions.h>
#include <sin_lookup.h>
#include <pathdesired.h>
#include <paths.h>
#include "plans.h"
#include <sanitycheck.h>
#include <homelocation.h>
#include <accelstate.h>
#include <fixedwingpathfollowersettings.h>
#include <fixedwingpathfollowerstatus.h>
#include <flightstatus.h>
#include <flightmodesettings.h>
#include <pathstatus.h>
#include <positionstate.h>
#include <velocitystate.h>
@ -50,14 +43,7 @@ extern "C" {
#include <stabilizationdesired.h>
#include <airspeedstate.h>
#include <attitudestate.h>
#include <takeofflocation.h>
#include <poilocation.h>
#include <manualcontrolcommand.h>
#include <systemsettings.h>
#include <stabilizationbank.h>
#include <stabilizationdesired.h>
#include <vtolselftuningstats.h>
#include <pathsummary.h>
}
// C++ includes
@ -80,6 +66,7 @@ void FixedWingFlyController::Activate(void)
SettingsUpdated();
resetGlobals();
mMode = pathDesired->Mode;
lastAirspeedUpdate = 0;
}
}
@ -244,9 +231,11 @@ void FixedWingFlyController::updatePathVelocity(float kFF, bool limited)
*/
uint8_t FixedWingFlyController::updateFixedDesiredAttitude()
{
uint8_t result = 1;
uint8_t result = 1;
bool cutThrust = false;
bool hasAirspeed = true;
const float dT = fixedWingSettings->UpdatePeriod / 1000.0f;
const float dT = fixedWingSettings->UpdatePeriod / 1000.0f;
VelocityDesiredData velocityDesired;
VelocityStateData velocityState;
@ -259,7 +248,7 @@ uint8_t FixedWingFlyController::updateFixedDesiredAttitude()
float groundspeedProjection;
float indicatedAirspeedState;
float indicatedAirspeedDesired;
float airspeedError;
float airspeedError = 0.0f;
float pitchCommand;
@ -282,56 +271,99 @@ uint8_t FixedWingFlyController::updateFixedDesiredAttitude()
AirspeedStateGet(&airspeedState);
SystemSettingsGet(&systemSettings);
/**
* Compute speed error and course
*/
// missing sensors for airspeed-direction we have to assume within
// reasonable error that measured airspeed is actually the airspeed
// component in forward pointing direction
// airspeedVector is normalized
airspeedVector[0] = cos_lookup_deg(attitudeState.Yaw);
airspeedVector[1] = sin_lookup_deg(attitudeState.Yaw);
// current ground speed projected in forward direction
groundspeedProjection = velocityState.North * airspeedVector[0] + velocityState.East * airspeedVector[1];
// note that airspeedStateBias is ( calibratedAirspeed - groundspeedProjection ) at the time of measurement,
// but thanks to accelerometers, groundspeedProjection reacts faster to changes in direction
// than airspeed and gps sensors alone
indicatedAirspeedState = groundspeedProjection + indicatedAirspeedStateBias;
// fluidMovement is a vector describing the aproximate movement vector of
// the surrounding fluid in 2d space (aka wind vector)
fluidMovement[0] = velocityState.North - (indicatedAirspeedState * airspeedVector[0]);
fluidMovement[1] = velocityState.East - (indicatedAirspeedState * airspeedVector[1]);
// calculate the movement vector we need to fly to reach velocityDesired -
// taking fluidMovement into account
courseComponent[0] = velocityDesired.North - fluidMovement[0];
courseComponent[1] = velocityDesired.East - fluidMovement[1];
indicatedAirspeedDesired = boundf(sqrtf(courseComponent[0] * courseComponent[0] + courseComponent[1] * courseComponent[1]),
fixedWingSettings->HorizontalVelMin,
fixedWingSettings->HorizontalVelMax);
// if we could fly at arbitrary speeds, we'd just have to move towards the
// courseComponent vector as previously calculated and we'd be fine
// unfortunately however we are bound by min and max air speed limits, so
// we need to recalculate the correct course to meet at least the
// velocityDesired vector direction at our current speed
// this overwrites courseComponent
bool valid = correctCourse(courseComponent, (float *)&velocityDesired.North, fluidMovement, indicatedAirspeedDesired);
// Error condition: wind speed too high, we can't go where we want anymore
fixedWingPathFollowerStatus.Errors.Wind = 0;
if ((!valid) &&
fixedWingSettings->Safetymargins.Wind > 0.5f) { // alarm switched on
fixedWingPathFollowerStatus.Errors.Wind = 1;
// check for airspeed sensor
fixedWingPathFollowerStatus.Errors.AirspeedSensor = 0;
if (fixedWingSettings->UseAirspeedSensor == FIXEDWINGPATHFOLLOWERSETTINGS_USEAIRSPEEDSENSOR_FALSE) {
// fallback algo triggered voluntarily
hasAirspeed = false;
fixedWingPathFollowerStatus.Errors.AirspeedSensor = 1;
} else if (PIOS_DELAY_GetuSSince(lastAirspeedUpdate) > 1000000) {
// no airspeed update in one second, assume airspeed sensor failure
hasAirspeed = false;
result = 0;
fixedWingPathFollowerStatus.Errors.AirspeedSensor = 1;
}
// Airspeed error
airspeedError = indicatedAirspeedDesired - indicatedAirspeedState;
if (hasAirspeed) {
// missing sensors for airspeed-direction we have to assume within
// reasonable error that measured airspeed is actually the airspeed
// component in forward pointing direction
// airspeedVector is normalized
airspeedVector[0] = cos_lookup_deg(attitudeState.Yaw);
airspeedVector[1] = sin_lookup_deg(attitudeState.Yaw);
// current ground speed projected in forward direction
groundspeedProjection = velocityState.North * airspeedVector[0] + velocityState.East * airspeedVector[1];
// note that airspeedStateBias is ( calibratedAirspeed - groundspeedProjection ) at the time of measurement,
// but thanks to accelerometers, groundspeedProjection reacts faster to changes in direction
// than airspeed and gps sensors alone
indicatedAirspeedState = groundspeedProjection + indicatedAirspeedStateBias;
// fluidMovement is a vector describing the aproximate movement vector of
// the surrounding fluid in 2d space (aka wind vector)
fluidMovement[0] = velocityState.North - (indicatedAirspeedState * airspeedVector[0]);
fluidMovement[1] = velocityState.East - (indicatedAirspeedState * airspeedVector[1]);
// calculate the movement vector we need to fly to reach velocityDesired -
// taking fluidMovement into account
courseComponent[0] = velocityDesired.North - fluidMovement[0];
courseComponent[1] = velocityDesired.East - fluidMovement[1];
indicatedAirspeedDesired = boundf(sqrtf(courseComponent[0] * courseComponent[0] + courseComponent[1] * courseComponent[1]),
fixedWingSettings->HorizontalVelMin,
fixedWingSettings->HorizontalVelMax);
// if we could fly at arbitrary speeds, we'd just have to move towards the
// courseComponent vector as previously calculated and we'd be fine
// unfortunately however we are bound by min and max air speed limits, so
// we need to recalculate the correct course to meet at least the
// velocityDesired vector direction at our current speed
// this overwrites courseComponent
bool valid = correctCourse(courseComponent, (float *)&velocityDesired.North, fluidMovement, indicatedAirspeedDesired);
// Error condition: wind speed too high, we can't go where we want anymore
fixedWingPathFollowerStatus.Errors.Wind = 0;
if ((!valid) &&
fixedWingSettings->Safetymargins.Wind > 0.5f) { // alarm switched on
fixedWingPathFollowerStatus.Errors.Wind = 1;
result = 0;
}
// Airspeed error
airspeedError = indicatedAirspeedDesired - indicatedAirspeedState;
// Error condition: plane too slow or too fast
fixedWingPathFollowerStatus.Errors.Highspeed = 0;
fixedWingPathFollowerStatus.Errors.Lowspeed = 0;
if (indicatedAirspeedState > systemSettings.AirSpeedMax * fixedWingSettings->Safetymargins.Overspeed) {
fixedWingPathFollowerStatus.Errors.Overspeed = 1;
result = 0;
}
if (indicatedAirspeedState > fixedWingSettings->HorizontalVelMax * fixedWingSettings->Safetymargins.Highspeed) {
fixedWingPathFollowerStatus.Errors.Highspeed = 1;
result = 0;
cutThrust = true;
}
if (indicatedAirspeedState < fixedWingSettings->HorizontalVelMin * fixedWingSettings->Safetymargins.Lowspeed) {
fixedWingPathFollowerStatus.Errors.Lowspeed = 1;
result = 0;
}
if (indicatedAirspeedState < systemSettings.AirSpeedMin * fixedWingSettings->Safetymargins.Stallspeed) {
fixedWingPathFollowerStatus.Errors.Stallspeed = 1;
result = 0;
}
if (indicatedAirspeedState < fixedWingSettings->HorizontalVelMin * fixedWingSettings->Safetymargins.Lowspeed - fixedWingSettings->SafetyCutoffLimits.MaxDecelerationDeltaMPS) {
cutThrust = true;
result = 0;
}
}
// Vertical speed error
descentspeedDesired = boundf(
@ -340,36 +372,19 @@ uint8_t FixedWingFlyController::updateFixedDesiredAttitude()
fixedWingSettings->VerticalVelMax);
descentspeedError = descentspeedDesired - velocityState.Down;
// Error condition: plane too slow or too fast
fixedWingPathFollowerStatus.Errors.Highspeed = 0;
fixedWingPathFollowerStatus.Errors.Lowspeed = 0;
if (indicatedAirspeedState > systemSettings.AirSpeedMax * fixedWingSettings->Safetymargins.Overspeed) {
fixedWingPathFollowerStatus.Errors.Overspeed = 1;
result = 0;
}
if (indicatedAirspeedState > fixedWingSettings->HorizontalVelMax * fixedWingSettings->Safetymargins.Highspeed) {
fixedWingPathFollowerStatus.Errors.Highspeed = 1;
result = 0;
}
if (indicatedAirspeedState < fixedWingSettings->HorizontalVelMin * fixedWingSettings->Safetymargins.Lowspeed) {
fixedWingPathFollowerStatus.Errors.Lowspeed = 1;
result = 0;
}
if (indicatedAirspeedState < systemSettings.AirSpeedMin * fixedWingSettings->Safetymargins.Stallspeed) {
fixedWingPathFollowerStatus.Errors.Stallspeed = 1;
result = 0;
}
/**
* Compute desired thrust command
*/
// Compute the cross feed from vertical speed to pitch, with saturation
float speedErrorToPowerCommandComponent = boundf(
(airspeedError / fixedWingSettings->HorizontalVelMin) * fixedWingSettings->AirspeedToPowerCrossFeed.Kp,
-fixedWingSettings->AirspeedToPowerCrossFeed.Max,
fixedWingSettings->AirspeedToPowerCrossFeed.Max
);
float speedErrorToPowerCommandComponent = 0.0f;
if (hasAirspeed) {
speedErrorToPowerCommandComponent = boundf(
(airspeedError / fixedWingSettings->HorizontalVelMin) * fixedWingSettings->AirspeedToPowerCrossFeed.Kp,
-fixedWingSettings->AirspeedToPowerCrossFeed.Max,
fixedWingSettings->AirspeedToPowerCrossFeed.Max
);
}
// Compute final thrust response
powerCommand = pid_apply(&PIDpower, -descentspeedError, dT) +
@ -390,57 +405,84 @@ uint8_t FixedWingFlyController::updateFixedDesiredAttitude()
if (fixedWingSettings->ThrustLimit.Neutral + powerCommand >= fixedWingSettings->ThrustLimit.Max && // thrust at maximum
velocityState.Down > 0.0f && // we ARE going down
descentspeedDesired < 0.0f && // we WANT to go up
airspeedError > 0.0f && // we are too slow already
fixedWingSettings->Safetymargins.Lowpower > 0.5f) { // alarm switched on
airspeedError > 0.0f) { // we are too slow already
fixedWingPathFollowerStatus.Errors.Lowpower = 1;
result = 0;
if (fixedWingSettings->Safetymargins.Lowpower > 0.5f) { // alarm switched on
result = 0;
}
}
// Error condition: plane keeps climbing despite minimum thrust (opposite of above)
fixedWingPathFollowerStatus.Errors.Highpower = 0;
if (fixedWingSettings->ThrustLimit.Neutral + powerCommand <= fixedWingSettings->ThrustLimit.Min && // thrust at minimum
velocityState.Down < 0.0f && // we ARE going up
descentspeedDesired > 0.0f && // we WANT to go down
airspeedError < 0.0f && // we are too fast already
fixedWingSettings->Safetymargins.Highpower > 0.5f) { // alarm switched on
airspeedError < 0.0f) { // we are too fast already
// this alarm is often switched off because of false positives, however we still want to cut throttle if it happens
cutThrust = true;
fixedWingPathFollowerStatus.Errors.Highpower = 1;
result = 0;
if (fixedWingSettings->Safetymargins.Highpower > 0.5f) { // alarm switched on
result = 0;
}
}
/**
* Compute desired pitch command
*/
// Compute the cross feed from vertical speed to pitch, with saturation
float verticalSpeedToPitchCommandComponent = boundf(-descentspeedError * fixedWingSettings->VerticalToPitchCrossFeed.Kp,
-fixedWingSettings->VerticalToPitchCrossFeed.Max,
fixedWingSettings->VerticalToPitchCrossFeed.Max
);
if (hasAirspeed) {
// Compute the cross feed from vertical speed to pitch, with saturation
float verticalSpeedToPitchCommandComponent = boundf(-descentspeedError * fixedWingSettings->VerticalToPitchCrossFeed.Kp,
-fixedWingSettings->VerticalToPitchCrossFeed.Max,
fixedWingSettings->VerticalToPitchCrossFeed.Max
);
// Compute the pitch command as err*Kp + errInt*Ki + X_feed.
pitchCommand = -pid_apply(&PIDspeed, airspeedError, dT) + verticalSpeedToPitchCommandComponent;
// Compute the pitch command as err*Kp + errInt*Ki + X_feed.
pitchCommand = -pid_apply(&PIDspeed, airspeedError, dT) + verticalSpeedToPitchCommandComponent;
fixedWingPathFollowerStatus.Error.Speed = airspeedError;
fixedWingPathFollowerStatus.ErrorInt.Speed = PIDspeed.iAccumulator;
fixedWingPathFollowerStatus.Command.Speed = pitchCommand;
fixedWingPathFollowerStatus.Error.Speed = airspeedError;
fixedWingPathFollowerStatus.ErrorInt.Speed = PIDspeed.iAccumulator;
fixedWingPathFollowerStatus.Command.Speed = pitchCommand;
stabDesired.Pitch = boundf(fixedWingSettings->PitchLimit.Neutral + pitchCommand,
fixedWingSettings->PitchLimit.Min,
fixedWingSettings->PitchLimit.Max);
stabDesired.Pitch = boundf(fixedWingSettings->PitchLimit.Neutral + pitchCommand,
fixedWingSettings->PitchLimit.Min,
fixedWingSettings->PitchLimit.Max);
// Error condition: high speed dive
fixedWingPathFollowerStatus.Errors.Pitchcontrol = 0;
if (fixedWingSettings->PitchLimit.Neutral + pitchCommand >= fixedWingSettings->PitchLimit.Max && // pitch demand is full up
velocityState.Down > 0.0f && // we ARE going down
descentspeedDesired < 0.0f && // we WANT to go up
airspeedError < 0.0f && // we are too fast already
fixedWingSettings->Safetymargins.Pitchcontrol > 0.5f) { // alarm switched on
// Error condition: high speed dive
fixedWingPathFollowerStatus.Errors.Pitchcontrol = 0;
if (fixedWingSettings->PitchLimit.Neutral + pitchCommand >= fixedWingSettings->PitchLimit.Max && // pitch demand is full up
velocityState.Down > 0.0f && // we ARE going down
descentspeedDesired < 0.0f && // we WANT to go up
airspeedError < 0.0f && // we are too fast already
fixedWingSettings->Safetymargins.Pitchcontrol > 0.5f) { // alarm switched on
fixedWingPathFollowerStatus.Errors.Pitchcontrol = 1;
result = 0;
cutThrust = true;
}
} else {
// no airspeed sensor means we fly with constant pitch, like for landing pathfollower
stabDesired.Pitch = fixedWingSettings->LandingPitch;
}
// Error condition: pitch way out of wack
if (fixedWingSettings->Safetymargins.Pitchcontrol > 0.5f &&
(attitudeState.Pitch < fixedWingSettings->PitchLimit.Min - fixedWingSettings->SafetyCutoffLimits.PitchDeg ||
attitudeState.Pitch > fixedWingSettings->PitchLimit.Max + fixedWingSettings->SafetyCutoffLimits.PitchDeg)) {
fixedWingPathFollowerStatus.Errors.Pitchcontrol = 1;
result = 0;
cutThrust = true;
}
/**
* Compute desired roll command
*/
courseError = RAD2DEG(atan2f(courseComponent[1], courseComponent[0])) - attitudeState.Yaw;
if (hasAirspeed) {
courseError = RAD2DEG(atan2f(courseComponent[1], courseComponent[0])) - attitudeState.Yaw;
} else {
// fallback based on effective movement direction when in fallback mode, hope that airspeed > wind velocity, or we will never get home
courseError = RAD2DEG(atan2f(velocityDesired.East, velocityDesired.North)) - RAD2DEG(atan2f(velocityState.East, velocityState.North));
}
if (courseError < -180.0f) {
courseError += 360.0f;
@ -473,7 +515,15 @@ uint8_t FixedWingFlyController::updateFixedDesiredAttitude()
fixedWingSettings->RollLimit.Min,
fixedWingSettings->RollLimit.Max);
// TODO: find a check to determine loss of directional control. Likely needs some check of derivative
// Error condition: roll way out of wack
fixedWingPathFollowerStatus.Errors.Rollcontrol = 0;
if (fixedWingSettings->Safetymargins.Rollcontrol > 0.5f &&
(attitudeState.Roll < fixedWingSettings->RollLimit.Min - fixedWingSettings->SafetyCutoffLimits.RollDeg ||
attitudeState.Roll > fixedWingSettings->RollLimit.Max + fixedWingSettings->SafetyCutoffLimits.RollDeg)) {
fixedWingPathFollowerStatus.Errors.Rollcontrol = 1;
result = 0;
cutThrust = true;
}
/**
@ -482,6 +532,10 @@ uint8_t FixedWingFlyController::updateFixedDesiredAttitude()
// TODO implement raw control mode for yaw and base on Accels.Y
stabDesired.Yaw = 0.0f;
// safety cutoff condition
if (cutThrust) {
stabDesired.Thrust = 0.0f;
}
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
@ -615,4 +669,6 @@ void FixedWingFlyController::AirspeedStateUpdatedCb(__attribute__((unused)) UAVO
// changes to groundspeed to offset the airspeed by the same measurement.
// This has a side effect that in the absence of any airspeed updates, the
// pathfollower will fly using groundspeed.
lastAirspeedUpdate = PIOS_DELAY_GetuS();
}

154
flight/modules/PathFollower/fixedwinglandcontroller.cpp Normal file
View File

@ -0,0 +1,154 @@
/*
******************************************************************************
*
* @file FixedWingLandController.cpp
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Fixed wing fly controller implementation
* @see The GNU Public License (GPL) Version 3
*
* @addtogroup LibrePilot LibrePilotModules Modules PathFollower Navigation
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
extern "C" {
#include <openpilot.h>
#include <pathdesired.h>
#include <fixedwingpathfollowersettings.h>
#include <flightstatus.h>
#include <pathstatus.h>
#include <stabilizationdesired.h>
}
// C++ includes
#include "fixedwinglandcontroller.h"
// Private constants
// pointer to a singleton instance
FixedWingLandController *FixedWingLandController::p_inst = 0;
FixedWingLandController::FixedWingLandController()
: fixedWingSettings(NULL), mActive(false), mMode(0)
{}
// Called when mode first engaged
void FixedWingLandController::Activate(void)
{
if (!mActive) {
mActive = true;
SettingsUpdated();
resetGlobals();
mMode = pathDesired->Mode;
}
}
uint8_t FixedWingLandController::IsActive(void)
{
return mActive;
}
uint8_t FixedWingLandController::Mode(void)
{
return mMode;
}
// Objective updated in pathdesired
void FixedWingLandController::ObjectiveUpdated(void)
{}
void FixedWingLandController::Deactivate(void)
{
if (mActive) {
mActive = false;
resetGlobals();
}
}
void FixedWingLandController::SettingsUpdated(void)
{}
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t FixedWingLandController::Initialize(FixedWingPathFollowerSettingsData *ptr_fixedWingSettings)
{
PIOS_Assert(ptr_fixedWingSettings);
fixedWingSettings = ptr_fixedWingSettings;
resetGlobals();
return 0;
}
/**
* reset globals, (integrals, accumulated errors and timers)
*/
void FixedWingLandController::resetGlobals()
{
pathStatus->path_time = 0.0f;
pathStatus->path_direction_north = 0.0f;
pathStatus->path_direction_east = 0.0f;
pathStatus->path_direction_down = 0.0f;
pathStatus->correction_direction_north = 0.0f;
pathStatus->correction_direction_east = 0.0f;
pathStatus->correction_direction_down = 0.0f;
pathStatus->error = 0.0f;
pathStatus->fractional_progress = 0.0f;
}
/**
* fixed wing autopilot
* use fixed attitude heading towards destination waypoint
*/
void FixedWingLandController::UpdateAutoPilot()
{
StabilizationDesiredData stabDesired;
stabDesired.Roll = 0.0f;
stabDesired.Pitch = fixedWingSettings->LandingPitch;
stabDesired.Thrust = 0.0f;
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
// find out vector direction of *runway* (if any)
// and align, otherwise just stay straight ahead
if (fabsf(pathDesired->Start.North - pathDesired->End.North) < 1e-3f &&
fabsf(pathDesired->Start.East - pathDesired->End.East) < 1e-3f) {
stabDesired.Yaw = 0.0f;
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
} else {
stabDesired.Yaw = RAD2DEG(atan2f(pathDesired->End.East - pathDesired->Start.East, pathDesired->End.North - pathDesired->Start.North));
if (stabDesired.Yaw < -180.0f) {
stabDesired.Yaw += 360.0f;
}
if (stabDesired.Yaw > 180.0f) {
stabDesired.Yaw -= 360.0f;
}
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
}
StabilizationDesiredSet(&stabDesired);
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
PathStatusSet(pathStatus);
}

98
flight/modules/PathFollower/inc/fixedwingautotakeoffcontroller.h Normal file
View File

@ -0,0 +1,98 @@
/**
******************************************************************************
* @addtogroup LibrePilotModules LibrePilot Modules
* @{
* @addtogroup FixedWing CONTROL interface class
* @brief CONTROL interface class for pathfollower fixed wing fly controller
* @{
*
* @file fixedwingautotakeoffcontroller.h
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Executes CONTROL for fixed wing fly objectives
*
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef FIXEDWINGAUTOTAKEOFFCONTROLLER_H
#define FIXEDWINGAUTOTAKEOFFCONTROLLER_H
#include "fixedwingflycontroller.h"
// AutoTakeoff state machine
typedef enum {
FW_AUTOTAKEOFF_STATE_INACTIVE = 0,
FW_AUTOTAKEOFF_STATE_LAUNCH,
FW_AUTOTAKEOFF_STATE_CLIMB,
FW_AUTOTAKEOFF_STATE_HOLD,
FW_AUTOTAKEOFF_STATE_ABORT,
FW_AUTOTAKEOFF_STATE_SIZE
} FixedWingAutoTakeoffControllerState_T;
class FixedWingAutoTakeoffController : public FixedWingFlyController {
protected:
static FixedWingAutoTakeoffController *p_inst;
public:
static FixedWingFlyController *instance()
{
if (!p_inst) {
p_inst = new FixedWingAutoTakeoffController();
}
return p_inst;
}
void Activate(void);
void UpdateAutoPilot(void);
private:
// variables
FixedWingAutoTakeoffControllerState_T state;
float initYaw;
float maxVelocity;
// functions
void setState(FixedWingAutoTakeoffControllerState_T setstate);
void setAttitude(bool unsafe);
float getAirspeed(void);
bool isUnsafe(void);
void run_inactive(void);
void run_launch(void);
void run_climb(void);
void run_hold(void);
void run_abort(void);
void init_inactive(void);
void init_launch(void);
void init_climb(void);
void init_hold(void);
void init_abort(void);
void(FixedWingAutoTakeoffController::*const runFunctionTable[FW_AUTOTAKEOFF_STATE_SIZE]) (void) = {
&FixedWingAutoTakeoffController::run_inactive,
&FixedWingAutoTakeoffController::run_launch,
&FixedWingAutoTakeoffController::run_climb,
&FixedWingAutoTakeoffController::run_hold,
&FixedWingAutoTakeoffController::run_abort
};
void(FixedWingAutoTakeoffController::*const initFunctionTable[FW_AUTOTAKEOFF_STATE_SIZE]) (void) = {
&FixedWingAutoTakeoffController::init_inactive,
&FixedWingAutoTakeoffController::init_launch,
&FixedWingAutoTakeoffController::init_climb,
&FixedWingAutoTakeoffController::init_hold,
&FixedWingAutoTakeoffController::init_abort
};
};
#endif // FIXEDWINGAUTOTAKEOFFCONTROLLER_H

23
flight/modules/PathFollower/inc/fixedwingflycontroller.h
View File

@ -1,13 +1,14 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @addtogroup LibrePilotModules LibrePilot Modules
* @{
* @addtogroup FixedWing CONTROL interface class
* @brief CONTROL interface class for pathfollower fixed wing fly controller
* @{
*
* @file FixedWingCONTROL.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @file fixedwingflycontroller.h
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Executes CONTROL for fixed wing fly objectives
*
* @see The GNU Public License (GPL) Version 3
@ -33,7 +34,7 @@
#include "pathfollowercontrol.h"
class FixedWingFlyController : public PathFollowerControl {
private:
protected:
static FixedWingFlyController *p_inst;
FixedWingFlyController();
@ -57,24 +58,26 @@ public:
uint8_t Mode(void);
void AirspeedStateUpdatedCb(__attribute__((unused)) UAVObjEvent * ev);
protected:
FixedWingPathFollowerSettingsData *fixedWingSettings;
uint8_t mActive;
uint8_t mMode;
// correct speed by measured airspeed
float indicatedAirspeedStateBias;
private:
void resetGlobals();
uint8_t updateAutoPilotFixedWing();
void updatePathVelocity(float kFF, bool limited);
uint8_t updateFixedDesiredAttitude();
bool correctCourse(float *C, float *V, float *F, float s);
FixedWingPathFollowerSettingsData *fixedWingSettings;
uint8_t mActive;
uint8_t mMode;
int32_t lastAirspeedUpdate;
struct pid PIDposH[2];
struct pid PIDposV;
struct pid PIDcourse;
struct pid PIDspeed;
struct pid PIDpower;
// correct speed by measured airspeed
float indicatedAirspeedStateBias;
};
#endif // FIXEDWINGFLYCONTROLLER_H

67
flight/modules/PathFollower/inc/fixedwinglandcontroller.h Normal file
View File

@ -0,0 +1,67 @@
/**
******************************************************************************
* @addtogroup LibrePilotModules LibrePilot Modules
* @{
* @addtogroup FixedWing CONTROL interface class
* @brief CONTROL interface class for pathfollower fixed wing fly controller
* @{
*
* @file fixedwinglandcontroller.h
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Executes CONTROL for fixed wing fly objectives
*
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef FIXEDWINGLANDCONTROLLER_H
#define FIXEDWINGLANDCONTROLLER_H
#include "pathfollowercontrol.h"
class FixedWingLandController : public PathFollowerControl {
private:
static FixedWingLandController *p_inst;
FixedWingLandController();
public:
static FixedWingLandController *instance()
{
if (!p_inst) {
p_inst = new FixedWingLandController();
}
return p_inst;
}
int32_t Initialize(FixedWingPathFollowerSettingsData *fixedWingSettings);
void Activate(void);
void Deactivate(void);
void SettingsUpdated(void);
void UpdateAutoPilot(void);
void ObjectiveUpdated(void);
uint8_t IsActive(void);
uint8_t Mode(void);
private:
void resetGlobals();
FixedWingPathFollowerSettingsData *fixedWingSettings;
uint8_t mActive;
uint8_t mMode;
};
#endif // FIXEDWINGLANDCONTROLLER_H

7
flight/modules/PathFollower/inc/vtolautotakeoffcontroller.h
View File

@ -1,13 +1,14 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @addtogroup LibrePilotModules LibrePilot Modules
* @{
* @addtogroup PathFollower CONTROL interface class
* @brief vtol land controller class
* @{
*
* @file vtollandcontroller.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Executes CONTROL for landing sequence
*
* @see The GNU Public License (GPL) Version 3
@ -70,6 +71,8 @@ private:
PIDControlDown controlDown;
PIDControlNE controlNE;
uint8_t mActive;
uint8_t mOverride;
StatusVtolAutoTakeoffControlStateOptions autotakeoffState;
};
#endif // VTOLAUTOTAKEOFFCONTROLLER_H

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

@ -1,13 +1,14 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @addtogroup LibrePilotModules LibrePilot Modules
* @{
* @addtogroup PathFollower FSM
* @brief Executes landing sequence via an FSM
* @{
*
* @file vtollandfsm.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Executes FSM for landing sequence
*
* @see The GNU Public License (GPL) Version 3
@ -47,7 +48,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 triggers fallback to land
AUTOTAKEOFF_STATE_SIZE
} PathFollowerFSM_AutoTakeoffState_T;

6
flight/modules/PathFollower/inc/vtollandcontroller.h
View File

@ -1,13 +1,14 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @addtogroup LibrePilotModules LibrePilot Modules
* @{
* @addtogroup PathFollower CONTROL interface class
* @brief vtol land controller class
* @{
*
* @file vtollandcontroller.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Executes CONTROL for landing sequence
*
* @see The GNU Public License (GPL) Version 3
@ -71,6 +72,7 @@ private:
PIDControlDown controlDown;
PIDControlNE controlNE;
uint8_t mActive;
uint8_t mOverride;
};
#endif // VTOLLANDCONTROLLER_H

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

@ -2,12 +2,14 @@
******************************************************************************
*
* @file pathfollower.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief This module compared @ref PositionActuatl to @ref ActiveWaypoint
* and sets @ref AttitudeDesired. It only does this when the FlightMode field
* of @ref ManualControlCommand is Auto.
*
* @see The GNU Public License (GPL) Version 3
* @addtogroup LibrePilot LibrePilotModules Modules PathFollower Navigation
*
*****************************************************************************/
/*
@ -93,6 +95,8 @@ extern "C" {
#include "vtolbrakecontroller.h"
#include "vtolflycontroller.h"
#include "fixedwingflycontroller.h"
#include "fixedwingautotakeoffcontroller.h"
#include "fixedwinglandcontroller.h"
#include "grounddrivecontroller.h"
// Private constants
@ -220,6 +224,8 @@ void pathFollowerInitializeControllersForFrameType()
case FRAME_TYPE_FIXED_WING:
if (!fixedwing_initialised) {
FixedWingFlyController::instance()->Initialize(&fixedWingPathFollowerSettings);
FixedWingAutoTakeoffController::instance()->Initialize(&fixedWingPathFollowerSettings);
FixedWingLandController::instance()->Initialize(&fixedWingPathFollowerSettings);
fixedwing_initialised = 1;
}
break;
@ -289,6 +295,14 @@ static void pathFollowerSetActiveController(void)
activeController = FixedWingFlyController::instance();
activeController->Activate();
break;
case PATHDESIRED_MODE_LAND: // land with optional velocity roam option
activeController = FixedWingLandController::instance();
activeController->Activate();
break;
case PATHDESIRED_MODE_AUTOTAKEOFF:
activeController = FixedWingAutoTakeoffController::instance();
activeController->Activate();
break;
default:
activeController = 0;
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_UNINITIALISED);
@ -451,6 +465,7 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
static void airspeedStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
FixedWingFlyController::instance()->AirspeedStateUpdatedCb(ev);
FixedWingAutoTakeoffController::instance()->AirspeedStateUpdatedCb(ev);
}

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

@ -2,9 +2,11 @@
******************************************************************************
*
* @file vtollandcontroller.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Vtol landing controller loop
* @see The GNU Public License (GPL) Version 3
* @addtogroup LibrePilot LibrePilotModules Modules PathFollower Navigation
*
*****************************************************************************/
/*
@ -57,6 +59,7 @@ extern "C" {
#include <stabilizationbank.h>
#include <stabilizationdesired.h>
#include <vtolselftuningstats.h>
#include <statusvtolautotakeoff.h>
#include <pathsummary.h>
}
@ -67,6 +70,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 +87,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,15 +131,40 @@ 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't 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);
}
}
// Controller deactivated
void VtolAutoTakeoffController::Deactivate(void)
{
if (mActive) {
@ -224,6 +280,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 +317,78 @@ 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();

30
flight/modules/PathFollower/vtollandcontroller.cpp
View File

@ -2,9 +2,11 @@
******************************************************************************
*
* @file vtollandcontroller.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Vtol landing controller loop
* @see The GNU Public License (GPL) Version 3
* @addtogroup LibrePilot LibrePilotModules Modules PathFollower Navigation
*
*****************************************************************************/
/*
@ -76,7 +78,8 @@ VtolLandController::VtolLandController()
void VtolLandController::Activate(void)
{
if (!mActive) {
mActive = true;
mActive = true;
mOverride = true;
SettingsUpdated();
fsm->Activate();
controlDown.Activate();
@ -97,11 +100,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)
{

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

@ -1,13 +1,14 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @addtogroup LibrePilotModules LibrePilot Modules
* @{
* @addtogroup PathPlanner Path Planner Module
* @brief Executes a series of waypoints
* @{
*
* @file pathplanner.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Executes a series of waypoints
*
* @see The GNU Public License (GPL) Version 3
@ -48,8 +49,6 @@
#include "plans.h"
#include <sanitycheck.h>
#include <vtolpathfollowersettings.h>
#include <statusvtolautotakeoff.h>
#include <statusvtolland.h>
#include <manualcontrolcommand.h>
// Private constants
@ -80,9 +79,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 +128,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 +230,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 +259,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 +310,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 +439,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)

4
flight/modules/System/systemmod.c
View File

@ -465,9 +465,11 @@ static void callbackSchedulerForEachCallback(int16_t callback_id, const struct p
return;
}
// delayed callback scheduler reports callback stack overflows as remaininng: -1
#if !defined(ARCH_POSIX) && !defined(ARCH_WIN32)
if (callback_info->stack_remaining < 0 && stackOverflow == STACKOVERFLOW_NONE) {
stackOverflow = STACKOVERFLOW_WARNING;
}
#endif
// By convention, there is a direct mapping between (not negative) callback scheduler callback_id's and members
// of the CallbackInfoXXXXElem enums
PIOS_DEBUG_Assert(callback_id < CALLBACKINFO_RUNNING_NUMELEM);
@ -681,6 +683,7 @@ void vApplicationIdleHook(void)
void vApplicationStackOverflowHook(__attribute__((unused)) xTaskHandle *pxTask,
__attribute__((unused)) signed portCHAR *pcTaskName)
{
#if !defined(ARCH_POSIX) && !defined(ARCH_WIN32)
stackOverflow = STACKOVERFLOW_CRITICAL;
#if DEBUG_STACK_OVERFLOW
static volatile bool wait_here = true;
@ -689,6 +692,7 @@ void vApplicationStackOverflowHook(__attribute__((unused)) xTaskHandle *pxTask,
}
wait_here = true;
#endif
#endif
}
/**

5
ground/gcs/src/plugins/hitl/fgsimulator.cpp
View File

@ -2,7 +2,8 @@
******************************************************************************
*
* @file flightgearbridge.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @addtogroup GCSPlugins GCS Plugins
* @{
* @addtogroup HITLPlugin HITL Plugin
@ -269,7 +270,7 @@ void FGSimulator::processUpdate(const QByteArray & inp)
// Get pressure (kpa)
float pressure = fields[20].toFloat() * INHG2KPA;
// Get VelocityState Down (m/s)
float velocityStateDown = -fields[21].toFloat() * FPS2CMPS * 1e-2f;
float velocityStateDown = fields[21].toFloat() * FPS2CMPS * 1e-2f;
// Get VelocityState East (m/s)
float velocityStateEast = fields[22].toFloat() * FPS2CMPS * 1e-2f;
// Get VelocityState Down (m/s)

71
shared/uavobjectdefinition/fixedwingpathfollowersettings.xml
View File

@ -3,54 +3,43 @@
<description>Settings for the @ref FixedWingPathFollowerModule</description>
<!-- these coefficients control desired movement in airspace -->
<field name="HorizontalVelMax" units="m/s" type="float" elements="1" defaultvalue="20"/>
<!-- Maximum speed the autopilot will try to achieve, usually for long distances -->
<field name="HorizontalVelMin" units="m/s" type="float" elements="1" defaultvalue="10"/>
<!-- V_y, Minimum speed the autopilot will try to fly, for example when loitering -->
<field name="VerticalVelMax" units="m/s" type="float" elements="1" defaultvalue="10"/>
<!-- maximum allowed climb or sink rate in guided flight-->
<field name="HorizontalVelMax" units="m/s" type="float" elements="1" defaultvalue="20" description="Maximum speed the autopilot will try to achieve, usually for long distances" />
<field name="HorizontalVelMin" units="m/s" type="float" elements="1" defaultvalue="10" description="Minimum speed the autopilot will try to fly, for example when loitering" />
<field name="VerticalVelMax" units="m/s" type="float" elements="1" defaultvalue="10" description="maximum allowed climb or sink rate in guided flight" />
<field name="CourseFeedForward" units="s" type="float" elements="1" defaultvalue="3.0"/>
<!-- how many seconds to plan the flight vector ahead when initiating necessary heading changes - increase for planes with sluggish response -->
<field name="ReverseCourseOverlap" units="deg" type="float" elements="1" defaultvalue="20.0"/>
<!-- how big the overlapping area behind the plane is, where, if the desired course lies behind, the current bank angle will determine if the plane goes left or right -->
<field name="CourseFeedForward" units="s" type="float" elements="1" defaultvalue="1.0" description="how many seconds to plan the flight vector ahead when initiating necessary heading changes - increase for planes with sluggish response" />
<field name="ReverseCourseOverlap" units="deg" type="float" elements="1" defaultvalue="20.0" description="how big the overlapping area behind the plane is, where, if the desired course lies in, the plane will not change bank angle while turning (to prevent oscillation)" />
<field name="HorizontalPosP" units="(m/s)/m" type="float" elements="1" defaultvalue="0.2"/>
<!-- proportional coefficient for correction vector in path vector field to get back on course - reduce for fast planes to prevent course oscillations -->
<field name="VerticalPosP" units="(m/s)/m" type="float" elements="1" defaultvalue="0.4"/>
<!-- proportional coefficient for desired vertical speed in relation to altitude displacement-->
<field name="HorizontalPosP" units="(m/s)/m" type="float" elements="1" defaultvalue="0.2" description="proportional coefficient for correction vector in path vector field to get back on course - reduce for fast planes to prevent course oscillations" />
<field name="VerticalPosP" units="(m/s)/m" type="float" elements="1" defaultvalue="0.4" description="proportional coefficient for desired vertical speed in relation to altitude displacement - reduce if plane is oscillating vertically, increase if altitude is not held reliably" />
<field name="UseAirspeedSensor" units="bool" type="enum" elements="1" options="False,True" defaultvalue="True" description="allows to ignore the airspeed sensor, set to false if you have none, which will trigger fallback algorithm without indicating a fault" />
<!-- these coefficients control actual control outputs -->
<field name="CoursePI" units="deg/deg" type="float" elements="3" elementnames="Kp,Ki,ILimit" defaultvalue="0.2, 0, 0"/>
<!-- coefficients for desired bank angle in relation to ground bearing error - this controls heading -->
<field name="CoursePI" units="deg/deg" type="float" elements="3" elementnames="Kp,Ki,ILimit" defaultvalue="0.2,0,0" description="coefficients for desired bank angle in relation to ground bearing error - this controls heading" />
<field name="SpeedPI" units="deg / (m/s)" type="float" elements="3" elementnames="Kp,Ki,ILimit" defaultvalue="2.5, .25, 10"/>
<!-- coefficients for desired pitch
in relation to speed error IASerror -->
<field name="VerticalToPitchCrossFeed" units="deg / (m/s)" type="float" elementnames="Kp,Max" defaultvalue="5, 10"/>
<!-- coefficients for adjusting desired pitch
in relation to "vertical speed error -->
<field name="AirspeedToPowerCrossFeed" units="1 / (m/s)" type="float" elementnames="Kp,Max" defaultvalue="0.2, 1"/>
<!-- proportional coefficient for adjusting vertical speed error for power calculation
in relation to airspeed error IASerror -->
<field name="PowerPI" units="1/(m/s)" type="float" elements="3" elementnames="Kp,Ki,ILimit" defaultvalue="0.01,0.05,0.5"/>
<!-- proportional coefficient for desired thrust
in relation to vertical speed error (absolute but including crossfeed) -->
<field name="SpeedPI" units="deg / (m/s)" type="float" elements="3" elementnames="Kp,Ki,ILimit" defaultvalue="2.5,.25,10" description="coefficients for desired pitch in relation to speed error" />
<field name="VerticalToPitchCrossFeed" units="deg / (m/s)" type="float" elementnames="Kp,Max" defaultvalue="5,10" description="coefficients for adjusting desired pitch in relation to vertical speed error" />
<field name="AirspeedToPowerCrossFeed" units="1 / (m/s)" type="float" elementnames="Kp,Max" defaultvalue="0.2,1" description="proportional coefficient for adjusting vertical speed error for power calculation in relation to airspeed error" />
<field name="PowerPI" units="1/(m/s)" type="float" elements="3" elementnames="Kp,Ki,ILimit" defaultvalue="0.01,0.05,0.5" description="proportional coefficient for desired thrust in relation to vertical speed error (absolute but including crossfeed)" />
<!-- output limits -->
<field name="RollLimit" units="deg" type="float" elements="3" elementnames="Min,Neutral,Max" defaultvalue="-45,0,45" />
<!-- maximum allowed bank angles in navigates flight -->
<field name="PitchLimit" units="deg" type="float" elements="3" elementnames="Min,Neutral,Max" defaultvalue="-10,5,20" />
<!-- maximum allowed pitch angles and setpoint for neutral pitch -->
<field name="ThrustLimit" units="" type="float" elements="3" elementnames="Min,Neutral,Max" defaultvalue="0.1,0.5,0.9" />
<!-- minimum and maximum allowed thrust and setpoint for cruise speed -->
<field name="Safetymargins" units="" type="float"
elementnames="Wind, Stallspeed, Lowspeed, Highspeed, Overspeed, Lowpower, Highpower, Pitchcontrol"
defaultvalue="90, 1.0, 0.5, 1.5, 1.0, 1, 0, 1" />
<!-- Wind: degrees of crabbing allowed
Speeds: percentage (1.0=100%) of the limit to be over/onder
Power & Control: flag to turn on/off 0.0 =off 1.0 = on -->
<field name="UpdatePeriod" units="ms" type="int32" elements="1" defaultvalue="100"/>
<field name="RollLimit" units="deg" type="float" elements="3" elementnames="Min,Neutral,Max" defaultvalue="-45,0,45" description="maximum allowed bank angles in navigates flight" />
<field name="PitchLimit" units="deg" type="float" elements="3" elementnames="Min,Neutral,Max" defaultvalue="-10,5,20" description="maximum allowed pitch angles and setpoint for neutral pitch" />
<field name="ThrustLimit" units="" type="float" elements="3" elementnames="Min,Neutral,Max" defaultvalue="0.1,0.5,0.9" description="minimum and maximum allowed thrust and setpoint for cruise speed" />
<field name="Safetymargins" units="" type="float"
elementnames="Wind,Stallspeed,Lowspeed,Highspeed,Overspeed,Lowpower,Highpower,Rollcontrol,Pitchcontrol"
defaultvalue="1,1.0,0.5,1.5,1.0,1,0,1,1"
description="Wind: degrees of crabbing allowed -- Speeds: percentage (1.0=100%) of the limit to be over/under -- Power &amp; Control: flag to turn on/off 0.0=off 1.0 = on" />
<field name="SafetyCutoffLimits" units="" type="float"
elementnames="RollDeg,PitchDeg,YawDeg,MaxDecelerationDeltaMPS" defaultvalue="25.0,25.0,25.0,4.0"
description="maximum margins from attempted attitude allowed during takeoff -- In flight Roll/Pitch are added to RollLimit/PitchLimit as max limits - throttle is cut when beyond if roll &gt; RollLimit.Max + TakeOffLimits.RollDeg the same happens when MaxDecelerationDeltaMPS below Lowspeed limit if speed &lt; Safetymargins.Lowspeed*HorizontalVelMin - TakeOffLimits.MaxDecelerationDeltaMPS" />
<field name="TakeOffPitch" units="deg" type="float" elements="1" defaultvalue="25.0" description="pitch angle in autotakeoff mode" />
<field name="LandingPitch" units="deg" type="float" elements="1" defaultvalue="7.5" description="pitch angle in autoland mode" />
<field name="UpdatePeriod" units="ms" type="int32" elements="1" defaultvalue="100" description="update period of pathfollower" />
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/>

2
shared/uavobjectdefinition/fixedwingpathfollowerstatus.xml
View File

@ -4,7 +4,7 @@
<field name="Error" units="" type="float" elementnames="Course,Speed,Power" />
<field name="ErrorInt" units="" type="float" elementnames="Course,Speed,Power" />
<field name="Command" units="" type="float" elementnames="Course,Speed,Power" />
<field name="Errors" units="" type="uint8" elementnames="Wind,Stallspeed,Lowspeed,Highspeed,Overspeed,Lowpower,Highpower,Pitchcontrol" />
<field name="Errors" units="" type="uint8" elementnames="Wind,Stallspeed,Lowspeed,Highspeed,Overspeed,Lowpower,Highpower,Rollcontrol,Pitchcontrol,AirspeedSensor" />
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="500"/>

1
shared/uavobjectdefinition/flightmodesettings.xml
View File

@ -94,6 +94,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" description="wether a path plan should continue when interrupted by flight mode change (False), or restart from waypoint 0 (True)"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
<telemetryflight acked="true" updatemode="onchange" period="0"/>