rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-30 15:52:12 +01:00
Code Issues Releases Activity

Merge branch 'abeck/OP-1847-gps-assist-fixes' into rel-15.05

Browse Source
This commit is contained in:
abeck70 2015-05-17 19:58:29 +10:00
commit 2a70c702f8
9 changed files with 87 additions and 82 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
flight/libraries/inc/plans.h
View File

@ -67,7 +67,7 @@ void plan_setup_AutoTakeoff();
/**
* @brief setup pathplanner/pathfollower for braking
*/
void plan_setup_assistedcontrol(uint8_t timeout_occurred);
void plan_setup_assistedcontrol();
#define PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_NORTH 0
#define PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_EAST 1

118
flight/libraries/plans.c
View File

@ -615,7 +615,7 @@ void plan_run_VelocityRoam()
// avoid brake then hold sequence to continue descent.
plan_setup_land_from_velocityroam();
} else {
plan_setup_assistedcontrol(false);
plan_setup_assistedcontrol();
}
}
// otherwise nothing to do in braking/hold modes
@ -824,7 +824,7 @@ void plan_run_AutoCruise()
#define ASSISTEDCONTROL_TIMETOSTOP_MAXIMUM 9.0f // seconds
#define ASSISTEDCONTROL_DELAY_TO_BRAKE 1.0f // seconds
#define ASSISTEDCONTROL_TIMEOUT_MULTIPLIER 2.0f // actual deceleration rate can be 50% of desired...timeouts need to cater for this
void plan_setup_assistedcontrol(uint8_t timeout_occurred)
void plan_setup_assistedcontrol()
{
PositionStateData positionState;
@ -832,75 +832,55 @@ void plan_setup_assistedcontrol(uint8_t timeout_occurred)
PathDesiredData pathDesired;
FlightStatusAssistedControlStateOptions assistedControlFlightMode;
FlightStatusAssistedControlStateGet(&assistedControlFlightMode);
if (timeout_occurred) {
FlightStatusFlightModeOptions flightMode;
FlightStatusFlightModeGet(&flightMode);
if (flightMode == FLIGHTSTATUS_FLIGHTMODE_LAND) {
plan_setup_land_helper(&pathDesired);
} else {
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;
}
assistedControlFlightMode = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_HOLD;
} else {
VelocityStateData velocityState;
VelocityStateGet(&velocityState);
float brakeRate;
VtolPathFollowerSettingsBrakeRateGet(&brakeRate);
if (brakeRate < ASSISTEDCONTROL_BRAKERATE_MINIMUM) {
brakeRate = ASSISTEDCONTROL_BRAKERATE_MINIMUM; // set a minimum to avoid a divide by zero potential below
}
// Calculate the velocity
float velocity = velocityState.North * velocityState.North + velocityState.East * velocityState.East + velocityState.Down * velocityState.Down;
velocity = sqrtf(velocity);
// Calculate the desired time to zero velocity.
float time_to_stopped = ASSISTEDCONTROL_DELAY_TO_BRAKE; // we allow at least 0.5 seconds to rotate to a brake angle.
time_to_stopped += velocity / brakeRate;
// Sanity check the brake rate by ensuring that the time to stop is within a range.
if (time_to_stopped < ASSISTEDCONTROL_TIMETOSTOP_MINIMUM) {
time_to_stopped = ASSISTEDCONTROL_TIMETOSTOP_MINIMUM;
} else if (time_to_stopped > ASSISTEDCONTROL_TIMETOSTOP_MAXIMUM) {
time_to_stopped = ASSISTEDCONTROL_TIMETOSTOP_MAXIMUM;
}
// calculate the distance we will travel
float north_delta = velocityState.North * ASSISTEDCONTROL_DELAY_TO_BRAKE; // we allow at least 0.5s to rotate to brake angle
north_delta += (time_to_stopped - ASSISTEDCONTROL_DELAY_TO_BRAKE) * 0.5f * velocityState.North; // area under the linear deceleration plot
float east_delta = velocityState.East * ASSISTEDCONTROL_DELAY_TO_BRAKE; // we allow at least 0.5s to rotate to brake angle
east_delta += (time_to_stopped - ASSISTEDCONTROL_DELAY_TO_BRAKE) * 0.5f * velocityState.East; // area under the linear deceleration plot
float down_delta = velocityState.Down * ASSISTEDCONTROL_DELAY_TO_BRAKE;
down_delta += (time_to_stopped - ASSISTEDCONTROL_DELAY_TO_BRAKE) * 0.5f * velocityState.Down; // area under the linear deceleration plot
float net_delta = east_delta * east_delta + north_delta * north_delta + down_delta * down_delta;
net_delta = sqrtf(net_delta);
pathDesired.Start.North = positionState.North;
pathDesired.Start.East = positionState.East;
pathDesired.Start.Down = positionState.Down;
pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_NORTH] = velocityState.North;
pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_EAST] = velocityState.East;
pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_DOWN] = velocityState.Down;
pathDesired.End.North = positionState.North + north_delta;
pathDesired.End.East = positionState.East + east_delta;
pathDesired.End.Down = positionState.Down + down_delta;
pathDesired.StartingVelocity = velocity;
pathDesired.EndingVelocity = 0.0f;
pathDesired.Mode = PATHDESIRED_MODE_BRAKE;
pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_TIMEOUT] = time_to_stopped * ASSISTEDCONTROL_TIMEOUT_MULTIPLIER;
assistedControlFlightMode = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_BRAKE;
VelocityStateData velocityState;
VelocityStateGet(&velocityState);
float brakeRate;
VtolPathFollowerSettingsBrakeRateGet(&brakeRate);
if (brakeRate < ASSISTEDCONTROL_BRAKERATE_MINIMUM) {
brakeRate = ASSISTEDCONTROL_BRAKERATE_MINIMUM; // set a minimum to avoid a divide by zero potential below
}
// Calculate the velocity
float velocity = velocityState.North * velocityState.North + velocityState.East * velocityState.East + velocityState.Down * velocityState.Down;
velocity = sqrtf(velocity);
// Calculate the desired time to zero velocity.
float time_to_stopped = ASSISTEDCONTROL_DELAY_TO_BRAKE; // we allow at least 0.5 seconds to rotate to a brake angle.
time_to_stopped += velocity / brakeRate;
// Sanity check the brake rate by ensuring that the time to stop is within a range.
if (time_to_stopped < ASSISTEDCONTROL_TIMETOSTOP_MINIMUM) {
time_to_stopped = ASSISTEDCONTROL_TIMETOSTOP_MINIMUM;
} else if (time_to_stopped > ASSISTEDCONTROL_TIMETOSTOP_MAXIMUM) {
time_to_stopped = ASSISTEDCONTROL_TIMETOSTOP_MAXIMUM;
}
// calculate the distance we will travel
float north_delta = velocityState.North * ASSISTEDCONTROL_DELAY_TO_BRAKE; // we allow at least 0.5s to rotate to brake angle
north_delta += (time_to_stopped - ASSISTEDCONTROL_DELAY_TO_BRAKE) * 0.5f * velocityState.North; // area under the linear deceleration plot
float east_delta = velocityState.East * ASSISTEDCONTROL_DELAY_TO_BRAKE; // we allow at least 0.5s to rotate to brake angle
east_delta += (time_to_stopped - ASSISTEDCONTROL_DELAY_TO_BRAKE) * 0.5f * velocityState.East; // area under the linear deceleration plot
float down_delta = velocityState.Down * ASSISTEDCONTROL_DELAY_TO_BRAKE;
down_delta += (time_to_stopped - ASSISTEDCONTROL_DELAY_TO_BRAKE) * 0.5f * velocityState.Down; // area under the linear deceleration plot
float net_delta = east_delta * east_delta + north_delta * north_delta + down_delta * down_delta;
net_delta = sqrtf(net_delta);
pathDesired.Start.North = positionState.North;
pathDesired.Start.East = positionState.East;
pathDesired.Start.Down = positionState.Down;
pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_NORTH] = velocityState.North;
pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_EAST] = velocityState.East;
pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_DOWN] = velocityState.Down;
pathDesired.End.North = positionState.North + north_delta;
pathDesired.End.East = positionState.East + east_delta;
pathDesired.End.Down = positionState.Down + down_delta;
pathDesired.StartingVelocity = velocity;
pathDesired.EndingVelocity = 0.0f;
pathDesired.Mode = PATHDESIRED_MODE_BRAKE;
pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_TIMEOUT] = time_to_stopped * ASSISTEDCONTROL_TIMEOUT_MULTIPLIER;
assistedControlFlightMode = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_BRAKE;
FlightStatusAssistedControlStateSet(&assistedControlFlightMode);
PathDesiredSet(&pathDesired);
}

