Skip to content
Snippets Groups Projects
Commit c2134927 authored by dawehr's avatar dawehr
Browse files

Merge branch 'pid-derivative-smooth'

parents 34c3e512 d888074f
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
Showing
with 223 additions and 28 deletions
quad/src/gen_diagram/network.dot
+ 36
− 4
View file @ c2134927
......@@ -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
quad/src/gen_diagram/network.png
+ 0
− 0
View replaced file @ 34c3e512 View file @ c2134927
quad/src/gen_diagram/network.png

363 KiB | W: | H:

quad/src/gen_diagram/network.png

482 KiB | W: | H:

quad/src/gen_diagram/network.png
quad/src/gen_diagram/network.png
quad/src/gen_diagram/network.png
quad/src/gen_diagram/network.png
  • 2-up
  • Swipe
  • Onion skin
quad/src/graph_blocks/node_pid.c
+ 13
− 6
View file @ c2134927
......@@ -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;
}
......
quad/src/quad_app/biquad_filter.c 0 → 100644
+ 29
− 0
View file @ c2134927
#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
quad/src/quad_app/biquad_filter.h 0 → 100644
+ 14
− 0
View file @ c2134927
#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
quad/src/quad_app/control_algorithm.c
+ 57
− 3
View file @ c2134927
......@@ -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);
......
quad/src/quad_app/iic_utils.c
+ 1
− 5
View file @ c2134927
......@@ -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;
......
quad/src/quad_app/initialize_components.c
+ 2
− 0
View file @ c2134927
......@@ -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;
}
quad/src/quad_app/log_data.c
+ 22
− 2
View file @ c2134927
......@@ -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;
......
quad/src/quad_app/sensor_processing.c
+ 34
− 8
View file @ c2134927
......@@ -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++;
......
quad/src/quad_app/sensor_processing.h
+ 1
− 0
View file @ c2134927
......@@ -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);
......
quad/src/quad_app/type_def.h
+ 14
− 0
View file @ c2134927
......@@ -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;
/**
......
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