Example #1
0
/**
 * 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 bearing of current path direction
 */
float VtolFlyController::updatePathBearing()
{
    PositionStateData positionState;

    PositionStateGet(&positionState);

    float cur[3] = { positionState.North,
                     positionState.East,
                     positionState.Down };
    struct path_status progress;

    path_progress(pathDesired, cur, &progress, true);

    // atan2f always returns in between + and - 180 degrees
    return RAD2DEG(atan2f(progress.path_vector[1], progress.path_vector[0]));
}
/**
 * Compute desired velocity from the current position and path
 */
void VtolFlyController::UpdateVelocityDesired()
{
    PositionStateData positionState;

    PositionStateGet(&positionState);

    VelocityStateData velocityState;
    VelocityStateGet(&velocityState);

    VelocityDesiredData velocityDesired;

    // look ahead kFF seconds
    float cur[3] = { positionState.North + (velocityState.North * vtolPathFollowerSettings->CourseFeedForward),
                     positionState.East + (velocityState.East * vtolPathFollowerSettings->CourseFeedForward),
                     positionState.Down + (velocityState.Down * vtolPathFollowerSettings->CourseFeedForward) };
    struct path_status progress;
    path_progress(pathDesired, cur, &progress, true);

    controlNE.ControlPositionWithPath(&progress);
    if (!mManualThrust) {
        controlDown.ControlPositionWithPath(&progress);
    }

    controlNE.UpdateVelocityState(velocityState.North, velocityState.East);
    controlDown.UpdateVelocityState(velocityState.Down);

    float north, east;
    controlNE.GetVelocityDesired(&north, &east);
    velocityDesired.North = north;
    velocityDesired.East  = east;
    if (!mManualThrust) {
        velocityDesired.Down = controlDown.GetVelocityDesired();
    } else { velocityDesired.Down = 0.0f; }

    // update pathstatus
    pathStatus->error = progress.error;
    pathStatus->fractional_progress  = progress.fractional_progress;
    pathStatus->path_direction_north = progress.path_vector[0];
    pathStatus->path_direction_east  = progress.path_vector[1];
    pathStatus->path_direction_down  = progress.path_vector[2];

    pathStatus->correction_direction_north = progress.correction_vector[0];
    pathStatus->correction_direction_east  = progress.correction_vector[1];
    pathStatus->correction_direction_down  = progress.correction_vector[2];

    VelocityDesiredSet(&velocityDesired);
}
Example #4
0
/**
 * 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()
{
	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[GROUNDPATHFOLLOWERSETTINGS_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;

	// No altitude control on a ground vehicle
	velocityDesired.Down = 0;

	VelocityDesiredSet(&velocityDesired);
}
Example #5
0
/**
 * 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()
{
	PositionActualData positionActual;
	PositionActualGet(&positionActual);
	VelocityActualData velocityActual;
	VelocityActualGet(&velocityActual);

	float cur[3] = {positionActual.North, positionActual.East, positionActual.Down};
	struct path_status progress;

	path_progress(&pathDesired, cur, &progress);
	
	float groundspeed = 0;
	float altitudeSetpoint = 0;
	switch (pathDesired.Mode) {
		case PATHDESIRED_MODE_CIRCLERIGHT:
		case PATHDESIRED_MODE_CIRCLELEFT:
			groundspeed = pathDesired.EndingVelocity;
			altitudeSetpoint = pathDesired.End[2];
			break;
		case PATHDESIRED_MODE_ENDPOINT:
		case PATHDESIRED_MODE_VECTOR:
		default:
			groundspeed = pathDesired.StartingVelocity + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) *
				bound_min_max(progress.fractional_progress,0,1);
			altitudeSetpoint = pathDesired.Start[2] + (pathDesired.End[2] - pathDesired.Start[2]) *
				bound_min_max(progress.fractional_progress,0,1);
			break;
	}
	// this ensures a significant forward component at least close to the real trajectory
	if (groundspeed<fixedWingAirspeeds.BestClimbRateSpeed/10.0f)
		groundspeed=fixedWingAirspeeds.BestClimbRateSpeed/10.0f;
	
	// calculate velocity - can be zero if waypoints are too close
	VelocityDesiredData velocityDesired;
	velocityDesired.North = progress.path_direction[0] * groundspeed;
	velocityDesired.East = progress.path_direction[1] * groundspeed;
	
	float error_speed = progress.error * fixedwingpathfollowerSettings.HorizontalPosP;

	// calculate correction - can also be zero if correction vector is 0 or no error present
	velocityDesired.North += progress.correction_direction[0] * error_speed;
	velocityDesired.East += progress.correction_direction[1] * error_speed;
	
	float downError = altitudeSetpoint - positionActual.Down;
	velocityDesired.Down = downError * fixedwingpathfollowerSettings.VerticalPosP;

	// update pathstatus
	pathStatus.error = progress.error;
	pathStatus.fractional_progress = progress.fractional_progress;

	pathStatus.fractional_progress = progress.fractional_progress;
	if (pathStatus.fractional_progress < 1)
		pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS;
	else
		pathStatus.Status = PATHSTATUS_STATUS_COMPLETED;

	pathStatus.Waypoint = pathDesired.Waypoint;

	VelocityDesiredSet(&velocityDesired);
}
Example #6
0
static void produce_command(void)
{
    static int target_surface = 0;
    static int cmd_index = 0;
    Command *command;

    if (has_secondary)
        target_surface = 1;

    ASSERT(g_num_commands);

    command = &g_commands[cmd_index];
    if (command->cb) {
        command->cb(command);
    }
    switch (command->command) {
        case SLEEP:
             printf("sleep %u seconds\n", command->sleep.secs);
             sleep(command->sleep.secs);
             break;
        case PATH_PROGRESS:
            path_progress(&path);
            break;
        case SIMPLE_UPDATE: {
            QXLRect rect = {.left = 0, .right = (target_surface == 0 ? test_width : SURF_WIDTH),
                            .top = 0, .bottom = (target_surface == 0 ? test_height : SURF_HEIGHT)};
            qxl_worker->update_area(qxl_worker, target_surface, &rect, NULL, 0, 1);
            break;
        }

        /* Drawing commands, they all push a command to the command ring */
        case SIMPLE_COPY_BITS:
        case SIMPLE_DRAW_SOLID:
        case SIMPLE_DRAW_BITMAP:
        case SIMPLE_DRAW: {
            SimpleSpiceUpdate *update;

            if (has_automated_tests)
            {
                if (control == 0) {
                     return;
                }

                regression_test();
            }

            switch (command->command) {
            case SIMPLE_COPY_BITS:
                update = test_spice_create_update_copy_bits(0);
                break;
            case SIMPLE_DRAW:
                update = test_spice_create_update_draw(0, path.t);
                break;
            case SIMPLE_DRAW_BITMAP:
                update = test_spice_create_update_from_bitmap(command->bitmap.surface_id,
                        command->bitmap.bbox, command->bitmap.bitmap,
                        command->bitmap.num_clip_rects, command->bitmap.clip_rects);
                break;
            case SIMPLE_DRAW_SOLID:
                update = test_spice_create_update_solid(command->solid.surface_id,
                        command->solid.bbox, command->solid.color);
                break;
            }
            push_command(&update->ext);
            break;
        }

        case SIMPLE_CREATE_SURFACE: {
            SimpleSurfaceCmd *update;
            target_surface = MAX_SURFACE_NUM - 1;
            update = create_surface(target_surface, SURF_WIDTH, SURF_HEIGHT,
                                    g_secondary_surface);
            push_command(&update->ext);
            has_secondary = 1;
            break;
        }

        case SIMPLE_DESTROY_SURFACE: {
            SimpleSurfaceCmd *update;
            has_secondary = 0;
            update = destroy_surface(target_surface);
            target_surface = 0;
            push_command(&update->ext);
            break;
        }

        case DESTROY_PRIMARY:
            qxl_worker->destroy_primary_surface(qxl_worker, 0);
            break;

        case CREATE_PRIMARY:
            create_primary_surface(qxl_worker, command->create_primary.width, command->create_primary.height);
            break;
    }
    cmd_index = (cmd_index + 1) % g_num_commands;
}

