From d20d1ab295fd8a8b8f13eab3393af1de5592488b Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Sat, 19 Sep 2015 16:00:25 +0200 Subject: [PATCH] LP-89 - Port OP_15.05.01 fixes. Release notes: --- RELEASE-15.05.01 HOTFIX --- Banana Split --- This release fixes an important bug. All Revolution hardware running 15.05 should upgrade to 15.05.01. Note that this is a hotfix; it can simply be flashed without an erase settings. Furthermore, please review your vtolpathfollowersettings:HorizontalVelMax; a value of around 4m/s would be more appropriate for preliminary trialing of a new release and will be changed in future. Release Notes - OP Next Generation - Version OP15.05.01 ** Bug * [NG-55] - 15.05 PositionHold exhibits fly-away behaviour at the vtolpathfollowersettings maxRollPitch and HorizontalVelMax values. --- WHATSNEW.txt | 13 ++++++++++- flight/libraries/pid/pidcontroldown.cpp | 26 ++++++++++----------- flight/libraries/plans.c | 28 +++++++++++++++++++---- flight/modules/PathFollower/inc/pidcontrolne.h | 1 - flight/modules/PathFollower/pidcontrolne.cpp | 23 +++++++++++++++---- flight/modules/PathFollower/vtolflycontroller.cpp | 16 ++++++++----- 6 files changed, 77 insertions(+), 30 deletions(-) --- WHATSNEW.txt | 13 ++++++++- flight/libraries/pid/pidcontroldown.cpp | 26 ++++++++--------- flight/libraries/plans.c | 28 +++++++++++++++---- .../modules/PathFollower/inc/pidcontrolne.h | 1 - flight/modules/PathFollower/pidcontrolne.cpp | 23 ++++++++++++--- .../PathFollower/vtolflycontroller.cpp | 16 +++++++---- 6 files changed, 77 insertions(+), 30 deletions(-) diff --git a/WHATSNEW.txt b/WHATSNEW.txt index a9e52e817..29b95a293 100644 --- a/WHATSNEW.txt +++ b/WHATSNEW.txt @@ -1,4 +1,15 @@ ---- RELEASE-15.05 --- Revolution Nano --- +--- RELEASE-15.05.01 HOTFIX --- Banana Split --- +This release fixes an important bug. All Revolution hardware running 15.05 should upgrade to 15.05.01. Note that this is a hotfix; it can +simply be flashed without an erase settings. Furthermore, please review your vtolpathfollowersettings:HorizontalVelMax; a value of around 4m/s would be more appropriate for preliminary trialing of a new release and will be changed in future. + +Release Notes - OP Next Generation - Version OP15.05.01 + +** Bug + * [NG-55] - 15.05 PositionHold exhibits fly-away behaviour at the vtolpathfollowersettings maxRollPitch and HorizontalVelMax values. + + + +--- RELEASE-15.05 --- Banana Split --- This release introduces new features and new hardware support for the Revolution Nano. Note that the CC3D is not supported by this feature release. diff --git a/flight/libraries/pid/pidcontroldown.cpp b/flight/libraries/pid/pidcontroldown.cpp index 7f06eac93..317441f97 100644 --- a/flight/libraries/pid/pidcontroldown.cpp +++ b/flight/libraries/pid/pidcontroldown.cpp @@ -41,7 +41,7 @@ extern "C" { #include #include "plans.h" #include -#include +// #include #include } @@ -348,7 +348,7 @@ float PIDControlDown::GetVelocityDesired(void) float PIDControlDown::GetDownCommand(void) { - PIDStatusData pidStatus; + // PIDStatusData pidStatus; float ulow = mMinThrust; float uhigh = mMaxThrust; @@ -356,17 +356,17 @@ float PIDControlDown::GetDownCommand(void) mCallback->BoundThrust(ulow, uhigh); } float downCommand = pid2_apply(&PID, mVelocitySetpointCurrent, mVelocityState, ulow, uhigh); - pidStatus.setpoint = mVelocitySetpointCurrent; - pidStatus.actual = mVelocityState; - pidStatus.error = mVelocitySetpointCurrent - mVelocityState; - pidStatus.setpoint = mVelocitySetpointCurrent; - pidStatus.ulow = ulow; - pidStatus.uhigh = uhigh; - pidStatus.command = downCommand; - pidStatus.P = PID.P; - pidStatus.I = PID.I; - pidStatus.D = PID.D; - PIDStatusSet(&pidStatus); + // pidStatus.setpoint = mVelocitySetpointCurrent; + // pidStatus.actual = mVelocityState; + // pidStatus.error = mVelocitySetpointCurrent - mVelocityState; + // pidStatus.setpoint = mVelocitySetpointCurrent; + // pidStatus.ulow = ulow; + // pidStatus.uhigh = uhigh; + // pidStatus.command = downCommand; + // pidStatus.P = PID.P; + // pidStatus.I = PID.I; + // pidStatus.D = PID.D; + // PIDStatusSet(&pidStatus); mDownCommand = downCommand; return mDownCommand; } diff --git a/flight/libraries/plans.c b/flight/libraries/plans.c index 1ed4d6e11..634b353c9 100644 --- a/flight/libraries/plans.c +++ b/flight/libraries/plans.c @@ -102,7 +102,8 @@ void plan_setup_positionHold() PositionStateGet(&positionState); PathDesiredData pathDesired; - PathDesiredGet(&pathDesired); + // re-initialise in setup stage + memset(&pathDesired, 0, sizeof(PathDesiredData)); FlightModeSettingsPositionHoldOffsetData offset; FlightModeSettingsPositionHoldOffsetGet(&offset); @@ -132,7 +133,8 @@ void plan_setup_returnToBase() PositionStateDownGet(&positionStateDown); PathDesiredData pathDesired; - PathDesiredGet(&pathDesired); + // re-initialise in setup stage + memset(&pathDesired, 0, sizeof(PathDesiredData)); TakeOffLocationData takeoffLocation; TakeOffLocationGet(&takeoffLocation); @@ -241,6 +243,8 @@ void plan_setup_AutoTakeoff() // stabi command. } PathDesiredData pathDesired; + // re-initialise in setup stage + memset(&pathDesired, 0, sizeof(PathDesiredData)); plan_setup_AutoTakeoff_helper(&pathDesired); PathDesiredSet(&pathDesired); } @@ -312,6 +316,8 @@ void plan_run_AutoTakeoff() 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); } @@ -348,6 +354,9 @@ void plan_setup_land() { PathDesiredData pathDesired; + // re-initialise in setup stage + memset(&pathDesired, 0, sizeof(PathDesiredData)); + plan_setup_land_helper(&pathDesired); PathDesiredSet(&pathDesired); } @@ -506,7 +515,10 @@ static void plan_run_PositionVario(vario_type type) float alpha; PathDesiredData pathDesired; + // Reuse the existing pathdesired object as setup in the setup to avoid + // updating values already set. PathDesiredGet(&pathDesired); + FlightModeSettingsPositionHoldOffsetData offset; FlightModeSettingsPositionHoldOffsetGet(&offset); @@ -587,10 +599,11 @@ void plan_run_VelocityRoam() { // float alpha; PathDesiredData pathDesired; + // velocity roam code completely sets pathdesired object. it was not set in setup phase + memset(&pathDesired, 0, sizeof(PathDesiredData)); FlightStatusAssistedControlStateOptions assistedControlFlightMode; FlightStatusFlightModeOptions flightMode; - PathDesiredGet(&pathDesired); FlightModeSettingsPositionHoldOffsetData offset; FlightModeSettingsPositionHoldOffsetGet(&offset); FlightStatusAssistedControlStateGet(&assistedControlFlightMode); @@ -711,10 +724,11 @@ static PiOSDeltatimeConfig actimeval; void plan_setup_AutoCruise() { PositionStateData positionState; - PositionStateGet(&positionState); + PathDesiredData pathDesired; - PathDesiredGet(&pathDesired); + // setup needs to reinitialise the pathdesired object + memset(&pathDesired, 0, sizeof(PathDesiredData)); FlightModeSettingsPositionHoldOffsetData offset; FlightModeSettingsPositionHoldOffsetGet(&offset); @@ -757,7 +771,9 @@ void plan_run_AutoCruise() PositionStateGet(&positionState); PathDesiredData pathDesired; + // re-use pathdesired that was setup correctly in setup stage. PathDesiredGet(&pathDesired); + FlightModeSettingsPositionHoldOffsetData offset; FlightModeSettingsPositionHoldOffsetGet(&offset); @@ -830,6 +846,8 @@ void plan_setup_assistedcontrol() PositionStateGet(&positionState); PathDesiredData pathDesired; + // setup function, avoid carry over from previous mode + memset(&pathDesired, 0, sizeof(PathDesiredData)); FlightStatusAssistedControlStateOptions assistedControlFlightMode; diff --git a/flight/modules/PathFollower/inc/pidcontrolne.h b/flight/modules/PathFollower/inc/pidcontrolne.h index 8d2fa0d3b..dba5d59bb 100644 --- a/flight/modules/PathFollower/inc/pidcontrolne.h +++ b/flight/modules/PathFollower/inc/pidcontrolne.h @@ -39,7 +39,6 @@ class PIDControlNE { public: PIDControlNE(); ~PIDControlNE(); - void Initialize(); void Deactivate(); void Activate(); void UpdateParameters(float kp, float ki, float kd, __attribute__((unused)) float ilimit, float dT, float velocityMax); diff --git a/flight/modules/PathFollower/pidcontrolne.cpp b/flight/modules/PathFollower/pidcontrolne.cpp index f6ac0f6cd..5aa6f7460 100644 --- a/flight/modules/PathFollower/pidcontrolne.cpp +++ b/flight/modules/PathFollower/pidcontrolne.cpp @@ -47,13 +47,24 @@ extern "C" { PIDControlNE::PIDControlNE() : deltaTime(0), mNECommand(0), mNeutral(0), mVelocityMax(0), mMinCommand(0), mMaxCommand(0), mVelocityFeedforward(0), mActive(false) -{} +{ + memset((void *)PIDvel, 0, 2 * sizeof(pid2)); + memset((void *)PIDposH, 0, 2 * sizeof(pid)); + + mVelocitySetpointTarget[0] = 0.0f; + mVelocitySetpointTarget[1] = 0.0f; + mVelocityState[0] = 0.0f; + mVelocityState[1] = 0.0f; + mPositionSetpointTarget[0] = 0.0f; + mPositionSetpointTarget[1] = 0.0f; + mPositionState[0] = 0.0f; + mPositionState[1] = 0.0f; + mVelocitySetpointCurrent[0] = 0.0f; + mVelocitySetpointCurrent[1] = 0.0f; +} PIDControlNE::~PIDControlNE() {} -void PIDControlNE::Initialize() -{} - void PIDControlNE::Deactivate() { mActive = false; @@ -62,6 +73,10 @@ void PIDControlNE::Deactivate() void PIDControlNE::Activate() { mActive = true; + pid2_transfer(&PIDvel[0], 0.0f); + pid2_transfer(&PIDvel[1], 0.0f); + pid_zero(&PIDposH[0]); + pid_zero(&PIDposH[1]); } void PIDControlNE::UpdateParameters(float kp, float ki, float kd, float beta, float dT, float velocityMax) diff --git a/flight/modules/PathFollower/vtolflycontroller.cpp b/flight/modules/PathFollower/vtolflycontroller.cpp index 20a387277..2ba3592a9 100644 --- a/flight/modules/PathFollower/vtolflycontroller.cpp +++ b/flight/modules/PathFollower/vtolflycontroller.cpp @@ -84,6 +84,7 @@ void VtolFlyController::Activate(void) mManualThrust = false; SettingsUpdated(); controlDown.Activate(); + controlNE.UpdateVelocitySetpoint(0.0f, 0.0f); controlNE.Activate(); mMode = pathDesired->Mode; @@ -175,8 +176,6 @@ void VtolFlyController::UpdateVelocityDesired() VelocityStateData velocityState; VelocityStateGet(&velocityState); - controlNE.UpdateVelocityState(velocityState.North, velocityState.East); - controlDown.UpdateVelocityState(velocityState.Down); VelocityDesiredData velocityDesired; @@ -192,6 +191,9 @@ void VtolFlyController::UpdateVelocityDesired() controlDown.ControlPositionWithPath(&progress); } + controlNE.UpdateVelocityState(velocityState.North, velocityState.East); + controlDown.UpdateVelocityState(velocityState.Down); + float north, east; controlNE.GetVelocityDesired(&north, &east); velocityDesired.North = north; @@ -368,10 +370,12 @@ void VtolFlyController::UpdateAutoPilot() // to the pathDesired to initiate a Landing sequence. This is the simpliest approach. plans.c // can't manage this. And pathplanner whilst similar does not manage this as it is not a // waypoint traversal and is not aware of flight modes other than path plan. - if ((uint8_t)pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_NEXTCOMMAND] == FLIGHTMODESETTINGS_RETURNTOBASENEXTCOMMAND_LAND) { - if (pathStatus->fractional_progress > RTB_LAND_FRACTIONAL_PROGRESS_START_CHECKS) { - if (fabsf(pathStatus->correction_direction_north) < RTB_LAND_NE_DISTANCE_REQUIRED_TO_START_LAND_SEQUENCE && fabsf(pathStatus->correction_direction_east) < RTB_LAND_NE_DISTANCE_REQUIRED_TO_START_LAND_SEQUENCE) { - plan_setup_land(); + if (flightStatus->FlightMode == FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE) { + if ((uint8_t)pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_NEXTCOMMAND] == FLIGHTMODESETTINGS_RETURNTOBASENEXTCOMMAND_LAND) { + if (pathStatus->fractional_progress > RTB_LAND_FRACTIONAL_PROGRESS_START_CHECKS) { + if (fabsf(pathStatus->correction_direction_north) < RTB_LAND_NE_DISTANCE_REQUIRED_TO_START_LAND_SEQUENCE && fabsf(pathStatus->correction_direction_east) < RTB_LAND_NE_DISTANCE_REQUIRED_TO_START_LAND_SEQUENCE) { + plan_setup_land(); + } } } }