rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-20 10:54:14 +01:00
Code Issues Releases Activity

REVONANO VelocityRoam fixes

Browse Source 
1. New VelocityRoamHorizontalVelPID to allow tuning of VR independent of pathfollower nav modes
This commit is contained in:
abeck70 2015-05-25 22:24:06 +10:00
parent c5715b26af
commit cdf1b88cc0
2 changed files with 12 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
flight/modules/PathFollower/vtolvelocitycontroller.cpp
View File

@ -30,6 +30,7 @@ extern "C" {
#include <math.h>
#include <pid.h>
#include <alarms.h>
#include <CoordinateConversions.h>
#include <sin_lookup.h>
#include <pathdesired.h>
@ -104,10 +105,10 @@ void VtolVelocityController::SettingsUpdated(void)
{
const float dT = vtolPathFollowerSettings->UpdatePeriod / 1000.0f;
controlNE.UpdateParameters(vtolPathFollowerSettings->HorizontalVelPID.Kp,
vtolPathFollowerSettings->HorizontalVelPID.Ki,
vtolPathFollowerSettings->HorizontalVelPID.Kd,
vtolPathFollowerSettings->HorizontalVelPID.Beta,
controlNE.UpdateParameters(vtolPathFollowerSettings->VelocityRoamHorizontalVelPID.Kp,
vtolPathFollowerSettings->VelocityRoamHorizontalVelPID.Ki,
vtolPathFollowerSettings->VelocityRoamHorizontalVelPID.Kd,
vtolPathFollowerSettings->VelocityRoamHorizontalVelPID.Beta,
dT,
vtolPathFollowerSettings->HorizontalVelMax);
@ -201,28 +202,12 @@ void VtolVelocityController::UpdateAutoPilot()
float yaw = 0.0f;
int8_t result = UpdateStabilizationDesired(yaw_attitude, yaw);
if (!result) {
fallback_to_hold();
if (result) {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
} else {
pathStatus->Status = PATHSTATUS_STATUS_CRITICAL;
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING);
}
PathStatusSet(pathStatus);
}
void VtolVelocityController::fallback_to_hold(void)
{
PositionStateData positionState;
PositionStateGet(&positionState);
pathDesired->End.North = positionState.North;
pathDesired->End.East = positionState.East;
pathDesired->End.Down = positionState.Down;
pathDesired->Start.North = positionState.North;
pathDesired->Start.East = positionState.East;
pathDesired->Start.Down = positionState.Down;
pathDesired->StartingVelocity = 0.0f;
pathDesired->EndingVelocity = 0.0f;
pathDesired->Mode = PATHDESIRED_MODE_GOTOENDPOINT;
PathDesiredSet(pathDesired);
}

3
shared/uavobjectdefinition/vtolpathfollowersettings.xml
View File

@ -18,7 +18,6 @@
<field name="EmergencyFallbackAttitude" units="deg" type="float" elementnames="Roll,Pitch" defaultvalue="0,-20.0"/>
<field name="EmergencyFallbackYawRate" units="(deg/s)/deg" type="float" elementnames="kP,Max" defaultvalue="2.0, 30.0"/>
<field name="MaxRollPitch" units="deg" type="float" elements="1" defaultvalue="25"/>
<field name="VelocityRoamMaxRollPitch" units="deg" type="float" elements="1" defaultvalue="20"/>
<field name="UpdatePeriod" units="ms" type="uint16" elements="1" defaultvalue="50"/>
<field name="BrakeRate" units="m/s2" type="float" elements="1" defaultvalue="2.5"/>
<field name="BrakeMaxPitch" units="deg" type="float" elements="1" defaultvalue="25"/>
@ -26,6 +25,8 @@
<field name="BrakeVelocityFeedforward" units="deg/(m/s)" type="float" elements="1" defaultvalue="0"/>
<field name="LandVerticalVelPID" units="" type="float" elementnames="Kp,Ki,Kd,Beta" defaultvalue="0.42, 3.0, 0.02, 0.95"/>
<field name="AutoTakeoffVerticalVelPID" units="" type="float" elementnames="Kp,Ki,Kd,Beta" defaultvalue="0.42, 3.0, 0.02, 0.95"/>
<field name="VelocityRoamMaxRollPitch" units="deg" type="float" elements="1" defaultvalue="20"/>
<field name="VelocityRoamHorizontalVelPID" units="deg/(m/s)" type="float" elementnames="Kp,Ki,Kd,Beta" defaultvalue="8.0, 0.5, 0.001, 0.95"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
<telemetryflight acked="true" updatemode="onchange" period="0"/>