Something went wrong on our end
commands.c 11.98 KiB
#include "commands.h"
// 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 getInt(unsigned char* str, int pos) {
int i = ((str[pos+3] << 24) | (str[pos+2] << 16) | (str[pos+1] << 8) | (str[pos]));
return i;
}
//------------------------------------------------
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 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;
}
// 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 response(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
printf("This is the response: %s\n", packet);
return 0;
}
// ------------------------------------------------------------------
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 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 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 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 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 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 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 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 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;
}
// ------------------------------------------------------------
// 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 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 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 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;
}
// 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 *packet, int dataLen, modular_structs_t *structs)
{
printf("function for accelreq\n");
return 0;
}
int pitchangleresp(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
printf("function for accelreq\n");
return 0;
}
int rollangleresp(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
printf("function for accelreq\n");
return 0;
}
int gyroreq(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
printf("function for accelreq\n");
return 0;
}
int pitchanglereq(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
printf("function for accelreq\n");
return 0;
}
int rollanglereq(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
printf("function for accelreq\n");
return 0;
}
int accelresp(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
printf("function for accelreq\n");
return 0;
}