18
flight/modules/ManualControl/manualcontrol.c
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;

4
flight/modules/ManualControl/pathfollowerhandler.c
View File

@ -72,7 +72,7 @@ void pathFollowerHandler(bool newinit)
if ((flightModeAssist != FLIGHTSTATUS_FLIGHTMODEASSIST_NONE) &&
(assistedControlFlightMode == FLIGHTSTATUS_ASSISTEDCONTROLSTATE_PRIMARY)) {
// Switch from primary (just entered this PH flight mode) into brake
plan_setup_assistedcontrol(false);
plan_setup_assistedcontrol();
} else {
plan_setup_positionHold();
}
@ -116,7 +116,7 @@ void pathFollowerHandler(bool newinit)
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6:
if (assistedControlFlightMode == FLIGHTSTATUS_ASSISTEDCONTROLSTATE_BRAKE) {
// Just initiated braking after returning from stabi control
plan_setup_assistedcontrol(false);
plan_setup_assistedcontrol();
}
break;

15
flight/modules/PathFollower/vtolbrakecontroller.cpp
View File

@ -193,6 +193,21 @@ void VtolBrakeController::UpdateVelocityDesired()
if (!mManualThrust) {
controlDown.UpdatePositionSetpoint(positionState.Down);
}
FlightStatusFlightModeAssistOptions flightModeAssist;
FlightStatusFlightModeAssistGet(&flightModeAssist);
if (flightModeAssist != FLIGHTSTATUS_FLIGHTMODEASSIST_NONE) {
// Notify manualcommand via setting hold state in flightstatus assistedcontrolstate
FlightStatusAssistedControlStateOptions assistedControlFlightMode;
FlightStatusAssistedControlStateGet(&assistedControlFlightMode);
// sanity check that we are in brake state according to flight status, which means
// we are being used for gpsassist
if (assistedControlFlightMode == FLIGHTSTATUS_ASSISTEDCONTROLSTATE_BRAKE) {
assistedControlFlightMode = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_HOLD;
FlightStatusAssistedControlStateSet(&assistedControlFlightMode);
}
}
}
// Update position state and control position to create inputs to velocity control

