diff --git a/quad/src/gen_diagram/network.dot b/quad/src/gen_diagram/network.dot
index 7321d05e93f9aec20e120f62657e66ad43cab924..25a5dc165c2dac141e2c6694b707ec96db8eaf4a 100644
--- a/quad/src/gen_diagram/network.dot
+++ b/quad/src/gen_diagram/network.dot
@@ -3,12 +3,12 @@ rankdir="LR"
"Roll PID"[shape=record
label="<f0>Roll PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=15.000] |<f5> [Ki=0.000] |<f6> [Kd=0.200] |<f7> [alpha=0.000]"]
"Roll" -> "Roll PID":f1 [label="Constant"]
-"RC Roll" -> "Roll PID":f2 [label="Constant"]
+"Y Vel PID" -> "Roll PID":f2 [label="Correction"]
"Ts_IMU" -> "Roll PID":f3 [label="Constant"]
"Pitch PID"[shape=record
label="<f0>Pitch PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=15.000] |<f5> [Ki=0.000] |<f6> [Kd=0.200] |<f7> [alpha=0.000]"]
"Pitch trim add" -> "Pitch PID":f1 [label="Sum"]
-"RC Pitch" -> "Pitch PID":f2 [label="Constant"]
+"X Vel PID" -> "Pitch PID":f2 [label="Correction"]
"Ts_IMU" -> "Pitch PID":f3 [label="Constant"]
"Yaw PID"[shape=record
label="<f0>Yaw PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=2.600] |<f5> [Ki=0.000] |<f6> [Kd=0.000] |<f7> [alpha=0.000]"]
@@ -28,7 +28,7 @@ label="<f0>Pitch Rate PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |
"Yaw Rate PID"[shape=record
label="<f0>Yaw Rate PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=29700.000] |<f5> [Ki=0.000] |<f6> [Kd=0.000] |<f7> [alpha=0.000]"]
"dPsi" -> "Yaw Rate PID":f1 [label="Constant"]
-"RC Yaw" -> "Yaw Rate PID":f2 [label="Constant"]
+"Yaw PID" -> "Yaw Rate PID":f2 [label="Correction"]
"Ts_IMU" -> "Yaw Rate PID":f3 [label="Constant"]
"X pos PID"[shape=record
label="<f0>X pos PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=-0.015] |<f5> [Ki=-0.005] |<f6> [Kd=-0.030] |<f7> [alpha=0.000]"]
@@ -106,7 +106,7 @@ label="<f0>RC Yaw |<f1> [Constant=0.000]"]
label="<f0>RC Throttle |<f1> [Constant=0.000]"]
"Signal Mixer"[shape=record
label="<f0>Signal Mixer |<f1> --\>Throttle |<f2> --\>Pitch |<f3> --\>Roll |<f4> --\>Yaw"]
-"RC Throttle" -> "Signal Mixer":f1 [label="Constant"]
+"T trim add" -> "Signal Mixer":f1 [label="Sum"]
"P PWM Clamp" -> "Signal Mixer":f2 [label="Bounded"]
"R PWM Clamp" -> "Signal Mixer":f3 [label="Bounded"]
"Y PWM Clamp" -> "Signal Mixer":f4 [label="Bounded"]
@@ -114,4 +114,36 @@ label="<f0>Signal Mixer |<f1> --\>Throttle |<f2> --\>Pitch |<f3> --\>Roll |<f4>
label="<f0>Ts_IMU |<f1> [Constant=0.005]"]
"Ts_VRPN"[shape=record
label="<f0>Ts_VRPN |<f1> [Constant=0.040]"]
+"zero"[shape=record
+label="<f0>zero |<f1> [Constant=0.000]"]
+"X Vel PID"[shape=record
+label="<f0>X Vel PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=0.000] |<f5> [Ki=0.000] |<f6> [Kd=0.000] |<f7> [alpha=0.000]"]
+"X Vel" -> "X Vel PID":f1 [label="Correction"]
+"X Vel Clamp" -> "X Vel PID":f2 [label="Bounded"]
+"Ts_VRPN" -> "X Vel PID":f3 [label="Constant"]
+"Y Vel PID"[shape=record
+label="<f0>Y Vel PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=0.000] |<f5> [Ki=0.000] |<f6> [Kd=0.000] |<f7> [alpha=0.000]"]
+"Y Vel" -> "Y Vel PID":f1 [label="Correction"]
+"Y vel Clamp" -> "Y Vel PID":f2 [label="Bounded"]
+"Ts_VRPN" -> "Y Vel PID":f3 [label="Constant"]
+"X Vel"[shape=record
+label="<f0>X Vel |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=0.000] |<f5> [Ki=0.000] |<f6> [Kd=1.000] |<f7> [alpha=0.000]"]
+"VRPN X" -> "X Vel":f1 [label="Constant"]
+"zero" -> "X Vel":f2 [label="Constant"]
+"Ts_VRPN" -> "X Vel":f3 [label="Constant"]
+"Y Vel"[shape=record
+label="<f0>Y Vel |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=0.000] |<f5> [Ki=0.000] |<f6> [Kd=1.000] |<f7> [alpha=0.000]"]
+"VRPN Y" -> "Y Vel":f1 [label="Constant"]
+"zero" -> "Y Vel":f2 [label="Constant"]
+"Ts_VRPN" -> "Y Vel":f3 [label="Constant"]
+"X Vel Clamp"[shape=record
+label="<f0>X Vel Clamp |<f1> --\>Bounds in |<f2> [Min=-2.000] |<f3> [Max=2.000]"]
+"X pos PID" -> "X Vel Clamp":f1 [label="Correction"]
+"Y vel Clamp"[shape=record
+label="<f0>Y vel Clamp |<f1> --\>Bounds in |<f2> [Min=-2.000] |<f3> [Max=2.000]"]
+"Y pos PID" -> "Y vel Clamp":f1 [label="Correction"]
+"X Stick Gain"[shape=record
+label="<f0>X Stick Gain |<f1> --\>Input |<f2> [Gain=5.000]"]
+"Y Stick Gain"[shape=record
+label="<f0>Y Stick Gain |<f1> --\>Input |<f2> [Gain=-5.000]"]
}
\ No newline at end of file
diff --git a/quad/src/gen_diagram/network.png b/quad/src/gen_diagram/network.png
index 17aa66c2d549bb483b5812ec7e80e0d53933d358..6e1c3ebc4db038a6d42839315233e453f2e123fe 100644
Binary files a/quad/src/gen_diagram/network.png and b/quad/src/gen_diagram/network.png differ
diff --git a/quad/src/graph_blocks/node_pid.c b/quad/src/graph_blocks/node_pid.c
index df6b611100aec808a8728a3fbe435ecbe5078557..e776b7ed28e9295c9f30be9adb104ae3e9598647 100644
--- a/quad/src/graph_blocks/node_pid.c
+++ b/quad/src/graph_blocks/node_pid.c
@@ -6,9 +6,10 @@ static double FLT_EPSILON = 0.0001;
struct pid_node_state {
- double prev_error; // Previous error
+ double prev_val; // Previous input value
double acc_error; // Accumulated error
double last_filtered; // Last output from the filtered derivative
+ int just_reset; // Set to 1 after a call to reset
};
// The generic PID diagram. This function takes in pid inputs (CUR_POINT and SETOINT) and calculates the output "pid_correction"
@@ -50,23 +51,29 @@ static void pid_computation(void *state, const double* params, const double *inp
pid_state->acc_error += error;
}
+ if (pid_state->just_reset) {
+ // On first call after a reset, set the previous value to
+ // the current value to prevent a spike in derivative
+ pid_state->prev_val = inputs[PID_CUR_POINT];
+ pid_state->just_reset = 0;
+ }
// Compute each term's contribution
P = params[PID_KP] * error;
I = params[PID_KI] * pid_state->acc_error * inputs[PID_DT];
// Low-pass filter on derivative
- double change_in_error = error - pid_state->prev_error;
+ double change_in_value = inputs[PID_CUR_POINT] - pid_state->prev_val;
double term1 = params[PID_ALPHA] * pid_state->last_filtered;
- double derivative = change_in_error / inputs[PID_DT];
+ double derivative = change_in_value / inputs[PID_DT];
if (inputs[PID_DT] == 0) { // Divide by zero check
derivative = 0;
}
double term2 = params[PID_KD] * (1.0f - params[PID_ALPHA]) * derivative;
D = term1 + term2;
pid_state->last_filtered = D; // Store filtered value for next filter iteration
- pid_state->prev_error = error; // Store the current error into the state
+ pid_state->prev_val = inputs[PID_CUR_POINT]; // Store the current error into the state
- outputs[PID_CORRECTION] = P + I + D; // Store the computed correction
+ outputs[PID_CORRECTION] = P + I - D; // Store the computed correction
}
// This function sets the accumulated error and previous error to 0
@@ -74,8 +81,8 @@ static void pid_computation(void *state, const double* params, const double *inp
static void reset_pid(void *state) {
struct pid_node_state* pid_state = (struct pid_node_state*)state;
pid_state->acc_error = 0;
- pid_state->prev_error = 0;
pid_state->last_filtered = 0;
+ pid_state->just_reset = 1;
}
diff --git a/quad/src/quad_app/biquad_filter.c b/quad/src/quad_app/biquad_filter.c
new file mode 100644
index 0000000000000000000000000000000000000000..3fb3327e4106aea63f68b1f017508b20879337e3
--- /dev/null
+++ b/quad/src/quad_app/biquad_filter.c
@@ -0,0 +1,29 @@
+#include "biquad_filter.h"
+
+struct biquadState filter_make_state(float a0, float a1, float a2,
+ float b1, float b2) {
+ struct biquadState state = {
+ .delayed = {0,0},
+ .a0 = a0,
+ .a1 = a1,
+ .a2 = a2,
+ .b1 = b1,
+ .b2 = b2
+ };
+ return state;
+}
+
+// http://www.earlevel.com/main/2003/02/28/biquads/
+// Direct form II
+float biquad_execute(struct biquadState* state, float new_input) {
+ float left_sum = new_input -
+ (state->delayed[0] * state->b1) -
+ (state->delayed[1] * state->b2);
+ float output = (left_sum * state->a0) +
+ (state->delayed[0] * state->a1) +
+ (state->delayed[1] * state->a2);
+
+ state->delayed[1] = state->delayed[0];
+ state->delayed[0] = left_sum;
+ return output;
+}
\ No newline at end of file
diff --git a/quad/src/quad_app/biquad_filter.h b/quad/src/quad_app/biquad_filter.h
new file mode 100644
index 0000000000000000000000000000000000000000..a9076573883767901440683df9fe1c45abd52ae7
--- /dev/null
+++ b/quad/src/quad_app/biquad_filter.h
@@ -0,0 +1,14 @@
+#ifndef BIQUAD_FILTER_H
+#define BIQUAD_FILTER_H
+
+struct biquadState {
+ float delayed[2];
+ float a0, a1, a2, b1, b2;
+};
+
+float biquad_execute(struct biquadState* state, float new_input);
+
+
+struct biquadState filter_make_state(float a0, float a1, float a2,
+ float b1, float b2);
+#endif // BIQUAD_FILTER_H
\ No newline at end of file
diff --git a/quad/src/quad_app/control_algorithm.c b/quad/src/quad_app/control_algorithm.c
index 0391ca2dd23cca86ae5b5c0fd9191da3e28eaca1..710f26b34144c2827d46aa0363575541a12aae97 100644
--- a/quad/src/quad_app/control_algorithm.c
+++ b/quad/src/quad_app/control_algorithm.c
@@ -19,9 +19,11 @@
void connect_autonomous(parameter_t* ps) {
struct computation_graph* graph = ps->graph;
- graph_set_source(graph, ps->pitch_pid, PID_SETPOINT, ps->x_pos_pid, PID_CORRECTION);
- graph_set_source(graph, ps->roll_pid, PID_SETPOINT, ps->y_pos_pid, PID_CORRECTION);
- //graph_set_source(graph, ps->mixer, MIXER_THROTTLE, ps->throttle_trim_add, ADD_SUM);
+ //graph_set_source(graph, ps->pitch_pid, PID_SETPOINT, ps->x_pos_pid, PID_CORRECTION);
+ //graph_set_source(graph, ps->roll_pid, PID_SETPOINT, ps->y_pos_pid, PID_CORRECTION);
+ graph_set_source(graph, ps->pitch_pid, PID_SETPOINT, ps->x_vel_pid, PID_CORRECTION);
+ graph_set_source(graph, ps->roll_pid, PID_SETPOINT, ps->y_vel_pid, PID_CORRECTION);
+ graph_set_source(graph, ps->mixer, MIXER_THROTTLE, ps->throttle_trim_add, ADD_SUM);
graph_set_source(graph, ps->yaw_r_pid, PID_SETPOINT, ps->yaw_pid, PID_CORRECTION);
}
@@ -195,6 +197,58 @@ int control_algorithm_init(parameter_t * ps)
graph_set_param_val(graph, ps->clamp_d_pwmY, BOUNDS_MIN, -PWM_DIFF_BOUNDS);
graph_set_param_val(graph, ps->clamp_d_pwmY, BOUNDS_MAX, PWM_DIFF_BOUNDS);
+
+ // Velocity stuff
+ int zero_block = graph_add_defined_block(graph, BLOCK_CONSTANT, "zero");
+
+ ps->x_vel_pid = graph_add_defined_block(graph, BLOCK_PID, "X Vel PID");
+ ps->y_vel_pid = graph_add_defined_block(graph, BLOCK_PID, "Y Vel PID");
+ ps->x_vel = graph_add_defined_block(graph, BLOCK_PID, "X Vel");
+ ps->y_vel = graph_add_defined_block(graph, BLOCK_PID, "Y Vel");
+ ps->x_vel_clamp = graph_add_defined_block(graph, BLOCK_BOUNDS, "X Vel Clamp");
+ ps->y_vel_clamp = graph_add_defined_block(graph, BLOCK_BOUNDS, "Y vel Clamp");
+ ps->vel_x_gain = graph_add_defined_block(graph, BLOCK_GAIN, "X Stick Gain");
+ ps->vel_y_gain = graph_add_defined_block(graph, BLOCK_GAIN, "Y Stick Gain");
+
+ graph_set_source(graph, ps->x_vel_pid, PID_DT, ps->pos_time, CONST_VAL);
+ graph_set_source(graph, ps->y_vel_pid, PID_DT, ps->pos_time, CONST_VAL);
+ graph_set_source(graph, ps->x_vel, PID_DT, ps->pos_time, CONST_VAL);
+ graph_set_source(graph, ps->y_vel, PID_DT, ps->pos_time, CONST_VAL);
+
+ graph_set_source(graph, ps->x_vel_pid, PID_SETPOINT, ps->x_vel, PID_CORRECTION);
+ graph_set_source(graph, ps->y_vel_pid, PID_SETPOINT, ps->y_vel, PID_CORRECTION);
+ graph_set_source(graph, ps->x_vel_pid, PID_CUR_POINT, ps->x_vel, PID_CORRECTION);
+ graph_set_source(graph, ps->y_vel_pid, PID_CUR_POINT, ps->y_vel, PID_CORRECTION);
+
+ graph_set_source(graph, ps->x_vel, PID_CUR_POINT, ps->vrpn_x, CONST_VAL);
+ graph_set_source(graph, ps->y_vel, PID_CUR_POINT, ps->vrpn_y, CONST_VAL);
+ graph_set_source(graph, ps->x_vel, PID_SETPOINT, zero_block, CONST_VAL);
+ graph_set_source(graph, ps->y_vel, PID_SETPOINT, zero_block, CONST_VAL);
+
+ graph_set_source(graph, ps->x_vel_pid, PID_SETPOINT, ps->x_vel_clamp, GAIN_RESULT);
+ graph_set_source(graph, ps->y_vel_pid, PID_SETPOINT, ps->y_vel_clamp, GAIN_RESULT);
+
+ graph_set_source(graph, ps->x_vel_clamp, BOUNDS_IN, ps->x_pos_pid, PID_CORRECTION);
+ graph_set_source(graph, ps->y_vel_clamp, BOUNDS_IN, ps->y_pos_pid, PID_CORRECTION);
+ //graph_set_source(graph, ps->vel_x_gain, GAIN_INPUT, ps->rc_pitch, CONST_VAL);
+ //graph_set_source(graph, ps->vel_y_gain, GAIN_INPUT, ps->rc_roll, CONST_VAL);
+ //graph_set_source(graph, ps->x_vel_clamp, BOUNDS_IN, ps->vel_x_gain, GAIN_RESULT);
+ //graph_set_source(graph, ps->y_vel_clamp, BOUNDS_IN, ps->vel_y_gain, GAIN_RESULT);
+
+ float vel_clamps = 2;
+ graph_set_param_val(graph, ps->x_vel_clamp, BOUNDS_MIN, -vel_clamps);
+ graph_set_param_val(graph, ps->x_vel_clamp, BOUNDS_MAX, vel_clamps);
+ graph_set_param_val(graph, ps->y_vel_clamp, BOUNDS_MIN, -vel_clamps);
+ graph_set_param_val(graph, ps->y_vel_clamp, BOUNDS_MAX, vel_clamps);
+
+
+ graph_set_param_val(graph, ps->x_vel, PID_KD, 1);
+ graph_set_param_val(graph, ps->y_vel, PID_KD, 1);
+
+ graph_set_param_val(graph, ps->vel_x_gain, GAIN_GAIN, 5);
+ graph_set_param_val(graph, ps->vel_y_gain, GAIN_GAIN, -5);
+
+
// Set trims
graph_set_param_val(graph, ps->pitch_trim, CONST_SET, 0.02);
diff --git a/quad/src/quad_app/iic_utils.c b/quad/src/quad_app/iic_utils.c
index a0061e6b00432768feded1b15495c850fad5a914..a9fbd272c723f5a81942a9edef2adcf7dae613fb 100644
--- a/quad/src/quad_app/iic_utils.c
+++ b/quad/src/quad_app/iic_utils.c
@@ -30,7 +30,7 @@ int iic0_mpu9150_start(){
// Set clock reference to Z Gyro
iic0_mpu9150_write(0x6B, 0x03);
// Configure Digital Low/High Pass filter
- iic0_mpu9150_write(0x1A,0x05); // Level 5 low pass on gyroscope
+ iic0_mpu9150_write(0x1A,0x03); // Level 3 low pass on gyroscope
// Configure Gyro to 2000dps, Accel. to +/-8G
iic0_mpu9150_write(0x1B, 0x18);
@@ -182,10 +182,6 @@ int iic0_mpu9150_read_gam(gam_t* gam) {
gam->accel_y = (raw_accel_y / 4096.0) + ACCEL_Y_BIAS;
gam->accel_z = (raw_accel_z / 4096.0) + ACCEL_Z_BIAS;
- //Get X and Y angles
- // javey: this assigns accel_(pitch/roll) in units of radians
- gam->accel_pitch = atan(gam->accel_x / sqrt(gam->accel_y*gam->accel_y + gam->accel_z*gam->accel_z));
- gam->accel_roll = -atan(gam->accel_y / sqrt(gam->accel_x*gam->accel_x + gam->accel_z*gam->accel_z)); // negative because sensor board is upside down
//Convert gyro data to rate (we're only using the most 12 significant bits)
gyro_x = (sensor_data[GYR_X_H] << 8) | (sensor_data[GYR_X_L]); //* G_GAIN;
diff --git a/quad/src/quad_app/initialize_components.c b/quad/src/quad_app/initialize_components.c
index 57d3e2b96eaef8951425652c4fb61ac5ec4b1529..967eaa6294b342641fc1dcdfb0b3327ea4490427 100644
--- a/quad/src/quad_app/initialize_components.c
+++ b/quad/src/quad_app/initialize_components.c
@@ -99,5 +99,7 @@ int init_structs(modular_structs_t *structs) {
if(sensor_init(&structs->raw_sensor_struct, &structs->sensor_struct) == -1)
return -1;
+ sensor_processing_init(&structs->sensor_struct);
+
return 0;
}
diff --git a/quad/src/quad_app/log_data.c b/quad/src/quad_app/log_data.c
index 3e67c14aa36862cae1ad885955e944a86056007b..e67a075394ee6099d89f0c326a07f656a72694ee 100644
--- a/quad/src/quad_app/log_data.c
+++ b/quad/src/quad_app/log_data.c
@@ -95,6 +95,7 @@ void initialize_logging(log_t* log_struct, parameter_t* ps) {
char* rad_s = "rad/s";
char* pwm_val = "10ns_dutycycle";
char* m = "m";
+ char* m_s = "m/s";
addOutputToLog(log_struct, ps->alt_pid, PID_CORRECTION, pwm_val);
addOutputToLog(log_struct, ps->x_pos_pid, PID_CORRECTION, rad);
addOutputToLog(log_struct, ps->y_pos_pid, PID_CORRECTION, rad);
@@ -104,7 +105,7 @@ void initialize_logging(log_t* log_struct, parameter_t* ps) {
addOutputToLog(log_struct, ps->pitch_r_pid, PID_CORRECTION, pwm_val);
addOutputToLog(log_struct, ps->roll_r_pid, PID_CORRECTION, pwm_val);
addOutputToLog(log_struct, ps->yaw_r_pid, PID_CORRECTION, pwm_val);
- addOutputToLog(log_struct, ps->cur_pitch, CONST_VAL, rad);
+ addOutputToLog(log_struct, ps->pitch_trim_add, CONST_VAL, rad);
addOutputToLog(log_struct, ps->cur_roll, CONST_VAL, rad);
addOutputToLog(log_struct, ps->cur_yaw, CONST_VAL, rad);
addOutputToLog(log_struct, ps->vrpn_x, CONST_VAL, m);
@@ -122,6 +123,10 @@ void initialize_logging(log_t* log_struct, parameter_t* ps) {
addOutputToLog(log_struct, ps->rc_throttle, PID_CORRECTION, pwm_val);
addOutputToLog(log_struct, ps->rc_pitch, PID_CORRECTION, pwm_val);
addOutputToLog(log_struct, ps->rc_roll, PID_CORRECTION, pwm_val);
+ addOutputToLog(log_struct, ps->x_vel_pid, PID_CORRECTION, rad);
+ addOutputToLog(log_struct, ps->y_vel_pid, PID_CORRECTION, rad);
+ addOutputToLog(log_struct, ps->x_vel, PID_CORRECTION, m_s);
+ addOutputToLog(log_struct, ps->y_vel, PID_CORRECTION, m_s);
// TODO: Make this not stupid. Adding 6 for IMU and 1 for timestamp
row_size = n_outputs + n_params + 6 + 1;
@@ -184,12 +189,27 @@ void printLogging(hardware_t *hardware_struct, log_t* log_struct, parameter_t* p
send_data(&hardware_struct->uart, LOG_ID, 0, buf.str, buf.size);
return;
}
+
+ int i;
// Comment header
safe_sprintf_cat(&buf, "# MicroCART On-board Quad Log\n# Sample size: %d\n", arrayIndex);
+
+ // List Pid Constants
+ for (i = 0; i < ps->graph->n_nodes; ++i) {
+ struct graph_node* node = &ps->graph->nodes[i];
+ if (node->type->type_id == BLOCK_PID) {
+ double kp, ki, kd;
+ kp = graph_get_param_val(ps->graph, i, 0);
+ ki = graph_get_param_val(ps->graph, i, 1);
+ kd = graph_get_param_val(ps->graph, i, 2);
+ safe_sprintf_cat(&buf, "# %s :\tKp = %lf Ki = %lf Kd = %lf\n", node->name, kp, ki, kd);
+
+ }
+ }
+
// Header names for the pre-defined values
safe_sprintf_cat(&buf, "%%Time\taccel_x\taccel_y\taccel_z\tgyro_x\tgyro_y\tgyro_z");
- int i;
// Print all the recorded block parameters
for (i = 0; i < n_params; i++) {
const char* block_name = ps->graph->nodes[log_params[i].block_id].name;
diff --git a/quad/src/quad_app/sensor_processing.c b/quad/src/quad_app/sensor_processing.c
index d7bd7b707ac23b2b87e8c5aab8029840dce80b31..3e3b48bd1bbcb16dca49f198494ce05f847b6dfd 100644
--- a/quad/src/quad_app/sensor_processing.c
+++ b/quad/src/quad_app/sensor_processing.c
@@ -14,8 +14,33 @@
#include "timer.h"
#include <math.h>
+#define ALPHA 0.99
+
+int sensor_processing_init(sensor_t* sensor_struct) {
+ float a0 = 0.0200833310260;
+ float a1 = 0.0401666620520;
+ float a2 = 0.0200833310260;
+ float b1 = -1.561015391253;
+ float b2 = 0.6413487153577;
+ sensor_struct->accel_x_filt = filter_make_state(a0, a1, a2, b1, b2);
+ sensor_struct->accel_y_filt = filter_make_state(a0, a1, a2, b1, b2);
+ sensor_struct->accel_z_filt = filter_make_state(a0, a1, a2, b1, b2);
+ return 0;
+}
+
int sensor_processing(log_t* log_struct, user_input_t *user_input_struct, raw_sensor_t* raw_sensor_struct, sensor_t* sensor_struct)
{
+ // Filter accelerometer values
+ gam_t* gam = &(raw_sensor_struct->gam);
+ float accel_x = biquad_execute(&sensor_struct->accel_x_filt, gam->accel_x);
+ float accel_y = biquad_execute(&sensor_struct->accel_y_filt, gam->accel_y);
+ float accel_z = biquad_execute(&sensor_struct->accel_z_filt, gam->accel_z);
+ //Get X and Y angles
+ // javey: this assigns accel_(pitch/roll) in units of radians
+ float accel_pitch = atan(accel_x / sqrt(accel_y*accel_y + accel_z*accel_z));
+ float accel_roll = -atan(accel_y / sqrt(accel_x*accel_x + accel_z*accel_z)); // negative because sensor board is upside down
+
+
// copy currentQuadPosition and trimmedRCValues from raw_sensor_struct to sensor_struct
deep_copy_Qpos(&(sensor_struct->currentQuadPosition), &(raw_sensor_struct->currentQuadPosition));
@@ -31,13 +56,14 @@ int sensor_processing(log_t* log_struct, user_input_t *user_input_struct, raw_se
// |local y| = |sin(yaw angle) cos(yaw angle) 0| |camera given y|
// |local z| | 0 0 1| |camera given z|
+
sensor_struct->currentQuadPosition.x_pos =
- sensor_struct->currentQuadPosition.x_pos * cos(sensor_struct->currentQuadPosition.yaw) +
- sensor_struct->currentQuadPosition.y_pos * -sin(sensor_struct->currentQuadPosition.yaw);
+ raw_sensor_struct->currentQuadPosition.x_pos * cos(raw_sensor_struct->currentQuadPosition.yaw) +
+ raw_sensor_struct->currentQuadPosition.y_pos * -sin(raw_sensor_struct->currentQuadPosition.yaw);
sensor_struct->currentQuadPosition.y_pos =
- sensor_struct->currentQuadPosition.x_pos * sin(sensor_struct->currentQuadPosition.yaw) +
- sensor_struct->currentQuadPosition.y_pos * cos(sensor_struct->currentQuadPosition.yaw);
+ raw_sensor_struct->currentQuadPosition.x_pos * sin(raw_sensor_struct->currentQuadPosition.yaw) +
+ raw_sensor_struct->currentQuadPosition.y_pos * cos(raw_sensor_struct->currentQuadPosition.yaw);
// Calculate Euler angles and velocities using Gimbal Equations below
@@ -85,11 +111,11 @@ int sensor_processing(log_t* log_struct, user_input_t *user_input_struct, raw_se
sensor_struct->gyr_z = raw_sensor_struct->gam.gyro_zVel_r;
// Complementary Filter Calculations
- sensor_struct->pitch_angle_filtered = 0.98 * (sensor_struct->pitch_angle_filtered + sensor_struct->theta_dot * get_last_loop_time())
- + 0.02 * raw_sensor_struct->gam.accel_pitch;
+ sensor_struct->pitch_angle_filtered = ALPHA * (sensor_struct->pitch_angle_filtered + sensor_struct->theta_dot * get_last_loop_time())
+ + (1. - ALPHA) * accel_pitch;
- sensor_struct->roll_angle_filtered = 0.98 * (sensor_struct->roll_angle_filtered + sensor_struct->phi_dot* get_last_loop_time())
- + 0.02 * raw_sensor_struct->gam.accel_roll;
+ sensor_struct->roll_angle_filtered = ALPHA * (sensor_struct->roll_angle_filtered + sensor_struct->phi_dot* get_last_loop_time())
+ + (1. - ALPHA) * accel_roll;
// static int loop_counter = 0;
// loop_counter++;
diff --git a/quad/src/quad_app/sensor_processing.h b/quad/src/quad_app/sensor_processing.h
index 04b5efebe48ab20bdecfcfaab11e1ffe2f63a540..52b626eafa173ac5c806ad29d52ca03138017060 100644
--- a/quad/src/quad_app/sensor_processing.h
+++ b/quad/src/quad_app/sensor_processing.h
@@ -77,6 +77,7 @@
*/
int sensor_processing(log_t* log_struct, user_input_t *user_input_struct, raw_sensor_t * raw_sensor_struct, sensor_t* sensor_struct);
+int sensor_processing_init(sensor_t* sensor_struct);
void deep_copy_Qpos(quadPosition_t * dest, quadPosition_t * src);
void set_pitch_angle_filtered(sensor_t * sensor_struct, float accel_roll);
void set_roll_angle_filtered(sensor_t * sensor_struct, float accel_roll);
diff --git a/quad/src/quad_app/type_def.h b/quad/src/quad_app/type_def.h
index c32b202fa201762212adc068a3cee21e43c109f4..d016134432b8e058601904a5013a296b7ecd7b00 100644
--- a/quad/src/quad_app/type_def.h
+++ b/quad/src/quad_app/type_def.h
@@ -12,6 +12,7 @@
#include "commands.h"
#include "computation_graph.h"
#include "hw_iface.h"
+#include "biquad_filter.h"
typedef unsigned char u8;
typedef unsigned short u16;
@@ -278,6 +279,10 @@ typedef struct sensor {
quadPosition_t currentQuadPosition;
quadTrims_t trims;
+ struct biquadState accel_x_filt;
+ struct biquadState accel_y_filt;
+ struct biquadState accel_z_filt;
+
} sensor_t;
/**
@@ -343,6 +348,15 @@ typedef struct parameter_t {
int throttle_trim_add;
int pitch_trim;
int pitch_trim_add;
+ // Velocity nodes
+ int x_vel_pid;
+ int y_vel_pid;
+ int x_vel;
+ int y_vel;
+ int x_vel_clamp;
+ int y_vel_clamp;
+ int vel_x_gain;
+ int vel_y_gain;
} parameter_t;
/**