From 3a863ecd6a0daa2425248f2d568dab5cbd079b77 Mon Sep 17 00:00:00 2001
From: javey <javey@iastate.edu>
Date: Wed, 4 Jan 2017 14:01:49 -0600
Subject: [PATCH] Autonomous mode is throttle-only. Fixed bug in stringBuilder
that would fail to resize byte data. Packets that don't have a matching
checksum are thrown away now.
---
quad/sw/modular_quad_pid/src/commands.c | 350 ++++--------------
quad/sw/modular_quad_pid/src/commands.h | 39 +-
quad/sw/modular_quad_pid/src/communication.c | 4 +-
.../modular_quad_pid/src/control_algorithm.c | 12 +-
quad/sw/modular_quad_pid/src/stringBuilder.c | 3 +-
5 files changed, 103 insertions(+), 305 deletions(-)
diff --git a/quad/sw/modular_quad_pid/src/commands.c b/quad/sw/modular_quad_pid/src/commands.c
index 06e9f5640..97ed7720a 100644
--- a/quad/sw/modular_quad_pid/src/commands.c
+++ b/quad/sw/modular_quad_pid/src/commands.c
@@ -37,34 +37,35 @@ struct MessageType MessageTypes[MAX_TYPE] =
}
}
},
+
// CALIBRATION
{
// Message Type ID
0x01,
- // Calibration Subtypes (PID coefficients)
+ // Calibration Subtypes
{
- // yaw p constant subtype
+ // yaw setpoint subtype
{
// ID
0x00,
// Command text
- "setyawp",
+ "setyaw",
// Type of the command data
floatType,
// Function pointer
- setyawp
+ &yawset
},
- // yaw i constant subtype
+ // yaw p constant subtype
{
// ID
0x01,
// Command text
- "setyawi",
+ "setyawp",
// Type of the command data
floatType,
// Function pointer
- setyawi
+ &yawp
},
// yaw d constant subtype
{
@@ -75,29 +76,29 @@ struct MessageType MessageTypes[MAX_TYPE] =
// Type of the command data
floatType,
// Function pointer
- setyawd
+ &yawd
},
- // roll p constant subtype
+ // roll setpoint subtype
{
// ID
0x03,
// Command text
- "setrollp",
+ "setroll",
// Type of the command data
floatType,
// Function pointer
- setrollp
+ &rollset
},
- // roll i constant subtype
+ // roll p constant subtype
{
// ID
0x04,
// Command text
- "setrolli",
+ "setrollp",
// Type of the command data
floatType,
// Function pointer
- setrolli
+ &rollp
},
// roll d constant subtype
{
@@ -108,29 +109,29 @@ struct MessageType MessageTypes[MAX_TYPE] =
// Type of the command data
floatType,
// Function pointer
- setrolld
+ &rolld
},
- // pitch p constant subtype
+ // pitch setpoint subtype
{
// ID
0x06,
// Command text
- "setpitchp",
+ "setpitch",
// Type of the command data
floatType,
// Function pointer
- setpitchp
+ &pitchset
},
- // pitch i constant subtype
+ // pitch p constant subtype
{
// ID
0x07,
// Command text
- "setpitchi",
+ "setpitchp",
// Type of the command data
floatType,
// Function pointer
- setpitchi
+ &pitchp
},
// pitch d constant subtype
{
@@ -141,208 +142,109 @@ struct MessageType MessageTypes[MAX_TYPE] =
// Type of the command data
floatType,
// Function pointer
- setpitchd
+ &pitchd
},
- // yawrate p constant subtype
+ // throttle setpoint subtype
{
// ID
0x09,
// Command text
- "setyawratep",
+ "setthrottle",
// Type of the command data
floatType,
// Function pointer
- NULL
+ &throttleset
},
- // yawrate i constant subtype
+ // throttle p constant subtype
{
// ID
0x0A,
// Command text
- "setyawratei",
+ "setthrottlep",
// Type of the command data
floatType,
// Function pointer
- setyawratei
+ &throttlep
},
- // yawrate d constant subtype
+ // throttle i constant subtype
{
// ID
0x0B,
// Command text
- "setyawrated",
+ "setthrottlei",
// Type of the command data
floatType,
// Function pointer
- setyawrated
+ &throttlei
},
- // rollrate p constant subtype
+ // throttle d constant subtype
{
// ID
0x0C,
// Command text
- "setrollratep",
- // Type of the command data
- floatType,
- // Function pointer
- setrollratep
- },
- // rollrate i constant subtype
- {
- // ID
- 0x0D,
- // Command text
- "setrollratei",
- // Type of the command data
- floatType,
- // Function pointer
- setrollratei
- },
- // rollrate d constant subtype
- {
- // ID
- 0x0E,
- // Command text
- "setrollrated",
- // Type of the command data
- floatType,
- // Function pointer
- setrollrated
- },
- // pitchrate p constant subtype
- {
- // ID
- 0x0F,
- // Command text
- "setpitchratep",
- // Type of the command data
- floatType,
- // Function pointer
- setpitchratep
- },
- // pitchrate i constant subtype
- {
- // ID
- 0x10,
- // Command text
- "setpitchratei",
- // Type of the command data
- floatType,
- // Function pointer
- setpitchratei
- },
- // pitchrate d constant subtype
- {
- // ID
- 0x11,
- // Command text
- "setpitchrated",
- // Type of the command data
- floatType,
- // Function pointer
- setpitchrated
- },
- // height p constant subtype
- {
- // ID
- 0x12,
- // Command text
- "setheightp",
- // Type of the command data
- floatType,
- // Function pointer
- setheightp
- },
- // height i constant subtype
- {
- // ID
- 0x13,
- // Command text
- "setheighti",
- // Type of the command data
- floatType,
- // Function pointer
- setheighti
- },
- // height d constant subtype
- {
- // ID
- 0x14,
- // Command text
- "setheightd",
- // Type of the command data
- floatType,
- // Function pointer
- setheightd
- },
- // lat p constant subtype
- {
- // ID
- 0x15,
- // Command text
- "setlatp",
- // Type of the command data
- floatType,
- // Function pointer
- setlatp
- },
- // lat i constant subtype
- {
- // ID
- 0x16,
- // Command text
- "setlati",
+ "setthrottled",
// Type of the command data
floatType,
// Function pointer
- setlati
- },
- // lat d constant subtype
+ &throttled
+ }
+ }
+ },
+
+ // REQUEST
+ {
+ // Message Type ID
+ 0x02,
+
+ // Request Subtypes
+ {
+ // accelerometer subtype
{
- // ID
- 0x17,
+ // ID
+ 0x00,
// Command text
- "setlatd",
+ "accelreq",
// Type of the command data
floatType,
// Function pointer
- setlatd
+ &accelreq
},
- // long p constant subtype
+ // gyroscope subtype
{
- // ID
- 0x18,
+ // ID
+ 0x01,
// Command text
- "setlongp",
+ "gyroreq",
// Type of the command data
floatType,
// Function pointer
- setlongp
+ &gyroreq
},
- // long i constant subtype
+ // pitch angle subtype
{
- // ID
- 0x19,
+ // ID
+ 0x02,
// Command text
- "setlongi",
+ "reqpitchangle",
// Type of the command data
floatType,
// Function pointer
- setlongi
+ &pitchanglereq
},
- // long d constant subtype
+ // roll angle subtype
{
- // ID
- 0x1A,
+ // ID
+ 0x03,
// Command text
- "setlongd",
+ "reqrollangle",
// Type of the command data
floatType,
// Function pointer
- setlongd
+ &rollanglereq
}
}
},
+
// RESPONSE
{
// Message Type ID
@@ -500,84 +402,7 @@ int response(unsigned char *packet, int dataLen, modular_structs_t *structs)
// TODO: Erase memory leaks
-
-int setlatp(unsigned char *packet, int dataLen, modular_structs_t *structs) {
- // TODO
- return 0;
-}
-
-int setlati(unsigned char *packet, int dataLen, modular_structs_t *structs) {
- // TODO
- return 0;
-}
-
-int setlatd(unsigned char *packet, int dataLen, modular_structs_t *structs) {
- // TODO
- return 0;
-}
-
-int setlongp(unsigned char *packet, int dataLen, modular_structs_t *structs) {
- // TODO
- return 0;
-}
-
-int setlongi(unsigned char *packet, int dataLen, modular_structs_t *structs) {
- // TODO
- return 0;
-}
-
-int setlongd(unsigned char *packet, int dataLen, modular_structs_t *structs) {
- // TODO
- return 0;
-}
-
-int setpitchratep(unsigned char *packet, int dataLen, modular_structs_t *structs) {
- // TODO
- return 0;
-}
-
-int setpitchratei(unsigned char *packet, int dataLen, modular_structs_t *structs) {
- // TODO
- return 0;
-}
-
-int setpitchrated(unsigned char *packet, int dataLen, modular_structs_t *structs) {
- // TODO
- return 0;
-}
-
-int setrollratep(unsigned char *packet, int dataLen, modular_structs_t *structs) {
- // TODO
- return 0;
-}
-
-int setrollratei(unsigned char *packet, int dataLen, modular_structs_t *structs) {
- // TODO
- return 0;
-}
-
-int setrollrated(unsigned char *packet, int dataLen, modular_structs_t *structs) {
- // TODO
- return 0;
-}
-
-int setyawratep(unsigned char *packet, int dataLen, modular_structs_t *structs) {
- // TODO
- return 0;
-}
-
-int setyawratei(unsigned char *packet, int dataLen, modular_structs_t *structs) {
- // TODO
- return 0;
-}
-
-int setyawrated(unsigned char *packet, int dataLen, modular_structs_t *structs) {
- // TODO
- return 0;
-}
-
-
-int setyaw(unsigned char *packet, int dataLen, modular_structs_t *structs)
+int yawset(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
float value;
char buf[255] = {};
@@ -619,7 +444,7 @@ int setyaw(unsigned char *packet, int dataLen, modular_structs_t *structs)
return 0;
}
-int setyawp(unsigned char *packet, int dataLen, modular_structs_t *structs)
+int yawp(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
float value;
char buf[255] = {0};
@@ -657,12 +482,7 @@ int setyawp(unsigned char *packet, int dataLen, modular_structs_t *structs)
return 0;
}
-int setyawi(unsigned char *packet, int dataLen, modular_structs_t *structs) {
- // TODO
- return 0;
-}
-
-int setyawd(unsigned char *packet, int dataLen, modular_structs_t *structs)
+int yawd(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
float value;
char buf[255] = {};
@@ -700,7 +520,7 @@ int setyawd(unsigned char *packet, int dataLen, modular_structs_t *structs)
return 0;
}
-int setroll(unsigned char *packet, int dataLen, modular_structs_t *structs)
+int rollset(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
float value;
char buf[255] = {};
@@ -736,7 +556,7 @@ int setroll(unsigned char *packet, int dataLen, modular_structs_t *structs)
return 0;
}
-int setrollp(unsigned char *packet, int dataLen, modular_structs_t *structs)
+int rollp(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
float value;
char buf[255] = {};
@@ -772,12 +592,7 @@ int setrollp(unsigned char *packet, int dataLen, modular_structs_t *structs)
return 0;
}
-int setrolli(unsigned char *packet, int dataLen, modular_structs_t *structs) {
- // TODO
- return 0;
-}
-
-int setrolld(unsigned char *packet, int dataLen, modular_structs_t *structs)
+int rolld(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
float value;
char buf[255] = {};
@@ -813,7 +628,7 @@ int setrolld(unsigned char *packet, int dataLen, modular_structs_t *structs)
return 0;
}
-int setpitch(unsigned char *packet, int dataLen, modular_structs_t *structs)
+int pitchset(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
float value;
char buf[255] = {};
@@ -849,7 +664,7 @@ int setpitch(unsigned char *packet, int dataLen, modular_structs_t *structs)
return 0;
}
-int setpitchp(unsigned char *packet, int dataLen, modular_structs_t *structs)
+int pitchp(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
float value;
char buf[255] = {};
@@ -885,12 +700,7 @@ int setpitchp(unsigned char *packet, int dataLen, modular_structs_t *structs)
return 0;
}
-int setpitchi(unsigned char *packet, int dataLen, modular_structs_t *structs) {
- // TODO
- return 0;
-}
-
-int setpitchd(unsigned char *packet, int dataLen, modular_structs_t *structs)
+int pitchd(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
float value;
char buf[255] = {};
@@ -928,7 +738,7 @@ int setpitchd(unsigned char *packet, int dataLen, modular_structs_t *structs)
// ------------------------------------------------------------
// These should be renamed to altitude!
-int setheight(unsigned char *packet, int dataLen, modular_structs_t *structs)
+int throttleset(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
float value;
char buf[255] = {};
@@ -964,7 +774,7 @@ int setheight(unsigned char *packet, int dataLen, modular_structs_t *structs)
return 0;
}
-int setheightp(unsigned char *packet, int dataLen, modular_structs_t *structs)
+int throttlep(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
float value;
char buf[255] = {};
@@ -1000,7 +810,7 @@ int setheightp(unsigned char *packet, int dataLen, modular_structs_t *structs)
return 0;
}
-int setheighti(unsigned char *packet, int dataLen, modular_structs_t *structs)
+int throttlei(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
float value;
char buf[255] = {};
@@ -1036,7 +846,7 @@ int setheighti(unsigned char *packet, int dataLen, modular_structs_t *structs)
return 0;
}
-int setheightd(unsigned char *packet, int dataLen, modular_structs_t *structs)
+int throttled(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
float value;
char buf[255] = {};
diff --git a/quad/sw/modular_quad_pid/src/commands.h b/quad/sw/modular_quad_pid/src/commands.h
index 810414469..7861a84bd 100644
--- a/quad/sw/modular_quad_pid/src/commands.h
+++ b/quad/sw/modular_quad_pid/src/commands.h
@@ -44,34 +44,19 @@ int update(unsigned char *c, int dataLen, modular_structs_t *structs);
int beginupdate(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 setheightp(unsigned char *c, int dataLen, modular_structs_t *structs);
-int setheighti(unsigned char *c, int dataLen, modular_structs_t *structs);
-int setheightd(unsigned char *c, int dataLen, modular_structs_t *structs);
-int setlatp(unsigned char *c, int dataLen, modular_structs_t *structs);
-int setlati(unsigned char *c, int dataLen, modular_structs_t *structs);
-int setlatd(unsigned char *c, int dataLen, modular_structs_t *structs);
-int setlongp(unsigned char *c, int dataLen, modular_structs_t *structs);
-int setlongi(unsigned char *c, int dataLen, modular_structs_t *structs);
-int setlongd(unsigned char *c, int dataLen, modular_structs_t *structs);
-int setpitchp(unsigned char *c, int dataLen, modular_structs_t *structs);
-int setpitchi(unsigned char *c, int dataLen, modular_structs_t *structs);
-int setpitchd(unsigned char *c, int dataLen, modular_structs_t *structs);
-int setrollp(unsigned char *c, int dataLen, modular_structs_t *structs);
-int setrolli(unsigned char *c, int dataLen, modular_structs_t *structs);
-int setrolld(unsigned char *c, int dataLen, modular_structs_t *structs);
-int setyawp(unsigned char *c, int dataLen, modular_structs_t *structs);
-int setyawi(unsigned char *c, int dataLen, modular_structs_t *structs);
-int setyawd(unsigned char *c, int dataLen, modular_structs_t *structs);
-int setpitchratep(unsigned char *c, int dataLen, modular_structs_t *structs);
-int setpitchratei(unsigned char *c, int dataLen, modular_structs_t *structs);
-int setpitchrated(unsigned char *c, int dataLen, modular_structs_t *structs);
-int setrollratep(unsigned char *c, int dataLen, modular_structs_t *structs);
-int setrollratei(unsigned char *c, int dataLen, modular_structs_t *structs);
-int setrollrated(unsigned char *c, int dataLen, modular_structs_t *structs);
-int setyawratep(unsigned char *c, int dataLen, modular_structs_t *structs);
-int setyawratei(unsigned char *c, int dataLen, modular_structs_t *structs);
-int setyawrated(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);
diff --git a/quad/sw/modular_quad_pid/src/communication.c b/quad/sw/modular_quad_pid/src/communication.c
index b1c23b845..84eacfd72 100644
--- a/quad/sw/modular_quad_pid/src/communication.c
+++ b/quad/sw/modular_quad_pid/src/communication.c
@@ -110,8 +110,10 @@ int parse_packet(unsigned char * packet, unsigned char ** data, metadata_t * met
}
// compare checksum
- if(packet_checksum != calculated_checksum)
+ if(packet_checksum != calculated_checksum) {
+ return 1;
printf("Checksums did not match (Quadlog): 0x%02x\t0x%02x\n", packet_checksum, calculated_checksum);
+ }
//////////////////////////////
// Send an acknowledgment packet
diff --git a/quad/sw/modular_quad_pid/src/control_algorithm.c b/quad/sw/modular_quad_pid/src/control_algorithm.c
index f1dd773cf..b057a30be 100644
--- a/quad/sw/modular_quad_pid/src/control_algorithm.c
+++ b/quad/sw/modular_quad_pid/src/control_algorithm.c
@@ -190,13 +190,13 @@
parameter_struct->pitch_angle_pid.current_point = sensor_struct->pitch_angle_filtered;
parameter_struct->pitch_angle_pid.setpoint =
- (user_defined_struct->flight_mode == AUTO_FLIGHT_MODE)?
- (parameter_struct->local_x_pid.pid_correction) + pitch_trim : user_input_struct->pitch_angle_manual_setpoint;
+ /*(user_defined_struct->flight_mode == AUTO_FLIGHT_MODE)?
+ (parameter_struct->local_x_pid.pid_correction) + pitch_trim :*/ user_input_struct->pitch_angle_manual_setpoint;
parameter_struct->roll_angle_pid.current_point = sensor_struct->roll_angle_filtered;
parameter_struct->roll_angle_pid.setpoint =
- (user_defined_struct->flight_mode == AUTO_FLIGHT_MODE)?
- (parameter_struct->local_y_pid.pid_correction) + roll_trim : user_input_struct->roll_angle_manual_setpoint;
+ /*(user_defined_struct->flight_mode == AUTO_FLIGHT_MODE)?
+ (parameter_struct->local_y_pid.pid_correction) + roll_trim :*/ user_input_struct->roll_angle_manual_setpoint;
//logging and PID computation
@@ -223,8 +223,8 @@
// psi_dot is the angular velocity about the z-axis
// it is calculated from using the gimbal equations
parameter_struct->yaw_ang_vel_pid.current_point = sensor_struct->psi_dot;
- parameter_struct->yaw_ang_vel_pid.setpoint = (user_defined_struct->flight_mode == AUTO_FLIGHT_MODE)?
- parameter_struct->yaw_angle_pid.pid_correction : user_input_struct->yaw_manual_setpoint; // no trim added because the controller already works well
+ parameter_struct->yaw_ang_vel_pid.setpoint = /*(user_defined_struct->flight_mode == AUTO_FLIGHT_MODE)?
+ parameter_struct->yaw_angle_pid.pid_correction :*/ user_input_struct->yaw_manual_setpoint; // no trim added because the controller already works well
//logging and PID computation
log_struct->ang_vel_pitch_PID_values = pid_computation(&(parameter_struct->pitch_ang_vel_pid));
diff --git a/quad/sw/modular_quad_pid/src/stringBuilder.c b/quad/sw/modular_quad_pid/src/stringBuilder.c
index 1f24039cd..d830cfb02 100644
--- a/quad/sw/modular_quad_pid/src/stringBuilder.c
+++ b/quad/sw/modular_quad_pid/src/stringBuilder.c
@@ -39,7 +39,8 @@ int stringBuilder_maybeExpand(stringBuilder_t* sb) {
return STRINGBUILDER_NO_MEM_FOR_EXPANSION;
} else {
// Got mem for new buf, copy from old buf
- strncpy(sb->buf, oldBuf, sb->length + 1);
+ //strncpy(sb->buf, oldBuf, sb->length + 1);
+ memcpy(sb->buf, oldBuf, sb->length);
sb->capacity = newCapacity;
free(oldBuf);
}
--
GitLab