2
shared/uavobjectdefinition/altitudeholdsettings.xml
View File

@ -6,7 +6,7 @@
<field name="ThrustRate" units="m/s" type="float" elements="1" defaultvalue="5" />
<field name="ThrustLimits" units="" type="float" elementnames="Min,Neutral,Max" defaultvalue="0.2, 0.5, 0.9"/>
<field name="VerticalPosP" units="(m/s)/m" type="float" elements="1" defaultvalue="0.4"/>
<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="VerticalVelPID" units="(m/s^2)/(m/s)" type="float" elementnames="Kp,Ki,Kd,Beta" defaultvalue="0.35, 0.45, 0.001, 0.95"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
<telemetryflight acked="true" updatemode="onchange" period="0"/>

2
shared/uavobjectdefinition/mpu6000settings.xml
View File

@ -18,7 +18,7 @@
</options>
</field>
<field name="FilterSetting" units="Hz" type="enum" elements="1" defaultvalue="Lowpass_256_Hz">
<field name="FilterSetting" units="Hz" type="enum" elements="1" defaultvalue="Lowpass_188_Hz">
<options>
<option>Lowpass_256_Hz</option>
<option>Lowpass_188_Hz</option>

2
shared/uavobjectdefinition/stabilizationsettings.xml
View File

@ -18,7 +18,7 @@
<field name="VbarPiroComp" units="" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="FALSE"/>
<field name="VbarMaxAngle" units="deg" type="uint8" elements="1" defaultvalue="10"/>
<field name="GyroTau" units="" type="float" elements="1" defaultvalue="0.005"/>
<field name="GyroTau" units="" type="float" elements="1" defaultvalue="0.003"/>
<field name="DerivativeCutoff" units="Hz" type="uint8" elements="1" defaultvalue="20"/>
<field name="DerivativeGamma" units="" type="float" elements="1" defaultvalue="1"/>

6
shared/uavobjectdefinition/vtolpathfollowersettings.xml
View File

@ -7,11 +7,11 @@
<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.35, 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"/>
<field name="ThrustControl" units="" type="enum" elements="1" options="manual,auto" defaultvalue="auto"/>
<field name="YawControl" units="" type="enum" elements="1" options="manual,tailin,movementdirection,pathdirection,poi" defaultvalue="manual"/>
<field name="FlyawayEmergencyFallback" units="switch" type="enum" elements="1" options="disabled,enabled,always,debugtest" defaultvalue="enabled"/>
<field name="FlyawayEmergencyFallbackTriggerTime" units="s" type="float" elements="1" defaultvalue="10.0"/>