/********************************************
* fn: GPIO 初始化，打开GPIO设备
* return : 0:成功 -1:失败
*********************************************/
static int WatchDogInit(void)
{
int ret = FI_FAIL;
int wdt_ret;
unsigned long timeout;
if(g_wdt_fd <= 0)
{
g_wdt_fd = open( WATCHDOG_DEVICE_PATH, O_RDWR );
if(g_wdt_fd <= 0)
{
ERRORPRINT("failed: WatchDogInit!\r\n");
ret = FI_FAIL;
}
else
{
// 设置复位时间
timeout = WATCHDOG_TIMEOUT/2;
wdt_ret = ioctl( g_wdt_fd, WDIOC_SETTIMEOUT, &timeout);
if(wdt_ret != 0)
{
ERRORPRINT("failed: watch dog SET TIMEOUT ioctl() !\r\n");
}
ret = FI_SUCCESS;
}
}
return ret;
}
ActionResult_enum AutopilotIsReadyToMove()
{
ActionResult_enum result = ACTION_SUCCESS;
//critical section
int s = pthread_mutex_lock(&pilotStateMtx);
if (s != 0)
{
ERRORPRINT("Pilot: pilotStateMtx lock %i", s);
}
if (pilotState == PILOT_STATE_INACTIVE)
{
lastLuaCallReason = "Inactive";
result = ACTION_FAIL;
}
s = pthread_mutex_unlock(&pilotStateMtx);
if (s != 0)
{
ERRORPRINT("Pilot: pilotStateMtx unlock %i", s);
}
//end critical section
return result;
}
/** \brief This function will register a callback function for the
* incoming signals SIGINT and SIGTERM.
* <p>
* All threads implemented in the generic part of the test system
* are blocking incoming signals. Thus you have to deal with
* signals in the main thread. This function will help you to
* do this.
*
* \type global
*
* \param[in] pfHandler Callback function you like to register for
* incoming signals (SIGINT and SIGTERM).
*
* \return <pre>
* BBB_SUCCESS on success
* BBB_SIGNAL_HANDLER if we could not register callback function
* BBB_ERR_PARAM if pfHandler is equal to <CODE>NULL</CODE>
* </pre>
*
******************************************************************************/
BBBError registerExitHandler(void (*pfHandler)(int32_t s32Signal)) {
BBBError error = BBB_SUCCESS;
sigset_t sSigmask;
struct sigaction sSigaction;
if (pfHandler != NULL) {
sigemptyset(&sSigmask);
sigaddset(&sSigmask, SIGTERM);
sigaddset(&sSigmask, SIGINT);
sSigaction.sa_flags = 0;
sSigaction.sa_mask = sSigmask;
sSigaction.sa_handler = pfHandler;
if (sigaction(SIGTERM, &sSigaction, NULL) < 0) {
ERRORPRINT("sigaction() SIGTERM failed");
error = BBB_SIGNAL_HANDLER;
}
if (sigaction(SIGINT, &sSigaction, NULL) < 0) {
ERRORPRINT("sigaction() SIGINT failed");
error = BBB_SIGNAL_HANDLER;
}
} else {
ERRORPRINT("parameter error");
error = BBB_ERR_PARAM;
}
return (error);
}
void WdtServiceStop()
{
int ret;
g_WdtTm.runFlag = 0;
ret = ThreadJoin( g_WdtTm.id, NULL );
if( 0 != ret )
{
ERRORPRINT( "error:ThreadJoin:%s\r\n", STRERROR_ERRNO );
}
if(g_wdt_fd > 0)
{
ret = close(g_wdt_fd);
if(ret < 0)
{
ERRORPRINT("watch dog driver close failed!\n");
}
else
{
g_wdt_fd = -1;
}
}
}
//feed dog
static void *WdtThread( void *arg )
{
int wdt_ret = 1;
SVPrint( "%s start!\r\n", __FUNCTION__ );
while( g_WdtTm.runFlag )
{
if(g_wdt_fd <= 0)
{
ERRORPRINT("failed: watch dog feed fd<0 !\r\n");
}
else
{
wdt_ret = ioctl( g_wdt_fd, WDIOC_KEEPALIVE, (unsigned long)NULL );
if(wdt_ret != 0)
{
ERRORPRINT("failed: watch dog feed ioctl() !\r\n");
}
}
usleep( 5000*1000 );
}
ERRORPRINT( "%s stop!\r\n", __FUNCTION__ );
return NULL;
}
/*
* fn: 获取rtc 时间
*/
int RtcGetTime( int *pyear, int *pmonth, int *pday,
int *phour, int *pminute, int *psecond, int *pweekday )
{
int rtcRet = FI_FAILED;
int rtcHandle = -1;
rtc_time_t tm;
memset( &tm, 0x00, sizeof(tm) );
g_rtcLock.Lock();
rtcHandle = Open( RTC_DEVICE_PATH, O_RDONLY );
if( -1 == rtcHandle )
{
ERRORPRINT( "failed:can't Open rtc dev(%s)!\r\n", RTC_DEVICE_PATH );
rtcRet = FI_FAILED;
}
else
{
if( 0 == ioctl( rtcHandle, RTC_RD_TIME, &tm) )
{
if( 1 == TimeIsValidDatetime(tm.year, tm.month, tm.date,
tm.hour, tm.minute, tm.second) )
{
if(NULL != pyear) *pyear = tm.year;
if(NULL != pmonth) *pmonth = tm.month;
if(NULL != pday) *pday = tm.date;
if(NULL != phour) *phour = tm.hour;
if(NULL != pminute) *pminute = tm.minute;
if(NULL != psecond) *psecond = tm.second;
if(NULL != pweekday) *pweekday = tm.weekday;
rtcRet = FI_SUCCESSFUL;
}
else
{
ERRORPRINT( "TimeIsValidDatetime failed!\r\n" );
}
}
else
{
ERRORPRINT( "ioctl error:%s!\r\n", STRERROR_ERRNO );
}
Close( rtcHandle );
}
g_rtcLock.Unlock();
SVPrint( "%s, rtcRet(%d): %04d-%02d-%02d %02d:%02d:%02d\r\n",
__FUNCTION__, rtcRet,
tm.year, tm.month, tm.date,
tm.hour, tm.minute, tm.second );
return rtcRet;
}
void *ShareMeCopy( void *ptr, const char *file, int line )
{
if ( ptr == NULL )
{
#if 0
Print( "Copy Share Memory Failed, Can't Copy NULL ptr; "
"file: %s, line: %d !\r\n", file, line );
#endif
return NULL;
}
long *pmem = (long *)ptr - 2;
if ( *pmem == (long)SHARE_MEMORY_FLAG )
{
s_ShareMemMutex.Lock();
*(pmem + 1) += 1;
s_ShareMemMutex.Unlock();
return ptr;
}
else
{
ERRORPRINT( "Copy Share Memory Failed, Copy Error ptr; "
"file: %s, line: %d !\r\n", file, line );
}
return NULL;
}
int AutopilotInit() {
pilotDebugFile = fopen_logfile("pilot");
DEBUGPRINT("Autopilot Logfile opened");
//create autopilot thread
pthread_t thread;
int s = pthread_create(&thread, NULL, AutopilotThread, NULL);
if (s != 0) {
ERRORPRINT("Pilot Thread fail %i", s);
return -1;
}
ps_subscribe(TICK_1S_TOPIC, ProcessTickMessage);
ps_subscribe(NAV_TOPIC, ProcessPoseMessage);
ps_subscribe(ODO_TOPIC, ProcessOdometryMessage);
ps_add_event_observer(BATTERY_SHUTDOWN_EVENT, HandleEvent, nullptr);
ps_add_event_observer(PROXIMITY_EVENT, HandleEvent, nullptr);
ps_add_event_observer(SERVOS_STARTED_EVENT, HandleEvent, nullptr);
ps_add_event_observer(SERVOS_STOPPED_EVENT, HandleEvent, nullptr);
ps_add_event_observer(MOTOR_INHIBIT_EVENT, HandleEvent, nullptr);
return 0;
}
// Reads a register
int LSM303_readReg(uint8_t reg)
{
last_status = i2c_smbus_read_byte_data(IMU_FD, reg);
if (last_status < 0)
{
ERRORPRINT("i2c_smbus_read_byte_data from imu %x fail - %s\n", reg, strerror(errno));
return last_status;
}
return (last_status & 0xff);
}
// Writes a register
int LSM303_writeReg(uint8_t reg, uint8_t value)
{
last_status = i2c_smbus_write_byte_data(IMU_FD, reg, value);
if (last_status < 0)
{
ERRORPRINT("i2c_smbus_write_byte_data to imu %x fail - %s\n", reg, strerror(errno));
return -1;
}
else return 0;
}
int ResponderInit()
{
pthread_t thread;
int s = pthread_create(&thread, NULL, ResponderMessageThread, NULL);
if (s != 0)
{
ERRORPRINT("responder: Thread error - %i\n", s);
return -1;
}
return 0;
}
/** \brief Calling this function in a thread will result in blocking
* all incoming signals for the calling thread.
* <p>
* All threads implemented in the system are calling this
* function. Thus you have to deal with signals in the main
* thread.
*
* \type global
*
* \return <pre>
* BBB_SUCCESS on success
* BBB_BLOCK_SIGNAL if we could not block signals for the
* calling thread
* </pre>
*
******************************************************************************/
BBBError blockAllSignalForThread(void) {
BBBError error = BBB_SUCCESS;
sigset_t sSigmask;
sigfillset(&sSigmask);
if (pthread_sigmask(SIG_BLOCK, &sSigmask, NULL) < 0) {
ERRORPRINT("pthread_sigmask() failed");
error = BBB_BLOCK_SIGNAL;
}
return (error);
}
void ProcessPoseMessage(const void *_msg, int len)
{
psMessage_t *rxMessage = (psMessage_t *) _msg;
//critical section
int s = pthread_mutex_lock(&pilotStateMtx);
if (s != 0)
{
ERRORPRINT("Pilot: pilotStateMtx lock %i", s);
}
pose = rxMessage->posePayload;
gettimeofday(&latestPoseTime, NULL);
reviewProgress = true;
s = pthread_mutex_unlock(&pilotStateMtx);
if (s != 0)
{
ERRORPRINT("Pilot: pilotStateMtx unlock %i", s);
}
//end critical section
}
/*
* fn: 设置rtc 时间
*/
int RtcSetTime( int year, int month, int day,
int hour, int minute, int second )
{
int rtcRet = FI_FAILED;
rtc_time_t tm;
int rtcHandle;
g_rtcLock.Lock();
rtcHandle = Open( RTC_DEVICE_PATH, O_WRONLY );
if(-1 == rtcHandle)
{
ERRORPRINT( "failed:can't Open rtc dev(%s)!\r\n", RTC_DEVICE_PATH );
}
else
{
tm.year = year;
tm.month = month;
tm.date = day;
tm.hour = hour;
tm.minute = minute;
tm.second = second;
tm.weekday = 0; //add by liulu,2016-1-6;此项不设置会导致时间设置不成功
rtcRet = ioctl( rtcHandle, RTC_SET_TIME, &tm );
if(rtcRet < 0)
{
ERRORPRINT("RTC ioctl SET time failed.return:%d\n!!!,rtcRet");
}
Close ( rtcHandle );
}
g_rtcLock.Unlock();
SVPrint( "%s, rtcRet(%d): %04d-%02d-%02d %02d:%02d:%02d!\r\n",
__FUNCTION__, rtcRet,
year, month, day,
hour, minute, second, rtcRet );
return rtcRet;
}
void WdtServiceStart()
{
int ret;
WatchDogInit();
g_WdtTm.runFlag = 1;
ret = ThreadCreateSched( &g_WdtTm.id, WdtThread, NULL,
THREAD_SCHED_POLICY, THREAD_PRIORITY_WDT );
if( ret )
{
g_WdtTm.runFlag = 0;
ERRORPRINT( "error:ThreadCreateCommonPriority:%s\r\n", STRERROR_ERRNO );
}
}
//人工日期转换成 LINUX UTC 时间
int FiTimeHumanToUtc( int year, int month, int day, int hour, int minute, int second )
{
time_t ret;
struct tm t;
t.tm_year = year-1900;
t.tm_mon = month-1;
t.tm_mday = day;
t.tm_hour = hour;
t.tm_min = minute;
t.tm_sec = second;
ret = mktime( &t );
if ( ret < 0 )
{
ERRORPRINT( "%04d-%02d-%02d %02d:%02d:%02d\r\n", year, month, day, hour, minute, second );
return 0;
}
return ret;
}
int ConfFileRead( char *pBuf, int len )
{
int fd;
int ret;
g_confFileLock.Lock();
fd = Open( CONFIG_FILE, O_RDONLY );
if( -1 == fd )
{
ERRORPRINT( "Open(%s) failed:%s!\r\n", CONFIG_FILE, STRERROR_ERRNO );
g_confFileLock.Unlock();
return -1;
}
ret = Read( fd, pBuf, len );
ret = ret > 0 ? 0 : -1;
Close( fd );
g_confFileLock.Unlock();
return ret;
}
void ShareMeFree( void *ptr, const char *file, int line )
{
if ( ptr != NULL )
{
long *pmem = (long *)ptr - 2;
if ( *pmem == (long)SHARE_MEMORY_FLAG )
{
s_ShareMemMutex.Lock();
*(pmem + 1) -= 1;
if ( *(pmem + 1) == 0 )
{
free( pmem );
}
s_ShareMemMutex.Unlock();
}
else
{
ERRORPRINT( "Free Share Memory Failed, file: %s, line: %d !\r\n", file, line );
}
}
}
int main(int argc, char *argv[]) {
int i;
UNUSED(argc);
UNUSED(argv);
signal(SIGINT, sigInt);
signal(SIGHUP, sigHup);
printf("\nMakeplate linux C example.\n\n\n");
INFOPRINT("Information printout\n");
DEBUGPRINT("Debug printout\n");
WARNINGPRINT("Warning printout\n");
ERRORPRINT("Error printout\n");
DEBUG_DO(printf("Debug do\n"));
i = 0;
printf("Var i = %2x\n", i);
BIT_SET(i, 4);
printf("Var i = %2x\n", i);
BIT_SET(i, 2);
printf("Var i = %2x\n", i);
BIT_CLEAR(i,4);
printf("Var i = %2x\n", i);
printf("Use CTRL-C to stop program\n");
while(1) {
}
return 0;
}
int ConfFileWrite( char *pBuf, int len )
{
int fd;
int ret;
g_confFileLock.Lock();
fd = Open( CONFIG_FILE, O_CREAT|O_WRONLY|O_TRUNC );
if( -1 == fd )
{
ERRORPRINT( "Open(%s) failed:%s!\r\n", CONFIG_FILE, STRERROR_ERRNO );
g_confFileLock.Unlock();
return -1;
}
ret = Write( fd, pBuf, len );
ret = ret > 0 ? 0 : -1;
Close( fd );
g_confFileLock.Unlock();
return ret;
}
static int PilotAction(lua_State *L)
{
PilotingAction_enum actionCode = (PilotingAction_enum) lua_tointeger(L, 1);
lastLuaCall = pilotingActionList[actionCode];
DEBUGPRINT("Pilot Action: %s ...", lastLuaCall.c_str());
switch (actionCode)
{
case isPilotReady:
return actionReply(L, AutopilotIsReadyToMove());
break;
case ComputeHomePosition:
return fail(L); //actionReply(L, pilotSetGoalWaypoint("Home"));
break;
case ComputeRandomExplorePosition:
return actionReply(L, pilotSetRandomGoal(500)); //1 meter
break;
case ComputeRandomClosePosition:
return actionReply(L, pilotSetRandomGoal(30)); //1 foot
break;
case Orient:
return actionReply(L, AutopilotAction(PILOT_ORIENT));
break;
case Engage:
return actionReply(L, AutopilotAction(PILOT_ENGAGE));
break;
default:
ERRORPRINT("Pilot action: %i", actionCode);
return fail(L);
break;
}
}
/**
* Main function of server applcation that creates the tunnel interface, starts threads and configures the connections
*/
int main(int argc, char **argv)
{
//initializing system io components
init_sysio();
const char* __progname__ = argv[0];
/*Setting up the interrupt and binding it to a handler*/
init_interrupt(&exit_signal, SIGINT, BOOL_FALSE);
set_interrupt_sh(&exit_signal, _exit_handler);
/*The program uses argtable2 implementation for parsing arguments
* More information and examples for argtable2:
*
* http://argtable.sourceforge.net/example/index.html
*
* */
struct arg_lit *help = arg_lit0("h","help","show help");
struct arg_int *time = arg_int0("t", "time", "<n>", "time interval");
struct arg_end *end = arg_end(20);
void* argtable[] = {help, time, end};
int nerrors;
int exitcode=EXIT_SUCCESS;
/* verify the argtable[] entries were allocated sucessfully */
if (arg_nullcheck(argtable) != 0)
{
/* NULL entries were detected, some allocations must have failed */
ERRORPRINT("%s: insufficient memory\n",__progname__);
exitcode=1;
goto exit;
}
/* Parse the command line as defined by argtable[] */
nerrors = arg_parse(argc,argv,argtable);
/* special case: '--help' takes precedence over error reporting */
if (help->count > 0)
{
INFOPRINT("Usage: %s", __progname__);
arg_print_syntax(stdout,argtable,"\n");
arg_print_glossary(stdout,argtable," %-20s %s\n");
exitcode=0;
goto exit;
}
/* If the parser returned any errors then display them and exit */
if (nerrors > 0)
{
/* Display the error details contained in the arg_end struct.*/
arg_print_errors(stdout,end,__progname__);
INFOPRINT("Try '%s --help' for more information.\n",__progname__);
exitcode=1;
goto exit;
}
INFOPRINT("Service build by using devclego version %s.\n\n", DEVCLEGO_VERSION);
/*devclego starts here
* Build the abstract finite state machine by ceating it
* and then running the program.
* After the program proc finished, fsm is doomed.*/
fsm_ctor();
program(time);
fsm_dtor();
exit:
return exitcode;
}
//callback from lua BT Leaf
static int PilotAction(lua_State *L)
{
PilotingAction_enum actionCode = lua_tointeger(L, 1);
lastLuaCall = pilotingActionList[actionCode]; //for error reporting
DEBUGPRINT("Pilot Action: %s\n", lastLuaCall);
switch (actionCode)
{
case isGoodPose:
return actionReply(L, NavigatorIsGoodPose(false));
break;
case isAtWaypoint:
return actionReply(L, pilotIsAtWaypoint());
break;
case WaitForFix:
return success(L);//actionReply(L, NavigatorIsGoodPose(true));
break;
case isPilotReady:
return actionReply(L, AutopilotIsReadyToMove());
break;
case ComputeHomePosition:
return actionReply(L, pilotSetGoalWaypoint("Home"));
break;
case ComputeSolarPosition:
return actionReply(L, pilotSetGoalWaypoint("Solar1"));
break;
case ComputeHeelPosition:
if (heelPositionValid)
{
return actionReply(L, pilotSetGoalPosition(heelPosition));
}
else
{
return fail(L);
}
break;
case ComputeRandomExplorePosition:
return actionReply(L, pilotSetRandomGoal(500)); //1 meter
break;
case ComputeRandomClosePosition:
return actionReply(L, pilotSetRandomGoal(30)); //1 foot
break;
case Orient:
return actionReply(L, AutopilotAction(PILOT_ORIENT));
break;
case Engage:
return actionReply(L, AutopilotAction(PILOT_ENGAGE));
break;
case Turn:
if (drand48() > 0.5)
return actionReply(L, AutopilotAction(PILOT_TURN_LEFT));
else
return actionReply(L, AutopilotAction(PILOT_TURN_RIGHT));
break;
case TurnLeft:
return actionReply(L, AutopilotAction(PILOT_TURN_LEFT));
break;
case TurnRight:
return actionReply(L, AutopilotAction(PILOT_TURN_RIGHT));
break;
case TurnN:
return actionReply(L, AutopilotAction(PILOT_TURN_N));
break;
case TurnS:
return actionReply(L, AutopilotAction(PILOT_TURN_S));
break;
case TurnE:
return actionReply(L, AutopilotAction(PILOT_TURN_E));
break;
case TurnW:
return actionReply(L, AutopilotAction(PILOT_TURN_W));
break;
case TurnLeft90:
return actionReply(L, AutopilotAction(PILOT_TURN_LEFT_90));
break;
case TurnRight90:
return actionReply(L, AutopilotAction(PILOT_TURN_RIGHT_90));
break;
case MoveForward:
return actionReply(L, AutopilotAction(PILOT_MOVE_FORWARD));
break;
case MoveBackward:
return actionReply(L, AutopilotAction(PILOT_MOVE_BACKWARD));
break;
case MoveForward10:
return actionReply(L, AutopilotAction(PILOT_MOVE_FORWARD_10));
break;
case MoveBackward10:
return actionReply(L, AutopilotAction(PILOT_MOVE_BACKWARD_10));
break;
case SetFastSpeed:
return actionReply(L, AutopilotAction(PILOT_MOVE_BACKWARD));
break;
case SetMediumSpeed:
return actionReply(L, AutopilotAction(PILOT_MOVE_BACKWARD));
break;
case SetLowSpeed:
return actionReply(L, AutopilotAction(PILOT_MOVE_BACKWARD));
break;
case EnableFrontContactStop:
pilotFlags |= ENABLE_FRONT_CONTACT_ABORT;
return success(L);
break;
case DisableFrontContactStop:
pilotFlags &= ~ENABLE_FRONT_CONTACT_ABORT;
return success(L);
break;
case EnableFrontCloseStop:
pilotFlags |= ENABLE_FRONT_CLOSE_ABORT;
return success(L);
break;
case DisableFrontCloseStop:
pilotFlags &= ~ENABLE_FRONT_CLOSE_ABORT;
return success(L);
break;
case EnableRearContactStop:
pilotFlags |= ENABLE_REAR_CONTACT_ABORT;
return success(L);
break;
case DisableRearContactStop:
pilotFlags &= ~ENABLE_REAR_CONTACT_ABORT;
return success(L);
break;
case EnableRearCloseStop:
pilotFlags |= ENABLE_REAR_CLOSE_ABORT;
return success(L);
break;
case DisableRearCloseStop:
pilotFlags &= ~ENABLE_REAR_CLOSE_ABORT;
return success(L);
break;
case EnableFrontFarStop:
pilotFlags |= ENABLE_FRONT_FAR_ABORT;
return success(L);
break;
case DisableFrontFarStop:
pilotFlags &= ~ENABLE_FRONT_FAR_ABORT;
return success(L);
break;
case EnableWaypointStop:
pilotFlags |= ENABLE_WAYPOINT_STOP;
return success(L);
break;
case DisableWaypointStop:
pilotFlags &= ~ENABLE_WAYPOINT_STOP;
return success(L);
break;
case EnableLostFixStop:
pilotFlags |= ENABLE_LOSTFIX_ABORT;
return success(L);
break;
case DisableLostFixStop:
pilotFlags &= ~ENABLE_LOSTFIX_ABORT;
return success(L);
break;
default:
ERRORPRINT("Nav action: %i\n", actionCode);
SetCondition(BT_SCRIPT_ERROR);
return fail(L);
break;
}
}
//Tx thread function
void *AgentTxThread(void *arg)
{
int errorReply = 0;
int checksum;
uint8_t msgSequence[MAX_AGENT_CONNECTIONS];
for (int i=0; i<MAX_AGENT_CONNECTIONS; i++) msgSequence[i] = 0;
//loop
while (1)
{
//wait for next message
psMessage_t *txMessage = GetNextMessage(&agentQueue);
for (int i=0; i<MAX_AGENT_CONNECTIONS; i++)
{
if (connected[i])
{
pthread_mutex_lock(&agentMtx);
int socket = txSocket[i];
//send STX
writeToSocket( socket, STX_CHAR, &checksum, &errorReply);
checksum = 0; //checksum starts from here
//send header
writeToSocket( socket, txMessage->header.length, &checksum, &errorReply);
writeToSocket( socket, ~txMessage->header.length, &checksum, &errorReply);
writeToSocket( socket, msgSequence[i]++, &checksum, &errorReply);
writeToSocket( socket, txMessage->header.source, &checksum, &errorReply);
writeToSocket( socket, txMessage->header.messageType, &checksum, &errorReply);
//send payload
uint8_t *buffer = (uint8_t *) &txMessage->packet;
uint8_t size = txMessage->header.length;
if (size > sizeof(psMessage_t) - SIZEOF_HEADER)
{
size = txMessage->header.length = sizeof(psMessage_t) - SIZEOF_HEADER;
}
while (size) {
writeToSocket( socket, *buffer, &checksum, &errorReply);
buffer++;
size--;
}
//write checksum
writeToSocket( socket, (checksum & 0xff), &checksum, &errorReply);
if (errorReply != 0)
{
ERRORPRINT("agent: Tx send error: %s\n", strerror(errno));
AGENTsendErrors;
close(socket);
txSocket[i] = -1;
connected[i] = false;
}
else
{
DEBUGPRINT("agent: %s message sent\n", psLongMsgNames[txMessage->header.messageType]);
AGENTmessagesSent++;
if (psDefaultTopics[txMessage->header.messageType] == LOG_TOPIC)
{
switch (txMessage->logPayload.severity)
{
case SYSLOG_INFO:
agentInfo--;
break;
case SYSLOG_WARNING:
agentWarning--;
break;
case SYSLOG_ERROR:
agentError--;
default:
break;
}
}
}
pthread_mutex_unlock(&agentMtx);
}
}
DoneWithMessage(txMessage);
}
return 0;
}
/*
int SVOsdSetAttr(int type,RGN_HANDLE handle,const MPP_CHN_S *pstChn,const RGN_CHN_ATTR_S *pstChnAttr)
{
int ret = FI_FAILED;
if(VI == type)
ret = SVOsdViSetAttr(handle,pstChn,pstChnAttr);
return ret;
}
int SVOsdGetAttr(int type,RGN_HANDLE handle,const MPP_CHN_S *pstChn,RGN_CHN_ATTR_S *pstChnAttr)
{
int ret = FI_FAILED;
if(VI == type)
ret = SVOsdViGetAttr(handle,pstChn,pstChnAttr);
return ret;
}
*/
int SVOsdInitFont(void)
{
int fd;
int ret = 0;
struct stat st;
unsigned int file_size = 0;
if(g_asc8x16Buf == NULL)
{
if ((fd = open(FONT_8EN_FILE ,O_RDONLY)) < 0)
{
ERRORPRINT("open(%s) error:%s\r\n",FONT_8EN_FILE,STRERROR_ERRNO);
return(-1);
}
if(fstat(fd, &st) != 0)
{
ERRORPRINT("fstat %s failed!\r\n",FONT_8EN_FILE);
close(fd);
return -1;
}
file_size = st.st_size;
if((g_asc8x16Buf = (unsigned char *)Calloc(1,file_size)) == NULL)
{
ERRORPRINT("g_asc8x16Buf Malloc \r\n");
close(fd);
return(-1);
}
ret = Readn(fd, (char *)g_asc8x16Buf, file_size);
if(-1 == ret)
{
ERRORPRINT("read() error:%s\r\n",STRERROR_ERRNO);
close(fd);
return(-1);
}
close(fd);
}
if(g_ch16x16Buf == NULL)
{
if ((fd = open(FONT_16CH_FILE, O_RDONLY)) < 0)
{
ERRORPRINT("open(%s) error:%s\r\n",FONT_16CH_FILE,STRERROR_ERRNO);
return(-1);
}
if(fstat(fd, &st) != 0)
{
ERRORPRINT("fstat %s failed!\r\n",FONT_16CH_FILE);
close(fd);
return -1;
}
file_size = st.st_size;
if ((g_ch16x16Buf = (unsigned char *)Calloc(1,file_size)) == NULL)
{
ERRORPRINT("g_asc_16x32_buf Malloc\r\n");
close(fd);
return(-1);
}
ret = Readn(fd, (char *)g_ch16x16Buf, file_size);
if(-1 == ret)
{
ERRORPRINT("read FONT_32CH_FILE\r\n");
close(fd);
return(-1);
}
close(fd);
}
return(0);
}
//thread function for Rx
void *AgentRxThread(void *arg)
{
int mychan = (int) arg;
int socket = rxSocket[mychan];
psMessage_t rxMessage;
status_t parseStatus;
DEBUGPRINT("agent: Rx %i thread: fd=%i\n",mychan, socket);
parseStatus.noCRC = 0; ///< Do not expect a CRC, if > 0
parseStatus.noSeq = 0; ///< Do not check seq #s, if > 0
parseStatus.noLength2 = 0; ///< Do not check for duplicate length, if > 0
parseStatus.noTopic = 1; ///< Do not check for topic ID, if > 0
ResetParseStatus(&parseStatus);
agentOnline++;
connected[mychan] = true;
while(connected[mychan])
{
uint8_t readByte;
int result;
do {
if (recv(socket, &readByte, 1, 0) <= 0)
{
//quit on failure, EOF, etc.
ERRORPRINT("agent: Rx %i recv(fd=%i) error: %s\n", mychan, socket, strerror(errno));
AGENTreceiveErrors++;
pthread_mutex_lock(&agentMtx);
close(socket);
rxSocket[mychan] = -1;
close(txSocket[mychan]);
txSocket[mychan] = -1;
connected[mychan] = false;
agentOnline--;
pthread_mutex_unlock(&agentMtx);
pthread_exit(NULL);
return 0;
}
result = ParseNextCharacter(readByte, &rxMessage, &parseStatus);
} while (result == 0);
if (rxMessage.header.messageType == KEEPALIVE)
{
if (loopback)
{
rxMessage.header.source = 0;
CopyMessageToQ(&agentQueue, &rxMessage);
}
else
{
rxMessage.header.source = APP_XBEE;
XBeeBrokerProcessMessage(&rxMessage);
}
}
else
{
DEBUGPRINT("agent: Rx %i. %s message received\n", mychan, psLongMsgNames[rxMessage.header.messageType]);
AGENTmessagesReceived++;
rxMessage.header.source = APP_XBEE;
RouteMessage(&rxMessage);
}
}
//Tx disconnected
DEBUGPRINT("agent: Rx %i exiting.\n", mychan);
pthread_mutex_lock(&agentMtx);
close(socket);
rxSocket[mychan] = -1;
agentOnline--;
pthread_mutex_unlock(&agentMtx);
pthread_exit(NULL);
return 0;
}
//thread to listen for connect requests
void *AgentListenThread(void *arg)
{
pthread_t rxThread[MAX_AGENT_CONNECTIONS];
struct sockaddr client_address;
socklen_t client_address_len;
//initialize the data structures
{
for (int i=0; i<MAX_AGENT_CONNECTIONS; i++)
{
rxSocket[i] = -1;
txSocket[i] = -1;
rxThread[i] = (pthread_t) -1;
connected[i] = false;
}
}
DEBUGPRINT("agent: Listen thread\n");
//create listen socket
int listenSocket;
while ((listenSocket = socket(AF_INET, SOCK_STREAM, 0)) == -1)
{
ERRORPRINT("agent: socket() error: %s\n", strerror(errno));
sleep(1);
}
int optval = 1;
setsockopt(listenSocket, SOL_SOCKET, SO_REUSEADDR, &optval, 4);
DEBUGPRINT("agent: listen socket created\n");
//bind socket address
struct sockaddr_in my_address;
memset(&my_address, 0, sizeof(my_address));
my_address.sin_family = AF_INET;
my_address.sin_port = htons(LISTEN_PORT_NUMBER);
my_address.sin_addr.s_addr = INADDR_ANY;
while (bind(listenSocket, (struct sockaddr*) &my_address, sizeof(my_address)) == -1)
{
ERRORPRINT("agent: bind() error: %s\n", strerror(errno));
if (errno == EADDRINUSE) sleep(10);
sleep(1);
}
DEBUGPRINT("agent: listen socket ready\n");
while(1)
{
//wait for connect
while(listen(listenSocket, 2) != 0)
{
ERRORPRINT("agent: listen() error %s\n", strerror(errno));
sleep(1);
//ignore errors, just retry
}
client_address_len = sizeof(client_address);
int acceptSocket = accept(listenSocket, &client_address, &client_address_len);
if (acceptSocket >= 0)
{
//print the address
struct sockaddr_in *client_address_in = (struct sockaddr_in *) &client_address;
uint8_t addrBytes[4];
memcpy(addrBytes, &client_address_in->sin_addr.s_addr, 4);
DEBUGPRINT("agent: connect from %i.%i.%i.%i\n", addrBytes[0], addrBytes[1], addrBytes[2], addrBytes[3]);
//find a free thread
int i;
int channel = -1;
for (i=0; i<MAX_AGENT_CONNECTIONS; i++)
{
if (!connected[i])
{
channel = i;
break;
}
}
if (channel < 0)
{
pthread_mutex_unlock(&agentMtx);
DEBUGPRINT("agent: no available server context\n");
}
else
{
rxSocket[channel] = acceptSocket;
txSocket[channel] = dup(acceptSocket); //separate rx & tx sockets
//create agent Rx thread
int s;
do {
s = pthread_create(&rxThread[channel], NULL, AgentRxThread, (void*) channel);
if (s == EAGAIN) sleep(1);
}
while (s == EAGAIN);
if (s != 0) {
LogError("agent: Rx %i create failed - %s\n", channel, strerror(s));
close(rxSocket[channel]);
close(txSocket[channel]);
}
else
{
DEBUGPRINT("agent: Rx %i thread created.\n", channel);
}
}
}
}
return 0;
}
void ProcessTickMessage(const void *_msg, int len)
{
//critical section
int s = pthread_mutex_lock(&pilotStateMtx);
if (s != 0)
{
ERRORPRINT("Pilot: pilotStateMtx lock %i", s);
}
//make pilot available
if ((pilotState == PILOT_STATE_INACTIVE) && moveOK && !motorsInhibit)
{
pilotState = PILOT_STATE_IDLE;
LogInfo("Pilot Available");
}
reviewProgress = true;
switch (pilotState)
{
case PILOT_STATE_FORWARD_SENT:
case PILOT_STATE_BACKWARD_SENT:
case PILOT_STATE_ORIENT_SENT:
case PILOT_STATE_MOVE_SENT:
MOVE_XXX_time = 0;
if (MOVE_XXX_SENT_time == 0)
MOVE_XXX_SENT_time = time(NULL);
else
{
if (MOVE_XXX_SENT_time + motorsStartTimeout < time(NULL))
{
CancelPilotOperation(PILOT_STATE_FAILED);
lastLuaCallReason = "StartTO";
MOVE_XXX_SENT_time = 0;
LogWarning("Motors Start TO");
}
}
break;
case PILOT_STATE_FORWARD:
case PILOT_STATE_BACKWARD:
case PILOT_STATE_ORIENT:
case PILOT_STATE_MOVE:
MOVE_XXX_SENT_time = 0;
if (MOVE_XXX_time == 0)
MOVE_XXX_time = time(NULL);
else
{
if (MOVE_XXX_time + motorsRunTimeout < time(NULL))
{
CancelPilotOperation(PILOT_STATE_FAILED);
lastLuaCallReason = "RunTO";
MOVE_XXX_time = 0;
LogWarning("Motors Run TO");
}
}
break;
default:
MOVE_XXX_time = 0;
MOVE_XXX_SENT_time = 0;
break;
}
s = pthread_mutex_unlock(&pilotStateMtx);
if (s != 0)
{
ERRORPRINT("Pilot: pilotStateMtx unlock %i", s);
}
//end critical section
}
void HandleEvent(void *arg, ps_event_id_t event)
{
//critical section
int s = pthread_mutex_lock(&pilotStateMtx);
if (s != 0)
{
ERRORPRINT("Pilot: pilotStateMtx lock %i", s);
}
switch(event)
{
case BATTERY_SHUTDOWN_EVENT:
CancelPilotOperation(PILOT_STATE_INACTIVE);
break;
case PROXIMITY_EVENT:
CancelPilotOperation(PILOT_STATE_ABORT);
break;
case SERVOS_STARTED_EVENT:
switch (pilotState)
{
case PILOT_STATE_FORWARD_SENT:
pilotState = PILOT_STATE_FORWARD;
LogRoutine("Pilot Move Fwd Started");
break;
case PILOT_STATE_BACKWARD_SENT:
pilotState = PILOT_STATE_BACKWARD;
LogRoutine("Pilot Move Back Started");
break;
case PILOT_STATE_ORIENT_SENT:
pilotState = PILOT_STATE_ORIENT;
LogRoutine("Pilot Orient Started");
break;
case PILOT_STATE_MOVE_SENT:
pilotState = PILOT_STATE_MOVE;
LogRoutine("Pilot Move Started");
break;
default:
break;
}
break;
case SERVOS_STOPPED_EVENT:
switch (pilotState)
{
case PILOT_STATE_FORWARD:
case PILOT_STATE_BACKWARD:
pilotState = PILOT_STATE_DONE;
LogRoutine("Pilot Move Done");
break;
case PILOT_STATE_ORIENT:
{
pilotState = PILOT_STATE_DONE;
LogRoutine("D/R Orient Done");
}
break;
case PILOT_STATE_MOVE:
reviewProgress = true;
default:
break;
}
break;
case MOTOR_INHIBIT_EVENT:
if (CancelPilotOperation(PILOT_STATE_INACTIVE))
{
lastLuaCallReason = "MotInhibit";
LogInfo("Pilot Motor Inhibit Stop");
}
break;
break;
default:
break;
}
s = pthread_mutex_unlock(&pilotStateMtx);
if (s != 0)
{
ERRORPRINT("Pilot: pilotStateMtx unlock %i", s);
}
//end critical section
}
//thread to send updates to the motor processor
void *AutopilotThread(void *arg) {
int priorPilotState; //used to cancel notifications
// PowerState_enum powerState = POWER_STATE_UNKNOWN;
DEBUGPRINT("Pilot thread ready");
//loop
while (1) {
//critical section
int s = pthread_mutex_lock(&pilotStateMtx);
if (s != 0)
{
ERRORPRINT("Pilot: pilotStateMtx lock %i", s);
}
priorPilotState = pilotState; //track state for Conditions
if (reviewProgress & ~motorsBusy)
{
//review what's happening
switch (pilotState)
{
//Orienting is just a turn to a specific compass direction
case PILOT_STATE_ORIENT:
PerformOrient();
break;
case PILOT_STATE_MOVE:
PerformMovement();
break;
default:
break;
}
}
//update notifications
if (priorPilotState != pilotState) {
LogRoutine("Pilot State: %s", pilotStateNames[pilotState]);
switch (priorPilotState) {
case PILOT_STATE_IDLE:
case PILOT_STATE_INACTIVE:
break;
case PILOT_STATE_FORWARD:
case PILOT_STATE_BACKWARD:
case PILOT_STATE_ORIENT:
case PILOT_STATE_MOVE:
ps_cancel_condition(PILOT_ENGAGED);
break;
case PILOT_STATE_DONE:
ps_cancel_condition(PILOT_IDLE);
break;
case PILOT_STATE_FAILED:
ps_cancel_condition(PILOT_FAILED);
break;
}
switch (pilotState) {
case PILOT_STATE_IDLE:
case PILOT_STATE_INACTIVE:
ps_cancel_condition(PILOT_ENGAGED);
break;
case PILOT_STATE_FORWARD_SENT:
case PILOT_STATE_BACKWARD_SENT:
case PILOT_STATE_ORIENT_SENT:
case PILOT_STATE_MOVE_SENT:
case PILOT_STATE_FORWARD:
case PILOT_STATE_BACKWARD:
case PILOT_STATE_ORIENT:
case PILOT_STATE_MOVE:
ps_set_condition(PILOT_ENGAGED);
break;
case PILOT_STATE_DONE:
ps_set_condition(PILOT_IDLE);
break;
case PILOT_STATE_FAILED:
case PILOT_STATE_ABORT:
ps_set_condition(PILOT_FAILED);
break;
}
}
s = pthread_mutex_unlock(&pilotStateMtx);
if (s != 0)
{
ERRORPRINT("Pilot: pilotStateMtx unlock %i", s);
}
//end critical section
usleep(100000);
}
}