void clConsole::clCommandsHistory::LoadFromStream( iIStream* IStream )
{
while ( !IStream->Eof() )
{
LString Cmd = IStream->ReadLine();
PutCommand( Cmd );
}
}
void clsCMM::ProcessPackage(ADDRESSEDPACKAGE *pPackage)
{
if (pPackage->to != _HELICOPTER && pPackage->to != ID_ALL) return; //drop packages not for this helicopter
int nFrom = pPackage->from;
char *package = pPackage->package.content;
COMMAND cmd;
cmd.code = (short &)package[0];
if ( GetCommandString(cmd.code) != NULL ) {
printf("[CMM] Command received %d(%s) from %d\n", cmd.code, GetCommandString(cmd.code), pPackage->from);
char string[256];
sprintf(string, "command received %d(%s) from %d", cmd.code, GetCommandString(cmd.code), pPackage->from);
SendMessage(string);
}
switch (cmd.code) {
case COMMAND_QUIT:
case COMMAND_MAGTEST:
case COMMAND_MAGSET:
case COMMAND_HOVER:
case COMMAND_FILTER:
case COMMAND_TAKEOFF:
case COMMAND_LAND:
case COMMAND_ENGINEUP:
case COMMAND_ENGINEDOWN:
case COMMAND_EMERGENCY:
case COMMAND_EMERGENCYGROUND:
case COMMAND_NOTIFY:
case COMMAND_NONOTIFY:
case COMMAND_LEADERUPDATE:
case COMMAND_COMPETITION:
break;
case COMMAND_LIFT:
case COMMAND_DESCEND:
case COMMAND_ENGINE:
COPYDOUBLE(cmd.parameter, package+2);
break;
case COMMAND_HEADTO:
case COMMAND_COORDINATE:
::memcpy(cmd.parameter, package+2, 3*sizeof(double));
break;
case COMMAND_RUN:
case COMMAND_PARA:
case COMMAND_TEST: //parameter is an integer
case COMMAND_GPATH:
// (int &)cmd.parameter[0] = (int &)package[2];
COPYLONG(cmd.parameter, package+2);
break;
case COMMAND_FORMATION: // parameter is the path number of the leader
// case COMMAND_ATTACK: // for later cooperative behavior usage
(COOP_PKT &)cmd.parameter[0] = (COOP_PKT &)package[2]; //extract coop_pkt from CMM
_net.ToggleNetStart();
break;
case COMMAND_STOPFORMATION:
_coop.ResetConnectFlag();
break;
case COMMAND_PATH:
// (int &)cmd.parameter[0] = (int &)package[2]; //path id
// (int &)cmd.parameter[4] = (int &)package[6]; //tracking option
COPYLONG(cmd.parameter, package+2);
COPYLONG(cmd.parameter+4, package+6);
break;
case COMMAND_PATHA: {
double nPoint;
// COPYWORD(&nPoint, package+2);
COPYDOUBLE(&nPoint, package+2);
int npoint = (int)nPoint;
LOCATION *pPoint = (LOCATION *)(package+10);
::memcpy(cmd.parameter, package+2, sizeof(double));
::memcpy(cmd.parameter+8, package+10, npoint*sizeof(LOCATION));
/* //generate dynamic path
LOCATION pos0;
_state.GetCoordination(&pos0.longitude, &pos0.latitude, &pos0.altitude);
_pathTmp.CreatePath(pPoint, nPoint, &_state.GetState(), &pos0);
//set command to path tracking
cmd.code = COMMAND_PATH;
PUTLONG(cmd.parameter, -1);
PUTLONG(cmd.parameter + 4, PATHTRACKING_FIXED);*/
break;
}
case COMMAND_TRACK:
::memcpy(cmd.parameter, package+2, 2*sizeof(int)+3*sizeof(double));
break;
case COMMAND_DYNAMICPATH:
case COMMAND_DYNAMICPATHRESET:
::memcpy(cmd.parameter, package+2, 5*sizeof(double)); //t,x,y,z,c
break;
case COMMAND_HFLY: //parameter (u,v,h,r)
case COMMAND_FLY: //parameter (u,v,w,r)
case COMMAND_HOLD: //(x,y,z,c)
case COMMAND_HOLDPI:
case COMMAND_CFLY: //(u,v,w,r)
case COMMAND_TAKEOFFA: //(x,y,z,c)
case COMMAND_LANDA: //(x,y,z,c)
::memcpy(cmd.parameter, package+2, 4*sizeof(double));
break;
case COMMAND_CHIRP:
::memcpy(cmd.parameter, package+2, sizeof(int)+4*sizeof(double));
break;
case COMMAND_PLAN:
// (int &)cmd.parameter[0] = (int &)package[2];
COPYLONG(cmd.parameter, package+2);
printf("[CMM] Command plan, parameter %d\n", (int)GETLONG(package+2));
break;
case COMMAND_CAMRUN: break;
case COMMAND_CAMSTOP: break;
case COMMAND_CAMQUIT: break;
case COMMAND_CAMTRACK: break;
case COMMAND_CAMVBS:
// _state.EnableVision();
case COMMAND_CAMTHOLD:
::memcpy(cmd.parameter, package+2, sizeof(double));
break;
case COMMAND_COOP_PKT: // coop pkts, futher decoding will be used in the clsCoop
::memcpy(cmd.parameter, package+2, sizeof(COOP_PKT));
break;
case COMMAND_MODE:
COPYLONG(cmd.parameter, package+2);
printf("[CMM] Command mode, parameter %d\n", (int)GETLONG(package+2));
break;
case DATA_VICON: {
SetViconFlag();
_state.SetbMeasurementUpdate();
// AccountCMMRecord(nFrom);
VICON_DATA viconData;
int nObjLen = GETLONG(package+12);
// viconData.x = GETDOUBLE(package+16+nObjLen); viconData.y = GETDOUBLE(package+16+nObjLen+8); viconData.z = GETDOUBLE(package+16+nObjLen+16);
// viconData.a = GETDOUBLE(package+16+nObjLen+24); viconData.b = GETDOUBLE(package+16+nObjLen+32); viconData.c = GETDOUBLE(package+16+nObjLen+40);
memcpy(&viconData, package+16+nObjLen, sizeof(VICON_DATA));
CollectViconData(viconData);
return;
break;
}
default:
break; //for unknown command only code is transfered
}
PutCommand(&cmd);
}