diff --git a/flight/libraries/paths.c b/flight/libraries/paths.c index 8656b920a..620493792 100644 --- a/flight/libraries/paths.c +++ b/flight/libraries/paths.c @@ -61,6 +61,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; diff --git a/flight/modules/PathFollower/fixedwingautotakeoffcontroller.cpp b/flight/modules/PathFollower/fixedwingautotakeoffcontroller.cpp new file mode 100644 index 000000000..437920d8f --- /dev/null +++ b/flight/modules/PathFollower/fixedwingautotakeoffcontroller.cpp @@ -0,0 +1,251 @@ +/* + ****************************************************************************** + * + * @file FixedWingAutoTakeoffController.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015. + * @brief Fixed wing fly controller implementation + * @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 + */ + +extern "C" { +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +} + +// C++ includes +#include "fixedwingautotakeoffcontroller.h" + +// Private constants + +// 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); + } else { + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK); + } + + PathStatusSet(pathStatus); +} + +/** + * check if situation is unsafe + */ +bool FixedWingAutoTakeoffController::isUnsafe(void) +{ + bool abort = false; + float speed = getAirspeed(); + + if (speed > maxVelocity) { + maxVelocity = speed; + } + AttitudeStateData attitude; + AttitudeStateGet(&attitude); + // too much bank angle + if (fabsf(attitude.Roll) > fixedWingSettings->TakeOffLimits.RollDeg) { + abort = true; + } + if (fabsf(attitude.Pitch - fixedWingSettings->TakeOffPitch) > fixedWingSettings->TakeOffLimits.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->TakeOffLimits.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 + 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->TakeOffLimits.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); +} diff --git a/flight/modules/PathFollower/fixedwingflycontroller.cpp b/flight/modules/PathFollower/fixedwingflycontroller.cpp index ed47d0069..d498d41eb 100644 --- a/flight/modules/PathFollower/fixedwingflycontroller.cpp +++ b/flight/modules/PathFollower/fixedwingflycontroller.cpp @@ -26,23 +26,13 @@ extern "C" { #include -#include - -#include #include -#include #include #include #include -#include "plans.h" -#include - -#include -#include #include #include #include -#include #include #include #include @@ -50,14 +40,7 @@ extern "C" { #include #include #include -#include -#include -#include #include -#include -#include -#include -#include } // C++ includes diff --git a/flight/modules/PathFollower/fixedwinglandcontroller.cpp b/flight/modules/PathFollower/fixedwinglandcontroller.cpp new file mode 100644 index 000000000..52727eceb --- /dev/null +++ b/flight/modules/PathFollower/fixedwinglandcontroller.cpp @@ -0,0 +1,143 @@ +/* + ****************************************************************************** + * + * @file FixedWingLandController.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015. + * @brief Fixed wing fly controller implementation + * @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 + */ + +extern "C" { +#include + +#include +#include +#include +#include +#include +} + +// 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 integrals + */ +void FixedWingLandController::resetGlobals() +{ + pathStatus->path_time = 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); +} diff --git a/flight/modules/PathFollower/inc/fixedwingautotakeoffcontroller.h b/flight/modules/PathFollower/inc/fixedwingautotakeoffcontroller.h new file mode 100644 index 000000000..8b092aa9e --- /dev/null +++ b/flight/modules/PathFollower/inc/fixedwingautotakeoffcontroller.h @@ -0,0 +1,87 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot 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. + * @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 { +public: + 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 diff --git a/flight/modules/PathFollower/inc/fixedwingflycontroller.h b/flight/modules/PathFollower/inc/fixedwingflycontroller.h index 19dcf4003..45cfcf743 100644 --- a/flight/modules/PathFollower/inc/fixedwingflycontroller.h +++ b/flight/modules/PathFollower/inc/fixedwingflycontroller.h @@ -57,6 +57,13 @@ 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(); @@ -64,17 +71,11 @@ private: uint8_t updateFixedDesiredAttitude(); bool correctCourse(float *C, float *V, float *F, float s); - FixedWingPathFollowerSettingsData *fixedWingSettings; - uint8_t mActive; - uint8_t mMode; - 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 diff --git a/flight/modules/PathFollower/inc/fixedwinglandcontroller.h b/flight/modules/PathFollower/inc/fixedwinglandcontroller.h new file mode 100644 index 000000000..17a4e9b44 --- /dev/null +++ b/flight/modules/PathFollower/inc/fixedwinglandcontroller.h @@ -0,0 +1,66 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot 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. + * @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 diff --git a/flight/modules/PathFollower/pathfollower.cpp b/flight/modules/PathFollower/pathfollower.cpp index f96328965..e73ad8c24 100644 --- a/flight/modules/PathFollower/pathfollower.cpp +++ b/flight/modules/PathFollower/pathfollower.cpp @@ -93,6 +93,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 +222,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 +293,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 +463,7 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) static void airspeedStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { FixedWingFlyController::instance()->AirspeedStateUpdatedCb(ev); + FixedWingAutoTakeoffController::instance()->AirspeedStateUpdatedCb(ev); }