/**
* Increment the waypoint index which triggers the active waypoint method
*/
static void advanceWaypoint()
{
WaypointActiveGet(&waypointActive);
// Store the currently active waypoint. This is used in activeWaypoint to plot
// a waypoint from this (previous) waypoint to the newly selected one
previous_waypoint = waypointActive.Index;
// Default implementation simply jumps to the next possible waypoint. Insert any
// conditional logic desired here.
// Note: In the case of conditional logic it is the responsibilty of the implementer
// to ensure all possible paths are valid.
waypointActive.Index++;
if (waypointActive.Index >= UAVObjGetNumInstances(WaypointHandle())) {
holdCurrentPosition();
// Do not reset path_status_updated here to avoid this method constantly being called
return;
} else {
WaypointActiveSet(&waypointActive);
}
// Invalidate any pending path status updates
path_status_updated = false;
}
void CombineController::doUpdate(const double& timestep,
math::Vector3& translationalForceOut,
math::Vector3& rotationalTorqueOut)
{
// Don't do anything during the initialization pause
if (m_initializationPause > 0)
{
m_initializationPause -= timestep;
// so at the end of the initialization pause we
// are steady
if(m_initHoldDepth)
holdCurrentDepth();
if(m_initHoldHeading)
holdCurrentHeading();
if(m_initHoldPosition)
holdCurrentPosition();
return;
}
//if visual servoing is enabled, fix the desired state for that DOF so that
//the integral terms don't go insane
math::Vector2 dxy = m_desiredState->getDesiredPosition();
math::Vector2 estxy = m_stateEstimator->getEstimatedPosition();
double ed = m_stateEstimator->getEstimatedDepth();
double dd = m_desiredState->getDesiredDepth();
if(m_desiredState->vx == true)
{
dxy.x = estxy.x;
}
if(m_desiredState->vy == true)
{
dxy.y = estxy.y;
}
if(m_desiredState->vz == true)
{
dd = ed;
}
m_desiredState->setDesiredPosition(dxy);
m_desiredState->setDesiredDepth(dd);
// Update controllers
math::Vector3 inPlaneControlForce(
m_transController->translationalUpdate(
timestep, m_stateEstimator, m_desiredState));
math::Vector3 depthControlForce(
m_depthController->depthUpdate(
timestep, m_stateEstimator, m_desiredState));
math::Vector3 rotControlTorque(
m_rotController->rotationalUpdate(
timestep, m_stateEstimator, m_desiredState));
// Combine into desired rotational control and torque
translationalForceOut = inPlaneControlForce + depthControlForce;
rotationalTorqueOut = rotControlTorque;
//moved
//if(m_desiredState->vz == true && vConz == false)
//{
// intTermz = m_depthController->getISum(); //steal the positional controllers z integral term
//}
//begin the velocity controller
//yes this is the wrong place, but the system doesn't really provide effective support for controller switching
//let alone using multiple controllers with different types of states simultaneously
math::Vector2 eVelocity = m_stateEstimator->getEstimatedVelocity();
math::Vector2 dVelocity = m_desiredState->getDesiredVelocity();
math::Vector3 eAccel = m_stateEstimator->getEstimatedLinearAcceleration();
math::Vector2 dAccelxy = m_desiredState->getDesiredAccel();
//double dAccel = m_desiredState->getDesiredDepthAccel();
double eRate = m_stateEstimator->getEstimatedDepthRate();
double dRate = m_desiredState->getDesiredDepthRate();
//rotate estimated velocity into the body frame, since its in the intertial frame
math::Vector3 toRot(eVelocity.x,eVelocity.y,0);
math::Vector3 rot = m_stateEstimator->getEstimatedOrientation() * toRot;
//then rotate acceleration
eAccel = m_stateEstimator->getEstimatedOrientation()*eAccel;
eVelocity.x = rot.x;
eVelocity.y = rot.y;
//eRate = rot.z;
//double eAccel = (eRate - lastDV)/timestep;
lastDV = eRate;
double fX, fY,fZ;
//math::Vector3 toRot(dVelocity.x-eVelocity.x,dVelocity.y - eVelocity.y,eRate - dRate);
//math::Vector3 rot = m_stateEstimator->getEstimatedOrientation() * toRot;
math::Vector2 errVxy = /*math::Vector2(rot.x,rot.y);*/dVelocity - eVelocity;
double errVz = /*rot.z;*/eRate - dRate;
intTermxy = intTermxy + errVxy*timestep;
intTermz = intTermz + errVz*timestep;
//holdCurrentDepth();
//holdCurrentPosition();
if((vConx == false && m_desiredState->vx == true))
{
intTermxy.x = copysign(pix,dVelocity.x);
//intTermxy.x = 0;
}
if((vCony == false && m_desiredState->vy == true))
{
//intTermxy.y = copysign(piy,dVelocity.y);
intTermxy.y = 0;
}
if((vConz == false && m_desiredState->vz == true))
{
//begin added
//a more sohpisticated stealing, which separates the transient sum from the steady state one
intTermz = m_depthController->getISum();
intTermz = copysign(piz-intTermz,dRate) + intTermz;
//end added
//intTermz = m_depthController->getISum(); //steal the positional controllers z integral term
}
//if turning off visual servoing, store the previous integral sums
//this is done to reuse them later in order to speed up our rise time
if((vConx == true && m_desiredState->vx == false))
{
pix = intTermxy.x;
holdCurrentPosition();
//holdCurrentHeading();
}
if(vCony == true && m_desiredState->vy == false)
{
piy = intTermxy.y;
holdCurrentPosition();
//holdCurrentHeading();
}
if(vConz == true && m_desiredState->vz == false)
{
piz = intTermz;
holdCurrentDepth();
//holdCurrentHeading();
}
vConx = m_desiredState->vx;
vCony = m_desiredState->vy;
vConz = m_desiredState->vz;
//std::cout<<vConx<<":"<<vCony<<"::"<<vConz<<std::endl;
math::Vector3 translationalForceOutp = m_stateEstimator->getEstimatedOrientation().UnitInverse() * translationalForceOut;
math::Vector3 translationalForceOutf(0,0,0);
//this freezes up the other translation DOF's motion
//since if we are rotated we will likely move in it
//this does allow for some drift, but we don't have
//an alternative due to the structure of the system
math::Vector2 padj = m_desiredState->getDesiredPosition();
math::Vector2 cp = m_stateEstimator->getEstimatedPosition();
if(vConx == true)
{
double eXv = errVxy.x;
double eXa = eAccel.x;
double eXi = intTermxy.x;
fX = kpvx * eXv + kivx*eXi + kdvx*eXa;
translationalForceOutf.x = fX;
translationalForceOutp.x = 0;
padj.y = cp.y;
}
if(vCony == true)
{
double eYv = errVxy.y;
double eYa = eAccel.y;
double eYi = intTermxy.y;
fY = kpvy * eYv + kivy*eYi + kdvy*eYa;
translationalForceOutf.y = fY;
translationalForceOutp.y = 0;
padj.x = cp.x;
}
if(vConz == true)
{
double eZv = errVz;
double eZa = eAccel.z;
double eZi = intTermz;
fZ = kpvz * eZv + kivz*eZi + kdvz*eZa;
translationalForceOutf.z = fZ;
translationalForceOutp.z = 0;
}
m_desiredState->setDesiredPosition(padj);
//this is done so that we can remove the other controllers ability to control a DOF
//the controller forces are removed in the body frame, then the outputs of the visual servoing controller are moved to the body frame
translationalForceOut = m_stateEstimator->getEstimatedOrientation() * translationalForceOutp + translationalForceOutf;
LOGGER.infoStream() << translationalForceOut[0] << " "
<< translationalForceOut[1] << " "
<< translationalForceOut[2] << " "
<< rotationalTorqueOut[0] << " "
<< rotationalTorqueOut[1] << " "
<< rotationalTorqueOut[2];
// publish the individual control signals
math::Vector3EventPtr dEvent = math::Vector3EventPtr(new math::Vector3Event());
dEvent->vector3 = depthControlForce;
math::Vector3EventPtr tEvent = math::Vector3EventPtr(new math::Vector3Event());
tEvent->vector3 = inPlaneControlForce;
math::Vector3EventPtr oEvent = math::Vector3EventPtr(new math::Vector3Event());
oEvent->vector3 = rotControlTorque;
publish(IController::DEPTH_CONTROL_SIGNAL_UPDATE, dEvent);
publish(IController::TRANSLATION_CONTROL_SIGNAL_UPDATE, tEvent);
publish(IController::ORIENTATION_CONTROL_SIGNAL_UPDATE, oEvent);
}
/**
* This method is called when a new waypoint is activated
*
* Note: The way this is called, it runs in an object callback. This is safe because
* the execution time is extremely short. If it starts to take a longer time then
* the main task look should monitor a flag (such as the waypoint changing) and call
* this method from the main task.
*/
static void activateWaypoint(int idx)
{
active_waypoint = idx;
if (idx >= UAVObjGetNumInstances(WaypointHandle())) {
// Attempting to access invalid waypoint. Fall back to position hold at current location
AlarmsSet(SYSTEMALARMS_ALARM_PATHPLANNER, SYSTEMALARMS_ALARM_ERROR);
holdCurrentPosition();
return;
}
// Get the activated waypoint
WaypointInstGet(idx, &waypoint);
PathDesiredData pathDesired;
pathDesired.End[PATHDESIRED_END_NORTH] = waypoint.Position[WAYPOINT_POSITION_NORTH];
pathDesired.End[PATHDESIRED_END_EAST] = waypoint.Position[WAYPOINT_POSITION_EAST];
pathDesired.End[PATHDESIRED_END_DOWN] = waypoint.Position[WAYPOINT_POSITION_DOWN];
pathDesired.ModeParameters = waypoint.ModeParameters;
// Use this to ensure the cases match up (catastrophic if not) and to cover any cases
// that don't make sense to come from the path planner
switch(waypoint.Mode) {
case WAYPOINT_MODE_FLYVECTOR:
pathDesired.Mode = PATHDESIRED_MODE_FLYVECTOR;
break;
case WAYPOINT_MODE_FLYENDPOINT:
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
break;
case WAYPOINT_MODE_FLYCIRCLELEFT:
pathDesired.Mode = PATHDESIRED_MODE_FLYCIRCLELEFT;
break;
case WAYPOINT_MODE_FLYCIRCLERIGHT:
pathDesired.Mode = PATHDESIRED_MODE_FLYCIRCLERIGHT;
break;
case WAYPOINT_MODE_LAND:
pathDesired.Mode = PATHDESIRED_MODE_LAND;
break;
default:
holdCurrentPosition();
AlarmsSet(SYSTEMALARMS_ALARM_PATHPLANNER, SYSTEMALARMS_ALARM_ERROR);
return;
}
pathDesired.EndingVelocity = waypoint.Velocity;
if(previous_waypoint < 0) {
// For first waypoint, get current position as start point
PositionActualData positionActual;
PositionActualGet(&positionActual);
pathDesired.Start[PATHDESIRED_START_NORTH] = positionActual.North;
pathDesired.Start[PATHDESIRED_START_EAST] = positionActual.East;
pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down - 1;
pathDesired.StartingVelocity = waypoint.Velocity;
} else {
// Get previous waypoint as start point
WaypointData waypointPrev;
WaypointInstGet(previous_waypoint, &waypointPrev);
pathDesired.Start[PATHDESIRED_END_NORTH] = waypointPrev.Position[WAYPOINT_POSITION_NORTH];
pathDesired.Start[PATHDESIRED_END_EAST] = waypointPrev.Position[WAYPOINT_POSITION_EAST];
pathDesired.Start[PATHDESIRED_END_DOWN] = waypointPrev.Position[WAYPOINT_POSITION_DOWN];
pathDesired.StartingVelocity = waypointPrev.Velocity;
}
PathDesiredSet(&pathDesired);
// Invalidate any pending path status updates
path_status_updated = false;
AlarmsClear(SYSTEMALARMS_ALARM_PATHPLANNER);
}
