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