/** * Compute desired velocity from the current position * * Takes in @ref PositionActual and compares it to @ref PositionDesired * and computes @ref VelocityDesired */ void updateEndpointVelocity() { float dT = guidanceSettings.UpdatePeriod / 1000.0f; PositionActualData positionActual; VelocityDesiredData velocityDesired; PositionActualGet(&positionActual); VelocityDesiredGet(&velocityDesired); float northError; float eastError; float downError; float northCommand; float eastCommand; float northPos = positionActual.North; float eastPos = positionActual.East; float downPos = positionActual.Down; // Compute desired north command velocity from position error northError = pathDesired.End[PATHDESIRED_END_NORTH] - northPos; northCommand = pid_apply_antiwindup(&vtol_pids[NORTH_POSITION], northError, -guidanceSettings.HorizontalVelMax, guidanceSettings.HorizontalVelMax, dT); // Compute desired east command velocity from position error eastError = pathDesired.End[PATHDESIRED_END_EAST] - eastPos; eastCommand = pid_apply_antiwindup(&vtol_pids[EAST_POSITION], eastError, -guidanceSettings.HorizontalVelMax, guidanceSettings.HorizontalVelMax, dT); // Limit the maximum velocity any direction (not north and east separately) float total_vel = sqrtf(powf(northCommand,2) + powf(eastCommand,2)); float scale = 1; if(total_vel > guidanceSettings.HorizontalVelMax) scale = guidanceSettings.HorizontalVelMax / total_vel; velocityDesired.North = northCommand * scale; velocityDesired.East = eastCommand * scale; // Compute the desired velocity from the position difference downError = pathDesired.End[PATHDESIRED_END_DOWN] - downPos; velocityDesired.Down = pid_apply_antiwindup(&vtol_pids[DOWN_POSITION], downError, -guidanceSettings.VerticalVelMax, guidanceSettings.VerticalVelMax, dT); VelocityDesiredSet(&velocityDesired); // Indicate whether we are in radius of this endpoint uint8_t path_status = PATHSTATUS_STATUS_INPROGRESS; float distance2 = powf(northError, 2) + powf(eastError, 2); if (distance2 < (guidanceSettings.EndpointRadius * guidanceSettings.EndpointRadius)) { path_status = PATHSTATUS_STATUS_COMPLETED; } PathStatusStatusSet(&path_status); }
/** * Compute desired velocity from the current position and path * * Takes in @ref PositionActual and compares it to @ref PathDesired * and computes @ref VelocityDesired */ static void updatePathVelocity() { float dT = guidanceSettings.UpdatePeriod / 1000.0f; PositionActualData positionActual; PositionActualGet(&positionActual); float cur[3] = {positionActual.North, positionActual.East, positionActual.Down}; struct path_status progress; path_progress(&pathDesired, cur, &progress); // Update the path status UAVO PathStatusData pathStatus; PathStatusGet(&pathStatus); pathStatus.fractional_progress = progress.fractional_progress; if (pathStatus.fractional_progress < 1) pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS; else pathStatus.Status = PATHSTATUS_STATUS_COMPLETED; PathStatusSet(&pathStatus); float groundspeed = pathDesired.StartingVelocity + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) * progress.fractional_progress; if(progress.fractional_progress > 1) groundspeed = 0; VelocityDesiredData velocityDesired; velocityDesired.North = progress.path_direction[0] * groundspeed; velocityDesired.East = progress.path_direction[1] * groundspeed; float error_speed = progress.error * guidanceSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP]; float correction_velocity[2] = {progress.correction_direction[0] * error_speed, progress.correction_direction[1] * error_speed}; float total_vel = sqrtf(powf(correction_velocity[0],2) + powf(correction_velocity[1],2)); float scale = 1; if(total_vel > guidanceSettings.HorizontalVelMax) scale = guidanceSettings.HorizontalVelMax / total_vel; // Currently not apply a PID loop for horizontal corrections velocityDesired.North += progress.correction_direction[0] * error_speed * scale; velocityDesired.East += progress.correction_direction[1] * error_speed * scale; // Interpolate desired velocity along the path float altitudeSetpoint = pathDesired.Start[2] + (pathDesired.End[2] - pathDesired.Start[2]) * bound_min_max(progress.fractional_progress,0,1); float downError = altitudeSetpoint - positionActual.Down; velocityDesired.Down = pid_apply_antiwindup(&vtol_pids[DOWN_POSITION], downError, -guidanceSettings.VerticalVelMax, guidanceSettings.VerticalVelMax, dT); VelocityDesiredSet(&velocityDesired); }
/** * 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 */ static void updateVtolDesiredAttitude() { float dT = guidanceSettings.UpdatePeriod / 1000.0f; VelocityDesiredData velocityDesired; VelocityActualData velocityActual; StabilizationDesiredData stabDesired; AttitudeActualData attitudeActual; NedAccelData nedAccel; StabilizationSettingsData stabSettings; float northError; float northCommand; float eastError; float eastCommand; float downError; float downCommand; VtolPathFollowerSettingsGet(&guidanceSettings); VelocityActualGet(&velocityActual); VelocityDesiredGet(&velocityDesired); StabilizationDesiredGet(&stabDesired); VelocityDesiredGet(&velocityDesired); AttitudeActualGet(&attitudeActual); StabilizationSettingsGet(&stabSettings); NedAccelGet(&nedAccel); float northVel = velocityActual.North; float eastVel = velocityActual.East; float downVel = velocityActual.Down; // Compute desired north command from velocity error northError = velocityDesired.North - northVel; northCommand = pid_apply_antiwindup(&vtol_pids[NORTH_VELOCITY], northError, -guidanceSettings.MaxRollPitch, guidanceSettings.MaxRollPitch, dT) + velocityDesired.North * guidanceSettings.VelocityFeedforward; // Compute desired east command from velocity error eastError = velocityDesired.East - eastVel; eastCommand = pid_apply_antiwindup(&vtol_pids[NORTH_VELOCITY], eastError, -guidanceSettings.MaxRollPitch, guidanceSettings.MaxRollPitch, dT) + velocityDesired.East * guidanceSettings.VelocityFeedforward; // Compute desired down command. Using NED accel as the damping term downError = velocityDesired.Down - downVel; // Negative is critical here since throttle is negative with down downCommand = -pid_apply_antiwindup(&vtol_pids[DOWN_VELOCITY], downError, -1, 1, dT) + nedAccel.Down * guidanceSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KD]; // If this setting is zero then the throttle level available when enabled is used for hover:wq float used_throttle_offset = (guidanceSettings.HoverThrottle == 0) ? throttleOffset : guidanceSettings.HoverThrottle; stabDesired.Throttle = bound_min_max(downCommand + used_throttle_offset, 0, 1); // Project the north and east command signals into the pitch and roll based on yaw. // For this to behave well the craft should move similarly for 5 deg roll versus 5 deg pitch. // Notice the inputs are crudely bounded by the anti-winded but if both N and E were // saturated and the craft were at 45 degrees that would result in a value greater than // the limit, so apply limit again here. stabDesired.Pitch = bound_min_max(-northCommand * cosf(attitudeActual.Yaw * DEG2RAD) + -eastCommand * sinf(attitudeActual.Yaw * DEG2RAD), -guidanceSettings.MaxRollPitch, guidanceSettings.MaxRollPitch); stabDesired.Roll = bound_min_max(-northCommand * sinf(attitudeActual.Yaw * DEG2RAD) + eastCommand * cosf(attitudeActual.Yaw * DEG2RAD), -guidanceSettings.MaxRollPitch, guidanceSettings.MaxRollPitch); if(guidanceSettings.ThrottleControl == VTOLPATHFOLLOWERSETTINGS_THROTTLECONTROL_FALSE) { // For now override throttle with manual control. Disable at your risk, quad goes to China. ManualControlCommandData manualControl; ManualControlCommandGet(&manualControl); stabDesired.Throttle = manualControl.Throttle; } stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDEPLUS; stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDEPLUS; float yaw; switch(guidanceSettings.YawMode) { case VTOLPATHFOLLOWERSETTINGS_YAWMODE_RATE: /* This is awkward. This allows the transmitter to control the yaw while flying navigation */ ManualControlCommandYawGet(&yaw); stabDesired.Yaw = stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] * yaw; stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; break; case VTOLPATHFOLLOWERSETTINGS_YAWMODE_AXISLOCK: ManualControlCommandYawGet(&yaw); stabDesired.Yaw = stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] * yaw; stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; break; case VTOLPATHFOLLOWERSETTINGS_YAWMODE_ATTITUDE: ManualControlCommandYawGet(&yaw); stabDesired.Yaw = stabSettings.YawMax * yaw; stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; break; case VTOLPATHFOLLOWERSETTINGS_YAWMODE_POI: stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_POI; break; } StabilizationDesiredSet(&stabDesired); }
/** * Module thread, should not return. */ static void altitudeHoldTask(void *parameters) { bool engaged = false; AltitudeHoldDesiredData altitudeHoldDesired; StabilizationDesiredData stabilizationDesired; AltitudeHoldSettingsData altitudeHoldSettings; UAVObjEvent ev; struct pid velocity_pid; // Listen for object updates. AltitudeHoldDesiredConnectQueue(queue); AltitudeHoldSettingsConnectQueue(queue); FlightStatusConnectQueue(queue); AltitudeHoldSettingsGet(&altitudeHoldSettings); pid_configure(&velocity_pid, altitudeHoldSettings.VelocityKp, altitudeHoldSettings.VelocityKi, 0.0f, 1.0f); AlarmsSet(SYSTEMALARMS_ALARM_ALTITUDEHOLD, SYSTEMALARMS_ALARM_OK); // Main task loop const uint32_t dt_ms = 5; const float dt_s = dt_ms * 0.001f; uint32_t timeout = dt_ms; while (1) { if (PIOS_Queue_Receive(queue, &ev, timeout) != true) { } else if (ev.obj == FlightStatusHandle()) { uint8_t flight_mode; FlightStatusFlightModeGet(&flight_mode); if (flight_mode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD && !engaged) { // Copy the current throttle as a starting point for integral StabilizationDesiredThrottleGet(&velocity_pid.iAccumulator); velocity_pid.iAccumulator *= 1000.0f; // pid library scales up accumulator by 1000 engaged = true; } else if (flight_mode != FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD) engaged = false; // Run loop at 20 Hz when engaged otherwise just slowly wait for it to be engaged timeout = engaged ? dt_ms : 100; } else if (ev.obj == AltitudeHoldDesiredHandle()) { AltitudeHoldDesiredGet(&altitudeHoldDesired); } else if (ev.obj == AltitudeHoldSettingsHandle()) { AltitudeHoldSettingsGet(&altitudeHoldSettings); pid_configure(&velocity_pid, altitudeHoldSettings.VelocityKp, altitudeHoldSettings.VelocityKi, 0.0f, 1.0f); } bool landing = altitudeHoldDesired.Land == ALTITUDEHOLDDESIRED_LAND_TRUE; // For landing mode allow throttle to go negative to allow the integrals // to stop winding up const float min_throttle = landing ? -0.1f : 0.0f; // When engaged compute altitude controller output if (engaged) { float position_z, velocity_z, altitude_error; PositionActualDownGet(&position_z); VelocityActualDownGet(&velocity_z); position_z = -position_z; // Use positive up convention velocity_z = -velocity_z; // Use positive up convention // Compute the altitude error altitude_error = altitudeHoldDesired.Altitude - position_z; // Velocity desired is from the outer controller plus the set point float velocity_desired = altitude_error * altitudeHoldSettings.PositionKp + altitudeHoldDesired.ClimbRate; float throttle_desired = pid_apply_antiwindup(&velocity_pid, velocity_desired - velocity_z, min_throttle, 1.0f, // positive limits since this is throttle dt_s); if (altitudeHoldSettings.AttitudeComp > 0) { // Throttle desired is at this point the mount desired in the up direction, we can // account for the attitude if desired AttitudeActualData attitudeActual; AttitudeActualGet(&attitudeActual); // Project a unit vector pointing up into the body frame and // get the z component float fraction = attitudeActual.q1 * attitudeActual.q1 - attitudeActual.q2 * attitudeActual.q2 - attitudeActual.q3 * attitudeActual.q3 + attitudeActual.q4 * attitudeActual.q4; // Add ability to scale up the amount of compensation to achieve // level forward flight fraction = powf(fraction, (float) altitudeHoldSettings.AttitudeComp / 100.0f); // Dividing by the fraction remaining in the vertical projection will // attempt to compensate for tilt. This acts like the thrust is linear // with the output which isn't really true. If the fraction is starting // to go negative we are inverted and should shut off throttle throttle_desired = (fraction > 0.1f) ? (throttle_desired / fraction) : 0.0f; } StabilizationDesiredGet(&stabilizationDesired); stabilizationDesired.Throttle = bound_min_max(throttle_desired, min_throttle, 1.0f); if (landing) { stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; stabilizationDesired.Roll = 0; stabilizationDesired.Pitch = 0; stabilizationDesired.Yaw = 0; } else { stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; stabilizationDesired.Roll = altitudeHoldDesired.Roll; stabilizationDesired.Pitch = altitudeHoldDesired.Pitch; stabilizationDesired.Yaw = altitudeHoldDesired.Yaw; } StabilizationDesiredSet(&stabilizationDesired); } } }
/** * Compute the desired acceleration based on the desired * velocity and actual velocity */ static int32_t vtol_follower_control_accel(float dT) { VelocityDesiredData velocityDesired; VelocityActualData velocityActual; AccelDesiredData accelDesired; NedAccelData nedAccel; float north_error, north_acceleration; float east_error, east_acceleration; float down_error; static float last_north_velocity; static float last_east_velocity; NedAccelGet(&nedAccel); VelocityActualGet(&velocityActual); VelocityDesiredGet(&velocityDesired); // Optionally compute the acceleration required component from a changing velocity desired if (guidanceSettings.VelocityChangePrediction == VTOLPATHFOLLOWERSETTINGS_VELOCITYCHANGEPREDICTION_TRUE && dT > 0) { north_acceleration = (velocityDesired.North - last_north_velocity) / dT; east_acceleration = (velocityDesired.East - last_east_velocity) / dT; last_north_velocity = velocityDesired.North; last_east_velocity = velocityDesired.East; } else { north_acceleration = 0; east_acceleration = 0; } // Convert the max angles into the maximum angle that would be requested const float MAX_ACCELERATION = GRAVITY * sinf(guidanceSettings.MaxRollPitch * DEG2RAD); // Compute desired north command from velocity error north_error = velocityDesired.North - velocityActual.North; north_acceleration += pid_apply_antiwindup(&vtol_pids[NORTH_VELOCITY], north_error, -MAX_ACCELERATION, MAX_ACCELERATION, dT) + velocityDesired.North * guidanceSettings.VelocityFeedforward + -nedAccel.North * guidanceSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KD]; // Compute desired east command from velocity error east_error = velocityDesired.East - velocityActual.East; east_acceleration += pid_apply_antiwindup(&vtol_pids[EAST_VELOCITY], east_error, -MAX_ACCELERATION, MAX_ACCELERATION, dT) + velocityDesired.East * guidanceSettings.VelocityFeedforward + -nedAccel.East * guidanceSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KD]; accelDesired.North = north_acceleration; accelDesired.East = east_acceleration; // Note: vertical controller really isn't currently in units of acceleration and the anti-windup may // not be appropriate. However, it is fine for now since it this output is just directly used on the // output. To use this appropriately we need a model of throttle to acceleration. // Compute desired down command. Using NED accel as the damping term down_error = velocityDesired.Down - velocityActual.Down; // Negative is critical here since throttle is negative with down accelDesired.Down = -pid_apply_antiwindup(&vtol_pids[DOWN_VELOCITY], down_error, -1, 0, dT); // Store the desired acceleration AccelDesiredSet(&accelDesired); return 0; }
/** * Controller to maintain/seek a position and optionally descend. * @param[in] dT time since last eval * @param[in] hold_pos_ned a position to hold * @param[in] landing whether to descend * @param[in] update_status does this update path_status, or does somoene else? */ static int32_t vtol_follower_control_simple(const float dT, const float *hold_pos_ned, bool landing, bool update_status) { PositionActualData positionActual; VelocityDesiredData velocityDesired; PositionActualGet(&positionActual); VelocityActualData velocityActual; VelocityActualGet(&velocityActual); /* Where would we be in ___ second at current rates? */ const float cur_pos_ned[3] = { positionActual.North + velocityActual.North * guidanceSettings.PositionFeedforward, positionActual.East + velocityActual.East * guidanceSettings.PositionFeedforward, positionActual.Down }; float errors_ned[3]; /* Calculate the difference between where we want to be and the * above position */ vtol_calculate_difference(cur_pos_ned, hold_pos_ned, errors_ned, false); float horiz_error_mag = vtol_magnitude(errors_ned, 2); float scale_horiz_error_mag = 0; /* Apply a cubic deadband; if we are already really close don't work * as hard to fix it as if we're far away. Prevents chasing high * frequency noise in direction of correction. * * That is, when we're far, noise in estimated position mostly results * in noise/dither in how fast we go towards the target. When we're * close, there's a large directional / hunting component. This * attenuates that. */ if (horiz_error_mag > 0.00001f) { scale_horiz_error_mag = vtol_deadband(horiz_error_mag, guidanceSettings.EndpointDeadbandWidth, guidanceSettings.EndpointDeadbandCenterGain, vtol_end_m, vtol_end_r) / horiz_error_mag; } float damped_ne[2] = { errors_ned[0] * scale_horiz_error_mag, errors_ned[1] * scale_horiz_error_mag }; float commands_ned[3]; // Compute desired north command velocity from position error commands_ned[0] = pid_apply_antiwindup(&vtol_pids[NORTH_POSITION], damped_ne[0], -guidanceSettings.HorizontalVelMax, guidanceSettings.HorizontalVelMax, dT); // Compute desired east command velocity from position error commands_ned[1] = pid_apply_antiwindup(&vtol_pids[EAST_POSITION], damped_ne[1], -guidanceSettings.HorizontalVelMax, guidanceSettings.HorizontalVelMax, dT); if (!landing) { // Compute desired down comand velocity from the position difference commands_ned[2] = pid_apply_antiwindup(&vtol_pids[DOWN_POSITION], errors_ned[2], -guidanceSettings.VerticalVelMax, guidanceSettings.VerticalVelMax, dT); } else { // Just use the landing rate. commands_ned[2] = guidanceSettings.LandingRate; } // Limit the maximum horizontal velocity any direction (not north and east separately) vtol_limit_velocity(commands_ned, guidanceSettings.HorizontalVelMax); velocityDesired.North = commands_ned[0]; velocityDesired.East = commands_ned[1]; velocityDesired.Down = commands_ned[2]; VelocityDesiredSet(&velocityDesired); if (update_status) { uint8_t path_status = PATHSTATUS_STATUS_INPROGRESS; if (!landing) { const bool criterion_altitude = (errors_ned[2]> -guidanceSettings.WaypointAltitudeTol) || (!guidanceSettings.ThrottleControl); // Indicate whether we are in radius of this endpoint // And at/above the altitude requested if ((vtol_magnitude(errors_ned, 2) < guidanceSettings.EndpointRadius) && criterion_altitude) { path_status = PATHSTATUS_STATUS_COMPLETED; } } // landing never terminates. PathStatusStatusSet(&path_status); } return 0; }
/** * Compute desired velocity to follow the desired path from the current location. * @param[in] dT the time since last evaluation * @param[in] pathDesired the desired path to follow * @param[out] progress the current progress information along that path * @returns 0 if successful, <0 if an error occurred * * The calculated velocity to attempt is stored in @ref VelocityDesired */ int32_t vtol_follower_control_path(const float dT, const PathDesiredData *pathDesired, struct path_status *progress) { PositionActualData positionActual; PositionActualGet(&positionActual); VelocityActualData velocityActual; VelocityActualGet(&velocityActual); PathStatusData pathStatus; PathStatusGet(&pathStatus); const float cur_pos_ned[3] = { positionActual.North + velocityActual.North * guidanceSettings.PositionFeedforward, positionActual.East + velocityActual.East * guidanceSettings.PositionFeedforward, positionActual.Down }; path_progress(pathDesired, cur_pos_ned, progress); // Check if we have already completed this leg bool current_leg_completed = (pathStatus.Status == PATHSTATUS_STATUS_COMPLETED) && (pathStatus.Waypoint == pathDesired->Waypoint); pathStatus.fractional_progress = progress->fractional_progress; pathStatus.error = progress->error; pathStatus.Waypoint = pathDesired->Waypoint; // Figure out how low (high) we should be and the error const float altitudeSetpoint = vtol_interpolate(progress->fractional_progress, pathDesired->Start[2], pathDesired->End[2]); const float downError = altitudeSetpoint - positionActual.Down; // If leg is completed signal this if (current_leg_completed || pathStatus.fractional_progress > 1.0f) { const bool criterion_altitude = (downError > -guidanceSettings.WaypointAltitudeTol) || (!guidanceSettings.ThrottleControl); // Once we complete leg and hit altitude criterion signal this // waypoint is done. Or if we're not controlling throttle, // ignore height for completion. // Waypoint heights are thus treated as crossing restrictions- // cross this point at or above... if (criterion_altitude || current_leg_completed) { pathStatus.Status = PATHSTATUS_STATUS_COMPLETED; PathStatusSet(&pathStatus); } else { pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS; PathStatusSet(&pathStatus); } // Wait here for new path segment return vtol_follower_control_simple(dT, pathDesired->End, false, false); } // Interpolate desired velocity and altitude along the path float groundspeed = vtol_interpolate(progress->fractional_progress, pathDesired->StartingVelocity, pathDesired->EndingVelocity); float error_speed = vtol_deadband(progress->error, guidanceSettings.PathDeadbandWidth, guidanceSettings.PathDeadbandCenterGain, vtol_path_m, vtol_path_r) * guidanceSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP]; /* Sum the desired path movement vector with the correction vector */ float commands_ned[3]; commands_ned[0] = progress->path_direction[0] * groundspeed + progress->correction_direction[0] * error_speed; commands_ned[1] = progress->path_direction[1] * groundspeed + progress->correction_direction[1] * error_speed; /* Limit the total velocity based on the configured value. */ vtol_limit_velocity(commands_ned, guidanceSettings.HorizontalVelMax); commands_ned[2] = pid_apply_antiwindup(&vtol_pids[DOWN_POSITION], downError, -guidanceSettings.VerticalVelMax, guidanceSettings.VerticalVelMax, dT); VelocityDesiredData velocityDesired; VelocityDesiredGet(&velocityDesired); velocityDesired.North = commands_ned[0]; velocityDesired.East = commands_ned[1]; velocityDesired.Down = commands_ned[2]; VelocityDesiredSet(&velocityDesired); pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS; PathStatusSet(&pathStatus); return 0; }