// If there is a radio packet, read and execute it. send info packets if appropriate
void DashBot::dashPacketHandler(void){
if(readRadioPacket()) {
executeRadioCommand();
}
debugBlinkOff();
//if in gyro-assisted mode, update controller and allow it to change the motor settings
if(mode == GYRO_MODE) {
dashRun(power, -heading);
}
//if in automatic info packet transmission mode is enabled, send an info packet
if(millis() - infoPacketTime > delay_between_sensor_emissions)
{
infoPacketTime = millis();
//if (infoPacketTransmissionMode == 1)
sendInfoPacket();
//else if (infoPacketTransmissionMode == 2)
// sendNamePacket();
//else if (infoPacketTransmissionMode == 3)
//sendMessagePacket();
}
//if there's not been a packet for the last 1/2 second, call an all-stop
if(millis() > lastPacketTime + 500)
{
allStop();
}
}
int sendControlPacket(int fd, char *controlPacket, int sizeControlPacket) {
unsigned char try = 0;
while (try < MAX_TRY) {
if (llwrite(fd, controlPacket, sizeControlPacket) > 0) return OK;
try++;
}
return -1;
}
int sendInfoPacket(int fd, char *infoPacket, int sizeInfoPacket) {
unsigned char try = 0;
while (try < MAX_TRY) {
if (llwrite(fd, infoPacket, sizeInfoPacket) > 0) return OK;
try++;
}
return -1;
}
int sendFile(int fd, unsigned char fileNameSize, const char *fileName, unsigned int image_bytes_length, const char *image_bytes, unsigned int* out_already_sent_bytes) {
char controlPacket[MAX_CTRL_P];
int sizeControlPacket;
char infoPacket[MAX_INFO_P];
int sizeInfoPacket;
char info[L2 * 256 + L1]; // maximum size of info
unsigned int infoSize;
unsigned int maxInfoSize = L2 * 256 + L1;
unsigned char N = 0;
/*struct stat st;
stat(fileName, &st);
unsigned int fileSize = st.st_size; // get file size from file statistics
printf("File size: %d\n", fileSize);
FILE *file;
file = fopen(fileName, "r");
sizeControlPacket = getControlPacket(START, fileSize, fileNameSize, fileName, controlPacket); // START control packet
if (sendControlPacket(fd, controlPacket, sizeControlPacket) == OK) {
int fileChar;
while (1) {
infoSize = 0;
while ((fileChar = fgetc(file)) != EOF && infoSize < maxInfoSize) {
info[infoSize] = (char) fileChar;
infoSize++;
}
if (infoSize == 0) break;
sizeInfoPacket = getInfoPacket(N, infoSize, info, infoPacket);
if (sendInfoPacket(fd, infoPacket, sizeInfoPacket) == OK) N++;
else {
fclose(file);
return -1;
}
}*/
//(unsigned int)image_bytes_length: this casting is not the ideal solution but works for now
sizeControlPacket = getControlPacket(START, (unsigned int)image_bytes_length, fileNameSize, fileName, controlPacket); // START control packet
if (sendControlPacket(fd, controlPacket, sizeControlPacket) != OK)
return -1;
//printf("\n--fileName;%s\nimglength:%l\n", fileName, image_bytes_length);
long i = 0;
while (1) {
infoSize = 0;
while (i < image_bytes_length && infoSize < maxInfoSize) {
info[infoSize] = image_bytes[i];
infoSize++;
i++;
}
if (infoSize == 0) break;
sizeInfoPacket = getInfoPacket(N, infoSize, info, infoPacket);
if (sendInfoPacket(fd, infoPacket, sizeInfoPacket) == OK)
{
++out_already_sent_bytes;
N++;
}
else {
//fclose(file);
return -1;
}
}
controlPacket[0] = CE; // END control packet
if (sendControlPacket(fd, controlPacket, sizeControlPacket) == OK)
return OK;
return -1;
/*
controlPacket[0] = CE; // END control packet
if (sendControlPacket(fd, controlPacket, sizeControlPacket) == OK) {
fclose(file);
return OK;
}
else {
fclose(file);
return -1;
}
}
else return -1; */
}
