void
shutdownBaseServer(RaiModule* mod)
{
fprintf(stderr, "BaseServer: Shutting down...\n");
watchdogTimer(0); /* turn off timer */
rotateLimp();
translateLimp();
SIM_sonarStop();
SIM_irStop();
}
void
dropBaseClient(TCX_MODULE_PTR dropClient)
{
int ii;
fprintf(stderr, "%s:%6d:%s()\n", __FILE__, __LINE__, __FUNCTION__);
for (ii=0; ii<TCX_MAX_CLIENTS; ii++) {
if (CLIENT[ii] == dropClient) {
CLIENT[ii] = NULL;
fprintf(stderr, "Client was dropped from the list.\n");
/* If the dropped client was holding the odometry lock, then */
/* free it up for someone else to grab. */
if (ii == bOdometryLock)
releaseOdometryLock();
}
}
fprintf(stderr, "%s:%6d:%s()\n", __FILE__, __LINE__, __FUNCTION__);
for (ii=0; ii<TCX_MAX_CLIENTS; ii++) {
if (CLIENT[ii] != NULL) {
return;
}
}
fprintf(stderr, "%s:%6d:%s() - shutting off sensors and base\n",
__FILE__, __LINE__, __FUNCTION__);
watchdogTimer(0); /* turn off timer */
rotateLimp();
translateLimp();
SIM_sonarStop();
SIM_irStop();
}
void dispatchFixedMessage(int operation, unsigned long param)
{
if (VERBOSE) {
fprintf(stderr,"Calling base operation %d\n",operation);
}
confirmCommandToClient(operation, param); /* sends a message back to all
* baseClients, confirming the
* command, sebastian 7-97 */
switch(operation) {
/**** GENERAL ****/
case BASE_baseKill: baseKill(); break;
case BASE_loadHeading: loadHeading(param); break;
case BASE_loadPosition: loadPosition(param); break;
case BASE_statusReportData: statusReportData(param);break;
case BASE_statusReportPeriod: statusReportPeriod(param);break;
case BASE_batteryVoltage: batteryVoltage();break;
case BASE_batteryCurrent: batteryCurrent();break;
case BASE_watchdogTimer: watchdogTimer(param);break;
case BASE_assumeWatchdog: assumeWatchdog();break;
/**** ROTATION ****/
case BASE_rotateLimp: rotateLimp(); break;
case BASE_rotateHalt: rotateHalt(); break;
case BASE_rotateVelocityPos: rotateVelocityPos(); break;
case BASE_rotateVelocityNeg: rotateVelocityNeg(); break;
case BASE_rotateRelativePos: rotateRelativePos(param); break;
case BASE_rotateRelativeNeg: rotateRelativeNeg(param); break;
case BASE_rotateTorquePos: rotateTorquePos(param); break;
case BASE_rotateTorqueNeg: rotateTorqueNeg(param); break;
case BASE_rotatePowerPos: rotatePowerPos(param); break;
case BASE_rotatePowerNeg: rotatePowerNeg(param); break;
case BASE_rotateToPosition: rotateToPosition(param); break;
case BASE_findRotIndex:
if (bRobot.base_hasIndex) {
findRotIndex();
}
else {
sendClientFixed(BASE_indexReport, 0xFFFFFF);
}
break;
case BASE_setRotateVelocity: setRotateVelocity(param); break;
case BASE_setRotateAcceleration: setRotateAcceleration(param); break;
case BASE_setRotateFriction: setRotateFriction(param); break;
case BASE_setRotateSlope: setRotateSlope(param); break;
case BASE_setRotateTorque: setRotateTorque(param); break;
case BASE_setRotateZero: setRotateZero(param); break;
case BASE_rotateCurrent: rotateCurrent(); break;
case BASE_rotateWhere: rotateWhere(); break;
/**** TRANSLATION ****/
case BASE_translateLimp: translateLimp(); break;
case BASE_translateHalt: translateHalt(); break;
case BASE_translateVelocityPos: translateVelocityPos(); break;
case BASE_translateVelocityNeg: translateVelocityNeg(); break;
case BASE_translateRelativePos: translateRelativePos(param); break;
case BASE_translateRelativeNeg: translateRelativeNeg(param); break;
case BASE_translateTorquePos: translateTorquePos(param); break;
case BASE_translateTorqueNeg: translateTorqueNeg(param); break;
case BASE_translatePowerPos: translatePowerPos(param); break;
case BASE_translatePowerNeg: translatePowerNeg(param); break;
case BASE_translateToPosition: translateToPosition(param); break;
case BASE_setTranslateVelocity: setTranslateVelocity(param); break;
case BASE_setTranslateAcceleration: setTranslateAcceleration(param); break;
case BASE_setTranslateSlope: setTranslateSlope(param); break;
case BASE_setTranslateTorque: setTranslateTorque(param); break;
case BASE_setTranslateZero: setTranslateZero(param); break;
case BASE_translateCurrent: translateCurrent(); break;
case BASE_translateWhere: translateWhere(); break;
/**** SONARS ****/
case BASE_sonarStart: SIM_sonarStart(); break;
case BASE_sonarStop: SIM_sonarStop(); break;
/**** Odometry stuff ****/
case BASE_odometryChangeX:
ntohf(param, bOriginX);
sendClientFixed(BASE_odometryChangeX, param);
break;
case BASE_odometryChangeY:
ntohf(param, bOriginY);
sendClientFixed(BASE_odometryChangeY, param);
break;
case BASE_odometryChangeH:
ntohf(param, bOriginHeading);
sendClientFixed(BASE_odometryChangeH, param);
break;
/**** Odometry lock requests ****/
case BASE_requestOdometryLock:
requestOdometryLock((unsigned short)param);
break;
case BASE_releaseOdometryLock:
releaseOdometryLock();
break;
default:
fprintf(stderr, "BaseServer: Operation %d not yet implemented.\n",
operation);
}
if (VERBOSE) {
fprintf(stderr,"Operation %d complete\n",operation);
}
}
void CYSFReflector::run()
{
bool ret = m_conf.read();
if (!ret) {
::fprintf(stderr, "YSFRefector: cannot read the .ini file\n");
return;
}
ret = ::LogInitialise(m_conf.getLogFilePath(), m_conf.getLogFileRoot(), m_conf.getLogFileLevel(), m_conf.getLogDisplayLevel());
if (!ret) {
::fprintf(stderr, "YSFReflector: unable to open the log file\n");
return;
}
#if !defined(_WIN32) && !defined(_WIN64)
bool m_daemon = m_conf.getDaemon();
if (m_daemon) {
// Create new process
pid_t pid = ::fork();
if (pid == -1) {
::LogWarning("Couldn't fork() , exiting");
return;
}
else if (pid != 0)
exit(EXIT_SUCCESS);
// Create new session and process group
if (::setsid() == -1) {
::LogWarning("Couldn't setsid(), exiting");
return;
}
// Set the working directory to the root directory
if (::chdir("/") == -1) {
::LogWarning("Couldn't cd /, exiting");
return;
}
::close(STDIN_FILENO);
::close(STDOUT_FILENO);
::close(STDERR_FILENO);
//If we are currently root...
if (getuid() == 0) {
struct passwd* user = ::getpwnam("mmdvm");
if (user == NULL) {
::LogError("Could not get the mmdvm user, exiting");
return;
}
uid_t mmdvm_uid = user->pw_uid;
gid_t mmdvm_gid = user->pw_gid;
//Set user and group ID's to mmdvm:mmdvm
if (setgid(mmdvm_gid) != 0) {
::LogWarning("Could not set mmdvm GID, exiting");
return;
}
if (setuid(mmdvm_uid) != 0) {
::LogWarning("Could not set mmdvm UID, exiting");
return;
}
//Double check it worked (AKA Paranoia)
if (setuid(0) != -1) {
::LogWarning("It's possible to regain root - something is wrong!, exiting");
return;
}
}
}
#endif
CNetwork network(m_conf.getNetworkPort(), m_conf.getName(), m_conf.getDescription(), m_conf.getNetworkDebug());
ret = network.open();
if (!ret)
return;
CStopWatch stopWatch;
stopWatch.start();
CTimer dumpTimer(1000U, 120U);
dumpTimer.start();
CTimer pollTimer(1000U, 5U);
pollTimer.start();
LogMessage("Starting YSFReflector-%s", VERSION);
CTimer watchdogTimer(1000U, 0U, 1500U);
unsigned char tag[YSF_CALLSIGN_LENGTH];
unsigned char src[YSF_CALLSIGN_LENGTH];
unsigned char dst[YSF_CALLSIGN_LENGTH];
for (;;) {
unsigned char buffer[200U];
unsigned int len = network.readData(buffer);
if (len > 0U) {
if (!watchdogTimer.isRunning()) {
::memcpy(tag, buffer + 4U, YSF_CALLSIGN_LENGTH);
if (::memcmp(buffer + 14U, " ", YSF_CALLSIGN_LENGTH) != 0)
::memcpy(src, buffer + 14U, YSF_CALLSIGN_LENGTH);
else
::memcpy(src, "??????????", YSF_CALLSIGN_LENGTH);
if (::memcmp(buffer + 24U, " ", YSF_CALLSIGN_LENGTH) != 0)
::memcpy(dst, buffer + 24U, YSF_CALLSIGN_LENGTH);
else
::memcpy(dst, "??????????", YSF_CALLSIGN_LENGTH);
LogMessage("Received data from %10.10s to %10.10s at %10.10s", src, dst, buffer + 4U);
} else {
if (::memcmp(tag, buffer + 4U, YSF_CALLSIGN_LENGTH) == 0) {
bool changed = false;
if (::memcmp(buffer + 14U, " ", YSF_CALLSIGN_LENGTH) != 0 && ::memcmp(src, "??????????", YSF_CALLSIGN_LENGTH) == 0) {
::memcpy(src, buffer + 14U, YSF_CALLSIGN_LENGTH);
changed = true;
}
if (::memcmp(buffer + 24U, " ", YSF_CALLSIGN_LENGTH) != 0 && ::memcmp(dst, "??????????", YSF_CALLSIGN_LENGTH) == 0) {
::memcpy(dst, buffer + 24U, YSF_CALLSIGN_LENGTH);
changed = true;
}
if (changed)
LogMessage("Received data from %10.10s to %10.10s at %10.10s", src, dst, buffer + 4U);
}
}
// Only accept transmission from an already accepted repeater
if (::memcmp(tag, buffer + 4U, YSF_CALLSIGN_LENGTH) == 0) {
watchdogTimer.start();
std::string callsign = std::string((char*)(buffer + 4U), YSF_CALLSIGN_LENGTH);
CYSFRepeater* rpt = findRepeater(callsign);
if (rpt != NULL) {
for (std::vector<CYSFRepeater*>::const_iterator it = m_repeaters.begin(); it != m_repeaters.end(); ++it) {
if ((*it)->m_callsign != callsign)
network.writeData(buffer, (*it)->m_address, (*it)->m_port);
}
}
if ((buffer[34U] & 0x01U) == 0x01U) {
LogMessage("Received end of transmission");
watchdogTimer.stop();
}
}
}
// Refresh/add repeaters based on their polls
std::string callsign;
in_addr address;
unsigned int port;
bool ret = network.readPoll(callsign, address, port);
if (ret) {
CYSFRepeater* rpt = findRepeater(callsign);
if (rpt == NULL) {
LogMessage("Adding %s", callsign.c_str());
rpt = new CYSFRepeater;
rpt->m_timer.start();
rpt->m_callsign = callsign;
rpt->m_address = address;
rpt->m_port = port;
m_repeaters.push_back(rpt);
network.setCount(m_repeaters.size());
} else {
rpt->m_timer.start();
rpt->m_address = address;
rpt->m_port = port;
}
}
unsigned int ms = stopWatch.elapsed();
stopWatch.start();
network.clock(ms);
pollTimer.clock(ms);
if (pollTimer.hasExpired()) {
for (std::vector<CYSFRepeater*>::const_iterator it = m_repeaters.begin(); it != m_repeaters.end(); ++it)
network.writePoll((*it)->m_address, (*it)->m_port);
pollTimer.start();
}
// Remove any repeaters that haven't reported for a while
for (std::vector<CYSFRepeater*>::iterator it = m_repeaters.begin(); it != m_repeaters.end(); ++it)
(*it)->m_timer.clock(ms);
for (std::vector<CYSFRepeater*>::iterator it = m_repeaters.begin(); it != m_repeaters.end(); ++it) {
if ((*it)->m_timer.hasExpired()) {
LogMessage("Removing %s", (*it)->m_callsign.c_str());
m_repeaters.erase(it);
network.setCount(m_repeaters.size());
break;
}
}
watchdogTimer.clock(ms);
if (watchdogTimer.isRunning() && watchdogTimer.hasExpired()) {
LogMessage("Network watchdog has expired");
watchdogTimer.stop();
}
dumpTimer.clock(ms);
if (dumpTimer.hasExpired()) {
dumpRepeaters();
dumpTimer.start();
}
if (ms < 5U) {
#if defined(_WIN32) || defined(_WIN64)
::Sleep(5UL); // 5ms
#else
::usleep(5000); // 5ms
#endif
}
}
network.close();
::LogFinalise();
}
