void BadExit( const char* Msg)
{
CloseSteppers();
CloseEncoderResetLogFile();
CloseSerial( EncoderComPort);
CloseSerial( LX200ComPort);
BadMsgExit( Msg);
}
static void SerparClose(void)
{
#ifdef COMPILING_ON_WINDOWS
CloseParallel();
CloseSerial();
#else
Unix_CloseParallel();
Unix_CloseSerial();
#endif
}
static void SerialClose(void)
{
#ifdef DO_TRACE
printf("SerialClose()\n");
#endif
#ifdef COMPILING_ON_WINDOWS
CloseSerial();
#else
Unix_CloseSerial();
#endif
}
int CloseRobby(void)
{
/*
Output = 0;
if( WriteSerial(~Output) == -1) // tous les relais à zéro avant de partir
{
return (-1);
}
*/
if(CloseSerial () == -1)
{
return (-1);
}
return (0);
}
int SerialWindows::OpenSerial(void)
{
if(handle != INVALID_HANDLE_VALUE) {
return 0;
}
handle = CreateFile((char *)portName.c_str(), GENERIC_READ | GENERIC_WRITE, 0, 0, OPEN_EXISTING, 0, 0);
if(handle == INVALID_HANDLE_VALUE){
return -1;
}
if(SetParameter(timeOut, baudRate, dataBits, stopBits, parity, flowControl) < 0) {
CloseSerial();
return -1;
}
return 0;
}
RS232Interface::~RS232Interface()
{
UserDebug(Destructor, "RS232Interface::~RS232Interface() Destructor\n");
CloseSerial();
}
int main(int argc, char *argv[])
{
long iter, repeat = 0;
double interval_sec = (double)1/20;
struct timespec start, end;
int opt;
/*
* get command line options
*/
while ((opt = getopt(argc, argv, "i:h")) != -1) {
switch (opt) {
case 'i': /* iterations */
repeat = strtol(optarg, NULL, 0);
PDEBUG("repeat=%ld\n", repeat);
break;
case 'h':
usage(argc, argv);
break;
}
}
/* Initialize model */
EKF_IFS_2_initialize();
/* Initialize hardware */
InitIMU(); /* vectornav */
InitSerial(); /* arduino */
clock_gettime(CLOCK_REALTIME, &start);
iter = 0;
while (1) {
double remain_us;
uint64_t tmpdiff;
/* Get sensor data */
GetIMUData(&EKF_IFS_2_U);
/* Get Arduino Data */
GetSerialData(&EKF_IFS_2_U);
/* Get moving points Data */
InitMovingWaypoints(&EKF_IFS_2_U);
/* Get waypoints Data */
InitStaticWaypoints(&EKF_IFS_2_U);
/* Get Servo deflection Data */
InitOther(&EKF_IFS_2_U);
/* Step the model */
EKF_IFS_2_step();
/* Output to the motor controller */
SendSerialData(&EKF_IFS_2_Y);
/* Time book keeping */
clock_gettime(CLOCK_REALTIME, &end);
tmpdiff = get_elapsed(&start, &end);
remain_us = (interval_sec * 1000000 - tmpdiff / 1000);
if (remain_us > 0) {
usleep((useconds_t)remain_us);
}
clock_gettime(CLOCK_REALTIME, &start);
iter++;
PDEBUG("iter %ld took %" PRIu64 "us\n", iter, tmpdiff/1000);
PDEBUG("Out: throttle=%f elevator=%f aileron=%f rudder=%f\n",
EKF_IFS_2_Y.ControlSurfaceCommands.throttle_cmd,
EKF_IFS_2_Y.ControlSurfaceCommands.elevator_cmd,
EKF_IFS_2_Y.ControlSurfaceCommands.aileron_cmd,
EKF_IFS_2_Y.ControlSurfaceCommands.rudder_cmd);
if (iter >= repeat)
break;
}
/* Matfile logging */
rt_StopDataLogging(MATFILE, EKF_IFS_2_M->rtwLogInfo);
/* Terminate model */
EKF_IFS_2_terminate();
/* Close hardware */
CloseIMU();
CloseSerial();
return 0;
}
void InitROM(void)
{
w32 saved_regs[16];
char qvers[6];
char *initstr="UQLX v%s, release\012 %s\012QDOS Version %s\012";
long sysvars,sxvars;
if((long)((Ptr)gPC-(Ptr)theROM)-2 != ROMINIT_CMD_ADDR)
{ exception=4;
extraFlag=true;
return;
}
#if 0
printf("a6=%x, basic at %x\n",aReg[6],ReadLong(0x28010));
#endif
save_regs(saved_regs);
do_update=1; /* flip in screen RAM */
#ifdef OLD_PATCH
/* lea $0C000,a3 */
aReg[3]=0x0c000;
gPC+=2;
#else
#warning UNUSED OLD_PATCH
WW((Ptr)gPC-2,0x0c93); /* restore original instruction */
#endif
#if 0
KillSound();
CloseSerial();
InitSerial();
ZeroKeyboardBuffer();
#endif
/* delete old MDV drivers (for optical reasons) */
WriteLong(0x28048,0);
InitFileDrivers();
InitDrivers();
#ifdef XSCREEN
/*printf("call init_xscreen\n");*/
init_xscreen();
#else
#warning UNUSED XSCREEN
#endif
SchedInit();
init_bas_exts();
QLtrap(1,0,20000l);
#if 0
printf("QDOS vars at %x, trap res=%d, RTOP=%d\n",aReg[0],reg[0],RTOP);
#endif
sysvars=aReg[0];
sxvars=RL((Ptr)theROM+sysvars+0x7c);
#if 0
if (isMinerva)
printf("Minerva extended vars at %x\n",sxvars);
#endif
if (V3)
printf("sysvars at %x, ux RAMTOP %d, sys.ramt %d, qlscreen at %d\n",
sysvars,RTOP,sysvar_l(20),qlscreen.qm_lo);
// QDOS version
WL((Ptr)qvers,reg[2]);qvers[4]=0;
#if 0
p=(Ptr)theROM+RTOP-0x07FFEl;
sprintf(p,initstr,uqlx_version,release,qvers);
WriteWord(aReg[1]=RTOP-0x08000l,strlen(p));
#if 1
QLvector(0xd0,200000);
#else
WriteLong((*sp)-=4,(w32)gPC-(w32)theROM);
gPC=(uw16*)((Ptr)theROM+RW((uw16*)((Ptr)theROM+0xd0))); /* write string */
#endif
#endif
/*HACK: allow breakpoints in ROM*/
#if DEBUG_ROM
RamMap[0]=RamMap[1]=RamMap[2]=3;
uqlx_protect(0,3*32768,QX_RAM);
#endif
/* now install TKII defaults */
reg[1]=0x6c;
reg[2]=0;
QLtrap(1,0x18,200000);
if(reg[0]==0){
if (V3)printf("Initialising TK2 device defaults\n");
WriteLong(0x28070+0x3c,aReg[0]);
WriteLong(0x28070+0x40,32+aReg[0]);
WriteLong(0x28070+0x44,64+aReg[0]);
WriteWord(aReg[0],strlen(DATAD));strcpy((char*)((Ptr)theROM+aReg[0]+2),DATAD);
WriteWord(aReg[0]+32,strlen(PROGD));strcpy((char*)((Ptr)theROM+aReg[0]+34),PROGD);
WriteWord(aReg[0]+64,strlen(SPOOLD));strcpy((char*)((Ptr)theROM+aReg[0]+66),SPOOLD);
}
/* link in Minerva keyboard handling */
#if 1
if (isMinerva)
{
reg[1]=8;
reg[2]=0;
QLtrap(1,0x18,200000);
if(reg[0]==0)
{
WW((Ptr)theROM+MIPC_CMD_ADDR,MIPC_CMD_CODE);
WL((Ptr)theROM+aReg[0],RL((Ptr)theROM+sxvars+0x14));
WL((Ptr)theROM+aReg[0]+4,MIPC_CMD_ADDR);
WL((Ptr)theROM+sxvars+0x14,aReg[0]);
}
WW((Ptr)theROM+KBENC_CMD_ADDR,KBENC_CMD_CODE);
orig_kbenc=RL((Ptr)theROM+sxvars+0x10);
WL((Ptr)theROM+sxvars+0x10,KBENC_CMD_ADDR);
#if 0
printf("orig_kbenc=%x\nreplacement kbenc=%x\n",orig_kbenc,KBENC_CMD_ADDR);
printf("sx_kbenc addr=%x\n",sxvars+0x10);
#endif
}
#endif
init_poll();
/* make sure it wasn't changed somewhere */
restore_regs(saved_regs);
#ifdef OLD_PATCH
aReg[3]=0x0c000;
#endif
#ifndef OLD_PATCH
table[code=0x0c93](); /* run the original routine */
#endif
}
/*****************************************************************************
*
* OpenSerialByName: open the port
*
*****************************************************************************/
status_t OpenSerialByName(unsigned int reader_index, char *dev_name)
{
struct termios current_termios;
unsigned int reader = reader_index;
/* 255 is MAX_DEVICENAME in pcscd.h */
char reader_name[255] = "GemPCTwin";
char *p;
status_t ret;
DEBUG_COMM3("Reader index: %X, Device: %s", reader_index, dev_name);
/* parse dev_name using the pattern "device:name" */
p = strchr(dev_name, ':');
if (p)
{
/* copy the second part of the string */
strlcpy(reader_name, p+1, sizeof(reader_name));
/* replace ':' by '\0' so that dev_name only contains the device name */
*p = '\0';
}
ret = set_ccid_descriptor(reader_index, reader_name, dev_name);
if (STATUS_UNSUCCESSFUL == ret)
return STATUS_UNSUCCESSFUL;
/* secondary slot so do not physically open the device */
if (STATUS_SECONDARY_SLOT == ret)
return STATUS_SUCCESS;
serialDevice[reader].fd = open(dev_name, O_RDWR | O_NOCTTY);
if (-1 == serialDevice[reader].fd)
{
DEBUG_CRITICAL3("open %s: %s", dev_name, strerror(errno));
return STATUS_UNSUCCESSFUL;
}
/* Set RTS signal to low to prevent the smart card reader
* from sending its plug and play string. */
{
int flags;
if (ioctl(serialDevice[reader].fd, TIOCMGET, &flags) < 0)
{
DEBUG_CRITICAL2("Get RS232 signals state failed: %s",
strerror(errno));
}
else
{
flags &= ~TIOCM_RTS;
if (ioctl(serialDevice[reader].fd, TIOCMSET, &flags) < 0)
{
DEBUG_CRITICAL2("Set RTS to low failed: %s", strerror(errno));
}
else
{
DEBUG_COMM("Plug-n-Play inhibition successful");
}
}
}
/* set channel used */
serialDevice[reader].device = strdup(dev_name);
/* empty in and out serial buffers */
if (tcflush(serialDevice[reader].fd, TCIOFLUSH))
DEBUG_INFO2("tcflush() function error: %s", strerror(errno));
/* get config attributes */
if (tcgetattr(serialDevice[reader].fd, ¤t_termios) == -1)
{
DEBUG_INFO2("tcgetattr() function error: %s", strerror(errno));
(void)close(serialDevice[reader].fd);
serialDevice[reader].fd = -1;
return STATUS_UNSUCCESSFUL;
}
/* IGNBRK: ignore BREAK condition on input
* IGNPAR: ignore framing errors and parity errors. */
current_termios.c_iflag = IGNBRK | IGNPAR;
current_termios.c_oflag = 0; /* Raw output modes */
/* CS8: 8-bits character size
* CSTOPB: set two stop bits
* CREAD: enable receiver
* CLOCAL: ignore modem control lines */
current_termios.c_cflag = CS8 | CSTOPB | CREAD | CLOCAL;
/* Do not echo characters because if you connect to a host it or your modem
* will echo characters for you. Don't generate signals. */
current_termios.c_lflag = 0;
#ifndef ANDROID
/* set serial port speed to 115200 bauds */
(void)cfsetspeed(¤t_termios, B115200);
DEBUG_INFO("Set serial port baudrate to 115200 and correct configuration");
#endif
if (tcsetattr(serialDevice[reader].fd, TCSANOW, ¤t_termios) == -1)
{
(void)close(serialDevice[reader].fd);
serialDevice[reader].fd = -1;
DEBUG_INFO2("tcsetattr error: %s", strerror(errno));
return STATUS_UNSUCCESSFUL;
}
/* perform a command to be sure a Gemalto reader is connected
* get the reader firmware */
{
unsigned char tx_buffer[] = { 0x02 };
unsigned char rx_buffer[50];
unsigned int rx_length = sizeof(rx_buffer);
/* 2 seconds timeout to not wait too long if no reader is connected */
serialDevice[reader].ccid.readTimeout = 2*1000;
if (IFD_SUCCESS != CmdEscape(reader_index, tx_buffer, sizeof(tx_buffer),
rx_buffer, &rx_length))
{
DEBUG_CRITICAL("Get firmware failed. Maybe the reader is not connected");
(void)CloseSerial(reader_index);
return STATUS_UNSUCCESSFUL;
}
/* normal timeout: 2 seconds */
serialDevice[reader].ccid.readTimeout = DEFAULT_COM_READ_TIMEOUT ;
rx_buffer[rx_length] = '\0';
DEBUG_INFO2("Firmware: %s", rx_buffer);
}
/* perform a command to configure GemPC Twin reader card movement
* notification to synchronous mode: the card movement is notified _after_
* the host command and _before_ the reader anwser */
{
unsigned char tx_buffer[] = { 0x01, 0x01, 0x01};
unsigned char rx_buffer[50];
unsigned int rx_length = sizeof(rx_buffer);
if (IFD_SUCCESS != CmdEscape(reader_index, tx_buffer, sizeof(tx_buffer),
rx_buffer, &rx_length))
{
DEBUG_CRITICAL("Change card movement notification failed.");
(void)CloseSerial(reader_index);
return STATUS_UNSUCCESSFUL;
}
}
serialDevice[reader_index].ccid.sIFD_serial_number = NULL;
serialDevice[reader_index].ccid.sIFD_iManufacturer = NULL;
serialDevice[reader_index].ccid.IFD_bcdDevice = 0;
return STATUS_SUCCESS;
} /* OpenSerialByName */
/**
* Closes the associated serial port
*/
int Transport::closeComm()
{
CloseSerial(this->serial);
//serial = 0;
return 0;
}
/*! \fn Create::~Create()
* \brief A destructor for Create class. The serial communication is cleaned up here.
*/
Create::~Create()
{
pthread_mutex_destroy(&_serialMutex);
CloseSerial();
}
SerialWindows::~SerialWindows(void)
{
CloseSerial();
}
void main( int argc, char** argv)
{
//struct Position Pos;
int Ix;
strcpy( ConfigFile, "config.dat");
Parity = NoneParity;
DataBits = 8;
StopBits = 1;
CalledByGuideFlag = No;
KeepGoingFlag = No;
ReadSlewFlag = No;
StartScrollFlag = No;
/* if '-k' and '-s' (after full init): slew to Ra, Dec in slew.dat and keep going, exiting when
desired, writing slew_out.dat file;
if '-k' (before full init): (no slew.dat), keep going until centered on init position, write
slew_out.dat file and exit;
if '-s': slew to Ra, Dec in slew.dat and exit, writing slew_out.dat file;
if no '-k' and no '-s': write slew_out.dat file and exit;
if -c, then use following string as configuration file name, ie scope.exe -c config.dat will
result in config.dat being used
if -x, then use following string as scroll file name, and execute scroll file upon program
startup, ie scope.exe -x nan.scr will cause nan.scr to be loaded and started */
/* argv[0] is name of executing program */
for( Ix = 1; Ix < argc; Ix++)
if( argv[Ix][0] == '-')
if( strcmpi( &argv[Ix][1], "GUIDE") == 0)
CalledByGuideFlag = Yes;
else if( argv[Ix][1] == 'k')
KeepGoingFlag = Yes;
else if( argv[Ix][1] == 's')
ReadSlewFlag = Yes;
else if( (argv[Ix][1] == 'c' || argv[Ix][1] == 'C') && Ix < argc-1)
strcpy( ConfigFile, argv[Ix+1]);
else if( (argv[Ix][1] == 'x' || argv[Ix][1] == 'X') && Ix < argc-1)
{
strcpy( ScrollFilename, argv[Ix+1]);
StartScrollFlag = Yes;
}
InitCommonVars();
ReadConfig();
/*
Pos.Ra = Pos.Dec = 0;
applyCorrectionsFromDataFileCoordYearToEpochNow(&Pos);
printf("\n%f %f %f %f %f %f %f %f", Pos.Precession.A*RadToArcsec, Pos.Precession.Z*RadToArcsec,
Pos.Nutation.A*RadToArcsec, Pos.Nutation.Z*RadToArcsec, Pos.AnnualAberration.A*RadToArcsec,
Pos.AnnualAberration.Z*RadToArcsec, Pos.Ra*RadToArcsec, Pos.Dec*RadToArcsec);
ContMsgRoutine();
*/
/*
HsRecFile = fopen( HsRecFilename, "w");
if( HsRecFile == NULL)
BadExit( strcat( "Could not create ", HsRecFilename));
HsRecIx = 0;
*/
/*
InitTimes( DisplayOpeningMsgs, Tz, DST, LongitudeDeg);
InitConvert();
TestConvert();
getch();
TestAltAltAzTrack();
*/
if( DisplayOpeningMsgs)
{
printf( "\nCopyright BBAstroDesigns Inc. 2009\n");
printf( "\nLIMITED WARRANTY This software is provided ``as is'' and any express or");
printf( "\nimplied warranties, including, but not limited to, the implied warranties");
printf( "\nof merchantability and fitness for a particular purpose are disclaimed.");
printf( "\nIn no event shall BBAstroDesigns be liable for any direct, indirect,");
printf( "\nincidental, special, exemplary, nor consequential damages (including, but");
printf( "\nnot limited to, procurement of substitute goods or services, loss of use,");
printf( "\ndate, or profits, or business interruption) however caused and on any");
printf( "\ntheory of liability, whether in contract, strict liability, or tort");
printf( "\n(including negligence or otherwise) arising in any way out of the use of");
printf( "\nthis software, even if advised of the possibility of such damage.\n");
printf( "\nThis software licensed under the GNU GENERAL PUBLIC LICENSE. You may");
printf( "\ndistribute this software per the GNU GPL. See the enclosed gpl.txt.\n\n");
ContMsgRoutine();
}
if( DisplayOpeningMsgs)
printf( "\ncalled by guide: %d, keep_going: %d, read slew.dat file %d",
CalledByGuideFlag, KeepGoingFlag, ReadSlewFlag);
/* if( strcmpi( TestString, "TestSerial") == 0)
{
InitSerial( EncoderComPort, EncoderBaudRate, Parity, DataBits, StopBits);
TestSerial( EncoderComPort);
CloseSerial( EncoderComPort);
} */
/* else if( strcmpi( TestString, "TestVideo") == 0)
{
InitVideo( DisplayOpeningMsgs);
TestVideo();
} */
/* else if( strcmpi( TestString, "TestATimes") == 0)
{
InitTimes( DisplayOpeningMsgs, Tz, DST, LongitudeDeg);
TestTimes();
} */
/* else if( strcmpi( TestString, "TestParallelPort") == 0)
{
InitPPort();
TestPPort();
ClosePPort();
} */
/* else if( strcmpi( TestString, "TestRefract") == 0)
{
InitRefract();
TestRefract();
} */
/* else if( strcmpi( TestString, "TestMouse") == 0)
{
TestMouse();
} */
/* else if( strcmpi( TestString, "TestEncoders") == 0)
{
InitSerial( EncoderComPort, EncoderBaudRate, Parity, DataBits, StopBits);
InitEncoders();
TestEncoders();
CloseSerial( EncoderComPort);
} */
/* else if( strcmpi( TestString, "TestHandpad") == 0)
{
InitPPort();
InitializeHandpad();
TestHandpad();
ClosePPort();
} */
/* else if( strcmpi( TestString, "TestConversion") == 0)
{
InitTimes( DisplayOpeningMsgs, Tz, DST, LongitudeDeg);
InitConvert();
TestConvert();
} */
/* else if( strcmpi( TestString, "TestAltOffset") == 0)
{
InitTimes( DisplayOpeningMsgs, Tz, DST, LongitudeDeg);
InitConvert();
TestAltOffset();
} */
/* else if( strcmpi( TestString, "TestIACA") == 0)
{
InitTimes( DisplayOpeningMsgs, Tz, DST, LongitudeDeg);
TestIACA();
InitIACA();
} */
/* else if( strcmpi( TestString, "WritePWMValues") == 0)
{
InitTimes( DisplayOpeningMsgs, Tz, DST, LongitudeDeg);
InitVideo( DisplayOpeningMsgs);
InitPPort();
InitMotors();
WritePWMValues();
CloseSteppers();
ClosePPort();
}
else */
{
if( strcmpi( TestString, "NoTest") != 0
&& strcmpi( TestString, "PreloadGuidexx.dat") != 0
&& strcmpi( TestString, "Track") != 0)
{
if( DisplayOpeningMsgs)
printf( "\nsetting unrecognized TestString to 'NoTest'");
strcpy( TestString, "NoTest");
}
InitSerial( EncoderComPort, EncoderBaudRate, Parity, DataBits, StopBits);
InitEncoders();
InitMouseControl();
InitTimes( DisplayOpeningMsgs, Tz, DST, LongitudeDeg);
InitVideo( DisplayOpeningMsgs);
InitPPort();
InitializeHandpad();
InitMotors();
InitConvert();
InitRefract();
InitPEC();
InitGuide();
if( strcmpi( TestString, "PreloadGuidexx.dat") == 0)
{
LoadGuideAlts();
LoadGuideAzs();
}
InitIACA();
InitLX200Input();
InitHPEvent();
if( !CalledByGuideFlag ||
(CalledByGuideFlag && (KeepGoingFlag || ReadSlewFlag)))
{
InitKBEvent();
if( ReadSlewFlag)
InputEquatSlewDat();
if( StartScrollFlag)
LoadScrollFileFromFile();
if( strcmpi( TestString, "Track") == 0)
Start2MotorTrackWithDefaultValues();
while( !QuitFlag)
{
SequentialTaskController();
/* GrandTourFlag used to flag next object: set in ProcessHPEventsModeSwitch() */
if( GrandTourLoaded && GrandTourFlag)
ProcessGrandTour();
else
if( ScrollLoaded && ScrollFlag)
ProcessScroll();
else
if( HPPolarAlignLoaded && HPPolarAlignFlag)
ProcessHPPolarAlign();
else
{
if( UseMouseFlag && ProcessMouseEvent())
;
else
if( KeyStroke)
ProcessKBEvents();
else
ProcessHPEvents();
}
}
CloseKBEvent();
if( DisplayOpeningMsgs)
{
AskAndWriteConfig();
WriteLogFile();
}
}
CloseSteppers();
ClosePPort();
CloseEncoderResetLogFile();
CloseSerial( EncoderComPort);
CloseSerial( LX200ComPort);
if( CalledByGuideFlag)
WriteAltazSlewOutFile();
CloseMouseControl();
}
/*
for( Ix = 0; Ix < HsRecSize; Ix++)
fprintf( HsRecFile, "%8ld %8ld\n", HsRec[Ix].A, HsRec[Ix].Z);
// first position is index 0
fprintf( HsRecFile, " last entry in circular queue at position %d", HsRecIx);
fclose( HsRecFile);
*/
}