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