// free up a feature with no visual effects bool removeFeature(FEATURE *psDel) { MESSAGE *psMessage; Vector3i pos; ASSERT_OR_RETURN(false, psDel != NULL, "Invalid feature pointer"); ASSERT_OR_RETURN(false, !psDel->died, "Feature already dead"); //remove from the map data StructureBounds b = getStructureBounds(psDel); for (int breadth = 0; breadth < b.size.y; ++breadth) { for (int width = 0; width < b.size.x; ++width) { if (tileOnMap(b.map.x + width, b.map.y + breadth)) { MAPTILE *psTile = mapTile(b.map.x + width, b.map.y + breadth); if (psTile->psObject == psDel) { psTile->psObject = NULL; auxClearBlocking(b.map.x + width, b.map.y + breadth, FEATURE_BLOCKED | AIR_BLOCKED); } } } } if (psDel->psStats->subType == FEAT_GEN_ARTE || psDel->psStats->subType == FEAT_OIL_DRUM) { pos.x = psDel->pos.x; pos.z = psDel->pos.y; pos.y = map_Height(pos.x, pos.z) + 30; addEffect(&pos, EFFECT_EXPLOSION, EXPLOSION_TYPE_DISCOVERY, false, NULL, 0, gameTime - deltaGameTime + 1); if (psDel->psStats->subType == FEAT_GEN_ARTE) { scoreUpdateVar(WD_ARTEFACTS_FOUND); intRefreshScreen(); } } if (psDel->psStats->subType == FEAT_GEN_ARTE || psDel->psStats->subType == FEAT_OIL_RESOURCE) { for (unsigned player = 0; player < MAX_PLAYERS; ++player) { psMessage = findMessage((MSG_VIEWDATA *)psDel, MSG_PROXIMITY, player); while (psMessage) { removeMessage(psMessage, player); psMessage = findMessage((MSG_VIEWDATA *)psDel, MSG_PROXIMITY, player); } } } debug(LOG_DEATH, "Killing off feature %s id %d (%p)", objInfo(psDel), psDel->id, psDel); killFeature(psDel); return true; }
void Context::merge(QDomNodeList& nodes) { for (uint i = 0; i < nodes.count(); ++i) { QDomNode node = nodes.item(i); if (node.isNull()) continue; QDomElement e = node.toElement(); if (e.isNull()) continue; QString tag = e.tagName(); QString text = e.text(); if (tag == "name") { assert(text == name); } else if (tag == "message") { QDomNodeList nodes = e.childNodes(); Message message; message.load(nodes); if (messageExists(message.source)) { Message& current = findMessage(message.source); current.translation = message.translation; current.type = message.type; } } else { qWarning("Error: unknown context tag: " + tag); } } }
TTErr TTObjectBase::sendMessage(const TTSymbol name, const TTValue& anInputValue, TTValue& anOutputValue) { TTMessagePtr message = NULL; TTErr err; // Retreive the registered message err = findMessage(name, &message); if (!err) { // Calling the method of the object depending of the arguments it takes // No argument case if (message->flags & kTTMessagePassNone) { TTMethodNone method = (TTMethodNone)message->method; return (this->*method)(); } // Name + arguments case else if (message->flags & kTTMessagePassNameAndValue) { return (this->*message->method)(name, anInputValue, anOutputValue); } // Only argument case (default : kTTMessagePassValue) else { TTMethodValue method = (TTMethodValue)message->method; return (this->*method)(anInputValue, anOutputValue); } } return kTTErrMethodNotFound; }
/* Mode Getter */ bool MAVLink::getMode(uint32_t &mode) { MAVLinkMessage * mm = findMessage(MAVLINK_MSG_ID_SET_MODE); if (mm == NULL) { return false; } mode = mavlink_msg_set_mode_get_mode(&mm->msg); return checkTimeout(mm,MAVLINK_MODE_TIMEOUT); }
Message& Context::findMessage(const QString& source) { for (unsigned int i = 0; i < messages.size(); ++i) if (messages[i].source == source) return messages[i]; Message message; message.source = source; return findMessage(source); }
TTErr TTObjectBase::removeMessage(const TTSymbol name) { TTMessage* message = NULL; TTErr err = findMessage(name, &message); if (!err) { err = messages->remove(name); delete message; } return err; }
/* Attitude Command Getter */ bool MAVLink::getAttitudeCommand(float &roll, float &pitch, float &yaw, float &thrust) { MAVLinkMessage * mm = findMessage(MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST); if (mm == NULL) { return false; } roll = mavlink_msg_set_roll_pitch_yaw_thrust_get_roll(&mm->msg); pitch = mavlink_msg_set_roll_pitch_yaw_thrust_get_pitch(&mm->msg); yaw = mavlink_msg_set_roll_pitch_yaw_thrust_get_yaw(&mm->msg); thrust = mavlink_msg_set_roll_pitch_yaw_thrust_get_thrust(&mm->msg); return checkTimeout(mm,MAVLINK_ATTITUDE_TIMEOUT); }
/* VFR HUD Getter */ bool MAVLink::getVFRHUD(float &airspeed, float &groundspeed, int16_t &heading, uint16_t &throttle, float &alt, float &climb) { MAVLinkMessage * mm = findMessage(MAVLINK_MSG_ID_VFR_HUD); if (mm == NULL) { return false; } airspeed = mavlink_msg_vfr_hud_get_airspeed(&mm->msg); groundspeed = mavlink_msg_vfr_hud_get_groundspeed(&mm->msg); heading = mavlink_msg_vfr_hud_get_heading(&mm->msg); throttle = mavlink_msg_vfr_hud_get_throttle(&mm->msg); alt = mavlink_msg_vfr_hud_get_alt(&mm->msg); climb = mavlink_msg_vfr_hud_get_climb(&mm->msg); return checkTimeout(mm,MAVLINK_VFR_HUD_TIMEOUT); }
/* Attitude Getter */ bool MAVLink::getAttitude(float &roll, float &pitch, float &yaw, float &p, float &q, float &r) { MAVLinkMessage * mm = findMessage(MAVLINK_MSG_ID_ATTITUDE); if (mm == NULL) { return false; } roll = mavlink_msg_attitude_get_roll(&mm->msg); pitch = mavlink_msg_attitude_get_pitch(&mm->msg); yaw = mavlink_msg_attitude_get_yaw(&mm->msg); p = mavlink_msg_attitude_get_rollspeed(&mm->msg); q = mavlink_msg_attitude_get_pitchspeed(&mm->msg); r = mavlink_msg_attitude_get_yawspeed(&mm->msg); return checkTimeout(mm,MAVLINK_ATTITUDE_TIMEOUT); }
bool MAVLink::getSystemStatus(uint8_t &mode, uint8_t &nav_mode, uint8_t &status, uint16_t &load, uint16_t &vbat, uint16_t &battery_remaining, uint16_t &packet_drop) { MAVLinkMessage * mm = findMessage(MAVLINK_MSG_ID_SYS_STATUS); if (mm == NULL) { return false; } mode = mavlink_msg_sys_status_get_mode(&mm->msg); nav_mode = mavlink_msg_sys_status_get_nav_mode(&mm->msg); status = mavlink_msg_sys_status_get_status(&mm->msg); load = mavlink_msg_sys_status_get_load(&mm->msg); vbat = mavlink_msg_sys_status_get_vbat(&mm->msg); battery_remaining = mavlink_msg_sys_status_get_battery_remaining(&mm->msg); packet_drop = mavlink_msg_sys_status_get_packet_drop(&mm->msg); return checkTimeout(mm,MAVLINK_STATUS_TIMEOUT); }
bool MAVLink::getGlobalPositionInt(int32_t &lat, int32_t &lng, int32_t &alt, int16_t &vx, int16_t &vy, int16_t &vz) { MAVLinkMessage * mm = findMessage(MAVLINK_MSG_ID_GLOBAL_POSITION_INT); if (mm == NULL) { return false; } lat = mavlink_msg_global_position_int_get_lat(&mm->msg); lng = mavlink_msg_global_position_int_get_lon(&mm->msg); alt = mavlink_msg_global_position_int_get_alt(&mm->msg); vx = mavlink_msg_global_position_int_get_vx(&mm->msg); vy = mavlink_msg_global_position_int_get_vy(&mm->msg); vz = mavlink_msg_global_position_int_get_vz(&mm->msg); return checkTimeout(mm,MAVLINK_RAW_GPS_TIMEOUT); }
/* Raw Servo Getter */ bool MAVLink::getScaledServos(int16_t &s1, int16_t &s2, int16_t &s3, int16_t &s4, int16_t &s5, int16_t &s6, int16_t &s7, int16_t &s8, uint8_t &rssi) { MAVLinkMessage * mm = findMessage(MAVLINK_MSG_ID_RC_CHANNELS_SCALED); if (mm == NULL) { return false; } s1 = mavlink_msg_rc_channels_scaled_get_chan1_scaled(&mm->msg); s2 = mavlink_msg_rc_channels_scaled_get_chan2_scaled(&mm->msg); s3 = mavlink_msg_rc_channels_scaled_get_chan3_scaled(&mm->msg); s4 = mavlink_msg_rc_channels_scaled_get_chan4_scaled(&mm->msg); s5 = mavlink_msg_rc_channels_scaled_get_chan5_scaled(&mm->msg); s6 = mavlink_msg_rc_channels_scaled_get_chan6_scaled(&mm->msg); s7 = mavlink_msg_rc_channels_scaled_get_chan7_scaled(&mm->msg); s8 = mavlink_msg_rc_channels_scaled_get_chan8_scaled(&mm->msg); return checkTimeout(mm,MAVLINK_SERVO_TIMEOUT); }
/* Raw Servo Getter */ bool MAVLink::getRawServos(uint16_t &s1, uint16_t &s2, uint16_t &s3, uint16_t &s4, uint16_t &s5, uint16_t &s6, uint16_t &s7, uint16_t &s8) { MAVLinkMessage * mm = findMessage(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW); if (mm == NULL) { return false; } s1 = mavlink_msg_servo_output_raw_get_servo1_raw(&mm->msg); s2 = mavlink_msg_servo_output_raw_get_servo2_raw(&mm->msg); s3 = mavlink_msg_servo_output_raw_get_servo3_raw(&mm->msg); s4 = mavlink_msg_servo_output_raw_get_servo4_raw(&mm->msg); s5 = mavlink_msg_servo_output_raw_get_servo5_raw(&mm->msg); s6 = mavlink_msg_servo_output_raw_get_servo6_raw(&mm->msg); s7 = mavlink_msg_servo_output_raw_get_servo7_raw(&mm->msg); s8 = mavlink_msg_servo_output_raw_get_servo8_raw(&mm->msg); return checkTimeout(mm,MAVLINK_SERVO_TIMEOUT); }
/* Raw GPS Getter */ bool MAVLink::getRawGPS(int &fix, float &lat, float &lng, float &alt, float &eph, float &epv, float &v, float &crs) { MAVLinkMessage * mm = findMessage(MAVLINK_MSG_ID_GPS_RAW); if (mm == NULL) { return false; } fix = mavlink_msg_gps_raw_get_fix_type(&mm->msg); lat = mavlink_msg_gps_raw_get_lat(&mm->msg); lng = mavlink_msg_gps_raw_get_lon(&mm->msg); alt = mavlink_msg_gps_raw_get_alt(&mm->msg); eph = mavlink_msg_gps_raw_get_eph(&mm->msg); epv = mavlink_msg_gps_raw_get_epv(&mm->msg); v = mavlink_msg_gps_raw_get_v(&mm->msg); crs = mavlink_msg_gps_raw_get_hdg(&mm->msg); return checkTimeout(mm,MAVLINK_RAW_GPS_TIMEOUT); }
/* RC Override Getter */ bool MAVLink::getRCOverride(uint16_t &c1, uint16_t &c2, uint16_t &c3, uint16_t &c4, uint16_t &c5, uint16_t &c6, uint16_t &c7, uint16_t &c8) { MAVLinkMessage * mm = findMessage(MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE); if (mm == NULL) { return false; } c1 = mavlink_msg_rc_channels_override_get_chan1_raw(&mm->msg); c2 = mavlink_msg_rc_channels_override_get_chan2_raw(&mm->msg); c3 = mavlink_msg_rc_channels_override_get_chan3_raw(&mm->msg); c4 = mavlink_msg_rc_channels_override_get_chan4_raw(&mm->msg); c5 = mavlink_msg_rc_channels_override_get_chan5_raw(&mm->msg); c6 = mavlink_msg_rc_channels_override_get_chan6_raw(&mm->msg); c7 = mavlink_msg_rc_channels_override_get_chan7_raw(&mm->msg); c8 = mavlink_msg_rc_channels_override_get_chan8_raw(&mm->msg); return checkTimeout(mm,MAVLINK_RC_OVERRIDE_TIMEOUT); }
void WebBrowser::markMessageAsRead(int id, bool read) { if (!m_root.isNull()) { Message *msg = findMessage(id); if (msg != nullptr && m_root->getParentServiceRoot()->onBeforeSetMessagesRead(m_root.data(), QList<Message>() << *msg, read ? RootItem::Read : RootItem::Unread)) { DatabaseQueries::markMessagesReadUnread(qApp->database()->connection(objectName(), DatabaseFactory::FromSettings), QStringList() << QString::number(msg->m_id), read ? RootItem::Read : RootItem::Unread); m_root->getParentServiceRoot()->onAfterSetMessagesRead(m_root.data(), QList<Message>() << *msg, read ? RootItem::Read : RootItem::Unread); emit markMessageRead(msg->m_id, read ? RootItem::Read : RootItem::Unread); msg->m_isRead = read ? RootItem::Read : RootItem::Unread; } } }
void WebBrowser::switchMessageImportance(int id, bool checked) { if (!m_root.isNull()) { Message *msg = findMessage(id); if (msg != nullptr && m_root->getParentServiceRoot()->onBeforeSwitchMessageImportance(m_root.data(), QList<ImportanceChange>() << ImportanceChange(*msg, msg->m_isImportant ? RootItem::NotImportant : RootItem::Important))) { DatabaseQueries::switchMessagesImportance(qApp->database()->connection(objectName(), DatabaseFactory::FromSettings), QStringList() << QString::number(msg->m_id)); m_root->getParentServiceRoot()->onBeforeSwitchMessageImportance(m_root.data(), QList<ImportanceChange>() << ImportanceChange(*msg, msg->m_isImportant ? RootItem::NotImportant : RootItem::Important)); emit markMessageImportant(msg->m_id, msg->m_isImportant ? RootItem::NotImportant : RootItem::Important); msg->m_isImportant = checked; } } }
// free up a feature with no visual effects bool removeFeature(FEATURE *psDel) { int mapX, mapY, width, breadth, player; MESSAGE *psMessage; Vector3i pos; ASSERT_OR_RETURN(false, psDel != NULL, "Invalid feature pointer"); ASSERT_OR_RETURN(false, !psDel->died, "Feature already dead"); if(bMultiMessages && !ingame.localJoiningInProgress) { SendDestroyFeature(psDel); // inform other players of destruction return true; // Wait for our message before really destroying the feature. } //remove from the map data mapX = map_coord(psDel->pos.x) - psDel->psStats->baseWidth/2; mapY = map_coord(psDel->pos.y) - psDel->psStats->baseBreadth/2; for (width = 0; width < psDel->psStats->baseWidth; width++) { for (breadth = 0; breadth < psDel->psStats->baseBreadth; breadth++) { if (tileOnMap(mapX + width, mapY + breadth)) { MAPTILE *psTile = mapTile(mapX + width, mapY + breadth); if (psTile->psObject == (BASE_OBJECT *)psDel) { psTile->psObject = NULL; auxClearBlocking(mapX + width, mapY + breadth, FEATURE_BLOCKED | AIR_BLOCKED); } } } } if(psDel->psStats->subType == FEAT_GEN_ARTE) { pos.x = psDel->pos.x; pos.z = psDel->pos.y; pos.y = map_Height(pos.x,pos.z); addEffect(&pos,EFFECT_EXPLOSION,EXPLOSION_TYPE_DISCOVERY,false,NULL,0); scoreUpdateVar(WD_ARTEFACTS_FOUND); intRefreshScreen(); } if (psDel->psStats->subType == FEAT_GEN_ARTE || psDel->psStats->subType == FEAT_OIL_RESOURCE) { for (player = 0; player < MAX_PLAYERS; player++) { psMessage = findMessage((MSG_VIEWDATA *)psDel, MSG_PROXIMITY, player); while (psMessage) { removeMessage(psMessage, player); psMessage = findMessage((MSG_VIEWDATA *)psDel, MSG_PROXIMITY, player); } } } killFeature(psDel); return true; }
void svc(SYSTEM_CALL_DATA *SystemCallData) { short call_type; static short do_print = 10; short i; //for hardware interface MEMORY_MAPPED_IO mmio; //for GET_TIME_OF_DAY INT32 Temp_Clock; //for SLEEP long Sleep_Time; struct Process_Control_Block *sleepPCB; //for RESTART_PROCESS long PID_restart; struct Process_Control_Block *restartPCB; struct Process_Control_Block *recreatedPCB; //for CREATE_PROCESS struct Process_Control_Block *newPCB; //for TERMINATE_PROCESS long termPID; struct Process_Control_Block *termPCB; //for GET_PROCESS_ID int ReturnedPID; char* ProcessName; struct Process_Control_Block *PCBbyProcessName; //for SUSPEND_PROCESS int suspendPID; struct Process_Control_Block *suspendPCB; //for RESUME_PROCESS int resumePID; struct Process_Control_Block *resumePCB; //for CHANGE_PRIORITY int changePrioPID; struct Process_Control_Block *changePrioPCB; int newPriority; //for SEND_MESSAGE long TargetPID; char *MessageBuffer; long SendLength; struct Message *MessageCreated; long *ErrorReturned_SendMessage; //for RECEIVE_MESSAGE long SourcePID; char *ReceiveBuffer; long ReceiveLength; long *ActualSendLength; long *ActualSourcePID; long *ErrorReturned_ReceiveMessage; struct Process_Control_Block *Mess_PCB; call_type = (short) SystemCallData->SystemCallNumber; if (do_print > 0) { printf("SVC handler: %s\n", call_names[call_type]); for (i = 0; i < SystemCallData->NumberOfArguments - 1; i++) { //Value = (long)*SystemCallData->Argument[i]; printf("Arg %d: Contents = (Decimal) %8ld, (Hex) %8lX\n", i, (unsigned long) SystemCallData->Argument[i], (unsigned long) SystemCallData->Argument[i]); } do_print--; } switch (call_type) { //get and return current system time case SYSNUM_GET_TIME_OF_DAY: mmio.Mode = Z502ReturnValue; mmio.Field1 = mmio.Field2 = mmio.Field3 = 0; MEM_READ(Z502Clock, &mmio); Temp_Clock = mmio.Field1; *SystemCallData->Argument[0] = Temp_Clock; break; //create a new PCB and put it into pcb table and ready queue case SYSNUM_CREATE_PROCESS: //create a new PCB newPCB = OSCreateProcess(SystemCallData->Argument[0], SystemCallData->Argument[1], SystemCallData->Argument[2], SystemCallData->Argument[3], SystemCallData->Argument[4]); //if create successfully, put it into PCB table and ready queue if (newPCB != NULL) { SchedularPrinter("Create", newPCB->ProcessID);//print states enPCBTable(newPCB); enReadyQueue(newPCB); } break; //return PID regarding process name case SYSNUM_GET_PROCESS_ID: ProcessName = (char*)SystemCallData->Argument[0]; //if no input process name, return the current running PID if (strcmp(ProcessName, "") == 0) { PCBbyProcessName = CurrentPCB(); *SystemCallData->Argument[1] = PCBbyProcessName->ProcessID; *SystemCallData->Argument[2] = ERR_SUCCESS; } //find the PCB in PCB table and return PID if found else { PCBbyProcessName = findPCBbyProcessName(ProcessName); //if found, return PID if (PCBbyProcessName != NULL) { ReturnedPID = PCBbyProcessName->ProcessID; *SystemCallData->Argument[1] = ReturnedPID; *SystemCallData->Argument[2] = ERR_SUCCESS; } //if not found, return error else { *SystemCallData->Argument[2] = ERR_BAD_PARAM; } } break; //if a PCB wanna sleep, put itself into timer queue and start //a new PCB case SYSNUM_SLEEP: //print states SchedularPrinter("Sleep", CurrentPID()); //Calculate WakeUpTime for PCB Sleep_Time = (long)SystemCallData->Argument[0]; sleepPCB = CurrentPCB(); sleepPCB->WakeUpTime = CurrentTime() + Sleep_Time; //Put current running PCB into timer queue and reset time lockTimer(); enTimerQueue(sleepPCB); if (sleepPCB == timerQueue->First_Element->PCB){ SetTimer(Sleep_Time); } unlockTimer(); //in uniprocessor, start a new PCB //in multiprocessor, only suspend itself if (ProcessorMode == Uniprocessor) { //first PCB in Ready Queue starts Dispatcher(); } else { OSSuspendCurrentProcess(); } break; //restart a PCB by terminate itself and created a new PCB with everything the same except PID case SYSNUM_RESTART_PROCESS: //initial return error *SystemCallData->Argument[2] = ERR_BAD_PARAM; PID_restart = (long)SystemCallData->Argument[0]; //if not restart itself if (PID_restart != CurrentPID()){ //find restarted PCB restartPCB = findPCBbyProcessID(PID_restart); //if PCB found, terminate itself and create a new one if (restartPCB != NULL){ TerminateProcess(restartPCB); recreatedPCB = OSCreateProcess(restartPCB->ProcessName, restartPCB->TestToRun, restartPCB->Priority, SystemCallData->Argument[1], SystemCallData->Argument[2]); //if create successfully, put it into PCB table and ready queue if (recreatedPCB != NULL) { *SystemCallData->Argument[2] = ERR_SUCCESS; SchedularPrinter("Create", recreatedPCB->ProcessID);//print states enPCBTable(recreatedPCB); enReadyQueue(recreatedPCB); SchedularPrinter("Restart", restartPCB->ProcessID); } } else{ *SystemCallData->Argument[2] = ERR_BAD_PARAM; } } else{ *SystemCallData->Argument[2] = ERR_BAD_PARAM; } break; //terminate a process case SYSNUM_TERMINATE_PROCESS: termPID = (long)SystemCallData->Argument[0]; //if PID = -1, terminate current running PCB if (termPID == -1) { if (PCBLiveNumber() > 1) { *SystemCallData->Argument[1] = ERR_SUCCESS; //print states SchedularPrinter("Terminate", termPID); //terminate current PCB TerminateProcess(CurrentPCB()); } else { *SystemCallData->Argument[1] = ERR_SUCCESS; HaltProcess(); } } //if PID = -2, terminate OS if (termPID == -2) { *SystemCallData->Argument[1] = ERR_SUCCESS; HaltProcess(); } //if PID positive, terminate specified PID else { termPCB = findPCBbyProcessID((long)SystemCallData->Argument[0]); //if PCB found, terminate it if (termPCB != NULL) { *SystemCallData->Argument[1] = ERR_SUCCESS; //if more than one PCB alive, simply terminate it if (PCBLiveNumber() > 1) { //print states SchedularPrinter("Terminate", termPID); //terminate specified PCB TerminateProcess(termPCB); } //if last alive PCB, terminate OS else { HaltProcess(); } } else { *SystemCallData->Argument[1] = ERR_BAD_PARAM; } } break; //suspend a PCB, which can be resumed case SYSNUM_SUSPEND_PROCESS: suspendPID = (int)SystemCallData->Argument[0]; //if PID = -1, suspend current running PCB if (suspendPID == -1) { //if more than one PCB alive, suspend it if (PCBLiveNumber() > 1) { *SystemCallData->Argument[1] = ERR_SUCCESS; //print states SchedularPrinter("Suspend", suspendPID); //Suspend Current Process SuspendProcess(CurrentPCB()); } //if last one PCB alive, return error else { *SystemCallData->Argument[1] = ERR_BAD_PARAM; } } //if PID positive, suspend specified PCB else { suspendPCB = findPCBbyProcessID((int)suspendPID); //if PCB found if (suspendPCB != NULL) { //if more than one PCB alive, suspend it if (suspendPCB->ProcessState == PCB_STATE_LIVE && PCBLiveNumber() > 1) { *SystemCallData->Argument[1] = ERR_SUCCESS; //print states SchedularPrinter("Suspend", suspendPID); //Suspend specified process SuspendProcess(suspendPCB); } //if last one PCB alive, return error else { *SystemCallData->Argument[1] = ERR_BAD_PARAM; } } else { *SystemCallData->Argument[1] = ERR_BAD_PARAM; } } break; //resumes a previously suspended PCB case SYSNUM_RESUME_PROCESS: resumePID = (int)SystemCallData->Argument[0]; resumePCB = findPCBbyProcessID(resumePID); //if PCB found if (resumePCB != NULL) { //if PCB is previously suspended if (resumePCB->ProcessState == PCB_STATE_SUSPEND) { *SystemCallData->Argument[1] = ERR_SUCCESS; //print states SchedularPrinter("Resume", resumePID); //Resume specified process ResumeProcess(resumePCB); } else { *SystemCallData->Argument[1] = ERR_BAD_PARAM; } } else { *SystemCallData->Argument[1] = ERR_BAD_PARAM; } break; //change the priority of a PCB case SYSNUM_CHANGE_PRIORITY: changePrioPID = (int)SystemCallData->Argument[0]; changePrioPCB = findPCBbyProcessID((int)changePrioPID); newPriority = (int)SystemCallData->Argument[1]; //if legal priority if (newPriority<=40 && newPriority>=0) { if (changePrioPCB != NULL) { *SystemCallData->Argument[2] = ERR_SUCCESS; //print states printf("Before changing Priority\n"); SchedularPrinter("ChangePrio", changePrioPID); //if PCB in ready queue, change order in ready queue if (changePrioPCB->ProcessLocation == PCB_LOCATION_READY_QUEUE && newPriority != changePrioPCB->Priority) { changePrioPCB = deCertainPCBFromReadyQueue(changePrioPID); changePrioPCB->Priority = newPriority; enReadyQueue(changePrioPCB); } else { changePrioPCB->Priority = newPriority; } //print states printf("After changing Priority\n"); SchedularPrinter("ChangePrio", changePrioPID); } else { *SystemCallData->Argument[2] = ERR_BAD_PARAM; } } else { *SystemCallData->Argument[2] = ERR_BAD_PARAM; } break; //PCB stores a message in message table case SYSNUM_SEND_MESSAGE: TargetPID = (long)SystemCallData->Argument[0]; MessageBuffer = (char*)SystemCallData->Argument[1]; SendLength = (long)SystemCallData->Argument[2]; ErrorReturned_SendMessage = SystemCallData->Argument[3]; //create a message MessageCreated = CreateMessage(TargetPID, MessageBuffer, SendLength, ErrorReturned_SendMessage); //if successfully create a message, put it into message table if (MessageCreated != NULL) { SchedularPrinter("SendMsg", TargetPID); enMessageTable(MessageCreated); } break; //retrive a message in message table case SYSNUM_RECEIVE_MESSAGE: SourcePID = (long)SystemCallData->Argument[0]; ReceiveBuffer = (char*)SystemCallData->Argument[1]; ReceiveLength = (long)SystemCallData->Argument[2]; ActualSendLength = SystemCallData->Argument[3]; ActualSourcePID = SystemCallData->Argument[4]; ErrorReturned_ReceiveMessage = SystemCallData->Argument[5]; Mess_PCB = CurrentPCB(); Mess_PCB->ProcessState = PCB_STATE_MSG_SUSPEND; pcbTable->Msg_Suspended_Number += 1; //PCB kept suspended by sleep until find a message while (findMessage(SourcePID, ReceiveBuffer, ReceiveLength, ActualSendLength, ActualSourcePID, ErrorReturned_ReceiveMessage) == 0) { Mess_PCB->WakeUpTime = CurrentTime() + 10; //Put current running PCB into timer queue and reset time enTimerQueue(Mess_PCB); if (Mess_PCB == timerQueue->First_Element->PCB) { SetTimer(10); } //first PCB in Ready Queue starts Dispatcher(); } Mess_PCB->ProcessState = PCB_STATE_LIVE; pcbTable->Msg_Suspended_Number -= 1; SchedularPrinter("ReceiveMsg", CurrentPID()); break; default: printf("ERROR! call_type not recognized!\n"); printf("Call_type is - %i\n", call_type); } } // End of svc
void ArduinoOnboard::arduinoDataCallback() { std::string msg; // These are super expensive on the jetson // m_nhPvt.getParam("srvBatteryCrit", srvBatteryCrit); // m_nhPvt.getParam("srvBatteryLow", srvBatteryLow); // m_nhPvt.getParam("camBatteryCrit", camBatteryCrit); // m_nhPvt.getParam("camBatteryLow", camBatteryLow); while(findMessage(msg)) { //allocate new wheelSpeeds message autorally_msgs::wheelSpeedsPtr wheelSpeeds(new autorally_msgs::wheelSpeeds); wheelSpeeds->header.stamp = ros::Time::now(); boost::char_separator<char> seps(":,\n"); tokenizer tok(msg, seps); tokenizer::iterator it=tok.begin(); //allocate new servo message for RC servoCommand autorally_msgs::servoMSGPtr servos(new autorally_msgs::servoMSG); servos->header.frame_id = "RC"; servos->backBrake = -5.0; servos->frontBrake = -5.0; double lf, rf, lb, rb; bool wheels = false; //bool servo = false; //bool camera = false; bool rc = false; while(it!=tok.end()) { if(*it == "wheels") { if(++it == tok.end()) break; lf = atof(it->c_str()); if(++it == tok.end()) break; rf = atof(it->c_str()); if(++it == tok.end()) break; lb = atof(it->c_str()); if(++it == tok.end()) break; rb = atof(it->c_str()); wheels = true; //} else if(*it == "servo") //{ // if(++it == tok.end()) break; // m_port.diag("Servo Battery Voltage", *it); // servo = true; //} else if(*it == "camera") //{ // if(++it == tok.end()) break; // m_port.diag("Camera Battery Voltage", *it); // camera = true; } else if(*it == "rc") { if(++it == tok.end()) break; servos->throttle = atof(it->c_str()); if(++it == tok.end()) break; servos->steering = atof(it->c_str()); rc = true; } else { NODELET_ERROR("ArduinoOnboard: Bad token %s", it->c_str()); m_port.diag_warn("ArduinoOnboard got a bad token"); } if(it!=tok.end()) { ++it; } } if(!(wheels && rc)) { NODELET_ERROR("ArduinoOnboard: Incomplete packet %d %d", wheels, rc); m_port.diag_warn("ArduinoOnboard: Incomplete packet"); } if(isnan(lf)) lf = 0.0; if(isnan(lb)) lb = 0.0; if(isnan(rf)) rf = 0.0; if(isnan(rb)) rb = 0.0; wheelSpeeds->lfSpeed = (lf)*m_wheelDiameter*PI; wheelSpeeds->lbSpeed = (lb)*m_wheelDiameter*PI; wheelSpeeds->rfSpeed = (rf)*m_wheelDiameter*PI; wheelSpeeds->rbSpeed = (rb)*m_wheelDiameter*PI; //check if one of the front rotation sensors is disabled if(m_lfEnabled && !m_rfEnabled) { wheelSpeeds->rfSpeed = wheelSpeeds->lfSpeed; } else if(!m_lfEnabled && m_rfEnabled) { wheelSpeeds->lfSpeed = wheelSpeeds->rfSpeed; } //check if one of the back rotation sensors is disabled if(m_lbEnabled && !m_rbEnabled) { wheelSpeeds->rbSpeed = wheelSpeeds->lbSpeed; } else if(!m_lbEnabled && m_rbEnabled) { wheelSpeeds->lbSpeed = wheelSpeeds->rbSpeed; } //Calculate servo commands //Raw data from arduino is in units of 500 ns. servos->throttle /= 2.0; servos->steering /= 2.0; std::map<std::string, ServoSettings>::const_iterator mapIt; if( (mapIt = m_servoSettings.find("steering")) != m_servoSettings.end()) { if(servos->steering > mapIt->second.center) { servos->steering = (servos->steering - mapIt->second.center) / (mapIt->second.max-mapIt->second.center); if (servos->steering > 1.0) servos->steering = 1.0; } else if(servos->steering < mapIt->second.center) { servos->steering = (servos->steering-mapIt->second.center)/(mapIt->second.center-mapIt->second.min); if (servos->steering < -1.0) servos->steering = -1.0; } else { servos->steering = 0.0; } if(mapIt->second.reverse) { servos->steering = -servos->steering; } } if( (mapIt = m_servoSettings.find("throttle")) != m_servoSettings.end()) { if(servos->throttle > mapIt->second.center) { servos->throttle = (servos->throttle - mapIt->second.center) / (mapIt->second.max-mapIt->second.center); if (servos->throttle > 1.0) servos->throttle = 1.0; } else if(servos->throttle < mapIt->second.center) { servos->throttle = (servos->throttle-mapIt->second.center)/(mapIt->second.center-mapIt->second.min); if (servos->throttle < -1.0) servos->throttle = -1.0; } else { servos->throttle = 0.0; } if(mapIt->second.reverse) { servos->throttle = -servos->throttle; } } servos->header.stamp = ros::Time::now(); if (m_lastTime + ros::Duration(2.0) > ros::Time::now()) { m_lastTime = ros::Time::now(); //set FPS int newFPS = 0; m_nhPvt.getParam("triggerFPS", newFPS); if(newFPS != m_triggerFPS) { m_triggerFPS = newFPS; m_port.lock(); m_port.writePort("#fps:" + std::to_string(m_triggerFPS) + "\r\n"); m_port.unlock(); } } //publish data m_wheelSpeedPub.publish(wheelSpeeds); m_servoPub.publish(servos); m_port.tick("arduinoData"); m_port.diag("Triggering FPS", std::to_string(m_triggerFPS)); } }