/** * @brief Handler to control Guided flightmodes. FlightControl is governed by PathFollower, control via PathDesired * @input: NONE: fully automated mode -- TODO recursively call handler for advanced stick commands * @output: PathDesired */ void pathFollowerHandler(bool newinit) { if (newinit) { plan_initialize(); } FlightStatusFlightModeOptions flightMode; FlightStatusAssistedControlStateOptions assistedControlFlightMode; FlightStatusFlightModeAssistOptions flightModeAssist; FlightStatusFlightModeGet(&flightMode); FlightStatusFlightModeAssistGet(&flightModeAssist); FlightStatusAssistedControlStateGet(&assistedControlFlightMode); if (newinit) { // After not being in this mode for a while init at current height switch (flightMode) { case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE: plan_setup_returnToBase(); break; case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD: if ((flightModeAssist != FLIGHTSTATUS_FLIGHTMODEASSIST_NONE) && (assistedControlFlightMode == FLIGHTSTATUS_ASSISTEDCONTROLSTATE_PRIMARY)) { // Switch from primary (just entered this PH flight mode) into brake plan_setup_assistedcontrol(); } else { plan_setup_positionHold(); } break; case FLIGHTSTATUS_FLIGHTMODE_COURSELOCK: plan_setup_CourseLock(); break; case FLIGHTSTATUS_FLIGHTMODE_VELOCITYROAM: plan_setup_VelocityRoam(); break; case FLIGHTSTATUS_FLIGHTMODE_HOMELEASH: plan_setup_HomeLeash(); break; case FLIGHTSTATUS_FLIGHTMODE_ABSOLUTEPOSITION: plan_setup_AbsolutePosition(); break; case FLIGHTSTATUS_FLIGHTMODE_LAND: if (flightModeAssist == FLIGHTSTATUS_FLIGHTMODEASSIST_NONE) { plan_setup_land(); } else { plan_setup_VelocityRoam(); } break; case FLIGHTSTATUS_FLIGHTMODE_AUTOTAKEOFF: plan_setup_AutoTakeoff(); break; case FLIGHTSTATUS_FLIGHTMODE_AUTOCRUISE: plan_setup_AutoCruise(); break; case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1: case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2: case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3: case FLIGHTSTATUS_FLIGHTMODE_STABILIZED4: case FLIGHTSTATUS_FLIGHTMODE_STABILIZED5: case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6: if (assistedControlFlightMode == FLIGHTSTATUS_ASSISTEDCONTROLSTATE_BRAKE) { // Just initiated braking after returning from stabi control plan_setup_assistedcontrol(); } break; default: plan_setup_positionHold(); break; } } switch (flightMode) { case FLIGHTSTATUS_FLIGHTMODE_COURSELOCK: plan_run_CourseLock(); break; case FLIGHTSTATUS_FLIGHTMODE_VELOCITYROAM: plan_run_VelocityRoam(); break; case FLIGHTSTATUS_FLIGHTMODE_HOMELEASH: plan_run_HomeLeash(); break; case FLIGHTSTATUS_FLIGHTMODE_ABSOLUTEPOSITION: plan_run_AbsolutePosition(); break; case FLIGHTSTATUS_FLIGHTMODE_LAND: if (flightModeAssist != FLIGHTSTATUS_FLIGHTMODEASSIST_NONE) { plan_run_VelocityRoam(); } break; case FLIGHTSTATUS_FLIGHTMODE_AUTOTAKEOFF: plan_run_AutoTakeoff(); break; case FLIGHTSTATUS_FLIGHTMODE_AUTOCRUISE: plan_run_AutoCruise(); break; default: break; } }
void plan_setup_assistedcontrol(uint8_t timeout_occurred) { PositionStateData positionState; PositionStateGet(&positionState); PathDesiredData pathDesired; PathDesiredGet(&pathDesired); FlightStatusAssistedControlStateOptions assistedControlFlightMode; FlightStatusAssistedControlStateGet(&assistedControlFlightMode); if (timeout_occurred) { pathDesired.End.North = positionState.North; pathDesired.End.East = positionState.East; pathDesired.End.Down = positionState.Down; pathDesired.Start.North = positionState.North; pathDesired.Start.East = positionState.East; pathDesired.Start.Down = positionState.Down; pathDesired.StartingVelocity = 0.0f; pathDesired.EndingVelocity = 0.0f; pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; assistedControlFlightMode = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_HOLD; } else { VelocityStateData velocityState; VelocityStateGet(&velocityState); float brakeRate; VtolPathFollowerSettingsBrakeRateGet(&brakeRate); if (brakeRate < ASSISTEDCONTROL_BRAKERATE_MINIMUM) { brakeRate = ASSISTEDCONTROL_BRAKERATE_MINIMUM; // set a minimum to avoid a divide by zero potential below } // Calculate the velocity float velocity = velocityState.North * velocityState.North + velocityState.East * velocityState.East + velocityState.Down * velocityState.Down; velocity = sqrtf(velocity); // Calculate the desired time to zero velocity. float time_to_stopped = ASSISTEDCONTROL_DELAY_TO_BRAKE; // we allow at least 0.5 seconds to rotate to a brake angle. time_to_stopped += velocity / brakeRate; // Sanity check the brake rate by ensuring that the time to stop is within a range. if (time_to_stopped < ASSISTEDCONTROL_TIMETOSTOP_MINIMUM) { time_to_stopped = ASSISTEDCONTROL_TIMETOSTOP_MINIMUM; } else if (time_to_stopped > ASSISTEDCONTROL_TIMETOSTOP_MAXIMUM) { time_to_stopped = ASSISTEDCONTROL_TIMETOSTOP_MAXIMUM; } // calculate the distance we will travel float north_delta = velocityState.North * ASSISTEDCONTROL_DELAY_TO_BRAKE; // we allow at least 0.5s to rotate to brake angle north_delta += (time_to_stopped - ASSISTEDCONTROL_DELAY_TO_BRAKE) * 0.5f * velocityState.North; // area under the linear deceleration plot float east_delta = velocityState.East * ASSISTEDCONTROL_DELAY_TO_BRAKE; // we allow at least 0.5s to rotate to brake angle east_delta += (time_to_stopped - ASSISTEDCONTROL_DELAY_TO_BRAKE) * 0.5f * velocityState.East; // area under the linear deceleration plot float down_delta = velocityState.Down * ASSISTEDCONTROL_DELAY_TO_BRAKE; down_delta += (time_to_stopped - ASSISTEDCONTROL_DELAY_TO_BRAKE) * 0.5f * velocityState.Down; // area under the linear deceleration plot float net_delta = east_delta * east_delta + north_delta * north_delta + down_delta * down_delta; net_delta = sqrtf(net_delta); pathDesired.Start.North = positionState.North; pathDesired.Start.East = positionState.East; pathDesired.Start.Down = positionState.Down; pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_NORTH] = velocityState.North; pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_EAST] = velocityState.East; pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_DOWN] = velocityState.Down; pathDesired.End.North = positionState.North + north_delta; pathDesired.End.East = positionState.East + east_delta; pathDesired.End.Down = positionState.Down + down_delta; pathDesired.StartingVelocity = velocity; pathDesired.EndingVelocity = 0.0f; pathDesired.Mode = PATHDESIRED_MODE_BRAKE; pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_TIMEOUT] = time_to_stopped * ASSISTEDCONTROL_TIMEOUT_MULTIPLIER; assistedControlFlightMode = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_BRAKE; } FlightStatusAssistedControlStateSet(&assistedControlFlightMode); PathDesiredSet(&pathDesired); }
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); } }