/********************************************
* 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;
}
Beispiel #2
0
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;
}
Beispiel #3
0
/** \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;
}
Beispiel #6
0
/*
* 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;
}
Beispiel #7
0
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;
}
Beispiel #8
0
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;
}
Beispiel #9
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);
}
Beispiel #10
0
// 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;
}
Beispiel #11
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;
}
Beispiel #12
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);
}
Beispiel #13
0
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
}
Beispiel #14
0
/*
* 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 );
    }
}
Beispiel #16
0
//人工日期转换成 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;    
}
Beispiel #18
0
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 );
            
        }
    }    
}
Beispiel #19
0
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; 
}
Beispiel #21
0
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;
	}

}
Beispiel #22
0
/**
 * 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;
}
Beispiel #23
0
//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;
	}

}
Beispiel #24
0
//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);
}
Beispiel #26
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;
}
Beispiel #27
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;
}
Beispiel #28
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
}
Beispiel #29
0
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
}
Beispiel #30
0
//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);
	}
}