static void q_debug_print(q_t *q)
{
/* #define Q_DEBUG_PRINT */
#ifdef Q_DEBUG_PRINT
int ii = 0;
q_node_t *node = NULL;
LOG_PRINTF(INFO, "Queue of %d nodes at 0x%08lx: ", q->len, ((long) q));
if (q == NULL)
{
LOG_PRINTF(INFO, "\n");
return;
}
node = q->head;
for (ii = 0; ii < q->len; ii++)
{
LOG_PRINTF(INFO, "<- (%d) [ p: 0x%08lx, q: 0x%08lx, d: 0x%08lx, n: 0x%08lx ] -> ",
ii,
(long) node->prev,
(long) node->q,
(long) node->data,
(long) node->next);
if (node->next == NULL)
{
break;
}
node = node->next;
}
LOG_PRINTF("\n");
#endif /* Q_DEBUG_PRINT */
return;
}
int main(int argc, char **argv)
{
int ret = 0;
char *grid = NULL;
char *dict_name = NULL;
word_list_t wl;
/* Simple args check. */
if (argc != 3)
{
usage(argv[0]);
return(E_ARGS);
}
else
{
dict_name = argv[1];
grid = argv[2];
}
wl.fname = dict_name;
wl.filters = grid;
ret = wl_read_list(&wl);
RET_MSG_ON_ERR(ret, "Dictionary file access error.\n");
LOG_PRINTF(INFO, "Starting up...\n");
ret = wg_word_build(grid, &wl);
RET_MSG_ON_ERR(ret, "Error %d finding words in given input.\n", ret);
LOG_PRINTF(INFO, "Done.\n");
return(ret);
}
void usart_call_cmd(struct usart_commands * commands)
{
uint32_t i = 0;
if(!usart_read_command()) {
return;
}
if (!command_len) {
LOG_PRINTF("#2");
return;
}
i=0;
while(commands[i].cmd != NULL) {
if (!strcmp((char*)command, (char*)commands[i].cmd)) {
if (commands[i].callback) {
if(command_argindex == command_len) {
commands[i].callback(NULL);
} else {
commands[i].callback(&command[command_argindex]);
}
}
LOG_PRINTF("\n>>");
command_len = 0;
command_argindex = 0;
return;
} else {
}
i++;
}
command_len = 0;
command_argindex = 0;
LOG_PRINTF("INVALID COMMAND\n>>");
}
static void event(usbh_device_t *dev, usbh_packet_callback_data_t status)
{
midi_device_t *midi = (midi_device_t *)dev->drvdata;
switch (midi->state) {
case 26:
{
switch (status.status) {
case USBH_PACKET_CALLBACK_STATUS_OK:
midi_in_message(midi, midi->endpoint_in_maxpacketsize);
midi->state = 25;
break;
case USBH_PACKET_CALLBACK_STATUS_ERRSIZ:
midi_in_message(midi, status.transferred_length);
midi->state = 25;
break;
case USBH_PACKET_CALLBACK_STATUS_EFATAL:
case USBH_PACKET_CALLBACK_STATUS_EAGAIN:
LOG_PRINTF("FATAL ERROR, MIDI DRIVER DEAD \n");
//~ dev->drv->remove();
midi->state = 0;
break;
}
}
break;
case 102:
{
midi->state = 101;
LOG_PRINTF("\n CAN'T TOUCH THIS... ignoring data\n");
}
break;
case 2:
{
LOG_PRINTF("|empty packet read|");
if (status.status == USBH_PACKET_CALLBACK_STATUS_OK) {
midi->state++;
device_xfer_control_read(0, 0, event, dev);
}
}
break;
case 3: // Configured
{
if (status.status == USBH_PACKET_CALLBACK_STATUS_OK) {
midi->state = 100;
midi->endpoint_in_toggle = 0;
LOG_PRINTF("\nMIDI CONFIGURED\n");
// Notify user
if (midi_config->notify_connected) {
midi_config->notify_connected(midi->device_id);
}
}
}
break;
default:
break;
}
}
shared_ptr<action::Action> FixedAngleTrace::control(const string& taskName)
{
float simTime = WM.getSimTime();
LOG_PRINTF("task","current time: %f", simTime);
//cout<<"TaskName: "<<taskName<<endl;
if( taskName!=mTaskName && true==mTask->changeable )
{
mEntryTaskName=taskName;
setCurrentTask(taskName);
LOG_PRINT("task"," start new task "+mTaskName);
LOG_PRINT("task","current pos is "+getCurrentPose());
LOG_FLUSH;
//return Timing::control(WM.predictedPerception(), mDesiredPose, mTask->time);
return Timing::control(WM.lastPerception(), mDesiredPose, mTask->time,taskName); //TT 1
}
float remainTime = mTaskBeginTime + mTask->time - simTime;
if ( remainTime > 0.005f )
{
LOG_PRINT("task"," continue task "+mTaskName);
LOG_PRINT("task","current pos is "+getCurrentPose());
LOG_FLUSH;
shared_ptr<action::Action> act
//= Timing::control(WM.predictedPerception(), mDesiredPose, remainTime);
= Timing::control(WM.lastPerception(), mDesiredPose, remainTime,taskName); //TT 2
int adjustFoot = mTask->adjustFoot;
if ( mFootExchanged ) adjustFoot = -adjustFoot;
// FOOT_ADJUSTER.adjust( adjustFoot, shared_static_cast<action::JointAction>(act) );
mIsFinished = false;
return act;
}
// the task should be finished
//// if no next task
if ( "null" == mTask->next )
{
//cout<<" stop at task "<<mTaskName<<' '<<mTask->next<<'\n';
mIsFinished = true;
mTaskName = "null";
//mEntryTaskName = "null";
LOG_PRINTF("task","do null");
LOG_FLUSH;
//return Timing::control(WM.predictedPerception(),mDesiredPose,(20.0f * serversetting::sim_step));
return Timing::control(WM.lastPerception(),mDesiredPose,(20.0f * serversetting::sim_step),taskName); //TT 3
}
//// start next task
setCurrentTask( mTask->next );
mIsStartNextTask = true;
LOG_PRINT("task"," start next task "+mTaskName);
LOG_FLUSH;
//return Timing::control(WM.predictedPerception(), mDesiredPose, mTask->time);
return Timing::control(WM.lastPerception(), mDesiredPose, mTask->time,taskName); //TT 4
}
int main(void)
{
clock_setup();
gpio_setup();
// provides time_curr_us to usbh_poll function
tim6_setup();
#ifdef USART_DEBUG
usart_init(USART6, 921600);
#endif
LOG_PRINTF("\n\n\n\n\n###################\nInit\n");
/**
* device driver initialization
*
* Pass configuration struct where the callbacks are defined
*/
hid_driver_init(&hid_config);
hub_driver_init();
gp_xbox_driver_init(&gp_xbox_config);
midi_driver_init(&midi_config);
gpio_set(GPIOD, GPIO13);
/**
* Pass array of supported low level drivers
* In case of stm32f407, there are up to two supported OTG hosts on one chip.
* Each one can be enabled or disabled in usbh_config.h - optimization for speed
*
* Pass array of supported device drivers
*/
usbh_init(lld_drivers, device_drivers);
gpio_clear(GPIOD, GPIO13);
LOG_PRINTF("USB init complete\n");
LOG_FLUSH();
while (1) {
// set busy led
gpio_set(GPIOD, GPIO14);
uint32_t time_curr_us = tim6_get_time_us();
usbh_poll(time_curr_us);
// clear busy led
gpio_clear(GPIOD, GPIO14);
LOG_FLUSH();
// approx 1ms interval between usbh_poll()
delay_ms_busy_loop(1);
}
return 0;
}
void init()
{
open(DEV_CONSOLE, O_RDWR);
printf("");
LOG_INIT("Leaks", LOGSYS_COMM, 0xffffffff);
LOG_PRINTF(("Leaks started"));
LOG_PRINTF((""));
}
/* Returns 0 on success, or a negative error code on failure. */
int fd32_chdir(/*const */char *DirName)
{
int Res;
char Aux[FD32_LFNPMAX];
char Drive[FD32_LFNMAX];
char *Path;
fd32_request_t *request;
void *DeviceId;
fd32_openfile_t Of;
fd32_close_t C;
tCds *D;
tCds **CdsList = (tCds **) fd32_get_cdslist();
LOG_PRINTF(("[CHDIR] In:\"%s\"\n", DirName));
/* Open the directory to check if it is a valid directory */
if ((Res = fd32_truename(Aux, DirName, FD32_TNSUBST)) < 0) return Res;
for (;;)
{
Res = fd32_get_drive(Aux, &request, &DeviceId, &Path);
if (Res < 0) return Res;
Of.Size = sizeof(fd32_openfile_t);
Of.DeviceId = DeviceId;
Of.FileName = Path;
Of.Mode = O_RDONLY | O_DIRECTORY;
Res = request(FD32_OPENFILE, &Of);
if (Res == FD32_OROPEN) break;
if (Res != -ENOTMOUNT) return Res;
}
/* If we arrive here, the directory is valid */
C.Size = sizeof(fd32_close_t);
C.DeviceId = Of.FileId;
request(FD32_CLOSE, &C);
if ((Res = fd32_truename(Aux, DirName, FD32_TNDOTS)) < 0) return Res;
for (Res = 0; Aux[Res] != ':'; Drive[Res] = Aux[Res], Res++);
Drive[Res] = 0;
LOG_PRINTF(("[CHDIR] Setting the current dir of \"%s\" to \"%s\"\n", Drive, &Aux[Res + 1]));
/* Search for the specified drive in the CDS list of the current process */
for (D = *CdsList; D; D = D->Next)
if (strcasecmp(D->Drive, Drive) == 0)
{
strcpy(D->CurDir, &Aux[Res + 1]);
break;
}
/* If no CDS is present for the specified drive, add the entry */
if ((D = (void *)mem_get(sizeof(tCds))) == NULL) return -ENOMEM;
D->Next = *CdsList;
strcpy(D->Drive, Drive);
strcpy(D->CurDir, &Aux[Res + 1]);
*CdsList = D;
return 0;
}
int sniff_icmp_fromwire(const byte *packet, size_t length) {
const struct icmp *header = (struct icmp *)packet;
LOG_PRINTF(ICMP, "-- ICMP (%lu bytes)\n", length);
if (length < ICMP_MINLEN || header->icmp_type > ICMP_MAXTYPE) {
LOG_PRINTF_INDENT(ICMP, 2, "\tinvalid packet\n");
return -1;
}
LOG_PRINTF_INDENT(ICMP, 2, "\ttype : %u\n", header->icmp_type); // type of message
LOG_PRINTF_INDENT(ICMP, 2, "\tcode : %u\n", header->icmp_code); // type sub code
LOG_PRINTF_INDENT(ICMP, 2, "\tcksum : %u\n", ntohs(header->icmp_cksum)); // ones complement cksum of struct
if (header->icmp_type == ICMP_ECHOREPLY || header->icmp_type == ICMP_ECHO) {
LOG_PRINTF_INDENT(ICMP, 2, "\tid : %u\n", ntohs(header->icmp_id));
LOG_PRINTF_INDENT(ICMP, 2, "\tseq : %u\n", ntohs(header->icmp_seq));
} else if (header->icmp_type == ICMP_UNREACH) {
if (header->icmp_code == ICMP_UNREACH_NEEDFRAG) {
LOG_PRINTF_INDENT(ICMP, 2, "\tpmvoid : %u\n", ntohs(header->icmp_pmvoid));
LOG_PRINTF_INDENT(ICMP, 2, "\tnextmtu: %u\n", ntohs(header->icmp_nextmtu));
} else {
LOG_PRINTF_INDENT(ICMP, 2, "\tvoid : %u\n", ntohl(header->icmp_void));
}
} else if (header->icmp_type == ICMP_REDIRECT) {
LOG_PRINTF_INDENT(ICMP, 2, "\tgwaddr : %s\n", inet_ntoa(*(struct in_addr *)&(header->icmp_gwaddr)));
} else if (header->icmp_type == ICMP_TIMXCEED) {
LOG_PRINTF_INDENT(ICMP, 2, "\tvoid : %u\n", ntohl(header->icmp_void));
}
return 0;
}
shared_ptr<action::Action> KeepBalance::perform()
{
updateState();
#ifdef ENABLE_LOG
float leftLegHeight = 0;// TODO WM.getJointTrans(JID_LEG_L_4).p().z();
float rightLegHeight = 0;//TODO WM.getJointTrans(JID_LEG_R_4).p().z();
float leftArmHeight = 0;//TODO WM.getJointTrans(JID_ARM_L_4).p().z();
float rightArmHeight = 0;//TODO WM.getJointTrans(JID_ARM_R_4).p().z();
LOG_PRINTF("keepBalance","leftLeg %f, rightLeg %f, leftArm %f, rightArm %f", leftLegHeight, rightLegHeight, leftArmHeight, rightArmHeight);
#endif
if (isDone() || isTerminable())
{
if (isDone())
{
LOG_PRINT("keepBalance","isDone");
}
else
{
LOG_PRINT("keepBalance","isTerminable");
}
analysisWhatToDo();
}
else
{
LOG_PRINT("keepBalance","is not done and not terminable");
}
LOG_FLUSH
return Task::perform();
}
static void midi_in_message_handler(int device_id, uint8_t *data)
{
(void)device_id;
switch (data[1]>>4) {
case 8:
LOG_PRINTF("\r\nNote Off");
break;
case 9:
LOG_PRINTF("\r\nNote On");
break;
default:
break;
}
}
static void *midi_init(void *usbh_dev)
{
if (!midi_config || !initialized) {
LOG_PRINTF("\n%s/%d : driver not initialized\n", __FILE__, __LINE__);
return 0;
}
uint32_t i;
midi_device_t *drvdata = 0;
// find free data space for midi device
for (i = 0; i < USBH_AC_MIDI_MAX_DEVICES; i++) {
if (midi_device[i].state == 0) {
drvdata = &midi_device[i];
drvdata->device_id = i;
drvdata->endpoint_in_address = 0;
drvdata->endpoint_out_address = 0;
drvdata->endpoint_in_toggle = 0;
drvdata->endpoint_out_toggle = 0;
drvdata->usbh_device = usbh_dev;
drvdata->write_callback_user = 0;
drvdata->sending = false;
break;
}
}
return drvdata;
}
void ORILIB_ByteReader_i (
IN ORILIB_ByteReader_t_State * const inpStateBuf,
OUT void * byteBuf,
OUT ORILIB_ByteReader_t_State * outStateBuf,
CF ORILIB_ByteReader_t_Conf * conf
) {
static far Uint32 nWraps = 0;
Uint32 nextReadOffset = inpStateBuf->nextReadOffset;
//nBytes is the number of Cplx16 samples, that is, the number of Int16 pairs in the trace file
if (nextReadOffset + conf->nBytesPerSymbol > (conf->nBytes)) {
if (nWraps < conf->nWrapArounds) {
nextReadOffset = 0;
nWraps++;
}
else {
LOG_PRINTF("ERROR: end of sample trace\n");
SYS_TimeStampPrintAllAliased();
exit(1);
}
}
_amem4cpy(byteBuf, &(conf->bytes[nextReadOffset]), conf->nBytesPerSymbol >> 2); // >> 2 because size is in words
nextReadOffset += conf->nBytesPerSymbol;
outStateBuf->nextReadOffset = nextReadOffset;
}
void otPlatLog(otLogLevel aLogLevel, otLogRegion aLogRegion, const char *aFormat, ...)
{
char logString[512];
unsigned int offset;
int charsWritten;
va_list args;
offset = 0;
LOG_PRINTF("[%d] ", NODE_ID);
va_start(args, aFormat);
charsWritten = vsnprintf(&logString[offset], sizeof(logString) - offset, aFormat, args);
va_end(args);
otEXPECT_ACTION(charsWritten >= 0, logString[offset] = 0);
exit:
#ifndef _WIN32
syslog(LOG_CRIT, "%s", logString);
#else
printf("%s\r\n", logString);
#endif
(void)aLogLevel;
(void)aLogRegion;
}
ghttp_request *init_request()
{
ghttp_request *request = NULL;
#ifdef DEBUG_MSG
openlog("http_request", LOG_PID, LOG_USER);
#endif
/* Allocate a new empty request object */
request = ghttp_request_new();
if (request == NULL)
{
LOG_PRINTF("Error: Func: %s Acclocate new empty request is NULL\n", __func__);
return NULL;
}
LOG_PRINTF("Info: Func: %s Acclocate new empty request is ok. \n", __func__);
return request;
}
/* TODO: Should return "invalid drive" on error. */
int fd32_getcwd(const char *Drive, char *Dest)
{
tCds *C;
tCds **CdsList = (tCds **) fd32_get_cdslist();
/* Search for the specified drive in the CDS list of the current process */
for (C = *CdsList; C; C = C->Next)
if (strcasecmp(C->Drive, Drive) == 0)
{
strcpy(Dest, C->CurDir);
LOG_PRINTF(("[GETCWD] Getting the current dir of \"%s\": \"%s\"\n", Drive, Dest));
return 0;
}
strcpy(Dest, "\\");
LOG_PRINTF(("[GETCWD] No CDS for \"%s\", getting root\n", Drive));
return 0;
}
int main( int argc, char *argv[] )
{
long int sleep_time;
int server_port;
char server_ip[] = "000.000.000.000";
LOG_OPEN();
if (argc != 5)
{
print_help();
exit(EXIT_SUCCESS);
}
else
{
// to do: user input validation!
sensor_id = atoi(argv[1]);
sleep_time = atoi(argv[2]);
strncpy(server_ip, argv[3],strlen(server_ip));
server_port = atoi(argv[4]);
//printf("%d %ld %s %d\n", sensor_id, sleep_time, server_ip, server_port);
}
srand48( time(NULL) );
// open TCP connection to the server; server is listening to SERVER_IP and PORT
client = tcp_active_open( server_port, server_ip );
int i=LOOPS;
signal(SIGINT, my_handler);
while(running)
{
temperature = temperature + TEMP_DEV * ((drand48() - 0.5)/10);
time(×tamp);
// send data to server in this order (!!): <sensor_id><temperature><timestamp>
// remark: don't send as a struct!
tcp_send( client, (void *)&sensor_id, sizeof(sensor_id));
tcp_send( client, (void *)&temperature, sizeof(temperature));
tcp_send( client, (void *)×tamp, sizeof(timestamp));
LOG_PRINTF(sensor_id,temperature,timestamp);
sleep(sleep_time);
UPDATE(i);
}
i = 0;
tcp_close( &client );
LOG_CLOSE();
exit(EXIT_SUCCESS);
}
int main()
{
init();
{
char *ptr = 0, *ptr1 = 0, *ptr2 = 0, *ptr4 = 0, *ptr5 = 0, *ptr6 = 0, *ptr7 = 0;
// Test MALLOC
ptr = (char*)DEBUG_MALLOC(256);
strcpy(ptr, "ptr");
// Test MALLOC
ptr1 = (char*)DEBUG_MALLOC(256);
strcpy(ptr1, "ptr1");
// Test malloc
ptr2 = (char*)malloc(256);
strcpy(ptr2, "ptr2");
// Test new
int* ptr3 = DEBUG_NEW int(65);
{
// checkpoint_charlie takes care of the checkpoint, giving it a name,
// and dumping out any memory leaks detected after scope drop
leaks::checkpoint_charlie checkpoint1("CHECKPOINT 1");
// Test new[]
ptr4 = DEBUG_NEW char[256];
strcpy(ptr4, "ptr4");
ptr5 = DEBUG_NEW char[256];
strcpy(ptr5, "ptr5");
//}
//{
leaks::checkpoint_charlie checkpoint2("CHECKPOINT 2");
ptr6 = new char[256];
strcpy(ptr6, "ptr6");
ptr7 = (char*)DEBUG_MALLOC(256);
strcpy(ptr7, "ptr7");
ptr7 = (char*)DEBUG_REALLOC(ptr7, 256);
}
// Housekeeping
LOG_PRINTF(("Housekeeping"));
DEBUG_FREE(ptr);
DEBUG_FREE(ptr1);
free(ptr2);
delete ptr3;
delete [] ptr4;
delete [] ptr5;
delete [] ptr6;
// Dump the current leaks
DEBUG_DUMP();
}
while(true) SVC_WAIT(1000); // Stay here
}
void KeepBalance::analysisWhatToDo()
{
shared_ptr<Task> getUp;
shared_ptr<Task> bufferAct;
if(mDuration>2.0f)
{
if(lieordive==IWANTTODIVE) lieordive = IWANTTOLIE;
else lieordive = IWANTTODIVE;
}
float stateKeepingTime = WM.getSimTime() - mStateKeepingStartTime;
if(mCurrentState == LEFTFALL_STATE && stateKeepingTime >0.3f ){
if(lieordive==IWANTTOLIE) getUp = shared_ptr<Task> (new LeftFallToLie(this));
else getUp = shared_ptr<Task> (new LeftFallToDive(this));
LOG_PRINTF("whatToDo","LeftFallToLie keeptime=%.3f",stateKeepingTime);
}
else if( mCurrentState == RIGHTFALL_STATE && stateKeepingTime > 0.3f ){
if(lieordive==IWANTTOLIE) getUp = shared_ptr<Task> (new RightFallToLie(this));
else getUp = shared_ptr<Task> (new RightFallToDive(this));
LOG_PRINTF("whatToDo","RightFallToLie keeptime=%.3f",stateKeepingTime);
}
else if ( mCurrentState == LIED_STATE && stateKeepingTime > 0.3f ){
// if it is timeout.
// cerr<<"GetUpFromLie"<<endl;
getUp = shared_ptr<Task> (new GetUpFromLie(this));// if the situation is 0.3 secs long.
LOG_PRINTF("whatToDo","GetUpFromLie keeptime=%.3f",stateKeepingTime);
}
else if ( mCurrentState == DIVED_STATE && stateKeepingTime > 0.3f ){
// cerr<<"GetUpFromDive"<<endl;
getUp = shared_ptr<Task> ( new GetUpFromDive(this) );
LOG_PRINTF("whatToDo","GetUpFromDive keepTime=%.3f",stateKeepingTime);
}
if ( 0 != getUp.get() )
{
stop();
mSubTaskList.push_back(getUp);
LOG_PRINT("whatToDo","getUp Action is not null");
}
else
{
LOG_PRINT("whatToDo","getUp Action is null");
}
}
void ORILIB_TraceReader_Init_i(
OUT ORILIB_TraceReader_t_State * outStateBuf
) {
outStateBuf->nextReadOffset = 0;
DEBUG(
LOG_PRINTF("ORILIB_TraceReader_Init: nextReadOffset = %d\n", outStateBuf->nextReadOffset);
)
}
static void hid_in_message_handler(uint8_t device_id, const uint8_t *data, uint32_t length)
{
(void)device_id;
(void)data;
if (length < 4) {
LOG_PRINTF("data too short, type=%d\n", hid_get_type(device_id));
return;
}
// print only first 4 bytes, since every mouse should have at least these four set.
// Report descriptors are not read by driver for now, so we do not know what each byte means
LOG_PRINTF("HID EVENT %02X %02X %02X %02X \n", data[0], data[1], data[2], data[3]);
if (hid_get_type(device_id) == HID_TYPE_KEYBOARD) {
static int x = 0;
if (x != data[2]) {
x = data[2];
hid_set_report(device_id, x);
}
}
}
void usart_interrupt(void)
{
if (usart_get_interrupt_source(usart, USART_SR_RXNE)) {
uint8_t data = usart_recv(usart);
usart_fifo_in_push(data);
if ( data != 3 && data != '\r' && data != '\n') {
usart_fifo_push(data);
} else {
LOG_PRINTF("\n>>");
}
}
}
void destory_request(ghttp_request *request)
{
if(request != NULL)
{
ghttp_request_destroy(request);
}
LOG_PRINTF("Info: Func: %s Destroy request is ok. \n", __func__);
#ifdef DEBUG_MSG
closelog();
#endif
return ;
}
void KeepBalance::updateState()
{
//to do better, change value after the situation has changed 0.1secs.
if ( WM.isLeftFall() ) {mPossibleState=LEFTFALL_STATE;LOG_PRINT("vision-me","11");}
else if ( WM.isRightFall() ) {mPossibleState = RIGHTFALL_STATE;LOG_PRINT("vision-me","22");}
else if ( WM.isDived() ) {mPossibleState = DIVED_STATE;LOG_PRINT("vision-me","33");}
else if ( WM.isLied() ) {mPossibleState = LIED_STATE;LOG_PRINT("vision-me","44");}
else if ( WM.isDiving() ) {mPossibleState = DIVING_STATE;LOG_PRINT("vision-me","55");}
else if ( WM.isLying() ) {mPossibleState = LYING_STATE;LOG_PRINT("vision-me","66");}
else{
if ( mLastPossibleState == DIVED_STATE ||
mLastPossibleState == LIED_STATE ||
mLastPossibleState == BALANCE_STATE ){
mPossibleState = BALANCE_STATE;
}
}
if(WM.getMyAcc().z()>9.0f) mPossibleState=BALANCE_STATE; //TT add
if ( mPossibleState == BALANCE_STATE ) unBalanceTime = 0.0f;
else unBalanceTime += 0.02f;
LOG_PRINTF("keepBalance","mPossibleState is %d", mPossibleState);
if ( mPossibleState == mLastPossibleState )
{
//LOG_PRINT("keepBalance","state continues");
LOG_PRINTF("keepBalance","stateKeepingTime %f",WM.getSimTime()-mStateKeepingStartTime);
}
else
{
mStateKeepingStartTime = WM.getSimTime();
LOG_PRINTF("keepBalance","stateKeepingTime %f",WM.getSimTime()-mStateKeepingStartTime);
}
mLastPossibleState = mPossibleState;
mCurrentState = ( WM.getSimTime()-mStateKeepingStartTime > 0.09f ) ? mPossibleState : mCurrentState; //根据状态持续时间判断是否需要更新当前状态
LOG_PRINTF("keepBalance","mCurrentState is %d",mCurrentState);
}
static uint8_t usart_read_command(void)
{
uint32_t fifo_len = usart_fifo_in_len;
while (fifo_len) {
uint8_t data = usart_fifo_in_pop();
if ((data >= 'A') && (data <= 'Z')) {
data += 'a'-'A';
}
if (((data >= 'a') && (data <= 'z')) || ((data >='0') && (data<='9'))) {
command[command_len++] = data;
} else if (data == ' ') {
if (command_len) {
if (command_argindex == 0) {
command[command_len++] = 0;
command_argindex = command_len;
} else {
command[command_len++] = ' ';
}
}
} else if (data == '\r' || data == '\n') {
if (command_len) {
command[command_len++] = 0;
if (!command_argindex) {
command_argindex = command_len;
}
return 1;
}
} else if (data == 127) {
if (command_len) {
if (command_argindex) {
if (command_len == command_argindex) {
command_argindex = 0;
}
}
command[command_len] = '\0';
command_len--;
}
} else if (data == 3) {
command_len = 0;
command_argindex = 0;
} else {
LOG_PRINTF("%d ",data);
}
fifo_len--;
}
return 0;
}
/* TODO: There's a bug here, p is never reassigned to Partial, but I'm too lazy to think on it now... */
int fd32_sfn_truename(char *Dest, char *Source)
{
int Res;
char Partial[FD32_LFNPMAX];
char Aux[FD32_LFNPMAX];
char *a = Aux;
char *d = Dest;
char *p = Partial;
char *n;
fd32_fs_lfnfind_t F;
LOG_PRINTF(("[SFN_TRUENAME] fd32_sfn_truename, Source=\"%s\"\n", Source));
if ((Res = fd32_truename(Aux, Source, FD32_TNSUBST)) < 0) return Res;
/* Copy drive specification */
for (; *a != '\\'; p++, d++, a++)
{
*d = *a;
*p = *a;
}
while (*a)
{
*(d++) = *a;
*(p++) = *(a++);
*d = 0;
if (*a == 0) break;
while ((*a != '\\') && *a) *(p++) = *(a++);
*p = 0;
LOG_PRINTF(("[SFN_TRUENAME] Searching \"%s\"\n", Partial));
if ((Res = fd32_lfn_findfirst(Partial, FD32_FRNONE | FD32_FAALL, &F)) < 0) return Res;
if (Res < 0) return Res;
if ((Res = fd32_lfn_findclose(Res)) < 0) return Res;
for (n = F.ShortName; (*d = *n); d++, n++);
*d = 0;
LOG_PRINTF(("[SFN_TRUENAME] Dest=\"%s\"\n", Dest));
}
return 0;
}
int sniff_udp_fromwire(const byte *packet, size_t length) {
const struct udphdr *header = (struct udphdr *)packet;
uint16_t sport = ntohs(header->uh_sport);
uint16_t dport = ntohs(header->uh_dport);
LOG_PRINTF(UDP, "-- UDP (%lu bytes)\n", length);
LOG_PRINTF_INDENT(UDP, 2, "\tsport: %u\n", sport); // source port
LOG_PRINTF_INDENT(UDP, 2, "\tdport: %u\n", dport); // destination port
LOG_PRINTF_INDENT(UDP, 2, "\tulen : %u\n", ntohs(header->uh_ulen)); // udp length
LOG_PRINTF_INDENT(UDP, 2, "\tsum : %u\n", header->uh_sum); // udp checksum
packet = (byte *)PTR_ADD(packet, UDP_HDR_LEN);
length = ntohs(header->uh_ulen) - UDP_HDR_LEN;
if (sport == 53 || dport == 53) {
sniff_dns_fromwire(packet, length);
}
#if LOG_ENABLED(UDP_DATA)
LOG_PRINTF(UDP_DATA, "showing %lu bytes:\n", length);
dump_hex(stdout, packet, length, 0);
#endif
return 0;
}
static void usart_fifo_push(uint8_t aData)
{
uint32_t i;
if( (usart_fifo_out_len + 1) == USART_FIFO_OUT_SIZE)//overflow
{
usart_fifo_out_len = 0;
LOG_PRINTF("OVERFLOW!");
return;
}
i = usart_fifo_out_index + usart_fifo_out_len;
if (i >= USART_FIFO_OUT_SIZE) {
i -= USART_FIFO_OUT_SIZE;
}
usart_fifo_out_data[i] = aData;
usart_fifo_out_len++;
}
int sniff_arp_fromwire(const byte *packet, size_t length) {
const struct ether_arp *header = (struct ether_arp *)packet;
uint16_t arphrd = ntohs(header->arp_hrd);
uint16_t arppro = ntohs(header->arp_pro);
uint16_t arpop = ntohs(header->arp_op);
LOG_PRINTF(ARP, "-- ARP (%lu bytes)\n", length);
LOG_PRINTF_INDENT(ARP, 2, "hrd: %u [%s]\n", arphrd, totext(ARP_ARRAY_HRD, arphrd)); // format of hardware address
LOG_PRINTF_INDENT(ARP, 2, "pro: 0x%04x [%s]\n", arppro, totext(ARP_ARRAY_PRO, arppro)); // format of protocol address
LOG_PRINTF_INDENT(ARP, 2, "hln: %u\n", header->arp_hln); // length of hardware address
LOG_PRINTF_INDENT(ARP, 2, "pln: %u\n", header->arp_pln); // length of protocol address
LOG_PRINTF_INDENT(ARP, 2, "op : %u [%s]\n", arpop, totext(ARP_ARRAY_OP, arpop));
LOG_PRINTF_INDENT(ARP, 2, "sha: %s\n", ether_ntoa((struct ether_addr *)&header->arp_sha)); // sender hardware address
LOG_PRINTF_INDENT(ARP, 2, "spa: %s\n", inet_ntoa(*(struct in_addr *)&header->arp_spa)); // sender protocol address
LOG_PRINTF_INDENT(ARP, 2, "tha: %s\n", ether_ntoa((struct ether_addr *)&header->arp_tha)); // target hardware address
LOG_PRINTF_INDENT(ARP, 2, "tpa: %s\n", inet_ntoa(*(struct in_addr *)&header->arp_tpa)); // target protocol address
return 0;
}
/* Converts a FD32 return code (< 0 meaning error)
* to a DOS return status in the specified register set.
* - on error: carry flag set, error (positive) in AX
* - on success: carry flag clear
*/
static void res2dos(int res, union rmregs *r)
{
LOG_PRINTF(("INT 21h - res=%i\n", res));
if (res < 0)
{
res = -res;
RMREGS_SET_CARRY;
if (res < sizeof(errno2dos))
r->x.ax = errno2dos[res];
else if (res == 0x4401)
r->x.ax = 0x4401; /* IOCTL capability not available */
else if (res == 0x7100)
r->x.ax = 0x7100; /* Yet another convention: LONG FILENAME */
else
r->x.ax = DOS_EUNKNOWN;
} else {
RMREGS_CLEAR_CARRY;
}
}
