mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-03-15 07:29:15 +01:00
REVONANO Fix BrakeController to use vtol VerticalVelPID instead of LandVerticalVelPID
This commit is contained in:
parent
89b40f3173
commit
50d60649aa
@ -66,7 +66,6 @@ public:
|
||||
void Activate(void);
|
||||
void Update(void);
|
||||
PathFollowerFSMState_T GetCurrentState(void);
|
||||
void Abort(void);
|
||||
uint8_t PositionHoldState(void);
|
||||
|
||||
protected:
|
||||
@ -103,7 +102,6 @@ protected:
|
||||
// void updateVtolBrakeFSMStatus();
|
||||
|
||||
void setStateTimeout(int32_t count);
|
||||
void fallback_to_hold(void);
|
||||
|
||||
static PathFollowerFSM_BrakeStateHandler_T sBrakeStateTable[BRAKE_STATE_SIZE];
|
||||
};
|
||||
|
@ -35,6 +35,7 @@ extern "C" {
|
||||
|
||||
#include <math.h>
|
||||
#include <pid.h>
|
||||
#include <alarms.h>
|
||||
#include <CoordinateConversions.h>
|
||||
#include <sin_lookup.h>
|
||||
#include <pathdesired.h>
|
||||
@ -138,10 +139,10 @@ void VtolBrakeController::SettingsUpdated(void)
|
||||
controlNE.UpdatePositionalParameters(vtolPathFollowerSettings->HorizontalPosP);
|
||||
controlNE.UpdateCommandParameters(-vtolPathFollowerSettings->BrakeMaxPitch, vtolPathFollowerSettings->BrakeMaxPitch, vtolPathFollowerSettings->BrakeVelocityFeedforward);
|
||||
|
||||
controlDown.UpdateParameters(vtolPathFollowerSettings->LandVerticalVelPID.Kp,
|
||||
vtolPathFollowerSettings->LandVerticalVelPID.Ki,
|
||||
vtolPathFollowerSettings->LandVerticalVelPID.Kd,
|
||||
vtolPathFollowerSettings->LandVerticalVelPID.Beta,
|
||||
controlDown.UpdateParameters(vtolPathFollowerSettings->VerticalVelPID.Kp,
|
||||
vtolPathFollowerSettings->VerticalVelPID.Ki,
|
||||
vtolPathFollowerSettings->VerticalVelPID.Kd,
|
||||
vtolPathFollowerSettings->VerticalVelPID.Beta,
|
||||
dT,
|
||||
10.0f * vtolPathFollowerSettings->VerticalVelMax); // avoid constraining initial fast entry into brake
|
||||
controlDown.UpdatePositionalParameters(vtolPathFollowerSettings->VerticalPosP);
|
||||
@ -364,8 +365,11 @@ void VtolBrakeController::UpdateAutoPilot()
|
||||
|
||||
int8_t result = UpdateStabilizationDesired();
|
||||
|
||||
if (!result) {
|
||||
fsm->Abort(); // enter PH
|
||||
if (result) {
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
|
||||
} else {
|
||||
pathStatus->Status = PATHSTATUS_STATUS_CRITICAL;
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING);
|
||||
}
|
||||
|
||||
PathStatusSet(pathStatus);
|
||||
|
Loading…
x
Reference in New Issue
Block a user