// TODO: Move mac packet creation from Radio_DMA to Mac_Packet
void telemSendB(unsigned int addr) {
MacPacket packet;
Payload pld;
TelemetryStructB telemetryB;
// Populate the telemetry fields
telemPopulateB(&telemetryB);
// Create a radio packet
packet = radioRequestPacket(TELEMETRY_B_SIZE);
if(packet == NULL) { return; }
macSetDestAddr(packet, addr);
macSetDestPan(packet, netGetLocalPanID());
// Write the telemetry struct into the packet payload
pld = macGetPayload(packet);
paySetType(pld, CMD_RESPONSE_TELEMETRY);
paySetData(pld, TELEMETRY_B_SIZE, (unsigned char *) &telemetryB);
if(!radioEnqueueTxPacket(packet)) {
radioReturnPacket(packet); // Delete packet if append fails
}
}
void cmdProcessBuffer(void) {
MacPacket packet;
Payload pld;
unsigned char command;
// Check for unprocessed packet
//packet = radioDequeueRxPacket();
packet = carrayPopTail(input_queue);
if(packet == NULL) {
return;
} else {
Nop();
Nop();
}
pld = macGetPayload(packet);
command = payGetType(pld);
if(command < MAX_CMD_FUNC_SIZE) {
cmd_func[command](packet);
}
radioReturnPacket(packet);
}
