/** * 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); }
/** * 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; }