Skip to content
Snippets Groups Projects
Commit 778fefc0 authored by javey's avatar javey
Browse files

Autonomous mode is throttle-only. Fixed bug in stringBuilder that would fail... 

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.
parent a7b1d92d
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
quad/sw/modular_quad_pid/src/commands.c
+ 80
− 270
View file @ 778fefc0
......@@ -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] = {};
......
quad/sw/modular_quad_pid/src/commands.h
+ 12
− 27
View file @ 778fefc0
......@@ -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);
......
quad/sw/modular_quad_pid/src/communication.c
+ 3
− 1
View file @ 778fefc0
......@@ -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
......
quad/sw/modular_quad_pid/src/control_algorithm.c
+ 6
− 6
View file @ 778fefc0
......@@ -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));
......
quad/sw/modular_quad_pid/src/stringBuilder.c
+ 2
− 1
View file @ 778fefc0
......@@ -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);
}
......
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