예제 #1
0
파일: cmd.c 프로젝트: abuchan/octoroach
static void cmdSetSteeringGains(unsigned char status, unsigned char length, unsigned char *frame) {
    //int Kp, Ki, Kd, Kaw, ff;
    //  int steerMode;
    //    int idx = 0;
    Payload pld;

    PKT_UNPACK(_args_cmdSetSteeringGains, argsPtr, frame);
    //_args_cmdSetSteeringGains* argsPtr = (_args_cmdSetSteeringGains*) (frame);

    steeringSetGains(argsPtr->Kp, argsPtr->Ki, argsPtr->Kd, argsPtr->Kaw, argsPtr->Kff);
    steeringSetMode(argsPtr->steerMode);

    pld = payCreateEmpty(sizeof(_args_cmdSetSteeringGains));
    pld->pld_data[0] = status;
    pld->pld_data[1] = CMD_SET_STEERING_GAINS;
    memcpy((pld->pld_data) + 2, frame, sizeof(_args_cmdSetSteeringGains));
    radioSendPayload((WordVal) macGetDestAddr(), pld);

}
예제 #2
0
static void cmdSetSteeringGains(unsigned char type, unsigned char status, unsigned char length, unsigned char *frame){
	int Kp, Ki, Kd, Kaw, ff;
	int idx = 0;
	MacPacket packet; Payload pld;
	Kp = frame[idx] + (frame[idx+1] << 8); idx+=2;
	Ki = frame[idx] + (frame[idx+1] << 8); idx+=2;
	Kd = frame[idx] + (frame[idx+1] << 8); idx+=2;
	Kaw = frame[idx] + (frame[idx+1] << 8); idx+=2;
	ff = frame[idx] + (frame[idx+1] << 8); idx+=2;
	steeringSetGains(Kp,Ki,Kd,Kaw, ff);
	//Send confirmation packet
	pld = payCreateEmpty(10);
	pld->pld_data[0] = status;
    pld->pld_data[1] = CMD_SET_STEERING_GAINS;
	memcpy((pld->pld_data)+2, frame, 10);
//	radioSendPayload((WordVal)macGetDestAddr(), pld);
// ip2.5c radio command:

    // Enqueue the packet for broadcast
    while(!radioEnqueueTxPacket(packet));
}