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

OP-1847 gpsassist fix

Browse Source 
Set assisted control state to HOLD when Brake controller transitions to HOLD.  This impacts throttle stick behavior as it is different during braking and on brake completion in the hold state.
Also cleanuped and removed unused/old code that triggered HOLD in plans.c
This commit is contained in:
abeck70 2015-05-16 20:33:03 +10:00
parent 5d923c4125
commit 7d603bc6cb
4 changed files with 67 additions and 72 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 * @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_NORTH 0
#define PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_EAST 1 #define PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_EAST 1

24
flight/libraries/plans.c
View File

@ -615,7 +615,7 @@ void plan_run_VelocityRoam()
// avoid brake then hold sequence to continue descent. // avoid brake then hold sequence to continue descent.
plan_setup_land_from_velocityroam(); plan_setup_land_from_velocityroam();
} else { } else {
plan_setup_assistedcontrol(false); plan_setup_assistedcontrol();
} }
} }
// otherwise nothing to do in braking/hold modes // 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_TIMETOSTOP_MAXIMUM 9.0f // seconds
#define ASSISTEDCONTROL_DELAY_TO_BRAKE 1.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 #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; PositionStateData positionState;
@ -832,26 +832,7 @@ void plan_setup_assistedcontrol(uint8_t timeout_occurred)
PathDesiredData pathDesired; PathDesiredData pathDesired;
FlightStatusAssistedControlStateOptions assistedControlFlightMode; 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; VelocityStateData velocityState;
VelocityStateGet(&velocityState); VelocityStateGet(&velocityState);
float brakeRate; float brakeRate;
@ -900,7 +881,6 @@ void plan_setup_assistedcontrol(uint8_t timeout_occurred)
pathDesired.Mode = PATHDESIRED_MODE_BRAKE; pathDesired.Mode = PATHDESIRED_MODE_BRAKE;
pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_TIMEOUT] = time_to_stopped * ASSISTEDCONTROL_TIMEOUT_MULTIPLIER; pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_TIMEOUT] = time_to_stopped * ASSISTEDCONTROL_TIMEOUT_MULTIPLIER;
assistedControlFlightMode = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_BRAKE; assistedControlFlightMode = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_BRAKE;
}
FlightStatusAssistedControlStateSet(&assistedControlFlightMode); FlightStatusAssistedControlStateSet(&assistedControlFlightMode);
PathDesiredSet(&pathDesired); PathDesiredSet(&pathDesired);
} }

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

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

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

@ -193,6 +193,21 @@ void VtolBrakeController::UpdateVelocityDesired()
if (!mManualThrust) { if (!mManualThrust) {
controlDown.UpdatePositionSetpoint(positionState.Down); 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 // Update position state and control position to create inputs to velocity control