mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-02-20 10:54:14 +01:00
OP-1760 autotakeoff
On landing stausvtolland state of DISARMED, armhandler now detects this and disarms by force equivalent to a pathplanner alarm trigger. To re-arm, one must leave the land flight mode.
This commit is contained in:
parent
3a0c36f7d7
commit
d943f5a0ce
@ -35,6 +35,7 @@
|
||||
#include <flightstatus.h>
|
||||
#include <flightmodesettings.h>
|
||||
#include <stabilizationdesired.h>
|
||||
#include <statusvtolland.h>
|
||||
|
||||
// Private constants
|
||||
#define ARMED_THRESHOLD 0.50f
|
||||
@ -339,6 +340,18 @@ static bool forcedDisArm(void)
|
||||
if (alarms.Receiver == SYSTEMALARMS_ALARM_CRITICAL) {
|
||||
return true;
|
||||
}
|
||||
|
||||
// check landing state if active
|
||||
FlightStatusData flightStatus;
|
||||
FlightStatusGet(&flightStatus);
|
||||
if (flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_LAND) {
|
||||
StatusVtolLandData statusland;
|
||||
StatusVtolLandGet(&statusland);
|
||||
if (statusland.State == STATUSVTOLLAND_STATE_DISARMED) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
|
@ -64,7 +64,6 @@ public:
|
||||
private:
|
||||
void UpdateVelocityDesired(void);
|
||||
int8_t UpdateStabilizationDesired(bool yaw_attitude, float yaw_direction);
|
||||
void setArmedIfChanged(uint8_t val);
|
||||
|
||||
PathFollowerFSM *fsm;
|
||||
VtolPathFollowerSettingsData *vtolPathFollowerSettings;
|
||||
|
@ -364,6 +364,7 @@ void VtolAutoTakeoffFSM::setup_checkstate(void)
|
||||
// If pathplanner, we need additional checks
|
||||
// E.g. if inflight, this mode is just positon hol
|
||||
StabilizationDesiredData stabDesired;
|
||||
|
||||
StabilizationDesiredGet(&stabDesired);
|
||||
if (stabDesired.Thrust > vtolPathFollowerSettings->ThrustLimits.Min) {
|
||||
setState(AUTOTAKEOFF_STATE_HOLD, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
|
||||
|
@ -176,7 +176,7 @@ void VtolLandController::UpdateVelocityDesired()
|
||||
|
||||
// Implement optional horizontal position hold.
|
||||
if ((((uint8_t)pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_OPTIONS]) == PATHDESIRED_MODEPARAMETER_LAND_OPTION_HORIZONTAL_PH) ||
|
||||
(flightStatus->ControlChain.PathPlanner == FLIGHTSTATUS_CONTROLCHAIN_TRUE) ) {
|
||||
(flightStatus->ControlChain.PathPlanner == FLIGHTSTATUS_CONTROLCHAIN_TRUE)) {
|
||||
// landing flight mode has stored original horizontal position in pathdesired
|
||||
PositionStateData positionState;
|
||||
PositionStateGet(&positionState);
|
||||
@ -271,17 +271,5 @@ void VtolLandController::UpdateAutoPilot()
|
||||
fsm->Abort();
|
||||
}
|
||||
|
||||
if (fsm->GetCurrentState() == PFFSM_STATE_DISARMED) {
|
||||
setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
|
||||
}
|
||||
|
||||
PathStatusSet(pathStatus);
|
||||
}
|
||||
|
||||
void VtolLandController::setArmedIfChanged(uint8_t val)
|
||||
{
|
||||
if (flightStatus->Armed != val) {
|
||||
flightStatus->Armed = val;
|
||||
FlightStatusSet(flightStatus);
|
||||
}
|
||||
}
|
||||
|
@ -529,7 +529,7 @@ void VtolLandFSM::run_wtg_for_groundeffect(__attribute__((unused)) uint8_t flTim
|
||||
mLandData->fsmLandStatus.WtgForGroundEffect.BounceAccel = 0.0f;
|
||||
}
|
||||
|
||||
if (flBounce ) { // || flBounceAccel) { // accel trigger can occur due to vibration and is too sensitive
|
||||
if (flBounce) { // || flBounceAccel) { // accel trigger can occur due to vibration and is too sensitive
|
||||
mLandData->observation2Count++;
|
||||
if (mLandData->observation2Count > BOUNCE_TRIGGER_COUNT) {
|
||||
setState(LAND_STATE_GROUNDEFFECT, (flBounce ? STATUSVTOLLAND_STATEEXITREASON_BOUNCEVELOCITY : STATUSVTOLLAND_STATEEXITREASON_BOUNCEACCEL));
|
||||
|
@ -556,6 +556,7 @@ static uint8_t conditionDistanceToTarget()
|
||||
static uint8_t conditionLegRemaining()
|
||||
{
|
||||
PathStatusData pathStatus;
|
||||
|
||||
PathStatusGet(&pathStatus);
|
||||
|
||||
if (pathStatus.fractional_progress >= (1.0f - pathAction.ConditionParameters[0])) {
|
||||
|
Loading…
x
Reference in New Issue
Block a user