Skip to content
Snippets Groups Projects
Commit 17f97027 authored by burneykb's avatar burneykb
Browse files

Fixed select and formatCommand bugs

parent 1bc65a32
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
groundStation/src/commands.c
+ 570
− 55
View file @ 17f97027
#include "commands.h"
static command_t registeredCommands[NUM_COMMANDS] = {
{ 0x00, 0x00, stringType, "debug", &debug }, //DEBUG
{ 0x01, 0x00, stringType, "setyaw", &setyaw }, //CALIBRATION
{ 0x01, 0x01, stringType, "setyawp", &setyawp },
{ 0x01, 0x02, stringType, "setyawd", &setyawd },
{ 0x01, 0x03, stringType, "setroll", &setroll },
{ 0x01, 0x04, stringType, "setrollp", &setrollp },
{ 0x01, 0x05, stringType, "setrolld", &setrolld },
{ 0x01, 0x06, stringType, "setpitch", &setpitch },
{ 0x01, 0x07, stringType, "setpitchp", &setpitchp },
{ 0x01, 0x08, stringType, "setpitchd", &setpitchd },
{ 0x01, 0x09, stringType, "setthrottle", &setthrottle },
{ 0x01, 0x0A, stringType, "setthrottlep", &setthrottlep },
{ 0x01, 0x0B, stringType, "setthrottlei", &setthrottlei },
{ 0x01, 0x0C, stringType, "setthrottled", &setthrottled },
{ 0x02, 0x00, stringType, "getaccel", &getaccel }, //REQUEST
{ 0x02, 0x01, stringType, "getgyro", &getgyro },
{ 0x02, 0x02, stringType, "getpitchangle", &getpitchangle },
{ 0x02, 0x03, stringType, "getrollangle", &getrollangle },
{ 0x03, 0x00, stringType, "accelresp", accelresp }, //RESPONSE
{ 0x03, 0x01, stringType, "gyroresp", &gyroresp },
{ 0x03, 0x02, stringType, "pitchangleresp", &pitchangleresp },
{ 0x03, 0x03, stringType, "rollangleresp", &rollangleresp },
{ 0x04, 0x00, stringType, "update", &update }, //UPDATE
{ 0x04, 0x01, stringType, "beginupdate", &beginupdate },
{ 0x05, 0x00, stringType, "log", &logdata }, //LOG
{ 0x05, 0x01, stringType, "response", &response },
};
// TAKE THESE OUT WHEN IMPLEMENTING ON THE QUAD SIDE
float getFloat(unsigned char* str, int pos) {
union {
float f;
int i;
} x;
x.i = ((str[pos+3] << 24) | (str[pos+2] << 16) | (str[pos+1] << 8) | (str[pos]));
return x.f;
}
int debug(unsigned char *c, int dataLen){
return 0;
int getInt(unsigned char* str, int pos) {
int i = ((str[pos+3] << 24) | (str[pos+2] << 16) | (str[pos+1] << 8) | (str[pos]));
return i;
}
int setyaw(unsigned char *c, int dataLen){
//------------------------------------------------
struct MessageType MessageTypes[MAX_TYPE] =
{
// DEBUG
{
// Message Type ID
0x00,
// Debug Subtypes
{
// NONE subtype
{
// ID
0x00,
// Command text
"debug",
// Type of the command data
stringType,
// Function pointer
&debug
}
}
},
// CALIBRATION
{
// Message Type ID
0x01,
// Calibration Subtypes
{
// yaw setpoint subtype
{
// ID
0x00,
// Command text
"setyaw",
// Type of the command data
floatType,
// Function pointer
&yawset
},
// yaw p constant subtype
{
// ID
0x01,
// Command text
"setyawp",
// Type of the command data
floatType,
// Function pointer
&yawp
},
// yaw d constant subtype
{
// ID
0x02,
// Command text
"setyawd",
// Type of the command data
floatType,
// Function pointer
&yawd
},
// roll setpoint subtype
{
// ID
0x03,
// Command text
"setroll",
// Type of the command data
floatType,
// Function pointer
&rollset
},
// roll p constant subtype
{
// ID
0x04,
// Command text
"setrollp",
// Type of the command data
floatType,
// Function pointer
&rollp
},
// roll d constant subtype
{
// ID
0x05,
// Command text
"setrolld",
// Type of the command data
floatType,
// Function pointer
&rolld
},
// pitch setpoint subtype
{
// ID
0x06,
// Command text
"setpitch",
// Type of the command data
floatType,
// Function pointer
&pitchset
},
// pitch p constant subtype
{
// ID
0x07,
// Command text
"setpitchp",
// Type of the command data
floatType,
// Function pointer
&pitchp
},
// pitch d constant subtype
{
// ID
0x08,
// Command text
"setpitchd",
// Type of the command data
floatType,
// Function pointer
&pitchd
},
// throttle setpoint subtype
{
// ID
0x09,
// Command text
"setthrottle",
// Type of the command data
floatType,
// Function pointer
&throttleset
},
// throttle p constant subtype
{
// ID
0x0A,
// Command text
"setthrottlep",
// Type of the command data
floatType,
// Function pointer
&throttlep
},
// throttle i constant subtype
{
// ID
0x0B,
// Command text
"setthrottlei",
// Type of the command data
floatType,
// Function pointer
&throttlei
},
// throttle d constant subtype
{
// ID
0x0C,
// Command text
"setthrottled",
// Type of the command data
floatType,
// Function pointer
&throttled
}
}
},
// REQUEST
{
// Message Type ID
0x02,
// Request Subtypes
{
// accelerometer subtype
{
// ID
0x00,
// Command text
"accelreq",
// Type of the command data
floatType,
// Function pointer
&accelreq
},
// gyroscope subtype
{
// ID
0x01,
// Command text
"gyroreq",
// Type of the command data
floatType,
// Function pointer
&gyroreq
},
// pitch angle subtype
{
// ID
0x02,
// Command text
"reqpitchangle",
// Type of the command data
floatType,
// Function pointer
&pitchanglereq
},
// roll angle subtype
{
// ID
0x03,
// Command text
"reqrollangle",
// Type of the command data
floatType,
// Function pointer
&rollanglereq
}
}
},
// RESPONSE
{
// Message Type ID
0x03,
// Response Subtypes
{
// accelerometer subtype
{
// ID
0x00,
// Command text
"respaccel",
// Type of the command data
floatType,
// Function pointer
&accelresp
},
// gyroscope subtype
{
// ID
0x01,
// Command text
"respgyro",
// Type of the command data
floatType,
// Function pointer
&gyroresp
},
// pitch angle subtype
{
// ID
0x02,
// Command text
"resppitchangle",
// Type of the command data
floatType,
// Function pointer
&pitchangleresp
},
// roll angle subtype
{
// ID
0x03,
// Command text
"resprollangle",
// Type of the command data
floatType,
// Function pointer
&rollangleresp
}
}
},
// UPDATE
{
// Message Type ID
0x04,
// Update Subtypes
{
// NONE subtype
{
// ID
0x00,
// Command text
"update",
// Type of the command data
stringType,
// Function pointer
&update
}
}
},
// LOG
{
// Message Type ID
0x05,
// Log Subtypes
{
// NONE subtype
{
// ID
0x00,
// Command text
"log",
// Type of the command data
stringType,
// Function pointer
&logdata
},
// Response subtype
{
// ID
0x01,
// Command text
"response",
// Type of the command data
stringType,
// Function pointer
&response
}
}
},
};
int debug(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
printf("function for debug\n");
return 0;
}
int setyawp(unsigned char *c, int dataLen){
int update(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
unsigned char update[28];
memcpy(update, ((float *)packet), 28);
int packetId = getInt(update, 0);
float y_pos = getFloat(update, 4);
float x_pos = getFloat(update, 8);
float alt_pos = getFloat(update, 12);
float roll = getFloat(update, 16);
float pitch = getFloat(update, 20);
float yaw = getFloat(update, 24);
structs->log_struct.currentQuadPosition.packetId = packetId;
structs->log_struct.currentQuadPosition.y_pos = y_pos;
structs->log_struct.currentQuadPosition.x_pos = x_pos;
structs->log_struct.currentQuadPosition.alt_pos = alt_pos;
structs->log_struct.currentQuadPosition.roll = roll;
structs->log_struct.currentQuadPosition.pitch = pitch;
structs->log_struct.currentQuadPosition.yaw = yaw;
printf("QUAD: VRPN Packet:");
printf("Packet ID: %d\n", packetId);
printf("Y Position: %f\n", y_pos);
printf("X Position: %f\n", x_pos);
printf("Altitude Position: %f\n", alt_pos);
printf("Roll: %f\n", roll);
printf("Pitch: %f\n", pitch);
printf("Yaw: %f\n", yaw);
printf("function for update\n");
return 0;
}
int setyawd(unsigned char *c, int dataLen){
// Why is this here?
// This should be on the ground station side
int logdata(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
printf("Logging: %s\n", packet);
return 0;
}
int setroll(unsigned char *c, int dataLen){
int response(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
printf("This is the response: %s\n", packet);
return 0;
}
int setrollp(unsigned char *c, int dataLen){
// ------------------------------------------------------------------
int yawset(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
float value;
memcpy(&value, ((float *)packet), dataLen);
printf("%f\n", value);
structs->setpoint_struct.desiredQuadPosition.yaw = value;
printf("function for yawset: %f\n", structs->setpoint_struct.desiredQuadPosition.yaw);
return 0;
}
int setrolld(unsigned char *c, int dataLen){
int yawp(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
float value;
memcpy(&value, ((float *)packet), dataLen);
structs->parameter_struct.yaw_angle_pid.Kp = value;
printf("function for yawp: %f\n", structs->parameter_struct.yaw_angle_pid.Kp);
return 0;
}
int setpitch(unsigned char *c, int dataLen){
int yawd(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
float value;
memcpy(&value, ((float *)packet), dataLen);
structs->parameter_struct.yaw_angle_pid.Kd = value;
printf("function for yawd: %f\n", structs->parameter_struct.yaw_angle_pid.Kd);
return 0;
}
int setpitchp(unsigned char *c, int dataLen){
int rollset(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
float value;
memcpy(&value, ((float *)packet), dataLen);
structs->setpoint_struct.desiredQuadPosition.roll = value;
printf("function for rollset: %f\n", structs->setpoint_struct.desiredQuadPosition.roll);
return 0;
}
int setpitchd(unsigned char *c, int dataLen){
int rollp(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
float value;
memcpy(&value, ((float *)packet), dataLen);
structs->parameter_struct.local_y_pid.Kp = value;
printf("function for rollp: %f\n", structs->parameter_struct.local_y_pid.Kp);
return 0;
}
int setthrottle(unsigned char *c, int dataLen){
int rolld(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
float value;
memcpy(&value, ((float *)packet), dataLen);
structs->parameter_struct.local_y_pid.Kd = value;
printf("function for rolld: %f\n", structs->parameter_struct.local_y_pid.Kd);
return 0;
}
int setthrottlep(unsigned char *c, int dataLen){
int pitchset(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
float value;
memcpy(&value, ((float *)packet), dataLen);
structs->setpoint_struct.desiredQuadPosition.pitch = value;
printf("function for pitchset: %f\n", structs->setpoint_struct.desiredQuadPosition.pitch);
return 0;
}
int setthrottlei(unsigned char *c, int dataLen){
int pitchp(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
float value;
memcpy(&value, ((float *)packet), dataLen);
structs->parameter_struct.local_x_pid.Kp = value;
printf("function for pitchp: %f\n", structs->parameter_struct.local_x_pid.Kp);
return 0;
}
int setthrottled(unsigned char *c, int dataLen){
int pitchd(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
float value;
memcpy(&value, ((float *)packet), dataLen);
structs->parameter_struct.local_x_pid.Kd = value;
printf("function for pitchd: %f\n", structs->parameter_struct.local_x_pid.Kd);
return 0;
}
int getaccel(unsigned char *c, int dataLen){
// ------------------------------------------------------------
// These should be renamed to altitude!
int throttleset(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
float value;
memcpy(&value, ((float *)packet), dataLen);
structs->setpoint_struct.desiredQuadPosition.alt_pos = value;
printf("function for throttleset: %f\n", structs->setpoint_struct.desiredQuadPosition.alt_pos);
return 0;
}
int getgyro(unsigned char *c, int dataLen){
int throttlep(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
float value;
memcpy(&value, ((float *)packet), dataLen);
structs->parameter_struct.alt_pid.Kp = value;
printf("function for throttlep: %f\n", structs->parameter_struct.alt_pid.Kp);
return 0;
}
int getpitchangle(unsigned char *c, int dataLen){
int throttlei(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
float value;
memcpy(&value, ((float *)packet), dataLen);
structs->parameter_struct.alt_pid.Ki = value;
printf("function for throttlei: %f\n", structs->parameter_struct.alt_pid.Ki);
return 0;
}
int getrollangle(unsigned char *c, int dataLen){
int throttled(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
float value;
memcpy(&value, ((float *)packet), dataLen);
structs->parameter_struct.alt_pid.Kd = value;
printf("function for throttled: %f\n", structs->parameter_struct.alt_pid.Kd);
return 0;
}
int accelresp(unsigned char *c, int dataLen){
// These should be renamed to altitude!
// ------------------------------------------------------------
int accelreq(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
printf("function for accelreq\n");
return 0;
}
int gyroresp(unsigned char *c, int dataLen){
int gyroresp(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
printf("function for accelreq\n");
return 0;
}
int pitchangleresp(unsigned char *c, int dataLen){
int pitchangleresp(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
printf("function for accelreq\n");
return 0;
}
int rollangleresp(unsigned char *c, int dataLen){
int rollangleresp(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
printf("function for accelreq\n");
return 0;
}
int update(unsigned char *c, int dataLen){
int gyroreq(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
printf("function for accelreq\n");
return 0;
}
int beginupdate(unsigned char *c, int dataLen){
int pitchanglereq(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
printf("function for accelreq\n");
return 0;
}
int logdata(unsigned char *c, int dataLen){
int rollanglereq(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
printf("function for accelreq\n");
return 0;
}
int response(unsigned char *packet, int dataLen){
int accelresp(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
printf("function for accelreq\n");
return 0;
}
\ No newline at end of file
groundStation/src/commands.h
+ 50
− 44
View file @ 17f97027
#ifndef _COMMANDS_H
#define _COMMANDS_H
#define NUM_COMMANDS 26
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "type_def.h"
// ----------------------
// Helper stuff
//TODO handle with enums
#define MAX_TYPE 6
#define MAX_SUBTYPE 100
int debug(unsigned char *c, int dataLen);
int setyaw(unsigned char *c, int dataLen);
int setyawp(unsigned char *c, int dataLen);
int setyawd(unsigned char *c, int dataLen);
int setroll(unsigned char *c, int dataLen);
int setrollp(unsigned char *c, int dataLen);
int setrolld(unsigned char *c, int dataLen);
int setpitch(unsigned char *c, int dataLen);
int setpitchp(unsigned char *c, int dataLen);
int setpitchd(unsigned char *c, int dataLen);
int setthrottle(unsigned char *c, int dataLen);
int setthrottlep(unsigned char *c, int dataLen);
int setthrottlei(unsigned char *c, int dataLen);
int setthrottled(unsigned char *c, int dataLen);
int getaccel(unsigned char *c, int dataLen);
int getgyro(unsigned char *c, int dataLen);
int getpitchangle(unsigned char *c, int dataLen);
int getrollangle(unsigned char *c, int dataLen);
int accelresp(unsigned char *c, int dataLen);
int gyroresp(unsigned char *c, int dataLen);
int pitchangleresp(unsigned char *c, int dataLen);
int rollangleresp(unsigned char *c, int dataLen);
int update(unsigned char *c, int dataLen);
int beginupdate(unsigned char *c, int dataLen);
int logdata(unsigned char *c, int dataLen);
int response(unsigned char *packet, int dataLen);
enum Message{
BEGIN_CHAR = 0xBE,
END_CHAR = 0xED
};
// This should also have double to avoid confusion with float values.
enum DataType
{
floatType,
......@@ -46,24 +26,50 @@ enum DataType
stringType
};
typedef struct command {
// MESSAGE SUBTYPES
struct MessageSubtype{
char ID;
char subID;
char dataType;
char commandText[100];
int (*functionPtr)(unsigned char *command, int dataLen);
}command_t;
char cmdText[100];
char cmdDataType;
int (*functionPtr)(unsigned char *command, int dataLen, modular_structs_t *structs);
};
enum CommandIDs{
DEBUG,
CALIBRATION,
REQUEST,
RESPONSE,
UPDATE,
LOG,
MAX_COMMAND_IDS
// MESSAGE TYPES
struct MessageType{
char ID;
struct MessageSubtype subtypes[MAX_SUBTYPE];
};
static command_t registeredCommands[NUM_COMMANDS];
int debug(unsigned char *c, int dataLen, modular_structs_t *structs);
int update(unsigned char *c, int dataLen, modular_structs_t *structs);
int logdata(unsigned char *c, int dataLen, modular_structs_t *structs);
int response(unsigned char *packet, int dataLen, modular_structs_t *structs);
int yawset(unsigned char *c, int dataLen, modular_structs_t *structs);
int yawp(unsigned char *c, int dataLen, modular_structs_t *structs);
int yawd(unsigned char *c, int dataLen, modular_structs_t *structs);
int rollset(unsigned char *c, int dataLen, modular_structs_t *structs);
int rollp(unsigned char *c, int dataLen, modular_structs_t *structs);
int rolld(unsigned char *c, int dataLen, modular_structs_t *structs);
int pitchset(unsigned char *c, int dataLen, modular_structs_t *structs);
int pitchp(unsigned char *c, int dataLen, modular_structs_t *structs);
int pitchd(unsigned char *c, int dataLen, modular_structs_t *structs);
int throttleset(unsigned char *c, int dataLen, modular_structs_t *structs);
int throttlep(unsigned char *c, int dataLen, modular_structs_t *structs);
int throttlei(unsigned char *c, int dataLen, modular_structs_t *structs);
int throttled(unsigned char *c, int dataLen, modular_structs_t *structs);
int accelreq(unsigned char *c, int dataLen, modular_structs_t *structs);
int gyroresp(unsigned char *c, int dataLen, modular_structs_t *structs);
int pitchangleresp(unsigned char *c, int dataLen, modular_structs_t *structs);
int rollangleresp(unsigned char *c, int dataLen, modular_structs_t *structs);
int gyroreq(unsigned char *c, int dataLen, modular_structs_t *structs);
int pitchanglereq(unsigned char *c, int dataLen, modular_structs_t *structs);
int rollanglereq(unsigned char *c, int dataLen, modular_structs_t *structs);
int accelresp(unsigned char *c, int dataLen, modular_structs_t *structs);
float getFloat(unsigned char* str, int pos);
int getInt(unsigned char* str, int pos);
// TODO add in string to be read from the command line when sending a subtype of message
extern struct MessageType MessageTypes[MAX_TYPE];
#endif
\ No newline at end of file
groundStation/src/communication.c
+ 68
− 75
View file @ 17f97027
......@@ -52,10 +52,13 @@ int checkInt(char *intString, int *value) {
//--------------------------------
// Formatting commands from ground station CLI
int formatCommand(unsigned char *command, unsigned char **formattedCommand) {
//command[strlen((char *)command) - 1] = 0;
tokenList_t tokens = tokenize((char *)command);
int formatCommand(char *command, unsigned char **formattedCommand) {
fprintf(stderr, "length = %li , received '%s'\n", strlen(command), command);
char cmd[strlen(command)];
strncpy(cmd, command, strlen(command));
cmd[strlen(command)] = '\0';
tokenList_t tokens = tokenize(cmd);
float floatValue = 0.0;
int intValue = 0;
int valid;
......@@ -63,72 +66,76 @@ int formatCommand(unsigned char *command, unsigned char **formattedCommand) {
// ----------------------------------------------
if(tokens.numTokens > 1) {
for(int cmdIndex = 0; cmdIndex < NUM_COMMANDS; ++cmdIndex)
for(int type = 0; type < MAX_TYPE; type++)
{
if(strcmp(tokens.tokens[0], registeredCommands[cmdIndex].commandText) == 0)
for(int subtype = 0; subtype < MAX_SUBTYPE; subtype++)
{
switch (registeredCommands[cmdIndex].dataType)
if(strcmp(tokens.tokens[0], MessageTypes[type].subtypes[subtype].cmdText) == 0)
{
// Validate the float input
case floatType:
valid = checkFloat(tokens.tokens[1], &floatValue);
if(!valid) {
return -1;
}
printf("%f, %s\n", floatValue, tokens.tokens[1]);
metadata.begin_char = (char) BEGIN_CHAR;
metadata.msg_type = registeredCommands[cmdIndex].ID;
metadata.msg_subtype = registeredCommands[cmdIndex].subID;
metadata.msg_id = msgNum++;
metadata.data_len = sizeof(floatValue);
formatPacket(&metadata, &floatValue, formattedCommand);
break;
// Validate the integer input
case intType:
valid = checkInt(tokens.tokens[1], &intValue);
if(!valid) {
return -1;
}
metadata.begin_char = (char) BEGIN_CHAR;
metadata.msg_type = registeredCommands[cmdIndex].ID;
metadata.msg_subtype = registeredCommands[cmdIndex].subID;
metadata.msg_id = msgNum++;
metadata.data_len = sizeof(intValue);
formatPacket(&metadata, &intValue, formattedCommand);
break;
printf("Sending\n\ttype: %d, \n\tsubtype: %d\n\tcommand: %s\n", type, subtype, MessageTypes[type].subtypes[subtype].cmdText);
// Validate the string input (doesn't need to happen)
case stringType:
metadata.begin_char = (char) BEGIN_CHAR;
metadata.msg_type = registeredCommands[cmdIndex].ID;
metadata.msg_subtype = registeredCommands[cmdIndex].subID;
metadata.msg_id = msgNum++;
metadata.data_len = strlen(tokens.tokens[1]);
// Make sure the second token is the right type
switch (MessageTypes[type].subtypes[subtype].cmdDataType)
{
// Validate the float input
case floatType:
valid = checkFloat(tokens.tokens[1], &floatValue);
if(!valid) {
return -1;
}
printf("%f, %s\n", floatValue, tokens.tokens[1]);
metadata.begin_char = (char) BEGIN_CHAR;
metadata.msg_type = MessageTypes[type].ID;
metadata.msg_subtype = MessageTypes[type].subtypes[subtype].ID;
metadata.msg_id = msgNum++;
metadata.data_len = sizeof(floatValue);
formatPacket(&metadata, &floatValue, formattedCommand);
break;
formatPacket(&metadata, &tokens.tokens[1], formattedCommand);
// Validate the integer input
case intType:
valid = checkInt(tokens.tokens[1], &intValue);
if(!valid) {
return -1;
}
metadata.begin_char = (char) BEGIN_CHAR;
metadata.msg_type = MessageTypes[type].ID;
metadata.msg_subtype = MessageTypes[type].subtypes[subtype].ID;
metadata.msg_id = msgNum++;
metadata.data_len = sizeof(intValue);
formatPacket(&metadata, &intValue, formattedCommand);
break;
break;
default:
return -1;
// Validate the string input (doesn't need to happen)
case stringType:
metadata.begin_char = (char) BEGIN_CHAR;
metadata.msg_type = MessageTypes[type].ID;
metadata.msg_subtype = MessageTypes[type].subtypes[subtype].ID;
metadata.msg_id = msgNum++;
metadata.data_len = strlen(tokens.tokens[1]);
formatPacket(&metadata, &tokens.tokens[1], formattedCommand);
break;
default:
return -1;
}
return 0;
}
return 0;
}
}
}
// Only gets here if the command does not exist
return -1;
return -1;
}
// QUAD & Ground Station
// Format the log data from log_message
//int formatData(unsigned char *log_msg, unsigned char *formattedCommand)
......@@ -231,7 +238,7 @@ int parse_packet(unsigned char * packet, unsigned char ** data, metadata_t * met
// QUAD & Ground Station
// Process the command received
int processCommand(unsigned char *packet, unsigned int cmdIndex) {
int processCommand(unsigned char *packet, modular_structs_t *structs) {
int validPacket;
unsigned char *data;
metadata_t metadata;
......@@ -244,25 +251,11 @@ int processCommand(unsigned char *packet, unsigned int cmdIndex) {
if(metadata.data_len >= 0) {
// Call the appropriate subtype function
(* (registeredCommands[cmdIndex].functionPtr))(data, metadata.data_len);
(* (MessageTypes[(unsigned char)metadata.msg_type].subtypes[(unsigned char)metadata.msg_subtype].functionPtr))(data, metadata.data_len, structs);
return 0;
}
// Only gets here if there is an error
return -1;
}
float getFloat(unsigned char* str, int pos) {
union {
float f;
int i;
} x;
x.i = ((str[pos+3] << 24) | (str[pos+2] << 16) | (str[pos+1] << 8) | (str[pos]));
return x.f;
}
int getInt(unsigned char* str, int pos) {
int i = ((str[pos+3] << 24) | (str[pos+2] << 16) | (str[pos+1] << 8) | (str[pos]));
return i;
}
}
\ No newline at end of file
groundStation/src/communication.h
+ 3
− 5
View file @ 17f97027
......@@ -10,12 +10,10 @@
tokenList_t tokenize(char* cmd);
int checkFloat(char *floatString, float *value);
int checkInt(char *intString, int *value);
int formatCommand(unsigned char *command, unsigned char **formattedCommand);
int formatCommand(char *command, unsigned char **formattedCommand);
int formatPacket(metadata_t *metadata, void *data, unsigned char **formattedCommand);
int parse_packet(unsigned char * packet, unsigned char ** data, metadata_t * meta_data);
int processCommand(unsigned char *command, unsigned int cmdIndex);
int processCommand(unsigned char *command, modular_structs_t *structs);
int logData(unsigned char *log_msg, unsigned char *formattedCommand);
float getFloat(unsigned char* str, int pos);
int getInt(unsigned char* str, int pos);
#endif
#endif
\ No newline at end of file
groundStation/src/microcart_cli.c
+ 59
− 62
View file @ 17f97027
/* Author: Kris Burney
/* Author: Kris Burney & Jake Drahos
*
* BlueTooth socket program for passing vrpn data to quad.
*/
......@@ -39,10 +39,6 @@ void sendStartPacket(void);
void getVRPNPacket(struct ucart_vrpn_TrackerData *);
void printVrpnData(struct ucart_vrpn_TrackerData *);
int connectToZybo();
// void *handleQuadResponse();
// void *handleCliInput();
// int atomic_check(int*, pthread_mutex_t*);
void performCommand(char *, char *);
int startsWith(const char *, const char *);
int safe_fd_set(int , fd_set* , int* );
int safe_fd_clr(int , fd_set* , int* );
......@@ -58,6 +54,8 @@ static char * get_client_buffer(int fd);
static int remove_client(int fd);
/* Receive data from client */
static void client_recv(int fd);
/* Checks to see if socket has disconnected. Returns 1 on disconnect, else returns 0 */
static int wasDisconnected(int fd);
/* Thread-safe wrappers */
pthread_mutex_t quadSocketMutex;
......@@ -75,6 +73,8 @@ const char *logHeader = "";//"#\n#\tDefault log header\n#\tEverything after '#'`
#define CLIENT_BUFFER_SIZE 1024
static char client_buffers[MAX_CLIENTS][CLIENT_BUFFER_SIZE];
static int client_fds[MAX_CLIENTS];
fd_set rfds_master;
int max_fd = 0;
pthread_mutex_t quadResponseMutex, cliInputMutex ;
unsigned char *respBuf, *commandBuf;
......@@ -109,10 +109,7 @@ static void cb(struct ucart_vrpn_TrackerData * td)
int main(int argc, char **argv)
{
// pthread_t quadResponse, cliInput;
fd_set rfds_master;
int activity;
int max_fd = 0;
FD_ZERO(&rfds_master);
/*
......@@ -156,7 +153,7 @@ int main(int argc, char **argv)
client_buffers[i][0] = '\n';
}
signal(SIGINT, killHandler);
//signal(SIGINT, killHandler);
if (pthread_mutex_lock(&quadSocketMutex)) {
err(-2, "pthrtead_mutex_lock (%s:%d):", __FILE__, __LINE__);
......@@ -201,7 +198,7 @@ int main(int argc, char **argv)
// ucart_vrpn_tracker_addCallback(tracker, cb);
// start the prompt
fprintf(stdout, "$microcart> ");
// fprintf(stdout, "$microcart> ");
struct timeval timeout = {
.tv_sec = 1,
......@@ -218,35 +215,35 @@ int main(int argc, char **argv)
fprintf(stderr, "Select activity = %d\n", activity);
for(int fd = 0; fd <= max_fd; ++fd) {
if (FD_ISSET(fd, &rfds)) {
fprintf(stderr, "Select woke for fd %d\n", fd);
if (fd == fileno(stdin)) {
unsigned char userCommand[CMD_MAX_LENGTH];
read(fileno(stdin), (char *)userCommand, sizeof(userCommand));
unsigned int cmdLen = strlen((char*) userCommand);
// if the user simply hit enter then let them try again
if((userCommand[0] != '\n') && (userCommand[0] != '\r'))
{
// remove newline and return line chars
if((userCommand[cmdLen - 1] == '\n') || (userCommand[cmdLen - 1] == '\r'))
userCommand[cmdLen - 1] = '\0';
unsigned char *packet;
formatCommand(userCommand, &packet);
printf("received input from cli: '%s'\n", userCommand);
fprintf(stdout, "CLI sees as: %f\n", getFloat(packet, 7));
// Write the command to the control_loop socket
// int n = writeQuad(packet, ((packet[6] << 8) | packet[5]) + 8);
// if(n < 0) {
// fprintf(stdout, "CLI: ERROR writing to socket\n");
// }
}
if(wasDisconnected(fd))
break;
fprintf(stdout, "$microcart> ");
memset(userCommand, 0, cmdLen);
if (fd == fileno(stdin)) {
// break;
// unsigned char userCommand[CMD_MAX_LENGTH];
// read(fileno(stdin), (char *)userCommand, sizeof(userCommand));
// unsigned int cmdLen = strlen((char*) userCommand);
// // if the user simply hit enter then let them try again
// if((userCommand[0] != '\n') && (userCommand[0] != '\r'))
// {
// // remove newline and return line chars
// if((userCommand[cmdLen - 1] == '\n') || (userCommand[cmdLen - 1] == '\r'))
// userCommand[cmdLen - 1] = '\0';
// unsigned char *packet;
// formatCommand(userCommand, &packet);
// printf("received input from cli: '%s'\n", userCommand);
// // Write the command to the control_loop socket
// // int n = writeQuad(packet, ((packet[6] << 8) | packet[5]) + 8);
// // if(n < 0) {
// // fprintf(stdout, "CLI: ERROR writing to socket\n");
// // }
// }
// fprintf(stdout, "$microcart> ");
// memset(userCommand, 0, cmdLen);
} else if (fd == zyboSocket) {
} else if (fd == backendSocket) {
......@@ -267,13 +264,6 @@ int main(int argc, char **argv)
}
}
} else {
for(int i = 0; i < MAX_CLIENTS; ++i) {
if(client_fds[i] != -1) {
char buff;
if(recv(client_fds[i],&buff, 1, MSG_PEEK | MSG_DONTWAIT) < 0)
remove_client(client_fds[i]);
}
}
timeout.tv_sec = 1;
timeout.tv_usec = 0;
}
......@@ -404,15 +394,6 @@ int connectToZybo() {
}
}
void performCommand(char *cmdName, char * command) {
for(int i = 0; i < NUM_COMMANDS; ++i)
{
if(startsWith(registeredCommands[i].commandText, command)) {
fprintf(stdout, "\r\n You used cmd '%s'\n",registeredCommands[i].commandText);
}
}
}
int startsWith(const char *pre, const char *str) {
size_t lenpre = strlen(pre),
lenstr = strlen(str);
......@@ -443,7 +424,7 @@ int safe_fd_clr(int fd, fd_set* fds, int* max_fd) {
static ssize_t writeQuad(const char * buf, size_t count) {
ssize_t retval;
if (getenv(NOQUAD_ENV)) {
if (1) {//getenv(NOQUAD_ENV)) {
return count;
}
if (pthread_mutex_lock(&quadSocketMutex)) {
......@@ -537,7 +518,6 @@ static void client_recv(int fd)
}
buffer[len_pre + r] = '\0';
/* Parse buffer and handle commands */
while (1) {
/* not using strtok because reasons */
......@@ -554,16 +534,33 @@ static void client_recv(int fd)
if (newline == -1) {
break;
}
buffer[newline] = '\0';
unsigned char * packet;
fprintf(stderr, "Client sent: %s\n", buffer);
formatCommand((unsigned char *) buffer, &packet);
writeQuad((char *) packet, 4);
free(packet);
// fprintf(stderr, "newline =%li, Client sent: '%s'\n", newline, buffer);
if(formatCommand(buffer, &packet) != -1) {
fprintf(stdout, "Backend sees as: %f\n", getFloat(packet, 7));
} else {
fprintf(stderr, "Could not recognize command '%s'\n", buffer);
}
writeQuad((char *) packet, len);
//free(packet);
char * rest = &buffer[newline] + 1;
size_t restLen = (strlen(rest) == 0) ? 1 : strlen(rest);
/* Delete parsed data and move the rest to the left */
memmove(buffer, rest, strlen(rest));
memmove(buffer, rest, restLen);
}
}
static int wasDisconnected(int fd) {
char buff;
if(recv(fd, &buff, 1, MSG_PEEK | MSG_DONTWAIT) == 0)
{
remove_client(fd);
safe_fd_clr(fd, &rfds_master, &max_fd);
fprintf(stderr, "fd %d has disconnect and was removed\n", fd);
return 1;
}
return 0;
}
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment