diff options
Diffstat (limited to 'src/modules')
-rw-r--r-- | src/modules/commander/accelerometer_calibration.c | 50 | ||||
-rw-r--r-- | src/modules/commander/accelerometer_calibration.h | 43 | ||||
-rw-r--r-- | src/modules/fixedwing_att_control/fixedwing_att_control_att.c | 4 | ||||
-rw-r--r-- | src/modules/fixedwing_att_control/fixedwing_att_control_rate.c | 6 | ||||
-rw-r--r-- | src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c | 8 | ||||
-rw-r--r-- | src/modules/multirotor_att_control/multirotor_attitude_control.c | 32 | ||||
-rw-r--r-- | src/modules/multirotor_att_control/multirotor_attitude_control.h | 22 | ||||
-rw-r--r-- | src/modules/multirotor_att_control/multirotor_rate_control.c | 55 | ||||
-rw-r--r-- | src/modules/multirotor_att_control/multirotor_rate_control.h | 24 | ||||
-rw-r--r-- | src/modules/sdlog2/logbuffer.c | 4 | ||||
-rw-r--r-- | src/modules/sdlog2/logbuffer.h | 4 | ||||
-rw-r--r-- | src/modules/sdlog2/sdlog2.c | 14 | ||||
-rw-r--r-- | src/modules/sdlog2/sdlog2_format.h | 4 | ||||
-rw-r--r-- | src/modules/sdlog2/sdlog2_messages.h | 16 | ||||
-rw-r--r-- | src/modules/systemlib/pid/pid.c | 65 | ||||
-rw-r--r-- | src/modules/systemlib/pid/pid.h | 27 |
16 files changed, 254 insertions, 124 deletions
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 <anton.babushkin@me.com> + * + * 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 <rk3dov@gmail.com> + * 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 <anton.babushkin@me.com> */ #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 <rk3dov@gmail.com> + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Anton Babushkin <anton.babushkin@me.com> + * + * 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 <anton.babushkin@me.com> */ #ifndef ACCELEROMETER_CALIBRATION_H_ diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_att.c b/src/modules/fixedwing_att_control/fixedwing_att_control_att.c index 769b8b0a8..2aeca3a98 100644 --- a/src/modules/fixedwing_att_control/fixedwing_att_control_att.c +++ b/src/modules/fixedwing_att_control/fixedwing_att_control_att.c @@ -128,8 +128,8 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att if (!initialized) { parameters_init(&h); parameters_update(&h, &p); - pid_init(&roll_controller, p.roll_p, 0, 0, 0, p.rollrate_lim, PID_MODE_DERIVATIV_NONE); //P Controller - pid_init(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitchrate_lim, PID_MODE_DERIVATIV_NONE); //P Controller + pid_init(&roll_controller, p.roll_p, 0, 0, 0, p.rollrate_lim, PID_MODE_DERIVATIV_NONE, 0.0f); //P Controller + pid_init(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitchrate_lim, PID_MODE_DERIVATIV_NONE, 0.0f); //P Controller initialized = true; } diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c b/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c index 4eccc118c..cdab39edc 100644 --- a/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c +++ b/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c @@ -179,9 +179,9 @@ int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, if (!initialized) { parameters_init(&h); parameters_update(&h, &p); - pid_init(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, 1, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the controller layout is with a PI rate controller - pid_init(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0, p.pitchrate_awu, 1, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher - pid_init(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu, 1, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher + pid_init(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, 1, PID_MODE_DERIVATIV_NONE, 0.0f); // set D part to 0 because the controller layout is with a PI rate controller + pid_init(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0, p.pitchrate_awu, 1, PID_MODE_DERIVATIV_NONE, 0.0f); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher + pid_init(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu, 1, PID_MODE_DERIVATIV_NONE, 0.0f); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher initialized = true; } diff --git a/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c b/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c index 48c0b9f9d..73df3fb9e 100644 --- a/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c +++ b/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c @@ -239,10 +239,10 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) parameters_init(&h); parameters_update(&h, &p); - pid_init(&heading_controller, p.heading_p, 0.0f, 0.0f, 0.0f, 10000.0f, PID_MODE_DERIVATIV_NONE); //arbitrary high limit - pid_init(&heading_rate_controller, p.headingr_p, p.headingr_i, 0.0f, 0.0f, p.roll_lim, PID_MODE_DERIVATIV_NONE); - pid_init(&altitude_controller, p.altitude_p, 0.0f, 0.0f, 0.0f, p.pitch_lim, PID_MODE_DERIVATIV_NONE); - pid_init(&offtrack_controller, p.xtrack_p, 0.0f, 0.0f, 0.0f , 60.0f * M_DEG_TO_RAD, PID_MODE_DERIVATIV_NONE); //TODO: remove hardcoded value + pid_init(&heading_controller, p.heading_p, 0.0f, 0.0f, 0.0f, 10000.0f, PID_MODE_DERIVATIV_NONE, 0.0f); //arbitrary high limit + pid_init(&heading_rate_controller, p.headingr_p, p.headingr_i, 0.0f, 0.0f, p.roll_lim, PID_MODE_DERIVATIV_NONE, 0.0f); + pid_init(&altitude_controller, p.altitude_p, 0.0f, 0.0f, 0.0f, p.pitch_lim, PID_MODE_DERIVATIV_NONE, 0.0f); + pid_init(&offtrack_controller, p.xtrack_p, 0.0f, 0.0f, 0.0f , 60.0f * M_DEG_TO_RAD, PID_MODE_DERIVATIV_NONE, 0.0f); //TODO: remove hardcoded value /* error and performance monitoring */ perf_counter_t fw_interval_perf = perf_alloc(PC_INTERVAL, "fixedwing_pos_control_interval"); diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.c b/src/modules/multirotor_att_control/multirotor_attitude_control.c index 76dbb36d3..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 <thomasgubler@student.ethz.ch> - * @author Julian Oes <joes@student.ethz.ch> - * @author Laurens Mackay <mackayl@student.ethz.ch> - * @author Tobias Naegeli <naegelit@student.ethz.ch> - * @author Martin Rutschmann <rutmarti@student.ethz.ch> - * @author Lorenz Meier <lm@inf.ethz.ch> + * Author: Thomas Gubler <thomasgubler@student.ethz.ch> + * Julian Oes <joes@student.ethz.ch> + * Laurens Mackay <mackayl@student.ethz.ch> + * Tobias Naegeli <naegelit@student.ethz.ch> + * Martin Rutschmann <rutmarti@student.ethz.ch> + * Lorenz Meier <lm@inf.ethz.ch> * * 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 <thomasgubler@student.ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> + * @author Laurens Mackay <mackayl@student.ethz.ch> + * @author Tobias Naegeli <naegelit@student.ethz.ch> + * @author Martin Rutschmann <rutmarti@student.ethz.ch> + * @author Lorenz Meier <lm@inf.ethz.ch> */ #include "multirotor_attitude_control.h" @@ -163,16 +171,12 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s static uint64_t last_run = 0; static uint64_t last_input = 0; float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; - float dT_input = (hrt_absolute_time() - last_input) / 1000000.0f; last_run = hrt_absolute_time(); if (last_input != att_sp->timestamp) { last_input = att_sp->timestamp; } - static int sensor_delay; - sensor_delay = hrt_absolute_time() - att->timestamp; - static int motor_skip_counter = 0; static PID_t pitch_controller; @@ -190,10 +194,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s parameters_init(&h); parameters_update(&h, &p); - pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, - 1000.0f, PID_MODE_DERIVATIV_SET); - pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, - 1000.0f, PID_MODE_DERIVATIV_SET); + pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, PID_MODE_DERIVATIV_SET, 0.0f); + pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, PID_MODE_DERIVATIV_SET, 0.0f); initialized = true; } 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 <thomasgubler@student.ethz.ch> - * @author Julian Oes <joes@student.ethz.ch> - * @author Laurens Mackay <mackayl@student.ethz.ch> - * @author Tobias Naegeli <naegelit@student.ethz.ch> - * @author Martin Rutschmann <rutmarti@student.ethz.ch> - * @author Lorenz Meier <lm@inf.ethz.ch> + * Author: Thomas Gubler <thomasgubler@student.ethz.ch> + * Julian Oes <joes@student.ethz.ch> + * Laurens Mackay <mackayl@student.ethz.ch> + * Tobias Naegeli <naegelit@student.ethz.ch> + * Martin Rutschmann <rutmarti@student.ethz.ch> + * Lorenz Meier <lm@inf.ethz.ch> * * 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 <thomasgubler@student.ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> + * @author Laurens Mackay <mackayl@student.ethz.ch> + * @author Tobias Naegeli <naegelit@student.ethz.ch> + * @author Martin Rutschmann <rutmarti@student.ethz.ch> + * @author Lorenz Meier <lm@inf.ethz.ch> */ #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 deba1ac03..e58d357d5 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.c +++ b/src/modules/multirotor_att_control/multirotor_rate_control.c @@ -1,8 +1,10 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2012-2013 PX4 Development Team. All rights reserved. * Author: Tobias Naegeli <naegelit@student.ethz.ch> * Lorenz Meier <lm@inf.ethz.ch> + * Anton Babushkin <anton.babushkin@me.com> + * Julian Oes <joes@student.ethz.ch> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -36,10 +38,12 @@ /** * @file multirotor_rate_control.c * - * Implementation of rate controller + * Implementation of rate controller for multirotors. * * @author Tobias Naegeli <naegelit@student.ethz.ch> * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Anton Babushkin <anton.babushkin@me.com> + * @author Julian Oes <joes@student.ethz.ch> */ #include "multirotor_rate_control.h" @@ -150,14 +154,10 @@ 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[], struct actuator_controls_s *actuators) { - static float roll_control_last = 0; - static float pitch_control_last = 0; static uint64_t last_run = 0; const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; static uint64_t last_input = 0; - float dT_input = (hrt_absolute_time() - last_input) / 1000000.0f; - if (last_input != rate_sp->timestamp) { last_input = rate_sp->timestamp; } @@ -166,6 +166,9 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, static int motor_skip_counter = 0; + static PID_t pitch_rate_controller; + static PID_t roll_rate_controller; + static struct mc_rate_control_params p; static struct mc_rate_control_param_handles h; @@ -176,43 +179,35 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, parameters_init(&h); parameters_update(&h, &p); initialized = true; + + pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f); + pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f); + } /* load new parameters with lower rate */ if (motor_skip_counter % 2500 == 0) { /* update parameters from storage */ parameters_update(&h, &p); - // warnx("rate ctrl: p.yawrate_p: %8.4f, loop: %d Hz, input: %d Hz", - // (double)p.yawrate_p, (int)(1.0f/deltaT), (int)(1.0f/dT_input)); + pid_set_parameters(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f); + pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f); } - /* calculate current control outputs */ + /* reset integral if on ground */ + if (rate_sp->thrust < 0.01f) { + pid_reset_integral(&pitch_rate_controller); + pid_reset_integral(&roll_rate_controller); + } /* control pitch (forward) output */ - float pitch_control = p.attrate_p * (rate_sp->pitch - rates[1]) - (p.attrate_d * pitch_control_last); - - /* increase resilience to faulty control inputs */ - if (isfinite(pitch_control)) { - pitch_control_last = pitch_control; - - } else { - pitch_control = 0.0f; - warnx("rej. NaN ctrl pitch"); - } + float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch , + rates[1], 0.0f, deltaT); /* control roll (left/right) output */ - float roll_control = p.attrate_p * (rate_sp->roll - rates[0]) - (p.attrate_d * roll_control_last); - - /* increase resilience to faulty control inputs */ - if (isfinite(roll_control)) { - roll_control_last = roll_control; - - } else { - roll_control = 0.0f; - warnx("rej. NaN ctrl roll"); - } + float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll , + rates[0], 0.0f, deltaT); - /* control yaw rate */ + /* control yaw rate */ //XXX use library here float yaw_rate_control = p.yawrate_p * (rate_sp->yaw - rates[2]); /* increase resilience to faulty control inputs */ 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 <thomasgubler@student.ethz.ch> - * @author Julian Oes <joes@student.ethz.ch> - * @author Laurens Mackay <mackayl@student.ethz.ch> - * @author Tobias Naegeli <naegelit@student.ethz.ch> - * @author Martin Rutschmann <rutmarti@student.ethz.ch> - * @author Lorenz Meier <lm@inf.ethz.ch> + * Copyright (C) 2012-2013 PX4 Development Team. All rights reserved. + * Author: Thomas Gubler <thomasgubler@student.ethz.ch> + * Julian Oes <joes@student.ethz.ch> + * Laurens Mackay <mackayl@student.ethz.ch> + * Tobias Naegeli <naegelit@student.ethz.ch> + * Martin Rutschmann <rutmarti@student.ethz.ch> + * Lorenz Meier <lm@inf.ethz.ch> * * 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 <thomasgubler@student.ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> + * @author Laurens Mackay <mackayl@student.ethz.ch> + * @author Tobias Naegeli <naegelit@student.ethz.ch> + * @author Martin Rutschmann <rutmarti@student.ethz.ch> + * @author Lorenz Meier <lm@inf.ethz.ch> */ #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 <rk3dov@gmail.com> + * Author: Anton Babushkin <anton.babushkin@me.com> * * 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 <rk3dov@gmail.com> + * @author Anton Babushkin <anton.babushkin@me.com> */ #include <string.h> 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 <rk3dov@gmail.com> + * Author: Anton Babushkin <anton.babushkin@me.com> * * 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 <rk3dov@gmail.com> + * @author Anton Babushkin <anton.babushkin@me.com> */ #ifndef SDLOG2_RINGBUFFER_H_ diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index e03295e6d..deac9e20b 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 <lm@inf.ethz.ch> - * Anton Babushkin <rk3dov@gmail.com> + * Anton Babushkin <anton.babushkin@me.com> * * 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 <lm@inf.ethz.ch> - * @author Anton Babushkin <rk3dov@gmail.com> + * @author Anton Babushkin <anton.babushkin@me.com> */ #include <nuttx/config.h> @@ -685,6 +685,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_AIRS_s log_AIRS; struct log_ARSP_s log_ARSP; struct log_FLOW_s log_FLOW; + struct log_GPOS_s log_GPOS; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -1056,7 +1057,14 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- GLOBAL POSITION --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(vehicle_global_position), subs.global_pos_sub, &buf.global_pos); - // TODO not implemented yet + log_msg.msg_type = LOG_GPOS_MSG; + log_msg.body.log_GPOS.lat = buf.global_pos.lat; + log_msg.body.log_GPOS.lon = buf.global_pos.lon; + log_msg.body.log_GPOS.alt = buf.global_pos.alt; + log_msg.body.log_GPOS.vel_n = buf.global_pos.vx; + log_msg.body.log_GPOS.vel_e = buf.global_pos.vy; + log_msg.body.log_GPOS.vel_d = buf.global_pos.vz; + LOGBUFFER_WRITE_AND_COUNT(GPOS); } /* --- VICON POSITION --- */ 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 <rk3dov@gmail.com> + * Author: Anton Babushkin <anton.babushkin@me.com> * * 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 <rk3dov@gmail.com> + * @author Anton Babushkin <anton.babushkin@me.com> */ /* diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index c100e921b..4a66fe116 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 <rk3dov@gmail.com> + * Author: Anton Babushkin <anton.babushkin@me.com> * * 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 <rk3dov@gmail.com> + * @author Anton Babushkin <anton.babushkin@me.com> */ #ifndef SDLOG2_MESSAGES_H_ @@ -198,6 +198,17 @@ struct log_FLOW_s { uint8_t quality; uint8_t sensor_id; }; + +/* --- GPOS - GLOBAL POSITION ESTIMATE --- */ +#define LOG_GPOS_MSG 16 +struct log_GPOS_s { + int32_t lat; + int32_t lon; + float alt; + float vel_n; + float vel_e; + float vel_d; +}; #pragma pack(pop) /* construct list of all message formats */ @@ -218,6 +229,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"), LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"), LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"), + LOG_FORMAT(GPOS, "LLffff", "Lat,Lon,Alt,VelN,VelE,VelD"), }; static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s); diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c index 49315cdc9..4996a8f66 100644 --- a/src/modules/systemlib/pid/pid.c +++ b/src/modules/systemlib/pid/pid.c @@ -1,9 +1,11 @@ /**************************************************************************** * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: @author Laurens Mackay <mackayl@student.ethz.ch> - * @author Tobias Naegeli <naegelit@student.ethz.ch> - * @author Martin Rutschmann <rutmarti@student.ethz.ch> + * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. + * Author: Laurens Mackay <mackayl@student.ethz.ch> + * Tobias Naegeli <naegelit@student.ethz.ch> + * Martin Rutschmann <rutmarti@student.ethz.ch> + * Anton Babushkin <anton.babushkin@me.com> + * Julian Oes <joes@student.ethz.ch> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -36,14 +38,21 @@ /** * @file pid.c - * Implementation of generic PID control interface + * + * Implementation of generic PID control interface. + * + * @author Laurens Mackay <mackayl@student.ethz.ch> + * @author Tobias Naegeli <naegelit@student.ethz.ch> + * @author Martin Rutschmann <rutmarti@student.ethz.ch> + * @author Anton Babushkin <anton.babushkin@me.com> + * @author Julian Oes <joes@student.ethz.ch> */ #include "pid.h" #include <math.h> __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 +60,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 +145,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 +161,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 +175,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 +183,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..714bf988f 100644 --- a/src/modules/systemlib/pid/pid.h +++ b/src/modules/systemlib/pid/pid.h @@ -1,9 +1,11 @@ /**************************************************************************** * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: @author Laurens Mackay <mackayl@student.ethz.ch> - * @author Tobias Naegeli <naegelit@student.ethz.ch> - * @author Martin Rutschmann <rutmarti@student.ethz.ch> + * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. + * Author: Laurens Mackay <mackayl@student.ethz.ch> + * Tobias Naegeli <naegelit@student.ethz.ch> + * Martin Rutschmann <rutmarti@student.ethz.ch> + * Anton Babushkin <anton.babushkin@me.com> + * Julian Oes <joes@student.ethz.ch> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -36,7 +38,14 @@ /** * @file pid.h - * Definition of generic PID control interface + * + * Definition of generic PID control interface. + * + * @author Laurens Mackay <mackayl@student.ethz.ch> + * @author Tobias Naegeli <naegelit@student.ethz.ch> + * @author Martin Rutschmann <rutmarti@student.ethz.ch> + * @author Anton Babushkin <anton.babushkin@me.com> + * @author Julian Oes <joes@student.ethz.ch> */ #ifndef PID_H_ @@ -47,8 +56,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 +74,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); |