LOCAL_C TInt SendDelimiterL(RBusDevComm& aSerial,const TDesC8& aFormatter,const TPtrC& aLogFile)
{
TBuf8<KSerialLogBufferSize> aLogFileDes8;
for (TInt i=0;i<aLogFile.Length();i++)
aLogFileDes8.Append((TChar)aLogFile[i]);
TBuf8<KSerialLogBufferSize> buffer;
buffer.Format(aFormatter,&aLogFileDes8);
return(SendSerialData(aSerial,buffer));
}
LOCAL_C void doSendTerminateL(TInt aPort)
{
// Open serial port.
RBusDevComm serial;
doOpenSerialL(serial,aPort);
SendSerialData(serial, _L8("!TERMINATE_MSG_LOGGING!"));
// Close serial port.
serial.Close();
}
LOCAL_C void doSendSerialL(const TPtrC& aLogFile, TInt aPort)
{
TInt error;
RFs aFs;
User::LeaveIfError(aFs.Connect());
// Open logfile.
RFile logFile;
test.Printf(_L("\nSending file: %S.\n"),&aLogFile);
User::LeaveIfError(logFile.Open(aFs,aLogFile,EFileRead));
// Open serial port.
RBusDevComm serial;
doOpenSerialL(serial, aPort);
// Send the start delimiter.
TBuf8<KSerialLogBufferSize> buffer;
error=SendDelimiterL(serial,KSerialLogStartDelimiterFormat,aLogFile);
// Send the logfile.
while (!error)
{
error=logFile.Read(buffer);
if (!error && buffer.Length())
error=SendSerialData(serial,buffer);
if (!buffer.Length())
error=KErrEof;
}
// Any errors?
if (error==KErrEof)
error=KErrNone;
// Send the stop delimiter.
if (!error)
error=SendDelimiterL(serial,KSerialLogStopDelimiterFormat,aLogFile);
User::After(1000000);
// Close serial port.
serial.Close();
// Close logfile.
logFile.Close();
User::LeaveIfError(error);
}
//
// OpenSerialPort()
// Open the serial port on COM3 to the Arduino.
//
int OpenSerialPort (void)
{
if( serialFD != -1 )
return 1;
// Set up the serial port
serialFD = serialport_init( "COM3", 9600 );
if( serialFD == -1 ) {
if( serialOpenRetry )
; // TODO: Start the retry timer
return 0;
}
// Send an initialization command
SendSerialData( 0, 0.5f );
return 1;
}
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 TestDialog_2::OnBnClickedButton2()
{
SendSerialData();
}
