rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-29 07:24:13 +01:00
Code Issues Releases Activity

LP-89 - Port OP_15.05.01 fixes. Release notes:

Browse Source 
--- 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(-)
This commit is contained in:
Alessio Morale 2015-09-19 16:00:25 +02:00
parent 498e77fc02
commit d20d1ab295
6 changed files with 77 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
WHATSNEW.txt
View File

@ -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.

26
flight/libraries/pid/pidcontroldown.cpp
View File

@ -41,7 +41,7 @@ extern "C" {
#include <paths.h>
#include "plans.h"
#include <stabilizationdesired.h>
#include <pidstatus.h>
// #include <pidstatus.h>
#include <vtolselftuningstats.h>
}
@ -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;
}

28
flight/libraries/plans.c
View File

@ -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;

1
flight/modules/PathFollower/inc/pidcontrolne.h
View File

@ -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);

23
flight/modules/PathFollower/pidcontrolne.cpp
View File

@ -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)

16
flight/modules/PathFollower/vtolflycontroller.cpp
View File

@ -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();
}
}
}
}