From 5cb1f4662fb28f68e539f2c8930c0f48ccea3521 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 20 Jun 2013 19:25:37 +0400 Subject: multirotor_attitude_control performance improved, tested in flight. PID library new functionality and bugfixes. --- src/modules/systemlib/pid/pid.c | 49 +++++++++++++++++++++++------------------ src/modules/systemlib/pid/pid.h | 11 ++++++--- 2 files changed, 36 insertions(+), 24 deletions(-) (limited to 'src/modules/systemlib') diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c index 49315cdc9..5eb6b279c 100644 --- a/src/modules/systemlib/pid/pid.c +++ b/src/modules/systemlib/pid/pid.c @@ -1,9 +1,10 @@ /**************************************************************************** * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. * Author: @author Laurens Mackay * @author Tobias Naegeli * @author Martin Rutschmann + * @author Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -43,7 +44,7 @@ #include __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, - float limit, uint8_t mode) + float limit, uint8_t mode, float dt_min) { pid->kp = kp; pid->ki = ki; @@ -51,13 +52,13 @@ __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, pid->intmax = intmax; pid->limit = limit; pid->mode = mode; - pid->count = 0; - pid->saturated = 0; - pid->last_output = 0; - - pid->sp = 0; - pid->error_previous = 0; - pid->integral = 0; + pid->dt_min = dt_min; + pid->count = 0.0f; + pid->saturated = 0.0f; + pid->last_output = 0.0f; + pid->sp = 0.0f; + pid->error_previous = 0.0f; + pid->integral = 0.0f; } __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit) { @@ -136,14 +137,14 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo // Calculated current error value float error = pid->sp - val; - if (isfinite(error)) { // Why is this necessary? DEW - pid->error_previous = error; - } - // Calculate or measured current error derivative - if (pid->mode == PID_MODE_DERIVATIV_CALC) { - d = (error - pid->error_previous) / dt; + d = (error - pid->error_previous) / fmaxf(dt, pid->dt_min); + pid->error_previous = error; + + } else if (pid->mode == PID_MODE_DERIVATIV_CALC_NO_SP) { + d = (-val - pid->error_previous) / fmaxf(dt, pid->dt_min); + pid->error_previous = -val; } else if (pid->mode == PID_MODE_DERIVATIV_SET) { d = -val_dot; @@ -152,6 +153,10 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo d = 0.0f; } + if (!isfinite(d)) { + d = 0.0f; + } + // Calculate the error integral and check for saturation i = pid->integral + (error * dt); @@ -162,7 +167,7 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo } else { if (!isfinite(i)) { - i = 0; + i = 0.0f; } pid->integral = i; @@ -170,17 +175,19 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo } // Calculate the output. Limit output magnitude to pid->limit - float output = (pid->error_previous * pid->kp) + (i * pid->ki) + (d * pid->kd); + float output = (error * pid->kp) + (i * pid->ki) + (d * pid->kd); - if (output > pid->limit) output = pid->limit; + if (isfinite(output)) { + if (output > pid->limit) { + output = pid->limit; - if (output < -pid->limit) output = -pid->limit; + } else if (output < -pid->limit) { + output = -pid->limit; + } - if (isfinite(output)) { pid->last_output = output; } - return pid->last_output; } diff --git a/src/modules/systemlib/pid/pid.h b/src/modules/systemlib/pid/pid.h index 64d668867..9ebd8e6d9 100644 --- a/src/modules/systemlib/pid/pid.h +++ b/src/modules/systemlib/pid/pid.h @@ -1,9 +1,10 @@ /**************************************************************************** * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. * Author: @author Laurens Mackay * @author Tobias Naegeli * @author Martin Rutschmann + * @author Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -47,8 +48,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 @@ -62,12 +66,13 @@ typedef struct { float error_previous; float last_output; float limit; + float dt_min; uint8_t mode; uint8_t count; uint8_t saturated; } PID_t; -__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, uint8_t mode); +__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, uint8_t mode, float dt_min); __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit); //void pid_set(PID_t *pid, float sp); __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt); -- cgit v1.2.3 From 72694825de93e0998d39f1296bc830c3ec23933d Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 22 Jun 2013 11:28:21 +0400 Subject: Copyright fixes --- src/modules/commander/accelerometer_calibration.c | 50 ++++++++++++++++++---- src/modules/commander/accelerometer_calibration.h | 43 +++++++++++++++++-- .../multirotor_attitude_control.c | 22 +++++++--- .../multirotor_attitude_control.h | 22 +++++++--- .../multirotor_rate_control.c | 6 ++- .../multirotor_rate_control.h | 24 +++++++---- src/modules/sdlog2/logbuffer.c | 4 +- src/modules/sdlog2/logbuffer.h | 4 +- src/modules/sdlog2/sdlog2.c | 4 +- src/modules/sdlog2/sdlog2_format.h | 4 +- src/modules/sdlog2/sdlog2_messages.h | 4 +- src/modules/systemlib/pid/pid.c | 18 +++++--- src/modules/systemlib/pid/pid.h | 18 +++++--- 13 files changed, 167 insertions(+), 56 deletions(-) (limited to 'src/modules/systemlib') diff --git a/src/modules/commander/accelerometer_calibration.c b/src/modules/commander/accelerometer_calibration.c index d79dd93dd..48a36ac26 100644 --- a/src/modules/commander/accelerometer_calibration.c +++ b/src/modules/commander/accelerometer_calibration.c @@ -1,12 +1,45 @@ -/* - * accelerometer_calibration.c +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Anton Babushkin + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: * - * Copyright (C) 2013 Anton Babushkin. All rights reserved. - * Author: Anton Babushkin + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. * - * Transform acceleration vector to true orientation and scale + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file accelerometer_calibration.c * - * * * * Model * * * + * Implementation of accelerometer calibration. + * + * Transform acceleration vector to true orientation, scale and offset + * + * ===== Model ===== * accel_corr = accel_T * (accel_raw - accel_offs) * * accel_corr[3] - fully corrected acceleration vector in body frame @@ -14,7 +47,7 @@ * accel_raw[3] - raw acceleration vector * accel_offs[3] - acceleration offset vector * - * * * * Calibration * * * + * ===== Calibration ===== * * Reference vectors * accel_corr_ref[6][3] = [ g 0 0 ] // nose up @@ -34,7 +67,6 @@ * * accel_offs[i] = (accel_raw_ref[i*2][i] + accel_raw_ref[i*2+1][i]) / 2 * - * * Find accel_T * * 9 unknown constants @@ -67,6 +99,8 @@ * * accel_T = A^-1 * g * g = 9.80665 + * + * @author Anton Babushkin */ #include "accelerometer_calibration.h" diff --git a/src/modules/commander/accelerometer_calibration.h b/src/modules/commander/accelerometer_calibration.h index a11cf93d3..f93a867ba 100644 --- a/src/modules/commander/accelerometer_calibration.h +++ b/src/modules/commander/accelerometer_calibration.h @@ -1,8 +1,43 @@ -/* - * accelerometer_calibration.h +/**************************************************************************** * - * Copyright (C) 2013 Anton Babushkin. All rights reserved. - * Author: Anton Babushkin + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Anton Babushkin + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file accelerometer_calibration.h + * + * Definition of accelerometer calibration. + * + * @author Anton Babushkin */ #ifndef ACCELEROMETER_CALIBRATION_H_ diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.c b/src/modules/multirotor_att_control/multirotor_attitude_control.c index 5c74f1e77..8f19c6a4b 100644 --- a/src/modules/multirotor_att_control/multirotor_attitude_control.c +++ b/src/modules/multirotor_att_control/multirotor_attitude_control.c @@ -1,12 +1,12 @@ /**************************************************************************** * * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: @author Thomas Gubler - * @author Julian Oes - * @author Laurens Mackay - * @author Tobias Naegeli - * @author Martin Rutschmann - * @author Lorenz Meier + * Author: Thomas Gubler + * Julian Oes + * Laurens Mackay + * Tobias Naegeli + * Martin Rutschmann + * Lorenz Meier * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -39,7 +39,15 @@ /* * @file multirotor_attitude_control.c - * Implementation of attitude controller + * + * Implementation of attitude controller for multirotors. + * + * @author Thomas Gubler + * @author Julian Oes + * @author Laurens Mackay + * @author Tobias Naegeli + * @author Martin Rutschmann + * @author Lorenz Meier */ #include "multirotor_attitude_control.h" diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.h b/src/modules/multirotor_att_control/multirotor_attitude_control.h index 2cf83e443..e78f45c47 100644 --- a/src/modules/multirotor_att_control/multirotor_attitude_control.h +++ b/src/modules/multirotor_att_control/multirotor_attitude_control.h @@ -1,12 +1,12 @@ /**************************************************************************** * * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Thomas Gubler - * @author Julian Oes - * @author Laurens Mackay - * @author Tobias Naegeli - * @author Martin Rutschmann - * @author Lorenz Meier + * Author: Thomas Gubler + * Julian Oes + * Laurens Mackay + * Tobias Naegeli + * Martin Rutschmann + * Lorenz Meier * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -39,7 +39,15 @@ /* * @file multirotor_attitude_control.h - * Attitude control for multi rotors. + * + * Definition of attitude controller for multirotors. + * + * @author Thomas Gubler + * @author Julian Oes + * @author Laurens Mackay + * @author Tobias Naegeli + * @author Martin Rutschmann + * @author Lorenz Meier */ #ifndef MULTIROTOR_ATTITUDE_CONTROL_H_ diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c index 61498b71f..e58d357d5 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.c +++ b/src/modules/multirotor_att_control/multirotor_rate_control.c @@ -1,9 +1,10 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Copyright (C) 2012-2013 PX4 Development Team. All rights reserved. * Author: Tobias Naegeli * Lorenz Meier * Anton Babushkin + * Julian Oes * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -37,11 +38,12 @@ /** * @file multirotor_rate_control.c * - * Implementation of rate controller + * Implementation of rate controller for multirotors. * * @author Tobias Naegeli * @author Lorenz Meier * @author Anton Babushkin + * @author Julian Oes */ #include "multirotor_rate_control.h" diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.h b/src/modules/multirotor_att_control/multirotor_rate_control.h index 03dec317a..362b5ed86 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.h +++ b/src/modules/multirotor_att_control/multirotor_rate_control.h @@ -1,12 +1,12 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Thomas Gubler - * @author Julian Oes - * @author Laurens Mackay - * @author Tobias Naegeli - * @author Martin Rutschmann - * @author Lorenz Meier + * Copyright (C) 2012-2013 PX4 Development Team. All rights reserved. + * Author: Thomas Gubler + * Julian Oes + * Laurens Mackay + * Tobias Naegeli + * Martin Rutschmann + * Lorenz Meier * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -39,7 +39,15 @@ /* * @file multirotor_attitude_control.h - * Attitude control for multi rotors. + * + * Definition of rate controller for multirotors. + * + * @author Thomas Gubler + * @author Julian Oes + * @author Laurens Mackay + * @author Tobias Naegeli + * @author Martin Rutschmann + * @author Lorenz Meier */ #ifndef MULTIROTOR_RATE_CONTROL_H_ diff --git a/src/modules/sdlog2/logbuffer.c b/src/modules/sdlog2/logbuffer.c index 2e1e4fd4d..b3243f7b5 100644 --- a/src/modules/sdlog2/logbuffer.c +++ b/src/modules/sdlog2/logbuffer.c @@ -1,7 +1,7 @@ /**************************************************************************** * * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: Anton Babushkin + * Author: Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -37,7 +37,7 @@ * * Ring FIFO buffer for binary log data. * - * @author Anton Babushkin + * @author Anton Babushkin */ #include diff --git a/src/modules/sdlog2/logbuffer.h b/src/modules/sdlog2/logbuffer.h index 31521f722..3a5e3a29f 100644 --- a/src/modules/sdlog2/logbuffer.h +++ b/src/modules/sdlog2/logbuffer.h @@ -1,7 +1,7 @@ /**************************************************************************** * * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: Anton Babushkin + * Author: Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -37,7 +37,7 @@ * * Ring FIFO buffer for binary log data. * - * @author Anton Babushkin + * @author Anton Babushkin */ #ifndef SDLOG2_RINGBUFFER_H_ diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index c543fb1b4..f31277831 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -2,7 +2,7 @@ * * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. * Author: Lorenz Meier - * Anton Babushkin + * Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -40,7 +40,7 @@ * does the heavy SD I/O in a low-priority worker thread. * * @author Lorenz Meier - * @author Anton Babushkin + * @author Anton Babushkin */ #include diff --git a/src/modules/sdlog2/sdlog2_format.h b/src/modules/sdlog2/sdlog2_format.h index 59b91d90d..5c175ef7e 100644 --- a/src/modules/sdlog2/sdlog2_format.h +++ b/src/modules/sdlog2/sdlog2_format.h @@ -1,7 +1,7 @@ /**************************************************************************** * * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: Anton Babushkin + * Author: Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -37,7 +37,7 @@ * * General log format structures and macro. * - * @author Anton Babushkin + * @author Anton Babushkin */ /* diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 7f7bf6053..2f7ddabf2 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -1,7 +1,7 @@ /**************************************************************************** * * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: Anton Babushkin + * Author: Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -37,7 +37,7 @@ * * Log messages and structures definition. * - * @author Anton Babushkin + * @author Anton Babushkin */ #ifndef SDLOG2_MESSAGES_H_ diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c index 5eb6b279c..4996a8f66 100644 --- a/src/modules/systemlib/pid/pid.c +++ b/src/modules/systemlib/pid/pid.c @@ -1,10 +1,11 @@ /**************************************************************************** * * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. - * Author: @author Laurens Mackay - * @author Tobias Naegeli - * @author Martin Rutschmann - * @author Anton Babushkin + * Author: Laurens Mackay + * Tobias Naegeli + * Martin Rutschmann + * Anton Babushkin + * Julian Oes * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -37,7 +38,14 @@ /** * @file pid.c - * Implementation of generic PID control interface + * + * Implementation of generic PID control interface. + * + * @author Laurens Mackay + * @author Tobias Naegeli + * @author Martin Rutschmann + * @author Anton Babushkin + * @author Julian Oes */ #include "pid.h" diff --git a/src/modules/systemlib/pid/pid.h b/src/modules/systemlib/pid/pid.h index 9ebd8e6d9..714bf988f 100644 --- a/src/modules/systemlib/pid/pid.h +++ b/src/modules/systemlib/pid/pid.h @@ -1,10 +1,11 @@ /**************************************************************************** * * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. - * Author: @author Laurens Mackay - * @author Tobias Naegeli - * @author Martin Rutschmann - * @author Anton Babushkin + * Author: Laurens Mackay + * Tobias Naegeli + * Martin Rutschmann + * Anton Babushkin + * Julian Oes * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -37,7 +38,14 @@ /** * @file pid.h - * Definition of generic PID control interface + * + * Definition of generic PID control interface. + * + * @author Laurens Mackay + * @author Tobias Naegeli + * @author Martin Rutschmann + * @author Anton Babushkin + * @author Julian Oes */ #ifndef PID_H_ -- cgit v1.2.3