diff options
-rw-r--r-- | src/modules/multirotor_att_control/multirotor_att_control_main.c | 8 | ||||
-rw-r--r-- | src/modules/multirotor_att_control/multirotor_attitude_control.c | 2 | ||||
-rw-r--r-- | src/modules/multirotor_att_control/multirotor_rate_control.c | 15 | ||||
-rw-r--r-- | src/modules/multirotor_att_control/multirotor_rate_control.h | 2 | ||||
-rw-r--r-- | src/modules/sdlog2/sdlog2.c | 3 | ||||
-rw-r--r-- | src/modules/sdlog2/sdlog2_messages.h | 5 | ||||
-rw-r--r-- | src/modules/systemlib/pid/pid.c | 12 | ||||
-rw-r--r-- | src/modules/systemlib/pid/pid.h | 5 |
8 files changed, 30 insertions, 22 deletions
diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c index dd38242d3..41a9f1df5 100644 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c @@ -410,13 +410,7 @@ mc_thread_main(int argc, char *argv[]) rates[1] = att.pitchspeed; rates[2] = att.yawspeed; - rates_acc[0] = att.rollacc; - rates_acc[1] = att.pitchacc; - rates_acc[2] = att.yawacc; - - - - multirotor_control_rates(&rates_sp, rates, rates_acc, &actuators, &control_debug_pub, &control_debug); + multirotor_control_rates(&rates_sp, rates, &actuators, &control_debug_pub, &control_debug); orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); orb_publish(ORB_ID(vehicle_control_debug), control_debug_pub, &control_debug); diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.c b/src/modules/multirotor_att_control/multirotor_attitude_control.c index 88cc65148..752f292e3 100644 --- a/src/modules/multirotor_att_control/multirotor_attitude_control.c +++ b/src/modules/multirotor_att_control/multirotor_attitude_control.c @@ -187,7 +187,7 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s /* control roll (left/right) output */ rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body , - att->roll, att->rollspeed, deltaT, &control_debug->roll_p, &control_debug->roll_i, &control_debug->roll_d); + att->roll, att->rollspeed, deltaT, NULL, NULL, NULL); // printf("rates_sp: %4.4f, att setpoint: %4.4f\n, pitch: %4.4f, pitchspeed: %4.4f, dT: %4.4f", rates_sp->pitch, att_sp->pitch_body, att->pitch, att->pitchspeed, deltaT); diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c index a0266e1b3..641d833f0 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.c +++ b/src/modules/multirotor_att_control/multirotor_rate_control.c @@ -151,7 +151,7 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru } void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, - const float rates[], const float rates_acc[], struct actuator_controls_s *actuators, + const float rates[], struct actuator_controls_s *actuators, const orb_advert_t *control_debug_pub, struct vehicle_control_debug_s *control_debug) { static uint64_t last_run = 0; @@ -179,6 +179,7 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, static bool initialized = false; + float diff_filter_factor = 1.0f; /* initialize the pid controllers when the function is called for the first time */ if (initialized == false) { @@ -186,8 +187,8 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, parameters_update(&h, &p); initialized = true; - pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f, PID_MODE_DERIVATIV_SET); - pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f, PID_MODE_DERIVATIV_SET); + pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, diff_filter_factor, PID_MODE_DERIVATIV_CALC_NO_SP); + pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, diff_filter_factor, PID_MODE_DERIVATIV_CALC_NO_SP); } @@ -195,8 +196,8 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, if (motor_skip_counter % 2500 == 0) { /* update parameters from storage */ parameters_update(&h, &p); - pid_set_parameters(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f); - pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f); + pid_set_parameters(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, diff_filter_factor); + pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, diff_filter_factor); } /* reset integral if on ground */ @@ -207,11 +208,11 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, /* control pitch (forward) output */ float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch , - rates[1], rates_acc[1], deltaT, &control_debug->pitch_rate_p, &control_debug->pitch_rate_i, &control_debug->pitch_rate_d); + rates[1], 0.0f, deltaT, &control_debug->pitch_rate_p, &control_debug->pitch_rate_i, &control_debug->pitch_rate_d); /* control roll (left/right) output */ float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll , - rates[0], rates_acc[0], deltaT, &control_debug->roll_rate_p, &control_debug->roll_rate_i, &control_debug->roll_rate_d); + rates[0], 0.0f, deltaT, &control_debug->roll_rate_p, &control_debug->roll_rate_i, &control_debug->roll_rate_d); /* increase resilience to faulty control inputs */ if (isfinite(pitch_control)) { diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.h b/src/modules/multirotor_att_control/multirotor_rate_control.h index fc741c378..465549540 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.h +++ b/src/modules/multirotor_att_control/multirotor_rate_control.h @@ -52,7 +52,7 @@ #include <uORB/topics/vehicle_control_debug.h> void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, - const float rates[], const float rates_acc[], struct actuator_controls_s *actuators, + const float rates[], struct actuator_controls_s *actuators, const orb_advert_t *control_debug_pub, struct vehicle_control_debug_s *control_debug); #endif /* MULTIROTOR_RATE_CONTROL_H_ */ diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index f967638c3..edcba4e7d 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1013,6 +1013,9 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_ATT.roll_rate = buf.att.rollspeed; log_msg.body.log_ATT.pitch_rate = buf.att.pitchspeed; log_msg.body.log_ATT.yaw_rate = buf.att.yawspeed; + log_msg.body.log_ATT.roll_acc = buf.att.rollacc; + log_msg.body.log_ATT.pitch_acc = buf.att.pitchacc; + log_msg.body.log_ATT.yaw_acc = buf.att.yawacc; LOGBUFFER_WRITE_AND_COUNT(ATT); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index b6537b2cd..04eae40b0 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -63,6 +63,9 @@ struct log_ATT_s { float roll_rate; float pitch_rate; float yaw_rate; + float roll_acc; + float pitch_acc; + float yaw_acc; }; /* --- ATSP - ATTITUDE SET POINT --- */ @@ -209,7 +212,7 @@ struct log_ARSP_s { static const struct log_format_s log_formats[] = { LOG_FORMAT(TIME, "Q", "StartTime"), - LOG_FORMAT(ATT, "ffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate"), + LOG_FORMAT(ATT, "fffffffff", "Roll,Pitch,Yaw,RollR,PitchR,YawR,RollA,PitchA,YawA"), LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"), LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), LOG_FORMAT(SENS, "ffff", "BaroPres,BaroAlt,BaroTemp,DiffPres"), diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c index 3685b2aeb..2aafe0636 100644 --- a/src/modules/systemlib/pid/pid.c +++ b/src/modules/systemlib/pid/pid.c @@ -144,15 +144,19 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo // Calculated current error value float error = pid->sp - val; - - float error_filtered = pid->diff_filter_factor*error + (1.0f-pid->diff_filter_factor)*pid->error_previous_filtered; + float error_filtered; // Calculate or measured current error derivative if (pid->mode == PID_MODE_DERIVATIV_CALC) { -// d = (error_filtered - pid->error_previous_filtered) / dt; - d = error_filtered - pid->error_previous_filtered ; + error_filtered = pid->error_previous_filtered + (error - pid->error_previous_filtered) * pid->diff_filter_factor; + d = (error_filtered - pid->error_previous_filtered) / fmaxf(dt, 0.003f); // fail-safe for too low dt + pid->error_previous_filtered = error_filtered; + } else if (pid->mode == PID_MODE_DERIVATIV_CALC_NO_SP) { + + error_filtered = pid->error_previous_filtered + (- val - pid->error_previous_filtered) * pid->diff_filter_factor; + d = (error_filtered - pid->error_previous_filtered) / fmaxf(dt, 0.003f); // fail-safe for too low dt pid->error_previous_filtered = error_filtered; } else if (pid->mode == PID_MODE_DERIVATIV_SET) { d = -val_dot; diff --git a/src/modules/systemlib/pid/pid.h b/src/modules/systemlib/pid/pid.h index e3d9038cd..eb9df96cc 100644 --- a/src/modules/systemlib/pid/pid.h +++ b/src/modules/systemlib/pid/pid.h @@ -47,8 +47,11 @@ /* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error * val_dot in pid_calculate() will be ignored */ #define PID_MODE_DERIVATIV_CALC 0 +/* PID_MODE_DERIVATIV_CALC_NO_SP calculates discrete derivative from previous value, setpoint derivative is ignored + * val_dot in pid_calculate() will be ignored */ +#define PID_MODE_DERIVATIV_CALC_NO_SP 1 /* Use PID_MODE_DERIVATIV_SET if you have the derivative already (Gyros, Kalman) */ -#define PID_MODE_DERIVATIV_SET 1 +#define PID_MODE_DERIVATIV_SET 2 // Use PID_MODE_DERIVATIV_NONE for a PI controller (vs PID) #define PID_MODE_DERIVATIV_NONE 9 |