rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-20 10:54:14 +01:00
Code Issues Releases Activity

OP-1760 autotakeoff

Browse Source 
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:
abeck70 2015-04-20 08:43:57 +10:00
parent 3a0c36f7d7
commit d943f5a0ce
6 changed files with 17 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
flight/modules/ManualControl/armhandler.c
View File

@ -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;
}

1
flight/modules/PathFollower/inc/vtollandcontroller.h
View File

@ -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;

1
flight/modules/PathFollower/vtolautotakeofffsm.cpp
View File

@ -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);

14
flight/modules/PathFollower/vtollandcontroller.cpp
View File

@ -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);
}
}

2
flight/modules/PathFollower/vtollandfsm.cpp
View File

@ -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));

1
flight/modules/PathPlanner/pathplanner.c
View File

@ -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])) {