1
0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-03-15 07:29:15 +01:00

OP-1847 gps assist fixes

1. Update vtol vertical pids
2. Reduce deadband on throttle in gpsassist auto throttle to avoid large deviations in throttle change from neutral
3. Reinitialise assisted control state on mode change where the flight is the same but only gpsassist is going from disabled to enabled or enabled to disabled.
This commit is contained in:
abeck70 2015-05-14 20:07:07 +10:00
parent c51bca44be
commit f4d1fc13ec
2 changed files with 16 additions and 6 deletions

View File

@ -59,8 +59,8 @@
#define CBTASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL
#define ASSISTEDCONTROL_NEUTRALTHROTTLERANGE_FACTOR 0.2f
#define ASSISTEDCONTROL_BRAKETHRUST_DEADBAND_FACTOR_LO 0.92f
#define ASSISTEDCONTROL_BRAKETHRUST_DEADBAND_FACTOR_HI 1.08f
#define ASSISTEDCONTROL_BRAKETHRUST_DEADBAND_FACTOR_LO 0.96f
#define ASSISTEDCONTROL_BRAKETHRUST_DEADBAND_FACTOR_HI 1.04f
// defined handlers
@ -242,7 +242,7 @@ static void manualControlTask(void)
// set assist mode to none to avoid an assisted flight mode position
// carrying over and impacting a newly selected non-assisted flight mode pos
newFlightModeAssist = FLIGHTSTATUS_FLIGHTMODEASSIST_NONE;
// The following are eqivalent to none effectively. Code should always
// The following are equivalent to none effectively. Code should always
// check the flightmodeassist state.
newAssistedControlState = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_PRIMARY;
newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL;
@ -264,6 +264,16 @@ static void manualControlTask(void)
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
newFlightModeAssist = isAssistedFlightMode(position, newMode, &modeSettings);
if (newFlightModeAssist != flightStatus.FlightModeAssist) {
// On change of assist mode reinitialise control state. This is required
// for the scenario where a flight position change reuses a flight mode
// but adds assistedcontrol.
newAssistedControlState = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_PRIMARY;
newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL;
}
if (newFlightModeAssist) {
// assess roll/pitch state
bool flagRollPitchHasInput = (fabsf(cmd.Roll) > 0.0f || fabsf(cmd.Pitch) > 0.0f);
@ -301,7 +311,7 @@ static void manualControlTask(void)
// retain thrust cmd for later comparison with actual in braking
thrustAtBrakeStart = cmd.Thrust;
// calculate hi and low value of +-8% as a mini-deadband
// calculate hi and low value of +-4% as a mini-deadband
// for use in auto-override in brake sequence
thrustLo = ASSISTEDCONTROL_BRAKETHRUST_DEADBAND_FACTOR_LO * thrustAtBrakeStart;
thrustHi = ASSISTEDCONTROL_BRAKETHRUST_DEADBAND_FACTOR_HI * thrustAtBrakeStart;

View File

@ -7,8 +7,8 @@
<field name="CourseFeedForward" units="s" type="float" elements="1" defaultvalue="1.0"/>
<field name="HorizontalPosP" units="(m/s)/m" type="float" elements="1" defaultvalue="0.25"/>
<field name="VerticalPosP" units="" type="float" elements="1" defaultvalue="0.4"/>
<field name="HorizontalVelPID" units="deg/(m/s)" type="float" elementnames="Kp,Ki,Kd,Beta" defaultvalue="8.0, 0.5, 0.0, 0.95"/>
<field name="VerticalVelPID" units="" type="float" elementnames="Kp,Ki,Kd,Beta" defaultvalue="0.3, 0.3, 0.0, 0.95"/>
<field name="HorizontalVelPID" units="deg/(m/s)" type="float" elementnames="Kp,Ki,Kd,Beta" defaultvalue="8.0, 0.5, 0.001, 0.95"/>
<field name="VerticalVelPID" units="(m/s^2)/(m/s)" type="float" elementnames="Kp,Ki,Kd,Beta" defaultvalue="0.5, 0.45, 0.001, 0.95"/>
<field name="ThrustLimits" units="" type="float" elementnames="Min,Neutral,Max" defaultvalue="0.2, 0.5, 0.9"/>
<field name="VelocityFeedforward" units="deg/(m/s)" type="float" elements="1" defaultvalue="2"/>
<field name="ThrustControl" units="" type="enum" elements="1" options="manual,auto" defaultvalue="manual"/>