//------------------------------------------------------------------------------
// circularScanController() -- controls the circular scans
//------------------------------------------------------------------------------
void ScanGimbal::circularScanController(const double)
{
static Basic::Integer iBar(1);
// Depending on our scan state, we will either start or stop the bar
switch(getScanState()) {
// reset state: move to ref position
case 0: {
setServoMode(POSITION_SERVO);
setFastSlewMode(true);
setCmdPos(getRefPosition());
setScanState(1);
}
break;
case 1: {
// wait state
if (isPositioned() || isAtLimits()) {
setScanState(2); // out of state 1
}
break;// fall into state 2
}
case 2: {
// start scan - switch to a rate servo, and begin spinning at a commanded rate
// Trigger the SCAN_START event handler
onStartScanEvent(&iBar);
setServoMode(RATE_SERVO);
setFastSlewMode(false);
myLastAngle = Basic::Angle::aepcdRad(getPosition().x() - getRefPosition().x());
setScanState(3);
}
break;
case 3: {
// end scan - finished with one rotation, start over again
bool onceAround = false;
double myAngle = Basic::Angle::aepcdRad(getPosition().x() - getRefPosition().x());
// clockwise
if(getCmdAzRate() >= 0.0) {
onceAround = (myLastAngle < 0.0 && myAngle >= 0.0);
}
// we are going counter-clockwise
else {
onceAround = (myLastAngle >= 0.0 && myAngle < 0.0);
}
myLastAngle = myAngle;
if (onceAround) {
// Trigger the SCAN_END event handler
onEndScanEvent(&iBar);
// start over with our scan (another revolution)
setScanState(2);
}
}
}
}
//------------------------------------------------------------------------------
// pseudoRandomScanController() -- steps through an array of vertices (fast slew)
//------------------------------------------------------------------------------
void ScanGimbal::pseudoRandomScanController(const double)
{
static Basic::Integer iBar(1);
// Depending on our scan state, we will either start or stop the bar
switch(getScanState()) {
// reset state, must be in electronic mode or we will not operate
case 0: {
if (prScanVertices != 0) {
if ( isGimbalType(ELECTRONIC) ) {
setServoMode(POSITION_SERVO);
setFastSlewMode(true);
setScanState(1);
}
else setScanMode(MANUAL_SCAN);
}
}
break;
case 1: {
// start state - go to the desired pseudo random point (must be in electric mode)
if (isPositioned() || isAtLimits()) {
// make sure we have at least one vertice, then move to it
if (nprv != 0 && cprv <= nprv) {
// if this is the first vertice, send a start event
if (cprv == 0) {
// Trigger the SCAN_START event handler
onStartScanEvent(&iBar);
}
else {
setScanPos(prScanVertices[cprv].x(), prScanVertices[cprv].y());
cprv++;
}
}
// when we reach the end
else setScanState(2);
}
}
break;
case 2: {
// end state - reset our vertice count and send an end event
if (isPositioned() || isAtLimits()) {
// Trigger the SCAN_END event handler
onEndScanEvent(&iBar);
// set us back to the first vertice
cprv = 0;
setScanState(1);
}
}
break;
}
// now set our commanded position accordingly
setCmdPos(getRefPosition()+getScanPos());
}
//------------------------------------------------------------------------------
// barScanController() -- control the bar scans
//------------------------------------------------------------------------------
void ScanGimbal::barScanController(const double)
{
static Basic::Integer iBar(1);
// Depending on our scan state, we will either start or stop the bar
switch(getScanState()) {
// reset state, we must set our bar number back to 1
case 0: {
setBarNumber(1);
computeNewBarPos(getBarNumber(), BEGINNING);
setScanState(1);
setServoMode(POSITION_SERVO);
setFastSlewMode(true);
}
break;
case 1: {
// start state - slow slew and compute the end position
if (isPositioned() || isAtLimits()) {
iBar = getBarNumber();
// Trigger the SCAN_START event handler
onStartScanEvent(&iBar);
computeNewBarPos(getBarNumber(), ENDING);
setScanState(2);
setFastSlewMode(false);
}
}
break;
case 2: {
// end state - fast slew and compute the next bar's position (if any)
if (isPositioned() || isAtLimits()) {
iBar = getBarNumber();
// Trigger the SCAN_END event handler
onEndScanEvent(&iBar);
nextBar();
computeNewBarPos(getBarNumber(), BEGINNING);
setScanState(1);
setFastSlewMode(true);
}
}
break;
}
// now set our commanded position accordingly
setCmdPos(getRefPosition()+getScanPos());
}
//------------------------------------------------------------------------------
// rollStabilizingController() -- Roll stabilizes the gimbal
//------------------------------------------------------------------------------
void StabilizingGimbal::rollStabilizingController(const LCreal)
{
if (getOwnship() == nullptr) return;
osg::Vec3 tpos = getCmdPosition();
if (mountPosition == NOSE){
tpos[ROLL_IDX] = static_cast<LCreal>(-getOwnship()->getRoll());
}
else if (mountPosition == TAIL){
tpos[ROLL_IDX] = static_cast<LCreal>(getOwnship()->getRoll());
}
else if (mountPosition == RIGHT_WING){
tpos[ELEV_IDX] = static_cast<LCreal>(-getOwnship()->getPitch());
}
else if (mountPosition == LEFT_WING){
tpos[ELEV_IDX] = static_cast<LCreal>(getOwnship()->getPitch());
}
setCmdPos( tpos );
}
//------------------------------------------------------------------------------
// spiralScanController() -- controls the spiral scan
//------------------------------------------------------------------------------
void ScanGimbal::spiralScanController(const double dt)
{
double degPerDT = (getRevPerSec() * 360.0) * dt;
static Basic::Integer iBar(1);
switch(getScanState()) {
// reset state: move to ref position
case 0: {
setServoMode(POSITION_SERVO);
setFastSlewMode(true);
setScanState(1);
setConAngle(0);
setNumRevs(0.0);
}
break;
case 1: {
// wait state
if (isPositioned() || isAtLimits()) {
setScanState(2); // out of state 1
}
break;// fall into state 2
}
case 2: {
// start scan
setFastSlewMode(false);
setConAngle(getConAngle() + degPerDT);
if (fabs(getConAngle()) > 360.0) {
setNumRevs(getNumRevs()+1);
if (getConAngle() >= 0.0) {
setConAngle(getConAngle() - 360.0);
} else {
setConAngle(getConAngle() + 360.0);
}
}
// Trigger the SCAN_START event handler
onStartScanEvent(&iBar);
setScanState(3);
}
break;
case 3: {
// turn revolutions per second into degrees per sec per frame
// now we get this to each time step
setConAngle(getConAngle() + degPerDT);
if (fabs(getConAngle()) > 360.0) {
setNumRevs(getNumRevs()+1);
if (getConAngle() >= 0.0) {
setConAngle(getConAngle() - 360.0);
} else {
setConAngle(getConAngle() + 360.0);
}
}
// end scan - finished with one rotation, check if our reference has moved
bool onceAround = false;
if (getNumRevs() >= getMaxNumRevs()) {
onceAround = true;
}
// after one revolution
if (onceAround) {
// Trigger the SCAN_END event handler
onEndScanEvent(&iBar);
setConAngle(0.0);
setNumRevs(0.0);
setScanState(2);
}
}
break;
}
double fullAngleRadians = getNumRevs() * 360.0;
if (getRevPerSec() < 0.0) {
fullAngleRadians = -fullAngleRadians;
}
fullAngleRadians = (fullAngleRadians + getConAngle()) * Basic::Angle::D2RCC;
// azimuth
double newX = getScanRadius() * (fullAngleRadians / (2.0 * PI)) * sin(fullAngleRadians);
// elevation
double newY = getScanRadius() * (fullAngleRadians / (2.0 * PI)) * cos(fullAngleRadians);
setScanPos(newX, newY);
// command our new position
setCmdPos(getRefPosition() + getScanPos());
}
//------------------------------------------------------------------------------
// conicalScanController() -- controls the conical scans
//------------------------------------------------------------------------------
void ScanGimbal::conicalScanController(const double dt)
{
double degPerDT = (getRevPerSec() * 360.0) * dt;
static Basic::Integer iBar(1);
switch(getScanState()) {
// reset state: move to ref position
case 0: {
setServoMode(POSITION_SERVO);
setFastSlewMode(true);
setScanState(1);
setConAngle(0);
}
break;
case 1: {
// wait state
if (isPositioned() || isAtLimits()) {
setScanState(2); // out of state 1
}
break;// fall into state 2
}
case 2: {
// start scan
setFastSlewMode(false);
setConAngle( Basic::Angle::aepcdDeg(degPerDT + getConAngle()) );
// Trigger the SCAN_START event handler
onStartScanEvent(&iBar);
setScanState(3);
}
break;
case 3: {
// turn revolutions per second into degrees per sec per frame
// now we get this to each time step
double conAngleN1 = getConAngle();
setConAngle( Basic::Angle::aepcdDeg(degPerDT + getConAngle()) );
// end scan - finished with one rotation, check if our reference has moved
bool onceAround = false;
// clockwise rotation
if (getRevPerSec() >= 0.0) {
if (conAngleN1 < 0.0 && getConAngle() >= 0.0) {
onceAround = true;
}
}
// counter-clockwise rotation
else {
if (conAngleN1 < 0.0 && getConAngle() >= 0.0) {
setConAngle(0);
onceAround = true;
}
}
// after one revolution
if (onceAround) {
// Trigger the SCAN_END event handler
onEndScanEvent(&iBar);
setScanState(2);
}
}
break;
}
// azimuth
double newX = getScanRadius() * sin(getConAngle() * Basic::Angle::D2RCC);
// elevation
double newY = getScanRadius() * cos(getConAngle() * Basic::Angle::D2RCC);
setScanPos(newX, newY);
// command our new position
setCmdPos(getRefPosition() + getScanPos());
}
