mc att control multiplatform alongside normal mc att control

This commit is contained in:
Thomas Gubler
2015-01-05 16:12:15 +01:00
parent 941ff05720
commit 5876ff11ec
15 changed files with 1349 additions and 88 deletions
+3 -3
View File
@@ -168,9 +168,9 @@ target_link_libraries(subscriber
## MC Attitude Control
add_executable(mc_att_control
src/modules/mc_att_control/mc_att_control_main.cpp
src/modules/mc_att_control/mc_att_control.cpp
src/modules/mc_att_control/mc_att_control_base.cpp)
src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp
src/modules/mc_att_control_multiplatform/mc_att_control.cpp
src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp)
add_dependencies(mc_att_control px4_generate_messages_cpp)
target_link_libraries(mc_att_control
${catkin_LIBRARIES}
+1
View File
@@ -86,6 +86,7 @@ MODULES += modules/position_estimator_inav
#MODULES += modules/fw_pos_control_l1
#MODULES += modules/fw_att_control
MODULES += modules/mc_att_control
MODULES += modules/mc_att_control_multiplatform
MODULES += modules/mc_pos_control
MODULES += modules/vtol_att_control
File diff suppressed because it is too large Load Diff
@@ -40,8 +40,6 @@
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#include <px4_defines.h>
#include "mc_att_control_params.h"
#include <systemlib/param/param.h>
/**
@@ -52,7 +50,7 @@
* @min 0.0
* @group Multicopter Attitude Control
*/
PX4_PARAM_DEFINE_FLOAT(MC_ROLL_P);
PARAM_DEFINE_FLOAT(MC_ROLL_P, 6.0f);
/**
* Roll rate P gain
@@ -62,7 +60,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_ROLL_P);
* @min 0.0
* @group Multicopter Attitude Control
*/
PX4_PARAM_DEFINE_FLOAT(MC_ROLLRATE_P);
PARAM_DEFINE_FLOAT(MC_ROLLRATE_P, 0.1f);
/**
* Roll rate I gain
@@ -72,7 +70,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_ROLLRATE_P);
* @min 0.0
* @group Multicopter Attitude Control
*/
PX4_PARAM_DEFINE_FLOAT(MC_ROLLRATE_I);
PARAM_DEFINE_FLOAT(MC_ROLLRATE_I, 0.0f);
/**
* Roll rate D gain
@@ -82,7 +80,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_ROLLRATE_I);
* @min 0.0
* @group Multicopter Attitude Control
*/
PX4_PARAM_DEFINE_FLOAT(MC_ROLLRATE_D);
PARAM_DEFINE_FLOAT(MC_ROLLRATE_D, 0.002f);
/**
* Pitch P gain
@@ -93,7 +91,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_ROLLRATE_D);
* @min 0.0
* @group Multicopter Attitude Control
*/
PX4_PARAM_DEFINE_FLOAT(MC_PITCH_P);
PARAM_DEFINE_FLOAT(MC_PITCH_P, 6.0f);
/**
* Pitch rate P gain
@@ -103,7 +101,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_PITCH_P);
* @min 0.0
* @group Multicopter Attitude Control
*/
PX4_PARAM_DEFINE_FLOAT(MC_PITCHRATE_P);
PARAM_DEFINE_FLOAT(MC_PITCHRATE_P, 0.1f);
/**
* Pitch rate I gain
@@ -113,7 +111,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_PITCHRATE_P);
* @min 0.0
* @group Multicopter Attitude Control
*/
PX4_PARAM_DEFINE_FLOAT(MC_PITCHRATE_I);
PARAM_DEFINE_FLOAT(MC_PITCHRATE_I, 0.0f);
/**
* Pitch rate D gain
@@ -123,7 +121,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_PITCHRATE_I);
* @min 0.0
* @group Multicopter Attitude Control
*/
PX4_PARAM_DEFINE_FLOAT(MC_PITCHRATE_D);
PARAM_DEFINE_FLOAT(MC_PITCHRATE_D, 0.002f);
/**
* Yaw P gain
@@ -134,7 +132,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_PITCHRATE_D);
* @min 0.0
* @group Multicopter Attitude Control
*/
PX4_PARAM_DEFINE_FLOAT(MC_YAW_P);
PARAM_DEFINE_FLOAT(MC_YAW_P, 2.0f);
/**
* Yaw rate P gain
@@ -144,7 +142,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_YAW_P);
* @min 0.0
* @group Multicopter Attitude Control
*/
PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_P);
PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f);
/**
* Yaw rate I gain
@@ -154,7 +152,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_P);
* @min 0.0
* @group Multicopter Attitude Control
*/
PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_I);
PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f);
/**
* Yaw rate D gain
@@ -164,7 +162,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_I);
* @min 0.0
* @group Multicopter Attitude Control
*/
PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_D);
PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f);
/**
* Yaw feed forward
@@ -175,7 +173,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_D);
* @max 1.0
* @group Multicopter Attitude Control
*/
PX4_PARAM_DEFINE_FLOAT(MC_YAW_FF);
PARAM_DEFINE_FLOAT(MC_YAW_FF, 0.5f);
/**
* Max yaw rate
@@ -187,7 +185,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_YAW_FF);
* @max 360.0
* @group Multicopter Attitude Control
*/
PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX);
PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX, 120.0f);
/**
* Max manual roll
@@ -197,7 +195,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX);
* @max 90.0
* @group Multicopter Attitude Control
*/
PX4_PARAM_DEFINE_FLOAT(MC_MAN_R_MAX);
PARAM_DEFINE_FLOAT(MC_MAN_R_MAX, 35.0f);
/**
* Max manual pitch
@@ -207,7 +205,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_MAN_R_MAX);
* @max 90.0
* @group Multicopter Attitude Control
*/
PX4_PARAM_DEFINE_FLOAT(MC_MAN_P_MAX);
PARAM_DEFINE_FLOAT(MC_MAN_P_MAX, 35.0f);
/**
* Max manual yaw rate
@@ -216,7 +214,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_MAN_P_MAX);
* @min 0.0
* @group Multicopter Attitude Control
*/
PX4_PARAM_DEFINE_FLOAT(MC_MAN_Y_MAX);
PARAM_DEFINE_FLOAT(MC_MAN_Y_MAX, 120.0f);
/**
* Max acro roll rate
@@ -226,7 +224,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_MAN_Y_MAX);
* @max 360.0
* @group Multicopter Attitude Control
*/
PX4_PARAM_DEFINE_FLOAT(MC_ACRO_R_MAX);
PARAM_DEFINE_FLOAT(MC_ACRO_R_MAX, 90.0f);
/**
* Max acro pitch rate
@@ -236,7 +234,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_ACRO_R_MAX);
* @max 360.0
* @group Multicopter Attitude Control
*/
PX4_PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX);
PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX, 90.0f);
/**
* Max acro yaw rate
@@ -245,4 +243,4 @@ PX4_PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX);
* @min 0.0
* @group Multicopter Attitude Control
*/
PX4_PARAM_DEFINE_FLOAT(MC_ACRO_Y_MAX);
PARAM_DEFINE_FLOAT(MC_ACRO_Y_MAX, 120.0f);
+1 -3
View File
@@ -38,6 +38,4 @@
MODULE_COMMAND = mc_att_control
SRCS = mc_att_control_main.cpp \
mc_att_control.cpp \
mc_att_control_base.cpp \
mc_att_control_params.c
mc_att_control_params.c
@@ -0,0 +1,139 @@
/****************************************************************************
*
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
*
* 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 mc_att_control_main.cpp
* Multicopter attitude controller.
*
* @author Tobias Naegeli <naegelit@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
* @author Thomas Gubler <thomasgubler@gmail.com>
* @author Julian Oes <julian@oes.ch>
* @author Roman Bapst <bapstr@ethz.ch>
*
* The controller has two loops: P loop for angular error and PD loop for angular rate error.
* Desired rotation calculated keeping in mind that yaw response is normally slower than roll/pitch.
* For small deviations controller rotates copter to have shortest path of thrust vector and independently rotates around yaw,
* so actual rotation axis is not constant. For large deviations controller rotates copter around fixed axis.
* These two approaches fused seamlessly with weight depending on angular error.
* When thrust vector directed near-horizontally (e.g. roll ~= PI/2) yaw setpoint ignored because of singularity.
* Controller doesn't use Euler angles for work, they generated only for more human-friendly control and logging.
* If rotation matrix setpoint is invalid it will be generated from Euler angles for compatibility with old position controllers.
*/
#include <string.h>
#include <cstdlib>
#include "mc_att_control.h"
static bool thread_running = false; /**< Deamon status flag */
static int daemon_task; /**< Handle of deamon task / thread */
namespace px4
{
bool task_should_exit = false;
}
using namespace px4;
PX4_MAIN_FUNCTION(mc_att_control_multiplatform);
#if !defined(__linux) && !(defined(__APPLE__) && defined(__MACH__))
/**
* Multicopter attitude control app start / stop handling function
*
* @ingroup apps
*/
extern "C" __EXPORT int mc_att_control_multiplatform_main(int argc, char *argv[]);
int mc_att_control_multiplatform_main(int argc, char *argv[])
{
if (argc < 1) {
errx(1, "usage: mc_att_control {start|stop|status}");
}
if (!strcmp(argv[1], "start")) {
if (thread_running) {
warnx("already running");
/* this is not an error */
exit(0);
}
task_should_exit = false;
daemon_task = task_spawn_cmd("mc_att_control",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
3000,
mc_att_control_multiplatform_task_main,
(argv) ? (char * const *)&argv[2] : (char * const *)NULL);
exit(0);
}
if (!strcmp(argv[1], "stop")) {
task_should_exit = true;
exit(0);
}
if (!strcmp(argv[1], "status")) {
if (thread_running) {
warnx("is running");
} else {
warnx("not started");
}
exit(0);
}
warnx("unrecognized command");
return 1;
}
#endif
PX4_MAIN_FUNCTION(mc_att_control_multiplatform)
{
px4::init(argc, argv, "mc_att_control_multiplatform");
PX4_INFO("starting");
MulticopterAttitudeControl attctl;
thread_running = true;
attctl.spin();
PX4_INFO("exiting.");
thread_running = false;
return 0;
}
@@ -0,0 +1,248 @@
/****************************************************************************
*
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
*
* 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 mc_att_control_params.c
* Parameters for multicopter attitude controller.
*
* @author Tobias Naegeli <naegelit@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#include <px4_defines.h>
#include "mc_att_control_params.h"
#include <systemlib/param/param.h>
/**
* Roll P gain
*
* Roll proportional gain, i.e. desired angular speed in rad/s for error 1 rad.
*
* @min 0.0
* @group Multicopter Attitude Control
*/
PX4_PARAM_DEFINE_FLOAT(MC_ROLL_P);
/**
* Roll rate P gain
*
* Roll rate proportional gain, i.e. control output for angular speed error 1 rad/s.
*
* @min 0.0
* @group Multicopter Attitude Control
*/
PX4_PARAM_DEFINE_FLOAT(MC_ROLLRATE_P);
/**
* Roll rate I gain
*
* Roll rate integral gain. Can be set to compensate static thrust difference or gravity center offset.
*
* @min 0.0
* @group Multicopter Attitude Control
*/
PX4_PARAM_DEFINE_FLOAT(MC_ROLLRATE_I);
/**
* Roll rate D gain
*
* Roll rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
*
* @min 0.0
* @group Multicopter Attitude Control
*/
PX4_PARAM_DEFINE_FLOAT(MC_ROLLRATE_D);
/**
* Pitch P gain
*
* Pitch proportional gain, i.e. desired angular speed in rad/s for error 1 rad.
*
* @unit 1/s
* @min 0.0
* @group Multicopter Attitude Control
*/
PX4_PARAM_DEFINE_FLOAT(MC_PITCH_P);
/**
* Pitch rate P gain
*
* Pitch rate proportional gain, i.e. control output for angular speed error 1 rad/s.
*
* @min 0.0
* @group Multicopter Attitude Control
*/
PX4_PARAM_DEFINE_FLOAT(MC_PITCHRATE_P);
/**
* Pitch rate I gain
*
* Pitch rate integral gain. Can be set to compensate static thrust difference or gravity center offset.
*
* @min 0.0
* @group Multicopter Attitude Control
*/
PX4_PARAM_DEFINE_FLOAT(MC_PITCHRATE_I);
/**
* Pitch rate D gain
*
* Pitch rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
*
* @min 0.0
* @group Multicopter Attitude Control
*/
PX4_PARAM_DEFINE_FLOAT(MC_PITCHRATE_D);
/**
* Yaw P gain
*
* Yaw proportional gain, i.e. desired angular speed in rad/s for error 1 rad.
*
* @unit 1/s
* @min 0.0
* @group Multicopter Attitude Control
*/
PX4_PARAM_DEFINE_FLOAT(MC_YAW_P);
/**
* Yaw rate P gain
*
* Yaw rate proportional gain, i.e. control output for angular speed error 1 rad/s.
*
* @min 0.0
* @group Multicopter Attitude Control
*/
PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_P);
/**
* Yaw rate I gain
*
* Yaw rate integral gain. Can be set to compensate static thrust difference or gravity center offset.
*
* @min 0.0
* @group Multicopter Attitude Control
*/
PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_I);
/**
* Yaw rate D gain
*
* Yaw rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
*
* @min 0.0
* @group Multicopter Attitude Control
*/
PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_D);
/**
* Yaw feed forward
*
* Feed forward weight for manual yaw control. 0 will give slow responce and no overshot, 1 - fast responce and big overshot.
*
* @min 0.0
* @max 1.0
* @group Multicopter Attitude Control
*/
PX4_PARAM_DEFINE_FLOAT(MC_YAW_FF);
/**
* Max yaw rate
*
* Limit for yaw rate, has effect for large rotations in autonomous mode, to avoid large control output and mixer saturation.
*
* @unit deg/s
* @min 0.0
* @max 360.0
* @group Multicopter Attitude Control
*/
PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX);
/**
* Max manual roll
*
* @unit deg
* @min 0.0
* @max 90.0
* @group Multicopter Attitude Control
*/
PX4_PARAM_DEFINE_FLOAT(MC_MAN_R_MAX);
/**
* Max manual pitch
*
* @unit deg
* @min 0.0
* @max 90.0
* @group Multicopter Attitude Control
*/
PX4_PARAM_DEFINE_FLOAT(MC_MAN_P_MAX);
/**
* Max manual yaw rate
*
* @unit deg/s
* @min 0.0
* @group Multicopter Attitude Control
*/
PX4_PARAM_DEFINE_FLOAT(MC_MAN_Y_MAX);
/**
* Max acro roll rate
*
* @unit deg/s
* @min 0.0
* @max 360.0
* @group Multicopter Attitude Control
*/
PX4_PARAM_DEFINE_FLOAT(MC_ACRO_R_MAX);
/**
* Max acro pitch rate
*
* @unit deg/s
* @min 0.0
* @max 360.0
* @group Multicopter Attitude Control
*/
PX4_PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX);
/**
* Max acro yaw rate
*
* @unit deg/s
* @min 0.0
* @group Multicopter Attitude Control
*/
PX4_PARAM_DEFINE_FLOAT(MC_ACRO_Y_MAX);
@@ -0,0 +1,43 @@
############################################################################
#
# Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
#
# 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.
#
############################################################################
#
# Multirotor attitude controller (vector based, no Euler singularities)
#
MODULE_COMMAND = mc_att_control_multiplatform
SRCS = mc_att_control_main.cpp \
mc_att_control.cpp \
mc_att_control_base.cpp \
mc_att_control_params.c