From 136e23f446ec2697a065960fb0aa3adedbf3e06f Mon Sep 17 00:00:00 2001
From: David Wehr <dawehr@iastate.edu>
Date: Mon, 3 Apr 2017 08:27:07 -0500
Subject: [PATCH] Added node for pitch trim so it can be set remotely.
---
quad/src/quad_app/control_algorithm.c | 12 +++++++++++-
quad/src/quad_app/type_def.h | 2 ++
2 files changed, 13 insertions(+), 1 deletion(-)
diff --git a/quad/src/quad_app/control_algorithm.c b/quad/src/quad_app/control_algorithm.c
index 3002cfc26..0391ca2dd 100644
--- a/quad/src/quad_app/control_algorithm.c
+++ b/quad/src/quad_app/control_algorithm.c
@@ -59,6 +59,10 @@ int control_algorithm_init(parameter_t * ps)
ps->cur_pitch = graph_add_defined_block(graph, BLOCK_CONSTANT, "Pitch"); // ID 15
ps->cur_roll = graph_add_defined_block(graph, BLOCK_CONSTANT, "Roll");
ps->cur_yaw = graph_add_defined_block(graph, BLOCK_CONSTANT, "Yaw");
+ // Sensor trims
+ ps->pitch_trim = graph_add_defined_block(graph, BLOCK_CONSTANT, "Pitch trim");
+ ps->pitch_trim_add = graph_add_defined_block(graph, BLOCK_ADD, "Pitch trim add");
+
// Yaw angular velocity PID
// theta_dot is the angular velocity about the y-axis
// phi_dot is the angular velocity about the x-axis
@@ -90,10 +94,14 @@ int control_algorithm_init(parameter_t * ps)
ps->pos_time = graph_add_defined_block(graph, BLOCK_CONSTANT, "Ts_VRPN");
// Connect pitch PID chain
+ // Trims
+ graph_set_source(graph, ps->pitch_trim_add, ADD_SUMMAND1, ps->pitch_trim, CONST_VAL);
+ graph_set_source(graph, ps->pitch_trim_add, ADD_SUMMAND2, ps->cur_pitch, CONST_VAL);
+ // Controllers
graph_set_source(graph, ps->pitch_r_pid, PID_SETPOINT, ps->pitch_pid, PID_CORRECTION);
graph_set_source(graph, ps->pitch_r_pid, PID_CUR_POINT, ps->theta_dot, CONST_VAL);
graph_set_source(graph, ps->pitch_r_pid, PID_DT, ps->angle_time, CONST_VAL);
- graph_set_source(graph, ps->pitch_pid, PID_CUR_POINT, ps->cur_pitch, CONST_VAL);
+ graph_set_source(graph, ps->pitch_pid, PID_CUR_POINT, ps->pitch_trim_add, ADD_SUM);
//graph_set_source(graph, ps->pitch_pid, PID_CUR_POINT, ps->vrpn_pitch, CONST_VAL);
graph_set_source(graph, ps->pitch_pid, PID_DT, ps->angle_time, CONST_VAL);
@@ -187,6 +195,8 @@ 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);
+ // Set trims
+ graph_set_param_val(graph, ps->pitch_trim, CONST_SET, 0.02);
// Initial value for sampling periods
graph_set_param_val(graph, ps->pos_time, CONST_SET, 0.04);
diff --git a/quad/src/quad_app/type_def.h b/quad/src/quad_app/type_def.h
index 53af7f2f8..c32b202fa 100644
--- a/quad/src/quad_app/type_def.h
+++ b/quad/src/quad_app/type_def.h
@@ -341,6 +341,8 @@ typedef struct parameter_t {
// "trim" for autonomous
int throttle_trim;
int throttle_trim_add;
+ int pitch_trim;
+ int pitch_trim_add;
} parameter_t;
/**
--
GitLab