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

REVONANO Fix BrakeController to use vtol VerticalVelPID instead of LandVerticalVelPID

Browse Source
This commit is contained in:
abeck70 2015-05-25 22:13:13 +10:00
parent 89b40f3173
commit 50d60649aa
2 changed files with 10 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
flight/modules/PathFollower/inc/vtolbrakefsm.h
View File

@ -66,7 +66,6 @@ public:
void Activate(void); void Activate(void);
void Update(void); void Update(void);
PathFollowerFSMState_T GetCurrentState(void); PathFollowerFSMState_T GetCurrentState(void);
void Abort(void);
uint8_t PositionHoldState(void); uint8_t PositionHoldState(void);
protected: protected:
@ -103,7 +102,6 @@ protected:
// void updateVtolBrakeFSMStatus(); // void updateVtolBrakeFSMStatus();
void setStateTimeout(int32_t count); void setStateTimeout(int32_t count);
void fallback_to_hold(void);
static PathFollowerFSM_BrakeStateHandler_T sBrakeStateTable[BRAKE_STATE_SIZE]; static PathFollowerFSM_BrakeStateHandler_T sBrakeStateTable[BRAKE_STATE_SIZE];
}; };

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

@ -35,6 +35,7 @@ extern "C" {
#include <math.h> #include <math.h>
#include <pid.h> #include <pid.h>
#include <alarms.h>
#include <CoordinateConversions.h> #include <CoordinateConversions.h>
#include <sin_lookup.h> #include <sin_lookup.h>
#include <pathdesired.h> #include <pathdesired.h>
@ -138,10 +139,10 @@ void VtolBrakeController::SettingsUpdated(void)
controlNE.UpdatePositionalParameters(vtolPathFollowerSettings->HorizontalPosP); controlNE.UpdatePositionalParameters(vtolPathFollowerSettings->HorizontalPosP);
controlNE.UpdateCommandParameters(-vtolPathFollowerSettings->BrakeMaxPitch, vtolPathFollowerSettings->BrakeMaxPitch, vtolPathFollowerSettings->BrakeVelocityFeedforward); controlNE.UpdateCommandParameters(-vtolPathFollowerSettings->BrakeMaxPitch, vtolPathFollowerSettings->BrakeMaxPitch, vtolPathFollowerSettings->BrakeVelocityFeedforward);
controlDown.UpdateParameters(vtolPathFollowerSettings->LandVerticalVelPID.Kp, controlDown.UpdateParameters(vtolPathFollowerSettings->VerticalVelPID.Kp,
vtolPathFollowerSettings->LandVerticalVelPID.Ki, vtolPathFollowerSettings->VerticalVelPID.Ki,
vtolPathFollowerSettings->LandVerticalVelPID.Kd, vtolPathFollowerSettings->VerticalVelPID.Kd,
vtolPathFollowerSettings->LandVerticalVelPID.Beta, vtolPathFollowerSettings->VerticalVelPID.Beta,
dT, dT,
10.0f * vtolPathFollowerSettings->VerticalVelMax); // avoid constraining initial fast entry into brake 10.0f * vtolPathFollowerSettings->VerticalVelMax); // avoid constraining initial fast entry into brake
controlDown.UpdatePositionalParameters(vtolPathFollowerSettings->VerticalPosP); controlDown.UpdatePositionalParameters(vtolPathFollowerSettings->VerticalPosP);
@ -364,8 +365,11 @@ void VtolBrakeController::UpdateAutoPilot()
int8_t result = UpdateStabilizationDesired(); int8_t result = UpdateStabilizationDesired();
if (!result) { if (result) {
fsm->Abort(); // enter PH AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
} else {
pathStatus->Status = PATHSTATUS_STATUS_CRITICAL;
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING);
} }
PathStatusSet(pathStatus); PathStatusSet(pathStatus);