void VtolLandFSM::Activate() { memset(mLandData, sizeof(VtolLandFSMData_T), 0); mLandData->currentState = LAND_STATE_INACTIVE; mLandData->flLowAltitude = false; mLandData->flAltitudeHold = false; mLandData->fsmLandStatus.averageDescentRate = MIN_LANDRATE; mLandData->fsmLandStatus.averageDescentThrust = vtolPathFollowerSettings->ThrustLimits.Neutral; mLandData->fsmLandStatus.calculatedNeutralThrust = vtolPathFollowerSettings->ThrustLimits.Neutral; mLandData->boundThrustMin = vtolPathFollowerSettings->ThrustLimits.Min; mLandData->boundThrustMax = vtolPathFollowerSettings->ThrustLimits.Max; TakeOffLocationGet(&(mLandData->takeOffLocation)); mLandData->fsmLandStatus.AltitudeAtState[LAND_STATE_INACTIVE] = 0.0f; assessAltitude(); if (pathDesired->Mode == PATHDESIRED_MODE_LAND) { #ifndef DEBUG_GROUNDIMPACT setState(LAND_STATE_INIT_ALTHOLD, STATUSVTOLLAND_STATEEXITREASON_NONE); #else setState(LAND_STATE_WTG_FOR_GROUNDEFFECT, STATUSVTOLLAND_STATEEXITREASON_NONE); #endif } else { // move to error state and callback to position hold setState(LAND_STATE_ABORT, STATUSVTOLLAND_STATEEXITREASON_NONE); } }
static void getVector(float controlVector[4], vario_type type) { FlightModeSettingsPositionHoldOffsetData offset; FlightModeSettingsPositionHoldOffsetGet(&offset); // scale controlVector[3] (thrust) by vertical/horizontal to have vertical plane less sensitive controlVector[3] *= offset.Vertical / offset.Horizontal; float length = sqrtf(controlVector[0] * controlVector[0] + controlVector[1] * controlVector[1] + controlVector[3] * controlVector[3]); if (length <= 1e-9f) { length = 1.0f; // should never happen as getVector is not called if control within deadband } { float direction[3] = { controlVector[1] / length, // pitch is north controlVector[0] / length, // roll is east controlVector[3] / length // thrust is down }; controlVector[0] = direction[0]; controlVector[1] = direction[1]; controlVector[2] = direction[2]; } controlVector[3] = length * offset.Horizontal; // rotate north and east - rotation angle based on type float angle; switch (type) { case NSEW: angle = 0.0f; // NSEW no rotation takes place break; case FPV: // local rotation, using current yaw AttitudeStateYawGet(&angle); break; case LOS: // determine location based on vector from takeoff to current location { PositionStateData positionState; PositionStateGet(&positionState); TakeOffLocationData takeoffLocation; TakeOffLocationGet(&takeoffLocation); angle = RAD2DEG(atan2f(positionState.East - takeoffLocation.East, positionState.North - takeoffLocation.North)); } break; } // rotate horizontally by angle { float rotated[2] = { controlVector[0] * cos_lookup_deg(angle) - controlVector[1] * sin_lookup_deg(angle), controlVector[0] * sin_lookup_deg(angle) + controlVector[1] * cos_lookup_deg(angle) }; controlVector[0] = rotated[0]; controlVector[1] = rotated[1]; } }
/** * Compute bearing of current takeoff location */ float VtolFlyController::updateTailInBearing() { PositionStateData p; PositionStateGet(&p); TakeOffLocationData t; TakeOffLocationGet(&t); // atan2f always returns in between + and - 180 degrees return RAD2DEG(atan2f(p.East - t.East, p.North - t.North)); }
/** * @brief setup pathplanner/pathfollower for return to base */ void plan_setup_returnToBase() { // Simple Return To Base mode - keep altitude the same applying configured delta, fly to takeoff position float positionStateDown; PositionStateDownGet(&positionStateDown); PathDesiredData pathDesired; // re-initialise in setup stage memset(&pathDesired, 0, sizeof(PathDesiredData)); TakeOffLocationData takeoffLocation; TakeOffLocationGet(&takeoffLocation); // TODO: right now VTOLPF does fly straight to destination altitude. // For a safer RTB destination altitude will be the higher between takeofflocation and current position (corrected with safety margin) float destDown; FlightModeSettingsReturnToBaseAltitudeOffsetGet(&destDown); destDown = MIN(positionStateDown, takeoffLocation.Down) - destDown; FlightModeSettingsPositionHoldOffsetData offset; FlightModeSettingsPositionHoldOffsetGet(&offset); pathDesired.End.North = takeoffLocation.North; pathDesired.End.East = takeoffLocation.East; pathDesired.End.Down = destDown; pathDesired.Start.North = takeoffLocation.North + offset.Horizontal; // in FlyEndPoint the direction of this vector does not matter pathDesired.Start.East = takeoffLocation.East; pathDesired.Start.Down = destDown; pathDesired.StartingVelocity = 0.0f; pathDesired.EndingVelocity = 0.0f; FlightModeSettingsReturnToBaseNextCommandOptions ReturnToBaseNextCommand; FlightModeSettingsReturnToBaseNextCommandGet(&ReturnToBaseNextCommand); pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_NEXTCOMMAND] = (float)ReturnToBaseNextCommand; pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_UNUSED1] = 0.0f; pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_UNUSED2] = 0.0f; pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_UNUSED3] = 0.0f; pathDesired.Mode = PATHDESIRED_MODE_GOTOENDPOINT; PathDesiredSet(&pathDesired); }
/** * @brief setup pathplanner/pathfollower for return to base */ void plan_setup_returnToBase() { // Simple Return To Base mode - keep altitude the same applying configured delta, fly to takeoff position float positionStateDown; PositionStateDownGet(&positionStateDown); PathDesiredData pathDesired; PathDesiredGet(&pathDesired); TakeOffLocationData takeoffLocation; TakeOffLocationGet(&takeoffLocation); // TODO: right now VTOLPF does fly straight to destination altitude. // For a safer RTB destination altitude will be the higher between takeofflocation and current position (corrected with safety margin) float destDown; FlightModeSettingsReturnToBaseAltitudeOffsetGet(&destDown); destDown = MIN(positionStateDown, takeoffLocation.Down) - destDown; FlightModeSettingsPositionHoldOffsetData offset; FlightModeSettingsPositionHoldOffsetGet(&offset); float startingVelocity; FlightModeSettingsPositionHoldStartingVelocityGet(&startingVelocity); pathDesired.End.North = takeoffLocation.North; pathDesired.End.East = takeoffLocation.East; pathDesired.End.Down = destDown; pathDesired.Start.North = takeoffLocation.North + offset.Horizontal; // in FlyEndPoint the direction of this vector does not matter pathDesired.Start.East = takeoffLocation.East; pathDesired.Start.Down = destDown; pathDesired.StartingVelocity = startingVelocity; pathDesired.EndingVelocity = 0.0f; pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; PathDesiredSet(&pathDesired); }