bool IncomingToRelay::haveWork()
{
bool work(false);
if (settle()) work = true;
if (Incoming::haveWork()) work = true;
return work;
}
RcppExport SEXP cfamounts(SEXP params){
SEXP rl=R_NilValue;
char* exceptionMesg=NULL;
try{
RcppParams rparam(params);
QuantLib::Date maturity(dateFromR(rparam.getDateValue("Maturity")));
QuantLib::Date settle(dateFromR(rparam.getDateValue("Settle")));
QuantLib::Date issue(dateFromR(rparam.getDateValue("IssueDate")));
double rate = rparam.getDoubleValue("CouponRate");
std::vector<double> rateVec(1, rate);
double faceAmount = rparam.getDoubleValue("Face");
double period = rparam.getDoubleValue("Period");
double basis = rparam.getDoubleValue("Basis");
DayCounter dayCounter = getDayCounter(basis);
Frequency freq = getFrequency(period);
Period p(freq);
double EMR = rparam.getDoubleValue("EMR");
Calendar calendar=UnitedStates(UnitedStates::GovernmentBond);
Schedule sch(settle, maturity, p, calendar,
Unadjusted, Unadjusted, DateGeneration::Backward,
(EMR == 1)? true : false);
FixedRateBond bond(1, faceAmount, sch, rateVec, dayCounter, Following,
100, issue);
//cashflow
int numCol = 2;
std::vector<std::string> colNames(numCol);
colNames[0] = "Date";
colNames[1] = "Amount";
RcppFrame frame(colNames);
Leg bondCashFlow = bond.cashflows();
for (unsigned int i = 0; i< bondCashFlow.size(); i++){
std::vector<ColDatum> row(numCol);
Date d = bondCashFlow[i]->date();
row[0].setDateValue(RcppDate(d.month(), d.dayOfMonth(), d.year()));
row[1].setDoubleValue(bondCashFlow[i]->amount());
frame.addRow(row);
}
RcppResultSet rs;
rs.add("cashFlow", frame);
rl = rs.getReturnList();
} catch(std::exception& ex) {
exceptionMesg = copyMessageToR(ex.what());
} catch(...) {
exceptionMesg = copyMessageToR("unknown reason");
}
if(exceptionMesg != NULL)
Rf_error(exceptionMesg);
return rl;
}
bool IncomingToRelay::doWork()
{
relay->check();
bool work(false);
if (settle()) work = true;
if (Incoming::doWork()) work = true;
return work;
}
int main()
{
int nSelection = -1;
printf("\n");
printf("欢迎进入计费管理系统\n");
do{
outputMenu();//输出系统菜单
scanf("%d",&nSelection);//输入菜单项编号
switch(nSelection){
case 1:{
add();// printf("添加卡\n");
break;
}
case 2:{
query();//printf("查询卡\n");
break;
}
case 3:{
logon();// printf("上机\n");
break;
}
case 4:{
settle();//printf("下机\n");
break;
}
case 5:{
addMoney();//printf("充值\n");
break;
}
case 6:{
refundMoney();//printf("退费\n");
break;
}
case 7:{
printf("查询统计\n");
break;
}
case 8:{
annul();//printf("注销卡\n");
break;
}
case 0:{
exitApp();
break;
}
default:printf("输入菜单序号错误！请选择(0~8)\n");break;
}
}while(nSelection != 0);
system("pause");
return 0;
}
int drop(PIECE ¤tPiece){
int i;
//printf("entered drop\n");
PIECE potentialPiece;
for(int i = 0; i < 4; i++){
potentialPiece.index[i].x = currentPiece.index[i].x;
potentialPiece.index[i].y = currentPiece.index[i].y;
}
potentialPiece.shape = currentPiece.shape;
potentialPiece.state = currentPiece.state;
while(blockTest(potentialPiece)){
for(i = 0; i < 4; i++){
potentialPiece.index[i].y = potentialPiece.index[i].y - 1;
if(potentialPiece.index[i].y < 0){
for(int i = 0; i < 4; i++){
currentPiece.index[i].y = potentialPiece.index[i].y + 1;
}
settle(currentPiece);
return 1;
}
}
}
for(int i = 0; i < 4; i++){
currentPiece.index[i].y = potentialPiece.index[i].y + 1;
}
settle(currentPiece);
return 1;
}
//--------- Begin of function Unit::ai_settle_new_town --------//
//
// Settle a new village next to one of the camps in this region.
//
int Unit::ai_settle_new_town()
{
//----- locate a suitable camp for the new town to settle next to ----//
Nation* ownNation = nation_array[nation_recno];
FirmCamp* firmCamp, *bestCamp=NULL;
int curRegionId = region_id();
int curRating, bestRating=0;
for( int i=ownNation->ai_camp_count-1 ; i>=0 ; i-- )
{
firmCamp = (FirmCamp*) firm_array[ ownNation->ai_camp_array[i] ];
if( firmCamp->region_id != curRegionId )
continue;
curRating = firmCamp->total_combat_level();
if( curRating > bestRating )
{
bestRating = curRating;
bestCamp = firmCamp;
}
}
if( !bestCamp )
return 0;
//--------- settle a new town now ---------//
int xLoc=bestCamp->loc_x1;
int yLoc=bestCamp->loc_y1;
if( world.locate_space(&xLoc, &yLoc, bestCamp->loc_x2, bestCamp->loc_y2,
STD_TOWN_LOC_WIDTH, STD_TOWN_LOC_HEIGHT,
UNIT_LAND, curRegionId, 1 ) ) // 1-build flag
{
settle( xLoc, yLoc );
return 1;
}
return 0;
}
void popFront() override { i++; settle(); }
explicit PreComputedEdgeRange(JSContext *cx, SimpleEdgeVector &edges)
: edges(edges),
i(0)
{
settle();
}
void SteadyState::resettleFunc()
{
settle( 1 );
}
void SteadyState::settleFunc()
{
settle( 0 );
}
static bool gps_query(/*@out@*/struct gps_data_t *gpsdata,
gps_mask_t expect,
const int timeout,
const char *fmt, ... )
/* ship a command and wait on an expected response type */
{
static fd_set rfds;
char buf[BUFSIZ];
va_list ap;
time_t starttime;
#ifdef COMPAT_SELECT
struct timeval tv;
#else
struct timespec tv;
sigset_t oldset, blockset;
(void)sigemptyset(&blockset);
(void)sigaddset(&blockset, SIGHUP);
(void)sigaddset(&blockset, SIGINT);
(void)sigaddset(&blockset, SIGTERM);
(void)sigaddset(&blockset, SIGQUIT);
(void)sigprocmask(SIG_BLOCK, &blockset, &oldset);
#endif /* COMPAT_SELECT */
va_start(ap, fmt);
(void)vsnprintf(buf, sizeof(buf)-2, fmt, ap);
va_end(ap);
if (buf[strlen(buf)-1] != '\n')
(void)strlcat(buf, "\n", BUFSIZ);
/*@-usedef@*/
if (write(gpsdata->gps_fd, buf, strlen(buf)) <= 0) {
gpsd_report(&context.errout, LOG_ERROR, "gps_query(), write failed\n");
return false;
}
/*@+usedef@*/
gpsd_report(&context.errout, LOG_PROG, "gps_query(), wrote, %s\n", buf);
FD_ZERO(&rfds);
starttime = time(NULL);
for (;;) {
FD_CLR(gpsdata->gps_fd, &rfds);
gpsd_report(&context.errout, LOG_PROG, "waiting...\n");
/*@ -usedef -type -nullpass -compdef @*/
tv.tv_sec = 2;
#ifdef COMPAT_SELECT
tv.tv_usec = 0;
if (select(gpsdata->gps_fd + 1, &rfds, NULL, NULL, &tv) == -1) {
#else
tv.tv_nsec = 0;
if (pselect(gpsdata->gps_fd + 1, &rfds, NULL, NULL, &tv, &oldset) == -1) {
#endif
if (errno == EINTR || !FD_ISSET(gpsdata->gps_fd, &rfds))
continue;
gpsd_report(&context.errout, LOG_ERROR, "select %s\n", strerror(errno));
exit(EXIT_FAILURE);
}
/*@ +usedef +type +nullpass +compdef @*/
gpsd_report(&context.errout, LOG_PROG, "reading...\n");
(void)gps_read(gpsdata);
if (ERROR_SET & gpsdata->set) {
gpsd_report(&context.errout, LOG_ERROR, "error '%s'\n", gpsdata->error);
return false;
}
/*@ +ignorequals @*/
if ((expect == NON_ERROR) || (expect & gpsdata->set) != 0)
return true;
else if (timeout > 0 && (time(NULL) - starttime > timeout)) {
gpsd_report(&context.errout, LOG_ERROR,
"timed out after %d seconds\n",
timeout);
return false;
}
/*@ -ignorequals @*/
}
return false;
}
static void onsig(int sig)
{
if (sig == SIGALRM) {
gpsd_report(&context.errout, LOG_ERROR, "packet recognition timed out.\n");
exit(EXIT_FAILURE);
} else {
gpsd_report(&context.errout, LOG_ERROR, "killed by signal %d\n", sig);
exit(EXIT_SUCCESS);
}
}
static char /*@observer@*/ *gpsd_id( /*@in@ */ struct gps_device_t *session)
/* full ID of the device for reports, including subtype */
{
static char buf[128];
if ((session == NULL) || (session->device_type == NULL) ||
(session->device_type->type_name == NULL))
return "unknown,";
(void)strlcpy(buf, session->device_type->type_name, sizeof(buf));
if (session->subtype[0] != '\0') {
(void)strlcat(buf, " ", sizeof(buf));
(void)strlcat(buf, session->subtype, sizeof(buf));
}
return (buf);
}
static void ctlhook(struct gps_device_t *device UNUSED, gps_mask_t changed UNUSED)
/* recognize when we've achieved sync */
{
static int packet_counter = 0;
/*
* If it's NMEA, go back around enough times for the type probes to
* reveal any secret identity (like SiRF or UBX) the chip might have.
* If it's not, getting more packets might fetch subtype information.
*/
if (packet_counter++ >= REDIRECT_SNIFF)
{
hunting = false;
(void) alarm(0);
}
}
int main(int argc, char **argv)
{
int option, status;
char *device = NULL, *devtype = NULL;
char *speed = NULL, *control = NULL, *rate = NULL;
bool to_binary = false, to_nmea = false, reset = false;
bool lowlevel=false, echo=false;
struct gps_data_t gpsdata;
const struct gps_type_t *forcetype = NULL;
const struct gps_type_t **dp;
#ifdef CONTROLSEND_ENABLE
char cooked[BUFSIZ];
ssize_t cooklen = 0;
#endif /* RECONFIGURE_ENABLE */
context.errout.label = "gpsctl";
#define USAGE "usage: gpsctl [-l] [-b | -n | -r] [-D n] [-s speed] [-c rate] [-T timeout] [-V] [-t devtype] [-x control] [-e] <device>\n"
while ((option = getopt(argc, argv, "bec:fhlnrs:t:x:D:T:V")) != -1) {
switch (option) {
case 'b': /* switch to vendor binary mode */
to_binary = true;
break;
case 'c':
#ifdef RECONFIGURE_ENABLE
rate = optarg;
#else
gpsd_report(&context.errout, LOG_ERROR,
"cycle-change capability has been conditioned out.\n");
#endif /* RECONFIGURE_ENABLE */
break;
case 'x': /* ship specified control string */
#ifdef CONTROLSEND_ENABLE
control = optarg;
lowlevel = true;
if ((cooklen = hex_escapes(cooked, control)) <= 0) {
gpsd_report(&context.errout, LOG_ERROR,
"invalid escape string (error %d)\n", (int)cooklen);
exit(EXIT_FAILURE);
}
#else
gpsd_report(&context.errout, LOG_ERROR,
"control_send capability has been conditioned out.\n");
#endif /* CONTROLSEND_ENABLE */
break;
case 'e': /* echo specified control string with wrapper */
lowlevel = true;
echo = true;
break;
case 'f': /* force direct access to the device */
lowlevel = true;
break;
case 'l': /* list known device types */
for (dp = gpsd_drivers; *dp; dp++) {
#ifdef RECONFIGURE_ENABLE
if ((*dp)->mode_switcher != NULL)
(void)fputs("-[bn]\t", stdout);
else
(void)fputc('\t', stdout);
if ((*dp)->speed_switcher != NULL)
(void)fputs("-s\t", stdout);
else
(void)fputc('\t', stdout);
if ((*dp)->rate_switcher != NULL)
(void)fputs("-c\t", stdout);
else
(void)fputc('\t', stdout);
#endif /* RECONFIGURE_ENABLE */
#ifdef CONTROLSEND_ENABLE
if ((*dp)->control_send != NULL)
(void)fputs("-x\t", stdout);
else
(void)fputc('\t', stdout);
#endif /* CONTROLSEND_ENABLE */
(void)puts((*dp)->type_name);
}
exit(EXIT_SUCCESS);
case 'n': /* switch to NMEA mode */
#ifdef RECONFIGURE_ENABLE
to_nmea = true;
#else
gpsd_report(&context.errout, LOG_ERROR,
"speed-change capability has been conditioned out.\n");
#endif /* RECONFIGURE_ENABLE */
break;
case 'r': /* force-switch to default mode */
#ifdef RECONFIGURE_ENABLE
reset = true;
lowlevel = false; /* so we'll abort if the daemon is running */
#else
gpsd_report(&context.errout, LOG_ERROR,
"reset capability has been conditioned out.\n");
#endif /* RECONFIGURE_ENABLE */
break;
case 's': /* change output baud rate */
#ifdef RECONFIGURE_ENABLE
speed = optarg;
#else
gpsd_report(&context.errout, LOG_ERROR,
"speed-change capability has been conditioned out.\n");
#endif /* RECONFIGURE_ENABLE */
break;
case 't': /* force the device type */
devtype = optarg;
break;
case 'T': /* set the timeout on packet recognition */
timeout = (unsigned)atoi(optarg);
explicit_timeout = true;
break;
case 'D': /* set debugging level */
debuglevel = atoi(optarg);
#ifdef CLIENTDEBUG_ENABLE
gps_enable_debug(debuglevel, stderr);
#endif /* CLIENTDEBUG_ENABLE */
break;
case 'V':
(void)fprintf(stderr, "version %s (revision %s)\n",
VERSION, REVISION);
exit(EXIT_SUCCESS);
case 'h':
default:
fprintf(stderr, USAGE);
break;
}
}
if (optind < argc)
device = argv[optind];
if (devtype != NULL) {
int matchcount = 0;
for (dp = gpsd_drivers; *dp; dp++) {
if (strstr((*dp)->type_name, devtype) != NULL) {
forcetype = *dp;
matchcount++;
}
}
if (matchcount == 0)
gpsd_report(&context.errout, LOG_ERROR,
"no driver type name matches '%s'.\n", devtype);
else if (matchcount == 1) {
assert(forcetype != NULL);
gpsd_report(&context.errout, LOG_PROG,
"%s driver selected.\n", forcetype->type_name);
} else {
forcetype = NULL;
gpsd_report(&context.errout, LOG_ERROR,
"%d driver type names match '%s'.\n",
matchcount, devtype);
}
}
if (((int)to_nmea + (int)to_binary + (int)reset) > 1) {
gpsd_report(&context.errout, LOG_ERROR, "make up your mind, would you?\n");
exit(EXIT_SUCCESS);
}
(void) signal(SIGINT, onsig);
(void) signal(SIGTERM, onsig);
(void) signal(SIGQUIT, onsig);
/*@-nullpass@*/ /* someday, add null annotation to the gpsopen() params */
if (!lowlevel) {
/* Try to open the stream to gpsd. */
if (gps_open(NULL, NULL, &gpsdata) != 0) {
gpsd_report(&context.errout, LOG_ERROR,
"no gpsd running or network error: %s.\n",
gps_errstr(errno));
lowlevel = true;
}
}
/*@-nullpass@*/
if (!lowlevel) {
int i, devcount;
if (!explicit_timeout)
timeout = HIGH_LEVEL_TIMEOUT;
/* what devices have we available? */
if (!gps_query(&gpsdata, DEVICELIST_SET, (int)timeout, "?DEVICES;\n")) {
gpsd_report(&context.errout, LOG_ERROR, "no DEVICES response received.\n");
(void)gps_close(&gpsdata);
exit(EXIT_FAILURE);
}
if (gpsdata.devices.ndevices == 0) {
gpsd_report(&context.errout, LOG_ERROR, "no devices connected.\n");
(void)gps_close(&gpsdata);
exit(EXIT_FAILURE);
} else if (gpsdata.devices.ndevices > 1 && device == NULL) {
gpsd_report(&context.errout, LOG_ERROR,
"multiple devices and no device specified.\n");
(void)gps_close(&gpsdata);
exit(EXIT_FAILURE);
}
gpsd_report(&context.errout, LOG_PROG,
"%d device(s) found.\n",gpsdata.devices.ndevices);
/* try to mine the devicelist return for the data we want */
if (gpsdata.devices.ndevices == 1 && device == NULL) {
device = gpsdata.dev.path;
i = 0;
} else {
assert(device != NULL);
for (i = 0; i < gpsdata.devices.ndevices; i++)
if (strcmp(device, gpsdata.devices.list[i].path) == 0) {
goto devicelist_entry_matches;
}
gpsd_report(&context.errout, LOG_ERROR,
"specified device not found in device list.\n");
(void)gps_close(&gpsdata);
exit(EXIT_FAILURE);
devicelist_entry_matches:;
}
(void)memcpy(&gpsdata.dev,
&gpsdata.devices.list[i],
sizeof(struct devconfig_t));
devcount = gpsdata.devices.ndevices;
/* if the device has not identified, watch it until it does so */
if (gpsdata.dev.driver[0] == '\0') {
if (gps_stream(&gpsdata, WATCH_ENABLE|WATCH_JSON, NULL) == -1) {
gpsd_report(&context.errout, LOG_ERROR, "stream set failed.\n");
(void)gps_close(&gpsdata);
exit(EXIT_FAILURE);
}
while (devcount > 0) {
errno = 0;
if (gps_read(&gpsdata) == -1) {
gpsd_report(&context.errout, LOG_ERROR, "data read failed.\n");
(void)gps_close(&gpsdata);
exit(EXIT_FAILURE);
}
if (gpsdata.set & DEVICE_SET) {
--devcount;
assert(gpsdata.dev.path[0]!='\0' && gpsdata.dev.driver[0]!='\0');
if (strcmp(gpsdata.dev.path, device) == 0) {
goto matching_device_seen;
}
}
}
gpsd_report(&context.errout, LOG_ERROR, "data read failed.\n");
(void)gps_close(&gpsdata);
exit(EXIT_FAILURE);
matching_device_seen:;
}
/* sanity check */
if (gpsdata.dev.driver[0] == '\0') {
gpsd_report(&context.errout, LOG_SHOUT, "%s can't be identified.\n",
gpsdata.dev.path);
(void)gps_close(&gpsdata);
exit(EXIT_SUCCESS);
}
/* if no control operation was specified, just ID the device */
if (speed==NULL && rate == NULL && !to_nmea && !to_binary && !reset) {
(void)printf("%s identified as a %s",
gpsdata.dev.path, gpsdata.dev.driver);
if (gpsdata.dev.subtype[0] != '\0') {
(void)fputc(' ', stdout);
(void)fputs(gpsdata.dev.subtype, stdout);
}
if (gpsdata.dev.baudrate > 0)
(void)printf(" at %u baud", gpsdata.dev.baudrate);
(void)fputc('.', stdout);
(void)fputc('\n', stdout);
}
status = 0;
#ifdef RECONFIGURE_ENABLE
if (reset)
{
gpsd_report(&context.errout, LOG_PROG,
"cannot reset with gpsd running.\n");
exit(EXIT_SUCCESS);
}
/*@-boolops@*/
/*
* We used to wait on DEVICE_SET here. That doesn't work
* anymore because when the demon generates its response it
* sets the mode bit in the response from the current packet
* type, which may not have changed (probably will not have
* changed) even though the command to switch modes has been
* sent and will shortly take effect.
*/
if (to_nmea) {
if (!gps_query(&gpsdata, NON_ERROR, (int)timeout,
"?DEVICE={\"path\":\"%s\",\"native\":0}\r\n",
device)) {
gpsd_report(&context.errout, LOG_ERROR,
"%s mode change to NMEA failed\n",
gpsdata.dev.path);
status = 1;
} else
gpsd_report(&context.errout, LOG_PROG,
"%s mode change succeeded\n", gpsdata.dev.path);
}
else if (to_binary) {
if (!gps_query(&gpsdata, NON_ERROR, (int)timeout,
"?DEVICE={\"path\":\"%s\",\"native\":1}\r\n",
device)) {
gpsd_report(&context.errout, LOG_ERROR,
"%s mode change to native mode failed\n",
gpsdata.dev.path);
status = 1;
} else
gpsd_report(&context.errout, LOG_PROG,
"%s mode change succeeded\n",
gpsdata.dev.path);
}
/*@+boolops@*/
if (speed != NULL) {
char parity = 'N';
char stopbits = '1';
if (strchr(speed, ':') == NULL)
(void)gps_query(&gpsdata,
DEVICE_SET, (int)timeout,
"?DEVICE={\"path\":\"%s\",\"bps\":%s}\r\n",
device, speed);
else {
char *modespec = strchr(speed, ':');
/*@ +charint @*/
status = 0;
if (modespec!=NULL) {
*modespec = '\0';
if (strchr("78", *++modespec) == NULL) {
gpsd_report(&context.errout, LOG_ERROR,
"No support for that word length.\n");
status = 1;
}
parity = *++modespec;
if (strchr("NOE", parity) == NULL) {
gpsd_report(&context.errout, LOG_ERROR,
"What parity is '%c'?\n", parity);
status = 1;
}
stopbits = *++modespec;
if (strchr("12", stopbits) == NULL) {
gpsd_report(&context.errout, LOG_ERROR,
"Stop bits must be 1 or 2.\n");
status = 1;
}
}
if (status == 0)
(void)gps_query(&gpsdata,
DEVICE_SET, (int)timeout,
"?DEVICE={\"path\":\"%s\",\"bps\":%s,\"parity\":\"%c\",\"stopbits\":%c}\r\n",
device, speed, parity, stopbits);
}
if (atoi(speed) != (int)gpsdata.dev.baudrate) {
gpsd_report(&context.errout, LOG_ERROR,
"%s driver won't support %s%c%c\n",
gpsdata.dev.path,
speed, parity, stopbits);
status = 1;
} else
gpsd_report(&context.errout, LOG_PROG,
"%s change to %s%c%c succeeded\n",
gpsdata.dev.path,
speed, parity, stopbits);
}
if (rate != NULL) {
(void)gps_query(&gpsdata,
DEVICE_SET, (int)timeout,
"?DEVICE={\"path\":\"%s\",\"cycle\":%s}\n",
device, rate);
}
#endif /* RECONFIGURE_ENABLE */
(void)gps_close(&gpsdata);
exit(status);
#ifdef RECONFIGURE_ENABLE
} else if (reset) {
/* hard reset will go through lower-level operations */
const int speeds[] = {2400, 4800, 9600, 19200, 38400, 57600, 115200};
static struct gps_device_t session; /* zero this too */
int i;
if (device == NULL || forcetype == NULL) {
gpsd_report(&context.errout, LOG_ERROR,
"device and type must be specified for the reset operation.\n");
exit(EXIT_FAILURE);
}
/*@ -mustfreeonly -immediatetrans @*/
gps_context_init(&context, "gpsctl");
context.errout.debug = debuglevel;
session.context = &context;
gpsd_tty_init(&session);
(void)strlcpy(session.gpsdata.dev.path, device, sizeof(session.gpsdata.dev.path));
session.device_type = forcetype;
(void)gpsd_open(&session);
(void)gpsd_set_raw(&session);
(void)session.device_type->speed_switcher(&session, 4800, 'N', 1);
(void)tcdrain(session.gpsdata.gps_fd);
for(i = 0; i < (int)(sizeof(speeds) / sizeof(speeds[0])); i++) {
(void)gpsd_set_speed(&session, speeds[i], 'N', 1);
(void)session.device_type->speed_switcher(&session, 4800, 'N', 1);
(void)tcdrain(session.gpsdata.gps_fd);
}
gpsd_set_speed(&session, 4800, 'N', 1);
for (i = 0; i < 3; i++)
if (session.device_type->mode_switcher)
session.device_type->mode_switcher(&session, MODE_NMEA);
gpsd_wrap(&session);
exit(EXIT_SUCCESS);
/*@ +mustfreeonly +immediatetrans @*/
#endif /* RECONFIGURE_ENABLE */
} else {
/* access to the daemon failed, use the low-level facilities */
static struct gps_device_t session; /* zero this too */
fd_set all_fds;
fd_set rfds;
/*
* Unless the user explicitly requested it, always run to end of
* hunt rather than timing out. Otherwise we can easily get messages
* that spuriously look like failure at high baud rates.
*/
/*@ -mustfreeonly -immediatetrans @*/
gps_context_init(&context, "gpsctl");
context.errout.debug = debuglevel;
session.context = &context; /* in case gps_init isn't called */
if (echo)
context.readonly = true;
if (timeout > 0) {
(void) alarm(timeout);
(void) signal(SIGALRM, onsig);
}
/*
* Unless the user has forced a type and only wants to see the
* string (not send it) we now need to try to open the device
* and find out what is actually there.
*/
if (!(forcetype != NULL && echo)) {
int maxfd = 0;
if (device == NULL) {
gpsd_report(&context.errout, LOG_ERROR,
"device must be specified for low-level access.\n");
exit(EXIT_FAILURE);
}
gpsd_init(&session, &context, device);
if (gpsd_activate(&session, O_PROBEONLY) < 0) {
gpsd_report(&context.errout, LOG_ERROR,
"initial GPS device %s open failed\n",
device);
exit(EXIT_FAILURE);
}
gpsd_report(&context.errout, LOG_INF,
"device %s activated\n", session.gpsdata.dev.path);
/*@i1@*/FD_SET(session.gpsdata.gps_fd, &all_fds);
if (session.gpsdata.gps_fd > maxfd)
maxfd = session.gpsdata.gps_fd;
/* initialize the GPS context's time fields */
gpsd_time_init(&context, time(NULL));
/*@-compdef@*/
/* grab packets until we time out, get sync, or fail sync */
for (hunting = true; hunting; )
{
fd_set efds;
switch(gpsd_await_data(&rfds, &efds, maxfd, &all_fds, &context.errout))
{
case AWAIT_GOT_INPUT:
break;
case AWAIT_NOT_READY:
/* no recovery from bad fd is possible */
if (FD_ISSET(session.gpsdata.gps_fd, &efds))
exit(EXIT_FAILURE);
continue;
case AWAIT_FAILED:
exit(EXIT_FAILURE);
}
switch(gpsd_multipoll(FD_ISSET(session.gpsdata.gps_fd, &rfds),
&session, ctlhook, 0))
{
case DEVICE_READY:
FD_SET(session.gpsdata.gps_fd, &all_fds);
break;
case DEVICE_UNREADY:
FD_CLR(session.gpsdata.gps_fd, &all_fds);
break;
case DEVICE_ERROR:
/* this is where a failure to sync lands */
gpsd_report(&context.errout, LOG_WARN,
"device error, bailing out.\n");
exit(EXIT_FAILURE);
case DEVICE_EOF:
gpsd_report(&context.errout, LOG_WARN,
"device signed off, bailing out.\n");
exit(EXIT_SUCCESS);
default:
break;
}
}
/*@+compdef@*/
gpsd_report(&context.errout, LOG_PROG,
"%s looks like a %s at %d.\n",
device, gpsd_id(&session),
session.gpsdata.dev.baudrate);
if (forcetype!=NULL && strcmp("NMEA0183", session.device_type->type_name) !=0 && strcmp(forcetype->type_name, session.device_type->type_name)!=0) {
gpsd_report(&context.errout, LOG_ERROR,
"'%s' doesn't match non-generic type '%s' of selected device.\n",
forcetype->type_name,
session.device_type->type_name);
}
}
(void)printf("%s identified as a %s at %u baud.\n",
device, gpsd_id(&session),
session.gpsdata.dev.baudrate);
/* if no control operation was specified, we're done */
if (speed==NULL && !to_nmea && !to_binary && control==NULL)
exit(EXIT_SUCCESS);
/* maybe user wants to see the packet rather than send it */
if (echo)
session.gpsdata.gps_fd = fileno(stdout);
/* control op specified; maybe we forced the type */
if (forcetype != NULL)
(void)gpsd_switch_driver(&session, forcetype->type_name);
/* now perform the actual control function */
status = 0;
#ifdef RECONFIGURE_ENABLE
/*@ -nullderef @*/
if (to_nmea || to_binary) {
bool write_enable = context.readonly;
context.readonly = false;
if (session.device_type->mode_switcher == NULL) {
gpsd_report(&context.errout, LOG_SHOUT,
"%s devices have no mode switch.\n",
session.device_type->type_name);
status = 1;
} else {
int target_mode = to_nmea ? MODE_NMEA : MODE_BINARY;
gpsd_report(&context.errout, LOG_SHOUT,
"switching to mode %s.\n",
to_nmea ? "NMEA" : "BINARY");
session.device_type->mode_switcher(&session, target_mode);
settle(&session);
}
context.readonly = write_enable;
}
if (speed) {
char parity = echo ? 'N': session.gpsdata.dev.parity;
int stopbits = echo ? 1 : session.gpsdata.dev.stopbits;
char *modespec;
modespec = strchr(speed, ':');
/*@ +charint @*/
status = 0;
if (modespec!=NULL) {
*modespec = '\0';
if (strchr("78", *++modespec) == NULL) {
gpsd_report(&context.errout, LOG_ERROR,
"No support for that word lengths.\n");
status = 1;
}
parity = *++modespec;
if (strchr("NOE", parity) == NULL) {
gpsd_report(&context.errout, LOG_ERROR,
"What parity is '%c'?\n", parity);
status = 1;
}
stopbits = *++modespec;
if (strchr("12", parity) == NULL) {
gpsd_report(&context.errout, LOG_ERROR,
"Stop bits must be 1 or 2.\n");
status = 1;
}
stopbits = (int)(stopbits-'0');
}
if (status == 0) {
if (session.device_type->speed_switcher == NULL) {
gpsd_report(&context.errout, LOG_ERROR,
"%s devices have no speed switch.\n",
session.device_type->type_name);
status = 1;
}
else if (session.device_type->speed_switcher(&session,
(speed_t)atoi(speed),
parity,
stopbits)) {
settle(&session);
gpsd_report(&context.errout, LOG_PROG,
"%s change to %s%c%d succeeded\n",
session.gpsdata.dev.path,
speed, parity, stopbits);
} else {
gpsd_report(&context.errout, LOG_ERROR,
"%s driver won't support %s%c%d.\n",
session.gpsdata.dev.path,
speed, parity, stopbits);
status = 1;
}
}
}
if (rate) {
bool write_enable = context.readonly;
context.readonly = false;
if (session.device_type->rate_switcher == NULL) {
gpsd_report(&context.errout, LOG_ERROR,
"%s devices have no rate switcher.\n",
session.device_type->type_name);
status = 1;
} else {
double rate_dbl = strtod(rate, NULL);
if (!session.device_type->rate_switcher(&session, rate_dbl)) {
gpsd_report(&context.errout, LOG_ERROR, "rate switch failed.\n");
status = 1;
}
settle(&session);
}
context.readonly = write_enable;
}
#endif /* RECONFIGURE_ENABLE */
#ifdef CONTROLSEND_ENABLE
/*@ -compdef @*/
if (control) {
bool write_enable = context.readonly;
context.readonly = false;
if (session.device_type->control_send == NULL) {
gpsd_report(&context.errout, LOG_ERROR,
"%s devices have no control sender.\n",
session.device_type->type_name);
status = 1;
} else {
if (session.device_type->control_send(&session,
cooked,
(size_t)cooklen) == -1) {
gpsd_report(&context.errout, LOG_ERROR,
"control transmission failed.\n");
status = 1;
}
settle(&session);
}
context.readonly = write_enable;
}
/*@ +compdef @*/
#endif /* CONTROLSEND_ENABLE */
exit(status);
/*@ +nullderef @*/
/*@ +mustfreeonly +immediatetrans @*/
}
}
int main(int argc, char **argv)
{
printf("ur5 Server Start\n");
// JointVector tcp_home= {0.0, -191.0, 600.0, 0.0, -2.2211, -2.2211};
JointVector joint_home_rad={0.0000, -1.5708, 0.0000, -1.5708, 0.0000, 0.0000};
JointVector joint_home={0.0000, -90, 0.0000, -90, 0.0000, 0.0000};
JointVector init_two ={-6.5018, -95.3469, -29.8123, -194.7758, -59.0822, -62.3032};
JointVector init_right_side = {0.309199, -1.240009, 0.323497, -1.039543, 0.541075, 0.486453};
JointVector initialP_rad = {-0.330294, -1.881878, -0.290919, -2.243194, -0.660115, -0.704284};
JointVector viereck[4] = {{43.24, -140.88, -30.05, -12.83, 129.28, -0.27},
{130.67, -140.87, -30.05, -12.75, 44.08, -0.27},
{130.67, -187.90, 6.46, -1.22, 43.35, -0.27},
{43.24, -187.90, -30.05, 3.87, 100.32, -0.27}
};
JointVector PosOne= {36.12, -112.88, -8.78, -24.53, 88.39, 16.64};
int i,h,n=0;
for(i=0; i<4;i++){
for(h=0;h<6;h++){
viereck[i][h]= deg_to_rad(viereck[i][h]);
}
}
for(i=0;i<MSG_BUFFER_SIZE;i++) msg_buffer[i].text = msg_text_buffers[i];
// JointVector initialP={-30.4347, -132.2621, -40.8718, -149.8236, -59.1151, -62.2781};
// int i;
// printf("joint_home ={");
// for(i=0; i<6; i++){
// joint_home_rad[i]= deg_to_rad(joint_home[i]);
// joint_home[i] = rad_to_deg(joint_home_rad[i]);
// printf("%3.4f%s",joint_home_rad[i], i<5 ? ", " : "};");
//// }
// MovePTPPacket move_ptp_packet;
for(i=0; i<6; i++){
PosOne[i]= deg_to_rad(PosOne[i]);}
// move_ptp(joint_home_rad, PosOne , &move_ptp_packet);
// return 0;
struct connection_data server;
fd_set masterfds, readfds;
struct timeval timeout,send_time_start, send_time_end;
int rc;
pthread_t tcp_server_thread;
MovePTPPacket move_ptp_packet;
server.initialize_direction = -1.0;
read_args(argc, argv, &server);
initialize_direction = server.initialize_direction;
printf("initialize direction: %f", initialize_direction);
rc = pthread_create(&tcp_server_thread, NULL, &start_tcp_server, &server);
if(rc < 0){
quit_program=true;
}
int connection_timeout_count=0;
pva_packet.header.protocol_id=PVA_PACKET_ID;
p_packet.header.protocol_id=PPACKET_ID;
//--------------------------- Init Robot -------------------------------------------------------
configuration_load();
open_interface();
power_up();
set_wrench();
if(!initialize(0)){
puts("could not initialize robot");
exit(EXIT_FAILURE);
}
settle();
setup_sigint();
setup_sigpipe();
memcpy(&last_pva_packet, &pva_packet,sizeof(PVAPacket));
for(i=0;i<6;i++) last_pva_packet.acceleration[i] = 0.0;
for(i=0;i<6;i++) last_pva_packet.velocity[i] = 0.0;
bool timeout_flag=false;
char *buff[sizeof(PVAPacket)];
//------------------------------------------------------------------------------------------------------
//------------------------------------------------------------------------------------------------------
// for(pva_packet.header.cycle_number =0; quit_program == false; pva_packet.header.cycle_number++) {
// robotinterface_read_state_blocking();
// pthread_mutex_lock(&connect_mutex);
// if(connection_timeout_count > TIMEOUT_TOLERANCE){
// connection_timeout_count=0;
// connected=false;
// }
// //--------------------------- Stuff roboter does -------------------------------------------------------
// robotinterface_get_actual(servo_packet.position, servo_packet.velocity);
// //------------------------------------------------------------------------------------------------------
// //------------------------------------------------------------------------------------------------------
// //---------------------------- send -------------------------------------------------------------------
// if(connected){
// gettimeofday(&send_time_start,0);
// if(send(client_fd, (char*) &pva_packet , sizeof(pva_packet),0) < 1){
// connected=false;
// }
// }
// //------------------------------------------------------------------------------------------------------
// //------------------------------------------------------------------------------------------------------
// //---------------------------- recieve -----------------------------------------------------------------
// timeout_flag=false;
// if(connected){
// FD_ZERO(&masterfds);
// FD_SET(client_fd, &masterfds);
// memcpy(&readfds, &masterfds, sizeof(fd_set));
// gettimeofday(&send_time_end,0);
// timeval_diff(&timeout, &send_time_start, &send_time_end);
// timeout.tv_usec = 6000 - timeout.tv_usec;
// if (select(client_fd +1, &readfds, NULL, NULL, &timeout) < 0) {
// pthread_mutex_lock(&connect_mutex);
// connected=false;
// pthread_mutex_unlock(&connect_mutex);
// }
// if(FD_ISSET(client_fd, &readfds)){
// n=recv(client_fd, buff, sizeof(buff), 0);
// if (n < 1 || n != sizeof(PVAPacket)){
// pthread_mutex_lock(&connect_mutex);
// connected=false;
// pthread_mutex_unlock(&connect_mutex);
// }
// }else{
// connection_timeout_count++;
// if(connection_timeout_count< TIMEOUT_TOLERANCE)
// timeout_flag=true;
// }
// if(!timeout_flag && connected){
// memcpy(&pva_packet, buff, n);
// if(connection_timeout_count) connection_timeout_count--;
// }
// }
// //------------------------------------------------------------------------------------------------------
// //------------------------------------------------------------------------------------------------------
// //---------------------------- do stuff with data ------------------------------------------------------
// // TODO check for errors-----
// //------------------------------------------------------------------------------------------------------
// if(!timeout_flag && connected){
// // save pva to last if something went wrong;
// memcpy(&last_pva_packet, &pva_packet, sizeof(PVAPacket);
// }
// //------------------------------------------------------------------------------------------------------
// //------------------------------------------------------------------------------------------------------
// //--------------------------- Stuff roboter does -------------------------------------------------------
// if(connected){
// if(!timeout){
// robotinterface_command_position_velocity_acceleration(pva_packet.position,
// pva_packet.velocity,
// pva_packet.acceleration);
// }else{ //timeout
// if(connection_timeout_count<3){
// connected=false;
// robotinterface_command_velocity(zero_vector);
// }
// // cause of timeout we take the last pva_packet
// robotinterface_command_position_velocity_acceleration(last_pva_packet.position,
// last_pva_packet.velocity,
// last_pva_packet.acceleration);
// }
// }else{
// robotinterface_command_velocity(zero_vector);
// }
// pthread_mutex_unlock(&connect_mutex);
// robotinterface_send();
// //------------------------------------------------------------------------------------------------------
// //------------------------------------------------------------------------------------------------------
// }
// // ---- quit programm
printf("shutdown robot...\n");
//---- shutdown robot ---------
// move to home ----
move_to_position(viereck[0]);
// move_to_position(joint_home_rad);
// move_to_position(PosOne);
// ---
puts("- Speeding down");
int j;
for (j = 0; j < 10; j++)
{
robotinterface_read_state_blocking();
robotinterface_command_velocity(zero_vector);
robotinterface_send();
}
puts("close robotinterface");
robotinterface_close();
puts("Done!");
printf("shutdown program\n");
return 0;
}
