diff --git a/groundStation/src/backend/backend.c b/groundStation/src/backend/backend.c index f56b7669825039c8dd38f53a0e2e4a33c81fcba0..5c4dc9fa6590757f051969825b6969c62bcf8fb5 100644 --- a/groundStation/src/backend/backend.c +++ b/groundStation/src/backend/backend.c @@ -116,7 +116,7 @@ static void cb(struct ucart_vrpn_TrackerData * td) { static int count = 0; if(!(count % 10)) { - //sendVrpnPacket(td); + sendVrpnPacket(td); //updateLogFile(td); } count++; @@ -239,6 +239,8 @@ int main(int argc, char **argv) * Ignore stdin from the backend */ } else if (fd == zyboSocket) { + printf("recieving from quad\n"); + /** * Read the response from the control loop */ @@ -256,7 +258,6 @@ int main(int argc, char **argv) //findTimeDiff(id); quad_recv(respBuf, respLen); - // if(respLen == 11) { // int id = getInt((unsigned char *)respBuf, 7); // findTimeDiff(id); @@ -697,7 +698,7 @@ static void quad_recv(const char * buf, size_t len) { onto the clients, do so. */ - unsigned char packet[len]; + char packet[len]; int validPacket; unsigned char *data; metadata_t metadata; @@ -705,15 +706,28 @@ static void quad_recv(const char * buf, size_t len) { memcpy(packet, buf, len); // Validate the message is correctly formatted - validPacket = parse_packet(packet, &data, &metadata); + validPacket = parse_packet((unsigned char *) packet, &data, &metadata); if(validPacket != 0) { warnx("Could not recognize packet from quad.\n"); return; } + int datalen = (packet[6] << 8) | (packet[5]); char * cmdText = MessageTypes[(int)metadata.msg_type].subtypes[(int)metadata.msg_subtype].cmdText; - float value = getFloat(packet, 7); + float value = getFloat((unsigned char *)packet, 7); printf("Quad : %s, %lf\n", cmdText, value); + /* + Assuming the quad sends the correct info.. This hasn't been tested yet due to a lack of + quad software. We can check how to format by the cmdText and pass to every client. + */ + char buffer[1048]; + sprintf(buffer, "%s %lf\n", cmdText, value); + + for(int fd = 0; fd <= max_fd; ++fd) { + if (get_client_index(fd) > -1) { + write(fd, buffer, datalen + 8); + } + } } static int wasDisconnected(int fd) { diff --git a/groundStation/src/backend/commands.h b/groundStation/src/backend/commands.h index 7ec5b4629f7c0d05831ab7ee82845846bbdf51e9..b9c9956ea0150c81291f93d3a3c665d07be67295 100644 --- a/groundStation/src/backend/commands.h +++ b/groundStation/src/backend/commands.h @@ -94,7 +94,7 @@ int respthrottled(unsigned char *c, int dataLen, modular_structs_t *structs); int respaccel(unsigned char *c, int dataLen, modular_structs_t *structs); int respgyro(unsigned char *c, int dataLen, modular_structs_t *structs); int resppitchangle(unsigned char *c, int dataLen, modular_structs_t *structs); -int getrollangle(unsigned char *c, int dataLen, modular_structs_t *structs); +int resprollangle(unsigned char *c, int dataLen, modular_structs_t *structs); float getFloat(unsigned char* str, int pos); int getInt(unsigned char* str, int pos);