// 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));
}
}