SpiceTimer *wakeup_timer;
int wakeup_ms = 50;
SpiceCoreInterface *g_core;

static int req_cmd_notification(QXLInstance *qin)
{
    g_core->timer_start(wakeup_timer, wakeup_ms);
    return TRUE;
}
/**
 * Compute desired velocity from the current position and path
 */
void FixedWingFlyController::updatePathVelocity(float kFF, bool limited)
{
    PositionStateData positionState;

    PositionStateGet(&positionState);
    VelocityStateData velocityState;
    VelocityStateGet(&velocityState);
    VelocityDesiredData velocityDesired;

    const float dT = fixedWingSettings->UpdatePeriod / 1000.0f;

    // look ahead kFF seconds
    float cur[3]   = { positionState.North + (velocityState.North * kFF),
                       positionState.East + (velocityState.East * kFF),
                       positionState.Down + (velocityState.Down * kFF) };
    struct path_status progress;
    path_progress(pathDesired, cur, &progress, true);

    // calculate velocity - can be zero if waypoints are too close
    velocityDesired.North = progress.path_vector[0];
    velocityDesired.East  = progress.path_vector[1];
    velocityDesired.Down  = progress.path_vector[2];

    if (limited &&
        // if a plane is crossing its desired flightpath facing the wrong way (away from flight direction)
        // it would turn towards the flightpath to get on its desired course. This however would reverse the correction vector
        // once it crosses the flightpath again, which would make it again turn towards the flightpath (but away from its desired heading)
        // leading to an S-shape snake course the wrong way
        // this only happens especially if HorizontalPosP is too high, as otherwise the angle between velocity desired and path_direction won't
        // turn steep unless there is enough space complete the turn before crossing the flightpath
        // in this case the plane effectively needs to be turned around
        // indicators:
        // difference between correction_direction and velocitystate >90 degrees and
        // difference between path_direction and velocitystate >90 degrees  ( 4th sector, facing away from everything )
        // fix: ignore correction, steer in path direction until the situation has become better (condition doesn't apply anymore)
        // calculating angles < 90 degrees through dot products
        (vector_lengthf(progress.path_vector, 2) > 1e-6f) &&
        ((progress.path_vector[0] * velocityState.North + progress.path_vector[1] * velocityState.East) < 0.0f) &&
        ((progress.correction_vector[0] * velocityState.North + progress.correction_vector[1] * velocityState.East) < 0.0f)) {
        ;
    } else {
        // calculate correction
        velocityDesired.North += pid_apply(&PIDposH[0], progress.correction_vector[0], dT);
        velocityDesired.East  += pid_apply(&PIDposH[1], progress.correction_vector[1], dT);
    }
    velocityDesired.Down += pid_apply(&PIDposV, progress.correction_vector[2], dT);

    // update pathstatus
    pathStatus->error     = progress.error;
    pathStatus->fractional_progress  = progress.fractional_progress;
    pathStatus->path_direction_north = progress.path_vector[0];
    pathStatus->path_direction_east  = progress.path_vector[1];
    pathStatus->path_direction_down  = progress.path_vector[2];

    pathStatus->correction_direction_north = progress.correction_vector[0];
    pathStatus->correction_direction_east  = progress.correction_vector[1];
    pathStatus->correction_direction_down  = progress.correction_vector[2];


    VelocityDesiredSet(&velocityDesired);
}
Example #8
0
static void produce_command(Test *test)
{
    Command *command;
    QXLWorker *qxl_worker = test->qxl_worker;

    spice_assert(qxl_worker);

    if (test->has_secondary)
        test->target_surface = 1;

    if (!test->num_commands) {
        usleep(1000);
        return;
    }

    command = &test->commands[test->cmd_index];
    if (command->cb) {
        command->cb(test, command);
    }
    switch (command->command) {
        case SLEEP:
             printf("sleep %u seconds\n", command->sleep.secs);
             sleep(command->sleep.secs);
             break;
        case PATH_PROGRESS:
            path_progress(&path);
            break;
        case SIMPLE_UPDATE: {
            QXLRect rect = {
                .left = 0,
                .right = (test->target_surface == 0 ? test->primary_width : test->width),
                .top = 0,
                .bottom = (test->target_surface == 0 ? test->primary_height : test->height)
            };
            if (rect.right > 0 && rect.bottom > 0) {
                spice_qxl_update_area(&test->qxl_instance, test->target_surface, &rect, NULL, 0, 1);
            }
            break;
        }

        /* Drawing commands, they all push a command to the command ring */
        case SIMPLE_COPY_BITS:
        case SIMPLE_DRAW_SOLID:
        case SIMPLE_DRAW_BITMAP:
        case SIMPLE_DRAW: {
            SimpleSpiceUpdate *update;

            if (has_automated_tests)
            {
                if (control == 0) {
                     return;
                }

                regression_test();
            }

            switch (command->command) {
            case SIMPLE_COPY_BITS:
                update = test_spice_create_update_copy_bits(test, 0);
                break;
            case SIMPLE_DRAW:
                update = test_spice_create_update_draw(test, 0, path.t);
                break;
            case SIMPLE_DRAW_BITMAP:
                update = test_spice_create_update_from_bitmap(command->bitmap.surface_id,
                        command->bitmap.bbox, command->bitmap.bitmap,
                        command->bitmap.num_clip_rects, command->bitmap.clip_rects);
                break;
            case SIMPLE_DRAW_SOLID:
                update = test_spice_create_update_solid(command->solid.surface_id,
                        command->solid.bbox, command->solid.color);
                break;
            default: /* Just to shut up GCC warning (-Wswitch) */
                break;
            }
            push_command(&update->ext);
            break;
        }

        case SIMPLE_CREATE_SURFACE: {
            SimpleSurfaceCmd *update;
            if (command->create_surface.data) {
                spice_assert(command->create_surface.surface_id > 0);
                spice_assert(command->create_surface.surface_id < MAX_SURFACE_NUM);
                spice_assert(command->create_surface.surface_id == 1);
                update = create_surface(command->create_surface.surface_id,
                                        command->create_surface.format,
                                        command->create_surface.width,
                                        command->create_surface.height,
                                        command->create_surface.data);
            } else {
                update = create_surface(test->target_surface, SPICE_SURFACE_FMT_32_xRGB,
                                        SURF_WIDTH, SURF_HEIGHT,
                                        test->secondary_surface);
            }
            push_command(&update->ext);
            test->has_secondary = 1;
            break;
        }

        case SIMPLE_DESTROY_SURFACE: {
            SimpleSurfaceCmd *update;
            test->has_secondary = 0;
            update = destroy_surface(test->target_surface);
            test->target_surface = 0;
            push_command(&update->ext);
            break;
        }

        case DESTROY_PRIMARY:
            spice_qxl_destroy_primary_surface(&test->qxl_instance, 0);
            break;

        case CREATE_PRIMARY:
            create_primary_surface(test,
                    command->create_primary.width, command->create_primary.height);
            break;
    }
    test->cmd_index = (test->cmd_index + 1) % test->num_commands;
}

static int req_cmd_notification(QXLInstance *qin)
{
    Test *test = SPICE_CONTAINEROF(qin, Test, qxl_instance);

    test->core->timer_start(test->wakeup_timer, test->wakeup_ms);
    return TRUE;
}
Example #9
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;
}