diff --git a/quad/sw/modular_quad_pid/src/callbacks.c b/quad/sw/modular_quad_pid/src/callbacks.c
index 4cf2d6937574f70e1e49457f1ac920b797ee4635..c5dbafd02ed8d4616de321ff329341e2063f402b 100644
--- a/quad/sw/modular_quad_pid/src/callbacks.c
+++ b/quad/sw/modular_quad_pid/src/callbacks.c
@@ -75,208 +75,14 @@ int cb_beginupdate(modular_structs_t *structs) {
/* Callbacks for configuration */
-int cb_setrollp(modular_structs_t *structs)
+int cb_setval(modular_structs_t *structs)
{
- structs->parameter_struct.roll_angle_pid.Kp = uart_buff_data_get_float(0);
- return 0;
-}
-int cb_setrolli(modular_structs_t *structs)
-{
- structs->parameter_struct.roll_angle_pid.Ki = uart_buff_data_get_float(0);
+ //structs->parameter_struct.roll_angle_pid.Kp = uart_buff_data_get_float(0);
return 0;
}
-int cb_setrolld(modular_structs_t *structs)
-{
- structs->parameter_struct.roll_angle_pid.Kd = uart_buff_data_get_float(0);
- return 0;
-}
-
-int cb_setpitchp(modular_structs_t *structs)
-{
- structs->parameter_struct.pitch_angle_pid.Kp = uart_buff_data_get_float(0);
- return 0;
-}
-
-int cb_setpitchi(modular_structs_t *structs)
-{
- structs->parameter_struct.pitch_angle_pid.Ki = uart_buff_data_get_float(0);
- return 0;
-}
-
-int cb_setpitchd(modular_structs_t *structs)
-{
- structs->parameter_struct.pitch_angle_pid.Kd = uart_buff_data_get_float(0);
- return 0;
-}
-
-int cb_setyawp(modular_structs_t *structs)
-{
- structs->parameter_struct.yaw_angle_pid.Kp = uart_buff_data_get_float(0);
- return 0;
-}
-
-int cb_setyawi(modular_structs_t *structs)
-{
- structs->parameter_struct.yaw_angle_pid.Ki = uart_buff_data_get_float(0);
- return 0;
-}
-
-int cb_setyawd(modular_structs_t *structs)
-{
- structs->parameter_struct.yaw_angle_pid.Kd = uart_buff_data_get_float(0);
- return 0;
-}
-
-int cb_setrollratep(modular_structs_t *structs)
-{
- structs->parameter_struct.roll_ang_vel_pid.Kp = uart_buff_data_get_float(0);
- return 0;
-}
-
-int cb_setrollratei(modular_structs_t *structs)
-{
- structs->parameter_struct.roll_ang_vel_pid.Ki = uart_buff_data_get_float(0);
- return 0;
-}
-
-int cb_setrollrated(modular_structs_t *structs)
-{
- structs->parameter_struct.roll_ang_vel_pid.Kd = uart_buff_data_get_float(0);
- return 0;
-}
-
-int cb_setpitchratep(modular_structs_t *structs)
-{
- structs->parameter_struct.pitch_ang_vel_pid.Kp = uart_buff_data_get_float(0);
- return 0;
-}
-
-int cb_setpitchratei(modular_structs_t *structs)
-{
- structs->parameter_struct.pitch_ang_vel_pid.Ki = uart_buff_data_get_float(0);
- return 0;
-}
-
-int cb_setpitchrated(modular_structs_t *structs)
-{
- structs->parameter_struct.pitch_ang_vel_pid.Kd = uart_buff_data_get_float(0);
- return 0;
-}
-
-int cb_setyawratep(modular_structs_t *structs)
-{
- structs->parameter_struct.yaw_ang_vel_pid.Kp = uart_buff_data_get_float(0);
- return 0;
-}
-
-int cb_setyawratei(modular_structs_t *structs)
-{
- structs->parameter_struct.yaw_ang_vel_pid.Ki = uart_buff_data_get_float(0);
- return 0;
-}
-
-int cb_setyawrated(modular_structs_t *structs)
-{
- structs->parameter_struct.yaw_ang_vel_pid.Kd = uart_buff_data_get_float(0);
- return 0;
-}
-
-int cb_setlocalxp(modular_structs_t *structs)
-{
- structs->parameter_struct.local_x_pid.Kp = uart_buff_data_get_float(0);
- return 0;
-}
-
-int cb_setlocalxi(modular_structs_t *structs)
-{
- structs->parameter_struct.local_x_pid.Ki = uart_buff_data_get_float(0);
- return 0;
-}
-
-int cb_setlocalxd(modular_structs_t *structs)
-{
- structs->parameter_struct.local_x_pid.Kd = uart_buff_data_get_float(0);
- return 0;
-}
-
-int cb_setlocalyp(modular_structs_t *structs)
-{
- structs->parameter_struct.local_y_pid.Kp = uart_buff_data_get_float(0);
- return 0;
-}
-
-int cb_setlocalyi(modular_structs_t *structs)
-{
- structs->parameter_struct.local_y_pid.Ki = uart_buff_data_get_float(0);
- return 0;
-}
-
-int cb_setlocalyd(modular_structs_t *structs)
-{
- structs->parameter_struct.local_y_pid.Kd = uart_buff_data_get_float(0);
- return 0;
-}
-
-int cb_setaltp(modular_structs_t *structs)
-{
- structs->parameter_struct.alt_pid.Kp = uart_buff_data_get_float(0);
- return 0;
-}
-
-int cb_setalti(modular_structs_t *structs)
-{
- structs->parameter_struct.alt_pid.Ki = uart_buff_data_get_float(0);
- return 0;
-}
-
-int cb_setaltd(modular_structs_t *structs)
-{
- structs->parameter_struct.alt_pid.Kd = uart_buff_data_get_float(0);
- return 0;
-}
-
-int cb_setrollsp(modular_structs_t *structs)
-{
- structs->setpoint_struct.desiredQuadPosition.roll = uart_buff_data_get_float(0);
- return 0;
-}
-
-int cb_setpitchsp(modular_structs_t *structs)
-{
- structs->setpoint_struct.desiredQuadPosition.pitch = uart_buff_data_get_float(0);
- return 0;
-}
-
-int cb_setyawsp(modular_structs_t *structs)
-{
- structs->setpoint_struct.desiredQuadPosition.yaw = uart_buff_data_get_float(0);
- return 0;
-}
-
-int cb_setaltsp(modular_structs_t *structs)
-{
- structs->setpoint_struct.desiredQuadPosition.alt_pos = uart_buff_data_get_float(0);
- return 0;
-}
-
-int cb_setlonsp(modular_structs_t *structs)
-{
- // TODO need to be able to take in a lat, lon position and convert it to quad x, y pos
- return 0;
-}
-
-int cb_setlatsp(modular_structs_t *structs)
-{
- // TODO need to be able to take in a lat, lon position and convert it to quad x, y pos
- return 0;
-}
-
-
-/* callbacks for getters */
-
-int cb_getyawp(modular_structs_t* structs) {
+int cb_getval(modular_structs_t* structs) {
char buf[255];
// Message logging number of messages received and size of payload received
@@ -285,182 +91,3 @@ int cb_getyawp(modular_structs_t* structs) {
send_data(MessageTypes[2].ID, MessageTypes[2].subtypes[0].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
return 0;
}
-
-int cb_getyawi(modular_structs_t* structs) {
- char buf[255];
-
- // Message logging number of messages received and size of payload received
- int length = snprintf(buf, sizeof buf, "%f", structs->parameter_struct.yaw_angle_pid.Ki);
-
- send_data(MessageTypes[2].ID, MessageTypes[2].subtypes[1].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
- return 0;
-}
-
-int cb_getyawd(modular_structs_t* structs) {
- char buf[255];
-
- // Message logging number of messages received and size of payload received
- int length = snprintf(buf, sizeof buf, "%f", structs->parameter_struct.yaw_angle_pid.Kd);
-
- send_data(MessageTypes[2].ID, MessageTypes[2].subtypes[2].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
- return 0;
-}
-
-int cb_getrollp(modular_structs_t* structs) {
- char buf[255];
-
- // Message logging number of messages received and size of payload received
- int length = snprintf(buf, sizeof buf, "%f", structs->parameter_struct.roll_angle_pid.Kp);
-
- send_data(MessageTypes[2].ID, MessageTypes[2].subtypes[3].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
- return 0;
-}
-
-int cb_getrolli(modular_structs_t* structs) {
- char buf[255];
-
- // Message logging number of messages received and size of payload received
- int length = snprintf(buf, sizeof buf, "%f", structs->parameter_struct.roll_angle_pid.Ki);
-
- send_data(MessageTypes[2].ID, MessageTypes[2].subtypes[4].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
- return 0;
-}
-
-int cb_getrolld(modular_structs_t* structs) {
- char buf[255];
-
- // Message logging number of messages received and size of payload received
- int length = snprintf(buf, sizeof buf, "%f", structs->parameter_struct.roll_angle_pid.Kd);
-
- send_data(MessageTypes[2].ID, MessageTypes[2].subtypes[5].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
- return 0;
-}
-
-int cb_getpitchp(modular_structs_t* structs) {
- char buf[255];
-
- // Message logging number of messages received and size of payload received
- int length = snprintf(buf, sizeof buf, "%f", structs->parameter_struct.pitch_angle_pid.Kp);
-
- send_data(MessageTypes[2].ID, MessageTypes[2].subtypes[6].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
- return 0;
-}
-
-int cb_getpitchi(modular_structs_t* structs) {
- char buf[255];
-
- // Message logging number of messages received and size of payload received
- int length = snprintf(buf, sizeof buf, "%f", structs->parameter_struct.pitch_angle_pid.Ki);
-
- send_data(MessageTypes[2].ID, MessageTypes[2].subtypes[7].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
- return 0;
-}
-
-int cb_getpitchd(modular_structs_t* structs) {
- char buf[255];
-
- // Message logging number of messages received and size of payload received
- int length = snprintf(buf, sizeof buf, "%f", structs->parameter_struct.pitch_angle_pid.Kd);
-
- send_data(MessageTypes[2].ID, MessageTypes[2].subtypes[8].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
- return 0;
-}
-
-
-
-
-
-
-
-
-
-
-int cb_getyawratep(modular_structs_t* structs) {
- char buf[255];
-
- // Message logging number of messages received and size of payload received
- int length = snprintf(buf, sizeof buf, "%f", structs->parameter_struct.yaw_ang_vel_pid.Kp);
-
- send_data(MessageTypes[2].ID, MessageTypes[2].subtypes[9].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
- return 0;
-}
-
-int cb_getyawratei(modular_structs_t* structs) {
- char buf[255];
-
- // Message logging number of messages received and size of payload received
- int length = snprintf(buf, sizeof buf, "%f", structs->parameter_struct.yaw_ang_vel_pid.Ki);
-
- send_data(MessageTypes[2].ID, MessageTypes[2].subtypes[10].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
- return 0;
-}
-
-int cb_getyawrated(modular_structs_t* structs) {
- char buf[255];
-
- // Message logging number of messages received and size of payload received
- int length = snprintf(buf, sizeof buf, "%f", structs->parameter_struct.yaw_ang_vel_pid.Kd);
-
- send_data(MessageTypes[2].ID, MessageTypes[2].subtypes[11].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
- return 0;
-}
-
-int cb_getrollratep(modular_structs_t* structs) {
- char buf[255];
-
- // Message logging number of messages received and size of payload received
- int length = snprintf(buf, sizeof buf, "%f", structs->parameter_struct.roll_ang_vel_pid.Kp);
-
- send_data(MessageTypes[2].ID, MessageTypes[2].subtypes[12].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
- return 0;
-}
-
-int cb_getrollratei(modular_structs_t* structs) {
- char buf[255];
-
- // Message logging number of messages received and size of payload received
- int length = snprintf(buf, sizeof buf, "%f", structs->parameter_struct.roll_ang_vel_pid.Ki);
-
- send_data(MessageTypes[2].ID, MessageTypes[2].subtypes[13].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
- return 0;
-}
-
-int cb_getrollrated(modular_structs_t* structs) {
- char buf[255];
-
- // Message logging number of messages received and size of payload received
- int length = snprintf(buf, sizeof buf, "%f", structs->parameter_struct.roll_ang_vel_pid.Kd);
-
- send_data(MessageTypes[2].ID, MessageTypes[2].subtypes[14].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
- return 0;
-}
-
-int cb_getpitchratep(modular_structs_t* structs) {
- char buf[255];
-
- // Message logging number of messages received and size of payload received
- int length = snprintf(buf, sizeof buf, "%f", structs->parameter_struct.pitch_ang_vel_pid.Kp);
-
- send_data(MessageTypes[2].ID, MessageTypes[2].subtypes[15].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
- return 0;
-}
-
-int cb_getpitchratei(modular_structs_t* structs) {
- char buf[255];
-
- // Message logging number of messages received and size of payload received
- int length = snprintf(buf, sizeof buf, "%f", structs->parameter_struct.pitch_ang_vel_pid.Ki);
-
- send_data(MessageTypes[2].ID, MessageTypes[2].subtypes[16].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
- return 0;
-}
-
-int cb_getpitchrated(modular_structs_t* structs) {
- char buf[255];
-
- // Message logging number of messages received and size of payload received
- int length = snprintf(buf, sizeof buf, "%f", structs->parameter_struct.pitch_ang_vel_pid.Kd);
-
- send_data(MessageTypes[2].ID, MessageTypes[2].subtypes[17].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
- return 0;
-}
diff --git a/quad/sw/modular_quad_pid/src/communication.c b/quad/sw/modular_quad_pid/src/communication.c
index 04a5705b1d5f6e0db4fefe8c7993ffe1cd8527cd..bba31571e0e5e6b26bf244f538d3719021d2b7d8 100644
--- a/quad/sw/modular_quad_pid/src/communication.c
+++ b/quad/sw/modular_quad_pid/src/communication.c
@@ -73,8 +73,7 @@ int process_packet(modular_structs_t *structs) {
metadata_t meta_data;
// parse metadata
meta_data.begin_char = uart_buff_get_u8(0);
- meta_data.msg_type = uart_buff_get_u8(1);
- meta_data.msg_subtype = uart_buff_get_u8(2);
+ meta_data.msg_type = uart_buff_get_u16(1);
meta_data.msg_id = uart_buff_get_u16(3);
meta_data.data_len = uart_buff_get_u16(5);
unsigned char packet_checksum = uart_buff_get_u8(7+meta_data.data_len);
@@ -93,7 +92,7 @@ int process_packet(modular_structs_t *structs) {
}
// Call appropriate function for packet
- (* (MessageTypes[meta_data.msg_type].subtypes[meta_data.msg_subtype].functionPtr))(structs);
+ (* (MessageTypes[meta_data.msg_type].functionPtr))(structs);
uart_buff_consume_packet();
@@ -167,24 +166,23 @@ void uart_interrupt_handler(XUartPs *InstancePtr) {
IsrStatus);
}
-int send_data(u16 type_id, u16 subtype_id, u16 msg_id, char* data, size_t size) {
+int send_data(u16 type_id, u16 msg_id, char* data, size_t size) {
//----------------------------------------------------------------------------------------------
// index|| 0 | 1 | 2 | 3 & 4 | 5 & 6 | 7+ | end |
//---------------------------------------------------------------------------------------------|
- // msg param|| beg char | msg type | msg subtype | msg id | data len (bytes) | data | checksum |
+ // msg param|| beg char | msg type | msg id | data len (bytes) | data | checksum |
//-------------------------------------------------------------------------------------------- |
- // bytes|| 1 | 1 | 1 | 2 | 2 | var | 1 |
+ // bytes|| 1 | 2 | 2 | 2 | var | 1 |
//----------------------------------------------------------------------------------------------
char formattedHeader[7];
// Begin Char:
formattedHeader[0] = BEGIN_CHAR;
- // Msg type:
- formattedHeader[1] = type_id;
- // Msg subtype
- formattedHeader[2] = subtype_id;
- //Msg id 2 bytes
+ // Msg type 2 bytes:
+ formattedHeader[1] = type_id & 0x000000ff;
+ formattedHeader[2] = (type_id >> 8) & 0x000000ff;
+ // Msg id 2 bytes
formattedHeader[3] = msg_id & 0x000000ff;
formattedHeader[4] = (msg_id >> 8) & 0x000000ff;
// Data length and data - bytes 5&6 for len, 7+ for data
@@ -195,10 +193,6 @@ int send_data(u16 type_id, u16 subtype_id, u16 msg_id, char* data, size_t size)
unsigned char packet_checksum = 0;
int i;
- // TODO: Look into uart0_sendBytes and see if it would be better to use
- // Send header
- //uart0_sendBytes(formattedHeader, 7);
-
for(i = 0; i < 7; i++) {
packet_checksum ^= formattedHeader[i];
uart0_sendByte(formattedHeader[i]);
diff --git a/quad/sw/modular_quad_pid/src/log_data.c b/quad/sw/modular_quad_pid/src/log_data.c
index f1e9e7a825a696486d3768708bc269841e745f4c..2b4bfe550e5facbd1dde865cc63cad1ed795cf2e 100644
--- a/quad/sw/modular_quad_pid/src/log_data.c
+++ b/quad/sw/modular_quad_pid/src/log_data.c
@@ -147,7 +147,7 @@ void printLogging(){
strcat(buf,header);
strcat(buf,units);
- send_data(MessageTypes[5].ID, MessageTypes[5].subtypes[0].ID, 0, buf, strlen(buf) + 1);
+ send_data(MessageTypes[LOG_TYPE_ID].ID, MessageTypes[5].subtypes[0].ID, 0, buf, strlen(buf) + 1);
//uart0_sendBytes(buf, strlen(buf));
//usleep(100000);
diff --git a/quad/sw/modular_quad_pid/src/type_def.h b/quad/sw/modular_quad_pid/src/type_def.h
index a13879a48884a77414b50ed5dbb421566b32f1c9..034f6b770b35f5e6764712043c1348888fb62ba6 100644
--- a/quad/sw/modular_quad_pid/src/type_def.h
+++ b/quad/sw/modular_quad_pid/src/type_def.h
@@ -28,8 +28,7 @@ enum flight_mode{
//----------------------------------------------------------------------------------------------
typedef struct {
char begin_char;
- char msg_type;
- char msg_subtype;
+ uint16_t msg_type;
uint16_t msg_id;
uint16_t data_len;
} metadata_t;
diff --git a/quad/sw/modular_quad_pid/src/uart.c b/quad/sw/modular_quad_pid/src/uart.c
index a31256e43d0afa9c1e30b5248ad558f04c0bbe8c..8328bed7b4f35388240e8c075b5e92cf4e7c4f70 100644
--- a/quad/sw/modular_quad_pid/src/uart.c
+++ b/quad/sw/modular_quad_pid/src/uart.c
@@ -298,8 +298,8 @@ void uart0_sendStr(char* str) {
void uart0_sendMetaData(metadata_t md)
{
uart0_sendByte(md.begin_char);
- uart0_sendByte(md.msg_type);
- uart0_sendByte(md.msg_subtype);
+ uart0_sendByte(md.msg_type & 0x00ff);
+ uart0_sendByte((md.msg_type >> 8) & 0x00ff);
uart0_sendByte(md.msg_id & 0x00ff);
uart0_sendByte((md.msg_id >> 8) & 0x00ff);
uart0_sendByte(md.data_len & 0x00ff);