/** * @brief setup pathplanner/pathfollower for positionhold */ void plan_setup_positionHold() { PositionStateData positionState; PositionStateGet(&positionState); PathDesiredData pathDesired; PathDesiredGet(&pathDesired); FlightModeSettingsPositionHoldOffsetData offset; FlightModeSettingsPositionHoldOffsetGet(&offset); float startingVelocity; FlightModeSettingsPositionHoldStartingVelocityGet(&startingVelocity); pathDesired.End.North = positionState.North; pathDesired.End.East = positionState.East; pathDesired.End.Down = positionState.Down; pathDesired.Start.North = positionState.North + offset.Horizontal; // in FlyEndPoint the direction of this vector does not matter pathDesired.Start.East = positionState.East; pathDesired.Start.Down = positionState.Down; pathDesired.StartingVelocity = startingVelocity; pathDesired.EndingVelocity = 0.0f; pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; PathDesiredSet(&pathDesired); }
static void SettingsUpdatedCb(UAVObjEvent * ev) { GroundPathFollowerSettingsGet(&guidanceSettings); // Configure the velocity control PID loops pid_configure(&ground_pids[VELOCITY], guidanceSettings.HorizontalVelPID[GROUNDPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP], // Kp guidanceSettings.HorizontalVelPID[GROUNDPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KI], // Ki 0, // Kd guidanceSettings.HorizontalVelPID[GROUNDPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT]); // Configure the position control (velocity output) PID loops pid_configure(&ground_pids[NORTH_POSITION], guidanceSettings.HorizontalPosPI[GROUNDPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP], guidanceSettings.HorizontalPosPI[GROUNDPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KI], 0, guidanceSettings.HorizontalPosPI[GROUNDPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT]); pid_configure(&ground_pids[EAST_POSITION], guidanceSettings.HorizontalPosPI[GROUNDPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP], guidanceSettings.HorizontalPosPI[GROUNDPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KI], 0, guidanceSettings.HorizontalPosPI[GROUNDPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT]); PathDesiredGet(&pathDesired); }
void PathDesired_Set(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs) { if (Param[0]->Val->Pointer == NULL) return; PathDesiredData data; PathDesiredGet(&data); // use the same struct like picoc. see below struct mystruct { double Start[3]; double End[3]; double StartingVelocity; double EndingVelocity; double ModeParameters; unsigned char Mode; } *pdata; pdata = Param[0]->Val->Pointer; data.Start[0] = pdata->Start[0]; data.Start[1] = pdata->Start[1]; data.Start[2] = pdata->Start[2]; data.End[0] = pdata->End[0]; data.End[1] = pdata->End[1]; data.End[2] = pdata->End[2]; data.StartingVelocity = pdata->StartingVelocity; data.EndingVelocity = pdata->EndingVelocity; data.ModeParameters = pdata->ModeParameters; data.Mode = pdata->Mode; PathDesiredSet(&data); }
void plan_setup_land() { float descendspeed; plan_setup_positionHold(); FlightModeSettingsLandingVelocityGet(&descendspeed); PathDesiredData pathDesired; PathDesiredGet(&pathDesired); pathDesired.StartingVelocity = descendspeed; pathDesired.EndingVelocity = descendspeed; PathDesiredSet(&pathDesired); PIOS_DELTATIME_Init(&landdT, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA); }
/** * @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); 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); }
void plan_setup_AutoCruise() { PositionStateData positionState; PositionStateGet(&positionState); PathDesiredData pathDesired; PathDesiredGet(&pathDesired); FlightModeSettingsPositionHoldOffsetData offset; FlightModeSettingsPositionHoldOffsetGet(&offset); float startingVelocity; FlightModeSettingsPositionHoldStartingVelocityGet(&startingVelocity); // initialization is flight in direction of the nose. // the velocity is not relevant, as it will be reset by the run function even during first call float angle; AttitudeStateYawGet(&angle); float vector[2] = { cos_lookup_deg(angle), sin_lookup_deg(angle) }; hold_position[0] = positionState.North; hold_position[1] = positionState.East; hold_position[2] = positionState.Down; pathDesired.End.North = hold_position[0] + vector[0]; pathDesired.End.East = hold_position[1] + vector[1]; pathDesired.End.Down = hold_position[2]; // start position has the same offset as in position hold pathDesired.Start.North = pathDesired.End.North + offset.Horizontal; // in FlyEndPoint the direction of this vector does not matter pathDesired.Start.East = pathDesired.End.East; pathDesired.Start.Down = pathDesired.End.Down; pathDesired.StartingVelocity = startingVelocity; pathDesired.EndingVelocity = 0.0f; pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; PathDesiredSet(&pathDesired); // re-iniztializing deltatime is valid and also good practice here since // getAverageSeconds() has not been called/updated in a long time if we were in a different flightmode. PIOS_DELTATIME_Init(&actimeval, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA); }
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); }
/** * @brief execute autocruise */ void plan_run_AutoCruise() { PositionStateData positionState; PositionStateGet(&positionState); PathDesiredData pathDesired; PathDesiredGet(&pathDesired); FlightModeSettingsPositionHoldOffsetData offset; FlightModeSettingsPositionHoldOffsetGet(&offset); float controlVector[4]; ManualControlCommandRollGet(&controlVector[0]); ManualControlCommandPitchGet(&controlVector[1]); ManualControlCommandYawGet(&controlVector[2]); controlVector[3] = 0.5f; // dummy, thrust is normalized separately normalizeDeadband(controlVector); // return value ignored ManualControlCommandThrustGet(&controlVector[3]); // no deadband as we are using thrust for velocity controlVector[3] = boundf(controlVector[3], 1e-6f, 1.0f); // bound to above zero, to prevent loss of vector direction // normalize old desired movement vector float vector[3] = { pathDesired.End.North - hold_position[0], pathDesired.End.East - hold_position[1], pathDesired.End.Down - hold_position[2] }; float length = sqrtf(vector[0] * vector[0] + vector[1] * vector[1] + vector[2] * vector[2]); if (length < 1e-9f) { length = 1.0f; // should not happen since initialized properly in setup() } vector[0] /= length; vector[1] /= length; vector[2] /= length; // start position is advanced according to actual movement - in the direction of desired vector only // projection using scalar product float kp = (positionState.North - hold_position[0]) * vector[0] + (positionState.East - hold_position[1]) * vector[1] + (positionState.Down - hold_position[2]) * vector[2]; if (kp > 0.0f) { hold_position[0] += kp * vector[0]; hold_position[1] += kp * vector[1]; hold_position[2] += kp * vector[2]; } // new angle is equal to old angle plus offset depending on yaw input and time // (controlVector is normalized with a deadband, change is zero within deadband) float angle = RAD2DEG(atan2f(vector[1], vector[0])); float dT = PIOS_DELTATIME_GetAverageSeconds(&actimeval); angle += 10.0f * controlVector[2] * dT; // TODO magic value could eventually end up in a to be created settings // resulting movement vector is scaled by velocity demand in controlvector[3] [0.0-1.0] vector[0] = cosf(DEG2RAD(angle)) * offset.Horizontal * controlVector[3]; vector[1] = sinf(DEG2RAD(angle)) * offset.Horizontal * controlVector[3]; vector[2] = -controlVector[1] * offset.Vertical * controlVector[3]; pathDesired.End.North = hold_position[0] + vector[0]; pathDesired.End.East = hold_position[1] + vector[1]; pathDesired.End.Down = hold_position[2] + vector[2]; // start position has the same offset as in position hold pathDesired.Start.North = pathDesired.End.North + offset.Horizontal; // in FlyEndPoint the direction of this vector does not matter pathDesired.Start.East = pathDesired.End.East; pathDesired.Start.Down = pathDesired.End.Down; PathDesiredSet(&pathDesired); }
static void plan_run_PositionVario(vario_type type) { float controlVector[4]; float alpha; PathDesiredData pathDesired; PathDesiredGet(&pathDesired); FlightModeSettingsPositionHoldOffsetData offset; FlightModeSettingsPositionHoldOffsetGet(&offset); ManualControlCommandRollGet(&controlVector[0]); ManualControlCommandPitchGet(&controlVector[1]); ManualControlCommandYawGet(&controlVector[2]); ManualControlCommandThrustGet(&controlVector[3]); FlightModeSettingsVarioControlLowPassAlphaGet(&alpha); vario_control_lowpass[0] = alpha * vario_control_lowpass[0] + (1.0f - alpha) * controlVector[0]; vario_control_lowpass[1] = alpha * vario_control_lowpass[1] + (1.0f - alpha) * controlVector[1]; vario_control_lowpass[2] = alpha * vario_control_lowpass[2] + (1.0f - alpha) * controlVector[2]; controlVector[0] = vario_control_lowpass[0]; controlVector[1] = vario_control_lowpass[1]; controlVector[2] = vario_control_lowpass[2]; // check if movement is desired if (normalizeDeadband(controlVector) == false) { // no movement desired, re-enter positionHold at current start-position if (!vario_hold) { vario_hold = true; // new hold position is the position that was previously the start position pathDesired.End.North = hold_position[0]; pathDesired.End.East = hold_position[1]; pathDesired.End.Down = hold_position[2]; // while the new start position has the same offset as in position hold pathDesired.Start.North = pathDesired.End.North + offset.Horizontal; // in FlyEndPoint the direction of this vector does not matter pathDesired.Start.East = pathDesired.End.East; pathDesired.Start.Down = pathDesired.End.Down; PathDesiredSet(&pathDesired); } } else { PositionStateData positionState; PositionStateGet(&positionState); // flip pitch to have pitch down (away) point north controlVector[1] = -controlVector[1]; getVector(controlVector, type); // layout of control Vector : unitVector in movement direction {0,1,2} vector length {3} velocity {4} if (vario_hold) { // start position is the position that was previously the hold position vario_hold = false; hold_position[0] = pathDesired.End.North; hold_position[1] = pathDesired.End.East; hold_position[2] = pathDesired.End.Down; } else { // start position is advanced according to movement - in the direction of ControlVector only // projection using scalar product float kp = (positionState.North - hold_position[0]) * controlVector[0] + (positionState.East - hold_position[1]) * controlVector[1] + (positionState.Down - hold_position[2]) * -controlVector[2]; if (kp > 0.0f) { hold_position[0] += kp * controlVector[0]; hold_position[1] += kp * controlVector[1]; hold_position[2] += kp * -controlVector[2]; } } // new destination position is advanced based on controlVector pathDesired.End.North = hold_position[0] + controlVector[0] * controlVector[3]; pathDesired.End.East = hold_position[1] + controlVector[1] * controlVector[3]; pathDesired.End.Down = hold_position[2] - controlVector[2] * controlVector[3]; // the new start position has the same offset as in position hold pathDesired.Start.North = pathDesired.End.North + offset.Horizontal; // in FlyEndPoint the direction of this vector does not matter pathDesired.Start.East = pathDesired.End.East; pathDesired.Start.Down = pathDesired.End.Down; PathDesiredSet(&pathDesired); } }
/** * Module thread, should not return. */ static void vtolPathFollowerTask(void *parameters) { SystemSettingsData systemSettings; FlightStatusData flightStatus; portTickType lastUpdateTime; VtolPathFollowerSettingsConnectCallback(SettingsUpdatedCb); PathDesiredConnectCallback(SettingsUpdatedCb); VtolPathFollowerSettingsGet(&guidanceSettings); PathDesiredGet(&pathDesired); // Main task loop lastUpdateTime = xTaskGetTickCount(); while (1) { // Conditions when this runs: // 1. Must have VTOL type airframe // 2. Flight mode is PositionHold and PathDesired.Mode is Endpoint OR // FlightMode is PathPlanner and PathDesired.Mode is Endpoint or Path SystemSettingsGet(&systemSettings); if ( (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_VTOL) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADP) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADX) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXA) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXAX) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXACOAX) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTO) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOV) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXP) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXX) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_TRI) ) { AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_WARNING); vTaskDelay(1000); continue; } // Continue collecting data if not enough time vTaskDelayUntil(&lastUpdateTime, MS2TICKS(guidanceSettings.UpdatePeriod)); // Convert the accels into the NED frame updateNedAccel(); FlightStatusGet(&flightStatus); // Check the combinations of flightmode and pathdesired mode switch(flightStatus.FlightMode) { /* This combination of RETURNTOHOME and HOLDPOSITION looks strange but * is correct. RETURNTOHOME mode uses HOLDPOSITION with the position * set to home */ case FLIGHTSTATUS_FLIGHTMODE_RETURNTOHOME: if (pathDesired.Mode == PATHDESIRED_MODE_HOLDPOSITION) { updateEndpointVelocity(); updateVtolDesiredAttitude(); } else { AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_ERROR); } break; case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD: if (pathDesired.Mode == PATHDESIRED_MODE_HOLDPOSITION) { updateEndpointVelocity(); updateVtolDesiredAttitude(); } else { AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_ERROR); } break; case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER: if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT || pathDesired.Mode == PATHDESIRED_MODE_HOLDPOSITION) { updateEndpointVelocity(); updateVtolDesiredAttitude(); } else if (pathDesired.Mode == PATHDESIRED_MODE_FLYVECTOR || pathDesired.Mode == PATHDESIRED_MODE_FLYCIRCLELEFT || pathDesired.Mode == PATHDESIRED_MODE_FLYCIRCLERIGHT) { updatePathVelocity(); updateVtolDesiredAttitude(); } else { AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_ERROR); } break; default: for (uint32_t i = 0; i < VTOL_PID_NUM; i++) pid_zero(&vtol_pids[i]); // Track throttle before engaging this mode. Cheap system ident StabilizationDesiredData stabDesired; StabilizationDesiredGet(&stabDesired); throttleOffset = stabDesired.Throttle; break; } AlarmsClear(SYSTEMALARMS_ALARM_PATHFOLLOWER); } }
/** * Module thread, should not return. */ static void pathfollowerTask(void *parameters) { SystemSettingsData systemSettings; FlightStatusData flightStatus; uint32_t lastUpdateTime; AirspeedActualConnectCallback(airspeedActualUpdatedCb); FixedWingPathFollowerSettingsConnectCallback(SettingsUpdatedCb); FixedWingAirspeedsConnectCallback(SettingsUpdatedCb); PathDesiredConnectCallback(SettingsUpdatedCb); // Force update of all the settings SettingsUpdatedCb(NULL); FixedWingPathFollowerSettingsGet(&fixedwingpathfollowerSettings); path_desired_updated = false; PathDesiredGet(&pathDesired); PathDesiredConnectCallback(pathDesiredUpdated); // Main task loop lastUpdateTime = PIOS_Thread_Systime(); while (1) { // Conditions when this runs: // 1. Must have FixedWing type airframe // 2. Flight mode is PositionHold and PathDesired.Mode is Endpoint OR // FlightMode is PathPlanner and PathDesired.Mode is Endpoint or Path SystemSettingsGet(&systemSettings); if ( (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWING) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWINGELEVON) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWINGVTAIL) ) { AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_CRITICAL); PIOS_Thread_Sleep(1000); continue; } // Continue collecting data if not enough time PIOS_Thread_Sleep_Until(&lastUpdateTime, fixedwingpathfollowerSettings.UpdatePeriod); static uint8_t last_flight_mode; FlightStatusGet(&flightStatus); PathStatusGet(&pathStatus); PositionActualData positionActual; static enum {FW_FOLLOWER_IDLE, FW_FOLLOWER_RUNNING, FW_FOLLOWER_ERR} state = FW_FOLLOWER_IDLE; // Check whether an update to the path desired occured and we should // process it. This makes sure that the follower alarm state is // updated. bool process_path_desired_update = (last_flight_mode == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER || last_flight_mode == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) && path_desired_updated; path_desired_updated = false; // Process most of these when the flight mode changes // except when in path following mode in which case // each iteration must make sure this has the latest // PathDesired if (flightStatus.FlightMode != last_flight_mode || process_path_desired_update) { last_flight_mode = flightStatus.FlightMode; switch(flightStatus.FlightMode) { case FLIGHTSTATUS_FLIGHTMODE_RETURNTOHOME: state = FW_FOLLOWER_RUNNING; PositionActualGet(&positionActual); pathDesired.Mode = PATHDESIRED_MODE_CIRCLEPOSITIONRIGHT; pathDesired.Start[0] = positionActual.North; pathDesired.Start[1] = positionActual.East; pathDesired.Start[2] = positionActual.Down; pathDesired.End[0] = 0; pathDesired.End[1] = 0; pathDesired.End[2] = -30.0f; pathDesired.ModeParameters = fixedwingpathfollowerSettings.OrbitRadius; pathDesired.StartingVelocity = fixedWingAirspeeds.CruiseSpeed; pathDesired.EndingVelocity = fixedWingAirspeeds.CruiseSpeed; PathDesiredSet(&pathDesired); break; case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD: state = FW_FOLLOWER_RUNNING; PositionActualGet(&positionActual); pathDesired.Mode = PATHDESIRED_MODE_CIRCLEPOSITIONRIGHT; pathDesired.Start[0] = positionActual.North; pathDesired.Start[1] = positionActual.East; pathDesired.Start[2] = positionActual.Down; pathDesired.End[0] = positionActual.North; pathDesired.End[1] = positionActual.East; pathDesired.End[2] = positionActual.Down; pathDesired.ModeParameters = fixedwingpathfollowerSettings.OrbitRadius; pathDesired.StartingVelocity = fixedWingAirspeeds.CruiseSpeed; pathDesired.EndingVelocity = fixedWingAirspeeds.CruiseSpeed; PathDesiredSet(&pathDesired); break; case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER: case FLIGHTSTATUS_FLIGHTMODE_TABLETCONTROL: state = FW_FOLLOWER_RUNNING; PathDesiredGet(&pathDesired); switch(pathDesired.Mode) { case PATHDESIRED_MODE_ENDPOINT: case PATHDESIRED_MODE_VECTOR: case PATHDESIRED_MODE_CIRCLERIGHT: case PATHDESIRED_MODE_CIRCLELEFT: break; default: state = FW_FOLLOWER_ERR; pathStatus.Status = PATHSTATUS_STATUS_CRITICAL; PathStatusSet(&pathStatus); AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_CRITICAL); break; } break; default: state = FW_FOLLOWER_IDLE; break; } } switch(state) { case FW_FOLLOWER_RUNNING: { updatePathVelocity(); uint8_t result = updateFixedDesiredAttitude(); if (result) { AlarmsClear(SYSTEMALARMS_ALARM_PATHFOLLOWER); } else { AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_WARNING); } PathStatusSet(&pathStatus); break; } case FW_FOLLOWER_IDLE: // Be cleaner and get rid of global variables northVelIntegral = 0; eastVelIntegral = 0; downVelIntegral = 0; bearingIntegral = 0; speedIntegral = 0; accelIntegral = 0; powerIntegral = 0; airspeedErrorInt = 0; AlarmsClear(SYSTEMALARMS_ALARM_PATHFOLLOWER); break; case FW_FOLLOWER_ERR: default: // Leave alarms set above break; } } }
/** * Module thread, should not return. */ static void groundPathFollowerTask(void *parameters) { SystemSettingsData systemSettings; FlightStatusData flightStatus; uint32_t lastUpdateTime; GroundPathFollowerSettingsConnectCallback(SettingsUpdatedCb); PathDesiredConnectCallback(SettingsUpdatedCb); GroundPathFollowerSettingsGet(&guidanceSettings); PathDesiredGet(&pathDesired); // Main task loop lastUpdateTime = PIOS_Thread_Systime(); while (1) { // Conditions when this runs: // 1. Must have GROUND type airframe // 2. Flight mode is PositionHold and PathDesired.Mode is Endpoint OR // FlightMode is PathPlanner and PathDesired.Mode is Endpoint or Path SystemSettingsGet(&systemSettings); if ( (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_GROUNDVEHICLECAR) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_GROUNDVEHICLEDIFFERENTIAL) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_GROUNDVEHICLEMOTORCYCLE) ) { AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_WARNING); PIOS_Thread_Sleep(1000); continue; } // Continue collecting data if not enough time PIOS_Thread_Sleep_Until(&lastUpdateTime, guidanceSettings.UpdatePeriod); // Convert the accels into the NED frame updateNedAccel(); FlightStatusGet(&flightStatus); // Check the combinations of flightmode and pathdesired mode switch(flightStatus.FlightMode) { /* This combination of RETURNTOHOME and HOLDPOSITION looks strange but * is correct. RETURNTOHOME mode uses HOLDPOSITION with the position * set to home */ case FLIGHTSTATUS_FLIGHTMODE_RETURNTOHOME: if (pathDesired.Mode == PATHDESIRED_MODE_HOLDPOSITION) { updateEndpointVelocity(); updateGroundDesiredAttitude(); } else { AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_ERROR); } break; case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD: if (pathDesired.Mode == PATHDESIRED_MODE_HOLDPOSITION) { updateEndpointVelocity(); updateGroundDesiredAttitude(); } else { AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_ERROR); } break; case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER: if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT || pathDesired.Mode == PATHDESIRED_MODE_HOLDPOSITION) { updateEndpointVelocity(); updateGroundDesiredAttitude(); } else if (pathDesired.Mode == PATHDESIRED_MODE_FLYVECTOR || pathDesired.Mode == PATHDESIRED_MODE_FLYCIRCLELEFT || pathDesired.Mode == PATHDESIRED_MODE_FLYCIRCLERIGHT) { updatePathVelocity(); updateGroundDesiredAttitude(); } else { AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_ERROR); } break; default: // Be cleaner and get rid of global variables northVelIntegral = 0; eastVelIntegral = 0; northPosIntegral = 0; eastPosIntegral = 0; // Track throttle before engaging this mode. Cheap system ident StabilizationDesiredData stabDesired; StabilizationDesiredGet(&stabDesired); throttleOffset = stabDesired.Throttle; break; } AlarmsClear(SYSTEMALARMS_ALARM_PATHFOLLOWER); } }
/** * Compute desired attitude from the desired velocity * * Takes in @ref NedActual which has the acceleration in the * NED frame as the feedback term and then compares the * @ref VelocityActual against the @ref VelocityDesired */ uint8_t waypointFollowing(uint8_t flightMode, FixedWingPathFollowerSettingsCCData fixedwingpathfollowerSettings) { float dT = fixedwingpathfollowerSettings.UpdatePeriod / 1000.0f; //Convert from [ms] to [s] VelocityActualData velocityActual; StabilizationDesiredData stabDesired; float trueAirspeed; float calibratedAirspeedActual; float airspeedDesired; float airspeedError; float pitchCommand; float powerCommand; float headingError_R; float rollCommand; //TODO: Move these out of the loop FixedWingAirspeedsData fixedwingAirspeeds; FixedWingAirspeedsGet(&fixedwingAirspeeds); VelocityActualGet(&velocityActual); StabilizationDesiredGet(&stabDesired); // TODO: Create UAVO that merges airspeed together AirspeedActualTrueAirspeedGet(&trueAirspeed); PositionActualData positionActual; PositionActualGet(&positionActual); PathDesiredData pathDesired; PathDesiredGet(&pathDesired); if (flightStatusUpdate) { //Reset integrals integral->totalEnergyError = 0; integral->airspeedError = 0; integral->lineError = 0; integral->circleError = 0; if (flightMode == FLIGHTSTATUS_FLIGHTMODE_RETURNTOHOME) { // Simple Return To Home mode: climb 10 meters and fly to home position pathDesired.Start[PATHDESIRED_START_NORTH] = positionActual.North; pathDesired.Start[PATHDESIRED_START_EAST] = positionActual.East; pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down; pathDesired.End[PATHDESIRED_END_NORTH] = 0; pathDesired.End[PATHDESIRED_END_EAST] = 0; pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down - 10; pathDesired.StartingVelocity = fixedwingAirspeeds.BestClimbRateSpeed; pathDesired.EndingVelocity = fixedwingAirspeeds.BestClimbRateSpeed; pathDesired.Mode = PATHDESIRED_MODE_FLYVECTOR; homeOrbit = false; } else if (flightMode == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) { // Simple position hold: stay at present altitude and position //Offset by one so that the start and end points don't perfectly coincide pathDesired.Start[PATHDESIRED_START_NORTH] = positionActual.North - 1; pathDesired.Start[PATHDESIRED_START_EAST] = positionActual.East; pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down; pathDesired.End[PATHDESIRED_END_NORTH] = positionActual.North; pathDesired.End[PATHDESIRED_END_EAST] = positionActual.East; pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down; pathDesired.StartingVelocity = fixedwingAirspeeds.BestClimbRateSpeed; pathDesired.EndingVelocity = fixedwingAirspeeds.BestClimbRateSpeed; pathDesired.Mode = PATHDESIRED_MODE_FLYVECTOR; } PathDesiredSet(&pathDesired); flightStatusUpdate = false; } /** * Compute speed error (required for throttle and pitch) */ // Current airspeed calibratedAirspeedActual = trueAirspeed; //BOOOOOOOOOO!!! Where's the conversion from TAS to CAS? // Current heading float headingActual_R = atan2f(velocityActual.East, velocityActual.North); // Desired airspeed airspeedDesired = pathDesired.EndingVelocity; // Airspeed error airspeedError = airspeedDesired - calibratedAirspeedActual; /** * Compute desired throttle command */ //Proxy because instead of m*(1/2*v^2+g*h), it's v^2+2*gh. This saves processing power float totalEnergyProxySetpoint = powf(pathDesired.EndingVelocity, 2.0f) - 2.0f * GRAVITY * pathDesired.End[2]; float totalEnergyProxyActual = powf(trueAirspeed, 2.0f) - 2.0f * GRAVITY * positionActual.Down; float errorTotalEnergy = totalEnergyProxySetpoint - totalEnergyProxyActual; float throttle_kp = fixedwingpathfollowerSettings. ThrottlePI[FIXEDWINGPATHFOLLOWERSETTINGSCC_THROTTLEPI_KP]; float throttle_ki = fixedwingpathfollowerSettings.ThrottlePI[FIXEDWINGPATHFOLLOWERSETTINGSCC_THROTTLEPI_KI]; float throttle_ilimit = fixedwingpathfollowerSettings.ThrottlePI[FIXEDWINGPATHFOLLOWERSETTINGSCC_THROTTLEPI_ILIMIT]; //Integrate with bound. Make integral leaky for better performance. Approximately 30s time constant. if (throttle_ilimit > 0.0f) { integral->totalEnergyError = bound(integral->totalEnergyError + errorTotalEnergy * dT, -throttle_ilimit / throttle_ki, throttle_ilimit / throttle_ki) * (1.0f - 1.0f / (1.0f + 30.0f / dT)); } powerCommand = errorTotalEnergy * throttle_kp + integral->totalEnergyError * throttle_ki; float throttlelimit_neutral = fixedwingpathfollowerSettings. ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGSCC_THROTTLELIMIT_NEUTRAL]; float throttlelimit_min = fixedwingpathfollowerSettings. ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGSCC_THROTTLELIMIT_MIN]; float throttlelimit_max = fixedwingpathfollowerSettings. ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGSCC_THROTTLELIMIT_MAX]; // set throttle stabDesired.Throttle = bound(powerCommand + throttlelimit_neutral, throttlelimit_min, throttlelimit_max); /** * Compute desired pitch command */ float airspeed_kp = fixedwingpathfollowerSettings. AirspeedPI[FIXEDWINGPATHFOLLOWERSETTINGSCC_AIRSPEEDPI_KP]; float airspeed_ki = fixedwingpathfollowerSettings. AirspeedPI[FIXEDWINGPATHFOLLOWERSETTINGSCC_AIRSPEEDPI_KI]; float airspeed_ilimit = fixedwingpathfollowerSettings. AirspeedPI[FIXEDWINGPATHFOLLOWERSETTINGSCC_AIRSPEEDPI_ILIMIT]; if (airspeed_ki > 0.0f) { //Integrate with saturation integral->airspeedError = bound(integral->airspeedError + airspeedError * dT, -airspeed_ilimit / airspeed_ki, airspeed_ilimit / airspeed_ki); } //Compute the cross feed from altitude to pitch, with saturation float pitchcrossfeed_kp = fixedwingpathfollowerSettings. VerticalToPitchCrossFeed [FIXEDWINGPATHFOLLOWERSETTINGSCC_VERTICALTOPITCHCROSSFEED_KP]; float pitchcrossfeed_min = fixedwingpathfollowerSettings. VerticalToPitchCrossFeed [FIXEDWINGPATHFOLLOWERSETTINGSCC_VERTICALTOPITCHCROSSFEED_MAX]; float pitchcrossfeed_max = fixedwingpathfollowerSettings. VerticalToPitchCrossFeed [FIXEDWINGPATHFOLLOWERSETTINGSCC_VERTICALTOPITCHCROSSFEED_MAX]; float alitudeError = -(pathDesired.End[2] - positionActual.Down); //Negative to convert from Down to altitude float altitudeToPitchCommandComponent = bound(alitudeError * pitchcrossfeed_kp, -pitchcrossfeed_min, pitchcrossfeed_max); //Compute the pitch command as err*Kp + errInt*Ki + X_feed. pitchCommand = -(airspeedError * airspeed_kp + integral->airspeedError * airspeed_ki) + altitudeToPitchCommandComponent; //Saturate pitch command float pitchlimit_neutral = fixedwingpathfollowerSettings. PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGSCC_PITCHLIMIT_NEUTRAL]; float pitchlimit_min = fixedwingpathfollowerSettings. PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGSCC_PITCHLIMIT_MIN]; float pitchlimit_max = fixedwingpathfollowerSettings. PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGSCC_PITCHLIMIT_MAX]; stabDesired.Pitch = bound(pitchlimit_neutral + pitchCommand, pitchlimit_min, pitchlimit_max); /** * Compute desired roll command */ float p[3] = { positionActual.North, positionActual.East, positionActual.Down }; float *c = pathDesired.End; float *r = pathDesired.Start; float q[3] = { pathDesired.End[0] - pathDesired.Start[0], pathDesired.End[1] - pathDesired.Start[1], pathDesired.End[2] - pathDesired.Start[2] }; float k_path = fixedwingpathfollowerSettings.VectorFollowingGain / pathDesired.EndingVelocity; //Divide gain by airspeed so that the turn rate is independent of airspeed float k_orbit = fixedwingpathfollowerSettings.OrbitFollowingGain / pathDesired.EndingVelocity; //Divide gain by airspeed so that the turn rate is independent of airspeed float k_psi_int = fixedwingpathfollowerSettings.FollowerIntegralGain; //======================================== //SHOULD NOT BE HARD CODED bool direction; float chi_inf = PI / 4.0f; //THIS NEEDS TO BE A FUNCTION OF HOW LONG OUR PATH IS. //Saturate chi_inf. I.e., never approach the path at a steeper angle than 45 degrees chi_inf = chi_inf < PI / 4.0f ? PI / 4.0f : chi_inf; //======================================== float rho; float headingCommand_R; float pncn = p[0] - c[0]; float pece = p[1] - c[1]; float d = sqrtf(pncn * pncn + pece * pece); //Assume that we want a 15 degree bank angle. This should yield a nice, non-agressive turn #define ROLL_FOR_HOLDING_CIRCLE 15.0f //Calculate radius, rho, using r*omega=v and omega = g/V_g * tan(phi) //THIS SHOULD ONLY BE CALCULATED ONCE, INSTEAD OF EVERY TIME rho = powf(pathDesired.EndingVelocity, 2) / (GRAVITY * tanf(fabs(ROLL_FOR_HOLDING_CIRCLE * DEG2RAD))); typedef enum { LINE, ORBIT } pathTypes_t; pathTypes_t pathType; //Determine if we should fly on a line or orbit path. switch (flightMode) { case FLIGHTSTATUS_FLIGHTMODE_RETURNTOHOME: if (d < rho + 5.0f * pathDesired.EndingVelocity || homeOrbit == true) { //When approx five seconds from the circle, start integrating into it pathType = ORBIT; homeOrbit = true; } else { pathType = LINE; } break; case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD: pathType = ORBIT; break; default: pathType = LINE; break; } //Check to see if we've gone past our destination. Since the path follower //is simply following a vector, it has no concept of where the vector ends. //It will simply keep following it to infinity if we don't stop it. So while //we don't know why the commutation to the next point failed, we don't know //we don't want the plane flying off. if (pathType == LINE) { //Compute the norm squared of the horizontal path length //IT WOULD BE NICE TO ONLY DO THIS ONCE PER WAYPOINT UPDATE, INSTEAD OF //EVERY LOOP float pathLength2 = q[0] * q[0] + q[1] * q[1]; //Perform a quick vector math operation, |a| < a.b/|a| = |b|cos(theta), //to test if we've gone past the waypoint. Add in a distance equal to 5s //of flight time, for good measure to make sure we don't add jitter. if (((p[0] - r[0]) * q[0] + (p[1] - r[1]) * q[1]) > pathLength2 + 5.0f * pathDesired.EndingVelocity) { //Whoops, we've really overflown our destination point, and haven't //received any instructions. Start circling //flightMode will reset the next time a waypoint changes, so there's //no danger of it getting stuck in orbit mode. flightMode = FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD; pathType = ORBIT; } } switch (pathType) { case ORBIT: if (pathDesired.Mode == PATHDESIRED_MODE_FLYCIRCLELEFT) { direction = false; } else { //In the case where the direction is undefined, always fly in a //clockwise fashion direction = true; } headingCommand_R = followOrbit(c, rho, direction, p, headingActual_R, k_orbit, k_psi_int, dT); break; case LINE: headingCommand_R = followStraightLine(r, q, p, headingActual_R, chi_inf, k_path, k_psi_int, dT); break; } //Calculate heading error headingError_R = headingCommand_R - headingActual_R; //Wrap heading error around circle if (headingError_R < -PI) headingError_R += 2.0f * PI; if (headingError_R > PI) headingError_R -= 2.0f * PI; //GET RID OF THE RAD2DEG. IT CAN BE FACTORED INTO HeadingPI float rolllimit_neutral = fixedwingpathfollowerSettings. RollLimit[FIXEDWINGPATHFOLLOWERSETTINGSCC_ROLLLIMIT_NEUTRAL]; float rolllimit_min = fixedwingpathfollowerSettings. RollLimit[FIXEDWINGPATHFOLLOWERSETTINGSCC_ROLLLIMIT_MIN]; float rolllimit_max = fixedwingpathfollowerSettings. RollLimit[FIXEDWINGPATHFOLLOWERSETTINGSCC_ROLLLIMIT_MAX]; float headingpi_kp = fixedwingpathfollowerSettings. HeadingPI[FIXEDWINGPATHFOLLOWERSETTINGSCC_HEADINGPI_KP]; rollCommand = (headingError_R * headingpi_kp) * RAD2DEG; //Turn heading stabDesired.Roll = bound(rolllimit_neutral + rollCommand, rolllimit_min, rolllimit_max); #ifdef SIM_OSX fprintf(stderr, " headingError_R: %f, rollCommand: %f\n", headingError_R, rollCommand); #endif /** * Compute desired yaw command */ // Coordinated flight, so we reset the desired yaw. stabDesired.Yaw = 0; stabDesired. StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabDesired. StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabDesired. StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_NONE; StabilizationDesiredSet(&stabDesired); //Stuff some debug variables into PathDesired, because right now these //fields aren't being used. pathDesired.ModeParameters[0] = pitchCommand; pathDesired.ModeParameters[1] = airspeedError; pathDesired.ModeParameters[2] = integral->airspeedError; pathDesired.ModeParameters[3] = alitudeError; pathDesired.UID = errorTotalEnergy; PathDesiredSet(&pathDesired); return 0; }
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); } }