aboutsummaryrefslogtreecommitdiff
path: root/src
diff options
context:
space:
mode:
Diffstat (limited to 'src')
-rw-r--r--src/modules/commander/accelerometer_calibration.c50
-rw-r--r--src/modules/commander/accelerometer_calibration.h43
-rw-r--r--src/modules/fixedwing_att_control/fixedwing_att_control_att.c4
-rw-r--r--src/modules/fixedwing_att_control/fixedwing_att_control_rate.c6
-rw-r--r--src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c8
-rw-r--r--src/modules/multirotor_att_control/multirotor_attitude_control.c32
-rw-r--r--src/modules/multirotor_att_control/multirotor_attitude_control.h22
-rw-r--r--src/modules/multirotor_att_control/multirotor_rate_control.c55
-rw-r--r--src/modules/multirotor_att_control/multirotor_rate_control.h24
-rw-r--r--src/modules/sdlog2/logbuffer.c4
-rw-r--r--src/modules/sdlog2/logbuffer.h4
-rw-r--r--src/modules/sdlog2/sdlog2.c4
-rw-r--r--src/modules/sdlog2/sdlog2_format.h4
-rw-r--r--src/modules/sdlog2/sdlog2_messages.h4
-rw-r--r--src/modules/systemlib/pid/pid.c65
-rw-r--r--src/modules/systemlib/pid/pid.h27
16 files changed, 233 insertions, 123 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 71c78f5b8..6059d9a44 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 982f843be..4cca46b9c 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>
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 1b2237d65..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_
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);
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-10 23:59:12 +0000