void plan_run_VelocityRoam() { // float alpha; PathDesiredData pathDesired; FlightStatusAssistedControlStateOptions assistedControlFlightMode; FlightStatusFlightModeOptions flightMode; PathDesiredGet(&pathDesired); FlightModeSettingsPositionHoldOffsetData offset; FlightModeSettingsPositionHoldOffsetGet(&offset); FlightStatusAssistedControlStateGet(&assistedControlFlightMode); FlightStatusFlightModeGet(&flightMode); StabilizationBankData stabSettings; StabilizationBankGet(&stabSettings); ManualControlCommandData cmd; ManualControlCommandGet(&cmd); cmd.Roll = applyExpo(cmd.Roll, stabSettings.StickExpo.Roll); cmd.Pitch = applyExpo(cmd.Pitch, stabSettings.StickExpo.Pitch); cmd.Yaw = applyExpo(cmd.Yaw, stabSettings.StickExpo.Yaw); bool flagRollPitchHasInput = (fabsf(cmd.Roll) > 0.0f || fabsf(cmd.Pitch) > 0.0f); if (!flagRollPitchHasInput) { // no movement desired, re-enter positionHold at current start-position if (assistedControlFlightMode == FLIGHTSTATUS_ASSISTEDCONTROLSTATE_PRIMARY) { // initiate braking and change assisted control flight mode to braking if (flightMode == FLIGHTSTATUS_FLIGHTMODE_LAND) { // avoid brake then hold sequence to continue descent. plan_setup_land_from_velocityroam(); } else { plan_setup_assistedcontrol(); } } // otherwise nothing to do in braking/hold modes } else { PositionStateData positionState; PositionStateGet(&positionState); // Revert assist control state to primary, which in this case implies // we are in roaming state (a GPS vector assisted velocity roam) assistedControlFlightMode = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_PRIMARY; // Calculate desired velocity in each direction float angle; AttitudeStateYawGet(&angle); angle = DEG2RAD(angle); float cos_angle = cosf(angle); float sine_angle = sinf(angle); float rotated[2] = { -cmd.Pitch * cos_angle - cmd.Roll * sine_angle, -cmd.Pitch * sine_angle + cmd.Roll * cos_angle }; // flip pitch to have pitch down (away) point north float horizontalVelMax; float verticalVelMax; VtolPathFollowerSettingsHorizontalVelMaxGet(&horizontalVelMax); VtolPathFollowerSettingsVerticalVelMaxGet(&verticalVelMax); float velocity_north = rotated[0] * horizontalVelMax; float velocity_east = rotated[1] * horizontalVelMax; float velocity_down = 0.0f; if (flightMode == FLIGHTSTATUS_FLIGHTMODE_LAND) { FlightModeSettingsLandingVelocityGet(&velocity_down); } float velocity = velocity_north * velocity_north + velocity_east * velocity_east; velocity = sqrtf(velocity); // if one stick input (pitch or roll) should we use fly by vector? set arbitrary distance of say 20m after which we // expect new stick input // if two stick input pilot is fighting wind manually and we use fly by velocity // in reality setting velocity desired to zero will fight wind anyway. pathDesired.Start.North = positionState.North; pathDesired.Start.East = positionState.East; pathDesired.Start.Down = positionState.Down; pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_NORTH] = velocity_north; pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_EAST] = velocity_east; pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_DOWN] = velocity_down; pathDesired.End.North = positionState.North; pathDesired.End.East = positionState.East; pathDesired.End.Down = positionState.Down; pathDesired.StartingVelocity = velocity; pathDesired.EndingVelocity = velocity; pathDesired.Mode = PATHDESIRED_MODE_VELOCITY; if (flightMode == FLIGHTSTATUS_FLIGHTMODE_LAND) { pathDesired.Mode = PATHDESIRED_MODE_LAND; pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_OPTIONS] = (float)PATHDESIRED_MODEPARAMETER_LAND_OPTION_NONE; } else { pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_UNUSED] = 0.0f; } PathDesiredSet(&pathDesired); FlightStatusAssistedControlStateSet(&assistedControlFlightMode); } }
int8_t VtolFlyController::UpdateStabilizationDesired(bool yaw_attitude, float yaw_direction) { uint8_t result = 1; StabilizationDesiredData stabDesired; AttitudeStateData attitudeState; StabilizationBankData stabSettings; float northCommand; float eastCommand; StabilizationDesiredGet(&stabDesired); AttitudeStateGet(&attitudeState); StabilizationBankGet(&stabSettings); controlNE.GetNECommand(&northCommand, &eastCommand); float angle_radians = DEG2RAD(attitudeState.Yaw); float cos_angle = cosf(angle_radians); float sine_angle = sinf(angle_radians); float maxPitch = vtolPathFollowerSettings->MaxRollPitch; stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabDesired.Pitch = boundf(-northCommand * cos_angle - eastCommand * sine_angle, -maxPitch, maxPitch); // this should be in the controller stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabDesired.Roll = boundf(-northCommand * sine_angle + eastCommand * cos_angle, -maxPitch, maxPitch); ManualControlCommandData manualControl; ManualControlCommandGet(&manualControl); // TODO The below need to be rewritten because the PID implementation has changed. #if 0 // DEBUG HACK: allow user to skew compass on purpose to see if emergency failsafe kicks in if (vtolPathFollowerSettings->FlyawayEmergencyFallback == VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_DEBUGTEST) { attitudeState.Yaw += 120.0f; if (attitudeState.Yaw > 180.0f) { attitudeState.Yaw -= 360.0f; } } if ( // emergency flyaway detection ( // integral already at its limit vtolPathFollowerSettings.HorizontalVelPID.ILimit - fabsf(global.PIDvel[0].iAccumulator) < 1e-6f || vtolPathFollowerSettings.HorizontalVelPID.ILimit - fabsf(global.PIDvel[1].iAccumulator) < 1e-6f ) && // angle between desired and actual velocity >90 degrees (by dot product) (velocityDesired.North * velocityState.North + velocityDesired.East * velocityState.East < 0.0f) && // quad is moving at significant speed (during flyaway it would keep speeding up) squaref(velocityState.North) + squaref(velocityState.East) > 1.0f ) { vtolEmergencyFallback += dT; if (vtolEmergencyFallback >= vtolPathFollowerSettings->FlyawayEmergencyFallbackTriggerTime) { // after emergency timeout, trigger alarm - everything else is handled by callers // (switch to emergency algorithm, switch to emergency waypoint in pathplanner, alarms, ...) result = 0; } } else { vtolEmergencyFallback = 0.0f; } #endif // if 0 if (yaw_attitude) { stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabDesired.Yaw = yaw_direction; } else { stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; stabDesired.Yaw = stabSettings.MaximumRate.Yaw * manualControl.Yaw; } // default thrust mode to cruise control stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL; if (mManualThrust) { stabDesired.Thrust = manualControl.Thrust; } else { stabDesired.Thrust = controlDown.GetDownCommand(); } StabilizationDesiredSet(&stabDesired); return result; }
/** * @brief Handler to control Stabilized flightmodes. FlightControl is governed by "Stabilization" * @input: ManualControlCommand * @output: StabilizationDesired */ void stabilizedHandler(bool newinit) { if (newinit) { StabilizationDesiredInitialize(); StabilizationBankInitialize(); } ManualControlCommandData cmd; ManualControlCommandGet(&cmd); FlightModeSettingsData settings; FlightModeSettingsGet(&settings); StabilizationDesiredData stabilization; StabilizationDesiredGet(&stabilization); StabilizationBankData stabSettings; StabilizationBankGet(&stabSettings); cmd.Roll = applyExpo(cmd.Roll, stabSettings.StickExpo.Roll); cmd.Pitch = applyExpo(cmd.Pitch, stabSettings.StickExpo.Pitch); cmd.Yaw = applyExpo(cmd.Yaw, stabSettings.StickExpo.Yaw); uint8_t *stab_settings; FlightStatusData flightStatus; FlightStatusGet(&flightStatus); switch (flightStatus.FlightMode) { case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1: stab_settings = FlightModeSettingsStabilization1SettingsToArray(settings.Stabilization1Settings); break; case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2: stab_settings = FlightModeSettingsStabilization2SettingsToArray(settings.Stabilization2Settings); break; case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3: stab_settings = FlightModeSettingsStabilization3SettingsToArray(settings.Stabilization3Settings); break; case FLIGHTSTATUS_FLIGHTMODE_STABILIZED4: stab_settings = FlightModeSettingsStabilization4SettingsToArray(settings.Stabilization4Settings); break; case FLIGHTSTATUS_FLIGHTMODE_STABILIZED5: stab_settings = FlightModeSettingsStabilization5SettingsToArray(settings.Stabilization5Settings); break; case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6: stab_settings = FlightModeSettingsStabilization6SettingsToArray(settings.Stabilization6Settings); break; default: // Major error, this should not occur because only enter this block when one of these is true AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); stab_settings = FlightModeSettingsStabilization1SettingsToArray(settings.Stabilization1Settings); return; } stabilization.Roll = (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) ? cmd.Roll : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd.Roll * stabSettings.ManualRate.Roll : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Roll * stabSettings.RollMax : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd.Roll * stabSettings.RollMax : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd.Roll * stabSettings.ManualRate.Roll : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd.Roll : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ACRO) ? cmd.Roll * stabSettings.ManualRate.Roll : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Roll * stabSettings.RollMax : 0; // this is an invalid mode stabilization.Pitch = (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) ? cmd.Pitch : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd.Pitch * stabSettings.ManualRate.Pitch : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Pitch * stabSettings.PitchMax : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd.Pitch * stabSettings.PitchMax : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd.Pitch * stabSettings.ManualRate.Pitch : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd.Pitch : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ACRO) ? cmd.Pitch * stabSettings.ManualRate.Pitch : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Pitch * stabSettings.PitchMax : 0; // this is an invalid mode // TOOD: Add assumption about order of stabilization desired and manual control stabilization mode fields having same order stabilization.StabilizationMode.Roll = stab_settings[0]; stabilization.StabilizationMode.Pitch = stab_settings[1]; // Other axes (yaw) cannot be Rattitude, so use Rate // Should really do this for Attitude mode as well? if (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) { stabilization.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; stabilization.Yaw = cmd.Yaw * stabSettings.ManualRate.Yaw; } else { stabilization.StabilizationMode.Yaw = stab_settings[2]; stabilization.Yaw = (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) ? cmd.Yaw : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd.Yaw * stabSettings.ManualRate.Yaw : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Yaw * stabSettings.YawMax : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd.Yaw * stabSettings.YawMax : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd.Yaw * stabSettings.ManualRate.Yaw : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd.Yaw : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ACRO) ? cmd.Yaw * stabSettings.ManualRate.Yaw : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Yaw * stabSettings.YawMax : 0; // this is an invalid mode } stabilization.StabilizationMode.Thrust = stab_settings[3]; stabilization.Thrust = cmd.Thrust; StabilizationDesiredSet(&stabilization); }