diff --git a/include/pid_controller.h b/include/pid_controller.h index 50aee69..87e4dbe 100644 --- a/include/pid_controller.h +++ b/include/pid_controller.h @@ -10,10 +10,8 @@ typedef struct { fix16_t kp; fix16_t ki; fix16_t kd; - // fix16_t tau; fix16_t x; - // fix16_t x_dot; fix16_t setpoint; fix16_t output; @@ -22,29 +20,29 @@ typedef struct { fix16_t integrator; fix16_t prev_x; - fix16_t prev_e; } pid_controller_t; -void pid_reset( pid_controller_t* pid, fix16_t prev_x ); + +void pid_init( pid_controller_t* pid, + fix16_t kp, + fix16_t ki, + fix16_t kd, + fix16_t initial_x, + fix16_t initial_setpoint, + fix16_t initial_output, + fix16_t min, + fix16_t max ); + void pid_set_gain_p( pid_controller_t* pid, fix16_t kp ); void pid_set_gain_i( pid_controller_t* pid, fix16_t ki ); void pid_set_gain_d( pid_controller_t* pid, fix16_t kd ); -// void pid_set_gain_tau(pid_controller_t *pid, fix16_t tau); -// void pid_set_gains(pid_controller_t *pid, fix16_t kp, fix16_t ki, fix16_t kd, -// fix16_t tau); void pid_set_gains( pid_controller_t* pid, fix16_t kp, fix16_t ki, fix16_t kd ); void pid_set_min_max( pid_controller_t* pid, fix16_t min, fix16_t max ); -// void pid_init(pid_controller_t *pid, fix16_t kp, fix16_t ki, fix16_t kd, -// fix16_t tau, fix16_t initial_x, fix16_t initial_x_dot, fix16_t -// initial_setpoint, fix16_t initial_output, fix16_t min, fix16_t max); -void pid_init( pid_controller_t* pid, fix16_t kp, fix16_t ki, fix16_t kd, - fix16_t initial_x, fix16_t initial_setpoint, - fix16_t initial_output, fix16_t min, fix16_t max ); -// fix16_t pid_step(pid_controller_t *pid, uint32_t time_now, fix16_t sp, -// fix16_t x, fix16_t x_dot, bool use_x_dot); +void pid_reset( pid_controller_t* pid, fix16_t prev_x ); + fix16_t pid_step( pid_controller_t* pid, fix16_t dt, fix16_t sp, fix16_t x ); #ifdef __cplusplus diff --git a/src/controller.c b/src/controller.c index 8dfe5a8..9514ec7 100644 --- a/src/controller.c +++ b/src/controller.c @@ -21,6 +21,8 @@ extern "C" { #include "safety.h" #include "sensors.h" +#include "drivers/drv_system.h" + system_status_t _system_status; sensor_readings_t _sensors; state_t _state_estimator; @@ -34,11 +36,17 @@ pid_controller_t _pid_roll_rate; pid_controller_t _pid_pitch_rate; pid_controller_t _pid_yaw_rate; +v3d integrator_; + static void controller_reset( void ) { pid_reset( &_pid_roll_rate, _state_estimator.p ); pid_reset( &_pid_pitch_rate, _state_estimator.q ); pid_reset( &_pid_yaw_rate, _state_estimator.r ); + integrator_.x = 0; + integrator_.y = 0; + integrator_.z = 0; + _control_input.r = 0; _control_input.p = 0; _control_input.y = 0; @@ -71,7 +79,7 @@ static void controller_set_input_failsafe( command_input_t* input ) { } // Technique adapted from the Pixhawk multirotor control scheme (~December 2018) -static void rot_error_from_attitude( v3d* Re, const qf16* q_sp, const qf16* q, +static void rot_error_from_attitude( v3d* e_R, qf16* qe, const qf16* q_sp, const qf16* q, const fix16_t yaw_w ) { /* //XXX: Shorthand method that doesn't allow for separate yaw tuning @@ -128,15 +136,20 @@ static void rot_error_from_attitude( v3d* Re, const qf16* q_sp, const qf16* q, qf16_mul( &qd, &qd_red, &q_mix ); qf16_normalize_to_unit( &qd, &qd ); - qf16_basis_error( e_R, q, &qd ); + qf16_basis_error( e_R, qe, q, &qd ); } +/* +static void calc_tracking_rate_error(v3d* we, const v3d* w, const qf16* qe, const v3d* w_sp) { + v3d w_sp_b; + qf16_rotate(&w_sp_b, qe, w_sp); + + v3d_sub(we, &w_sp_b, w); +} +*/ + static void controller_run( uint32_t time_now ) { // Variables that store the computed attitude goal rates - fix16_t goal_r = 0; - fix16_t goal_p = 0; - fix16_t goal_y = 0; - fix16_t goal_throttle = 0; command_input_t input; controller_set_input_failsafe( &input ); // Failsafe input to be safe @@ -212,6 +225,8 @@ static void controller_run( uint32_t time_now ) { _control_input.input_mask = input.input_mask; //==-- Throttle Control + fix16_t goal_throttle = 0; + // Trottle if ( !( _control_input.input_mask & CMD_IN_IGNORE_THROTTLE ) ) { // Use the commanded throttle @@ -232,6 +247,7 @@ static void controller_run( uint32_t time_now ) { //==-- Attitude Control // Save intermittent goals qf16_normalize_to_unit( &_control_input.q, &input.q ); + v3d rates_ref = {0,0,0}; // If we should listen to attitude input if ( !( _control_input.input_mask & CMD_IN_IGNORE_ATTITUDE ) ) { @@ -242,66 +258,65 @@ static void controller_run( uint32_t time_now ) { yaw_w = 0; } - v3d e_R; - rot_error_from_attitude( &e_R, + v3d angle_error = {0,0,0}; + qf16 qe = {_fc_1,0,0,0}; //No angle reference, so no angle error (R==R_sp==Identity) + rot_error_from_attitude( &angle_error, + &qe, &_control_input.q, &_state_estimator.attitude, yaw_w ); - v3d rates_sp; - v3d_mul_s( &rates_sp, &e_R, get_param_fix16( PARAM_MC_ANGLE_P ) ); - - // Set goal rates - goal_r = rates_sp.x; - goal_p = rates_sp.y; - goal_y = rates_sp.z; - - // If we're in offboard mode, and we aren't going to override yaw rate, and - // we want to fuse - // XXX: Ideally this would be handled as an additional case using the IGNORE - // flags, somehow... - if ( ( _system_status.control_mode == MAIN_MODE_OFFBOARD ) && ( _control_input.input_mask & CMD_IN_IGNORE_YAW_RATE ) && get_param_uint( PARAM_CONTROL_OB_FUSE_YAW_RATE ) ) { - // Add in the additional yaw rate input - goal_y = fix16_add( goal_y, input.y ); - } + v3d_mul_s(&rates_ref, &angle_error, get_param_fix16( PARAM_MC_ANGLE_P )); } //==-- Rate Control PIDs // Roll Rate if ( !( _control_input.input_mask & CMD_IN_IGNORE_ROLL_RATE ) ) { // Use the commanded roll rate goal - goal_r = input.r; + rates_ref.x = input.r; } // Pitch Rate if ( !( _control_input.input_mask & CMD_IN_IGNORE_PITCH_RATE ) ) { // Use the commanded pitch rate goal - goal_p = input.p; + rates_ref.y = input.p; } // Yaw Rate if ( !( _control_input.input_mask & CMD_IN_IGNORE_YAW_RATE ) ) { // Use the commanded yaw rate goal - goal_y = input.y; + rates_ref.z = input.y; + } else { + // If we're in offboard mode, and we aren't going to override yaw rate, and + // we want to fuse + // XXX: Ideally this would be handled as an additional case using the IGNORE + // flags, somehow... + if ( ( _system_status.control_mode == MAIN_MODE_OFFBOARD ) && + get_param_uint( PARAM_CONTROL_OB_FUSE_YAW_RATE ) ) { + // Add in the additional yaw rate input + // TODO: This is more of a hack. Needs to be conerted from euler rate to body rates (should effect goal_y, goal_p, and goal_r) + rates_ref.z = fix16_add( rates_ref.z, input.y ); + } } // Constrain rates to set params - goal_r = fix16_constrain( goal_r, -get_param_fix16( PARAM_MAX_ROLL_RATE ), + rates_ref.x = fix16_constrain( rates_ref.x, -get_param_fix16( PARAM_MAX_ROLL_RATE ), get_param_fix16( PARAM_MAX_ROLL_RATE ) ); - goal_p = fix16_constrain( goal_p, -get_param_fix16( PARAM_MAX_PITCH_RATE ), + rates_ref.y = fix16_constrain( rates_ref.y, -get_param_fix16( PARAM_MAX_PITCH_RATE ), get_param_fix16( PARAM_MAX_PITCH_RATE ) ); - goal_y = fix16_constrain( goal_y, -get_param_fix16( PARAM_MAX_YAW_RATE ), + rates_ref.z= fix16_constrain( rates_ref.z, -get_param_fix16( PARAM_MAX_YAW_RATE ), get_param_fix16( PARAM_MAX_YAW_RATE ) ); - // Save intermittent goals - _control_input.r = goal_r; - _control_input.p = goal_p; - _control_input.y = goal_y; + // Save intermittent goals and calculate rate error + _control_input.r = rates_ref.x; + _control_input.p = rates_ref.y; + _control_input.y = rates_ref.z; // Rate PID Controllers - fix16_t command_r = pid_step( &_pid_roll_rate, dt, goal_r, _state_estimator.p ); - fix16_t command_p = pid_step( &_pid_pitch_rate, dt, goal_p, _state_estimator.q ); - fix16_t command_y = pid_step( &_pid_yaw_rate, dt, goal_y, _state_estimator.r ); + v3d tau; + tau.x = pid_step( &_pid_roll_rate, dt, rates_ref.x, _state_estimator.p ); + tau.y = pid_step( &_pid_pitch_rate, dt, rates_ref.y, _state_estimator.q ); + tau.z = pid_step( &_pid_yaw_rate, dt, rates_ref.z, _state_estimator.r ); // XXX: //"Post-Scale/Normalize" the commands to act within @@ -309,9 +324,9 @@ static void controller_run( uint32_t time_now ) { // allows us to have higher PID gains for the rates //(by a factor of 100), and avoids complications // of getting close to the fixed-point step size - _control_output.r = fix16_div( command_r, _fc_100 ); - _control_output.p = fix16_div( command_p, _fc_100 ); - _control_output.y = fix16_div( command_y, _fc_100 ); + _control_output.r = fix16_div( tau.x, _fc_100 ); + _control_output.p = fix16_div( tau.y, _fc_100 ); + _control_output.y = fix16_div( tau.z, _fc_100 ); } void control_init( void ) { diff --git a/src/pid_controller.c b/src/pid_controller.c index baee018..aa247b7 100644 --- a/src/pid_controller.c +++ b/src/pid_controller.c @@ -9,7 +9,6 @@ extern "C" { // By default, pass prev_x as 0, unless you know the first measurement void pid_reset( pid_controller_t* pid, fix16_t prev_x ) { pid->integrator = 0; - pid->prev_e = 0; pid->prev_x = prev_x; } @@ -84,9 +83,10 @@ fix16_t pid_step( pid_controller_t* pid, fix16_t dt, fix16_t sp, fix16_t x ) { if ( dt > 0 ) { //==-- Derivative if ( pid->kd > 0 ) { - fix16_t de = fix16_div( fix16_sub( error, pid->prev_e ), dt ); + //Use dx instead of de to improve stability (removes noise due to changing reference) + fix16_t dx = fix16_div( fix16_sub( x, pid->prev_x ), dt ); - d_term = fix16_mul( pid->kd, de ); + d_term = -fix16_mul( pid->kd, dx ); } //==-- Integrator @@ -99,8 +99,7 @@ fix16_t pid_step( pid_controller_t* pid, fix16_t dt, fix16_t sp, fix16_t x ) { } } - // Sum three terms: u = p_term + i_term - d_term - // Sum three terms: u = p_term + i_term + de_term + // Sum three terms: u = p_term + i_term + d_term fix16_t u = fix16_add( p_term, d_term ); fix16_t ui = fix16_add( u, i_term ); @@ -119,7 +118,6 @@ fix16_t pid_step( pid_controller_t* pid, fix16_t dt, fix16_t sp, fix16_t x ) { // Save running values pid->prev_x = pid->x; - pid->prev_e = error; return pid->output; }