Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-29 19:57:12 +08:00
Code Issues Actions 14 Packages Projects Releases Wiki Activity

unmanned ground vehicle (UGV) controllers and Traxxas Stampede configuration (#7175)

Browse Source
This commit is contained in:
Marco Zorzi
2017-06-06 19:26:51 +02:00
committed by Daniel Agar
parent 184b190513
commit aef522553e
29 changed files with 2270 additions and 72 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ROMFS/px4fmu_common/init.d/50002_traxxas_stampede_2wd
+61
View File
@@ -0,0 +1,61 @@
#!nsh
#
# @name Traxxas stampede vxl 2wd
#
# @url https://traxxas.com/products/models/electric/stampede-vxl-tsm
#
# @type Rover
#
# @output MAIN2 steering
# @output MAIN4 throttle
#
# @maintainer Marco Zorzi
#
sh /etc/init.d/rc.gnd_defaults
if [ $AUTOCNF == yes ]
then
param set BAT_N_CELLS 7
param set FW_AIRSPD_MIN 0
param set FW_AIRSPD_TRIM 1
param set FW_AIRSPD_MAX 3
param set NAV_ACC_RAD 0.5
param set MIS_LTRMIN_ALT 0.01
param set MIS_TAKEOFF_ALT 0.01
param set EKF2_GBIAS_INIT 0.01
param set EKF2_ANGERR_INIT 0.01
param set EKF2_MAG_TYPE 1
param set GND_WR_P 2
param set GND_WR_I 0.9674
param set GND_WR_IMAX 0.1
param set GND_WR_D 1.2
param set GND_SP_CTRL_MODE 1
param set GND_L1_DIST 10
param set GND_THR_IDLE 0
param set GND_THR_CRUISE 0
param set GND_THR_MAX 0.5
param set GND_THR_MIN 0
param set GND_SPEED_P 0.25
param set GND_SPEED_I 0.001
param set GND_SPEED_D 3
param set GND_SPEED_IMAX 0.125
param set GND_SPEED_THR_SC 1
fi
# Configure this as ugv
set MAV_TYPE 10
# Set mixer
set MIXER stampede
# Provide ESC a constant 1500 us pulse
set PWM_DISARMED 1500
set PWM_MAIN_REV2 1
set PWM_MAX 2000
set PWM_MIN 1000
ROMFS/px4fmu_common/init.d/rc.axialracing_ax10_defaults
+1 -1
View File
@@ -1,6 +1,6 @@
#!nsh
set VEHICLE_TYPE rover
set VEHICLE_TYPE ugv
# This section can be enabled once tuning parameters for this particular
# rover model are known. It allows to configure default gains via the GUI
ROMFS/px4fmu_common/init.d/rc.gnd_apps
+24
View File
@@ -0,0 +1,24 @@
#!nsh
#
# Standard apps for unmanned ground vehicles (UGV)
#
#
# Start the attitude and position estimator
#
ekf2 start
#attitude_estimator_q start
#local_position_estimator start
#
# Start attitude controllers
#
gnd_att_control start
gnd_pos_control start
#
# Start Land Detector
#
land_detector start ugv
ROMFS/px4fmu_common/init.d/rc.gnd_defaults
+32
View File
@@ -0,0 +1,32 @@
#!nsh
set VEHICLE_TYPE ugv
if [ $AUTOCNF == yes ]
then
#
# Default parameters for UGVs
#
param set NAV_DLL_ACT 0
param set NAV_ACC_RAD 2.0
# temporary
param set NAV_FW_ALT_RAD 1000
param set MIS_LTRMIN_ALT 0.01
param set MIS_TAKEOFF_ALT 0.01
fi
# Enable servo output on pins 3 and 4 (steering and thrust)
# but also include 1+2 as they form together one output group
# and need to be set together.
set PWM_OUT 1234
# PWM Hz - 50 Hz is the normal rate in RC cars, higher rates
# may damage analog servos.
set PWM_RATE 50
# This is the gimbal pass mixer
set MIXER_AUX pass
set PWM_AUX_RATE 50
set PWM_AUX_OUT 1234
ROMFS/px4fmu_common/init.d/rcS
+17 -8
View File
@@ -1042,20 +1042,29 @@ then
fi
#
# Rover setup
# UGV setup
#
if [ $VEHICLE_TYPE == rover ]
if [ $VEHICLE_TYPE == ugv ]
then
# 10 is MAV_TYPE_GROUND_ROVER
set MAV_TYPE 10
if [ $MIXER == none ]
then
# Set default mixer for UGV if not defined
set MIXER stampede
fi
if [ $MAV_TYPE == none ]
then
# Use MAV_TYPE = 10 (UGV) if not defined
set MAV_TYPE 10
fi
param set MAV_TYPE ${MAV_TYPE}
# Load mixer and configure outputs
sh /etc/init.d/rc.interface
# Start standard rover apps
sh /etc/init.d/rc.axialracing_ax10_apps
param set MAV_TYPE 10
# Start standard UGV apps
sh /etc/init.d/rc.gnd_apps
fi
#
ROMFS/px4fmu_common/mixers/stampede.main.mix
+37
View File
@@ -0,0 +1,37 @@
Traxxas Stampede Rc Car mixer for PX4FMU
===========================
Designed for Traxxas Stampede
This file defines mixers suitable for controlling a Traxxas Stampede rover using
PX4FMU. The configuration assumes the steering is connected to PX4FMU
servo outputs 1 and the motor speed control to output 3. Output 0 and 2 is
assumed to be unused.
Inputs to the mixer come from channel group 0 (vehicle attitude), channels 2 (yaw), and 3 (thrust).
See the README for more information on the scaler format.
Output 0
---------------------------------------
Z:
Steering mixer using roll on output 1
---------------------------------------
M: 1
O: 10000 10000 0 -10000 10000
S: 0 2 10000 10000 0 -10000 10000
Output 2
---------------------------------------
This mixer is empty.
Z:
Output 3
---------------------------------------
M: 1
O: 10000 10000 0 -10000 10000
S: 0 3 10000 10000 0 -10000 10000
ROMFS/sitl/mixers/rover_sitl.main.mix
+31 -22
View File
@@ -1,37 +1,46 @@
Mixer for SITL rover
=========================================================
# mixer for the rudder
Output 0
---------------------------------------
M: 1
O: 10000 10000 0 -10000 10000
S: 0 0 10000 10000 0 -10000 10000
O: 10000 10000 0 -10000 10000
S: 0 2 10000 10000 0 -10000 10000
# mixer for the rudder
Output 1
---------------------------------------
M: 1
O: 10000 10000 0 -10000 10000
S: 0 0 10000 10000 0 -10000 10000
O: 10000 10000 0 -10000 10000
S: 0 2 10000 10000 0 -10000 10000
# mixer for the elevator
M: 1
O: 10000 10000 0 -10000 10000
S: 0 1 10000 10000 0 -10000 10000
Output 2
---------------------------------------
Z:
# mixer for throttle
Output 3
---------------------------------------
Z:
Output 4
---------------------------------------
Z:
Output 5
---------------------------------------
M: 1
O: 10000 10000 0 -10000 10000
S: 0 3 0 20000 -10000 -10000 10000
S: 0 3 10000 10000 0 -10000 10000
# mixer for throttle
Output 6
---------------------------------------
M: 1
O: 10000 10000 0 -10000 10000
S: 0 3 0 20000 -10000 -10000 10000
S: 0 3 10000 10000 0 -10000 10000
# mixer for throttle
M: 1
O: 10000 10000 0 -10000 10000
S: 0 3 0 20000 -10000 -10000 10000
Output 7
---------------------------------------
Z:
# mixer for throttle
M: 1
O: 10000 10000 0 -10000 10000
S: 0 3 0 20000 -10000 -10000 10000
Output 8
---------------------------------------
Z:
cmake/configs/nuttx_aerocore2_default.cmake
+3 -1
View File
@@ -86,8 +86,10 @@ set(config_module_list
#
# Vehicle Control
#
modules/fw_pos_control_l1
modules/fw_att_control
modules/fw_pos_control_l1
modules/gnd_att_control
modules/gnd_pos_control
modules/mc_att_control
modules/mc_pos_control
modules/vtol_att_control
cmake/configs/nuttx_aerofc-v1_default.cmake
+3 -1
View File
@@ -67,8 +67,10 @@ set(config_module_list
#
# Vehicle Control
#
modules/fw_pos_control_l1
modules/fw_att_control
modules/fw_pos_control_l1
modules/gnd_att_control
modules/gnd_pos_control
modules/mc_att_control
modules/mc_pos_control
modules/vtol_att_control
cmake/configs/nuttx_auav-x21_default.cmake
+3 -1
View File
@@ -108,8 +108,10 @@ set(config_module_list
#
# Vehicle Control
#
modules/fw_pos_control_l1
modules/fw_att_control
modules/fw_pos_control_l1
modules/gnd_att_control
modules/gnd_pos_control
modules/mc_att_control
modules/mc_pos_control
modules/vtol_att_control
cmake/configs/nuttx_mindpx-v2_default.cmake
+3 -1
View File
@@ -110,8 +110,10 @@ set(config_module_list
#
# Vehicle Control
#
modules/fw_pos_control_l1
modules/fw_att_control
modules/fw_pos_control_l1
modules/gnd_att_control
modules/gnd_pos_control
modules/mc_att_control
modules/mc_pos_control
modules/vtol_att_control
cmake/configs/nuttx_px4fmu-v2_default.cmake
+3 -1
View File
@@ -111,8 +111,10 @@ set(config_module_list
#
# Vehicle Control
#
modules/fw_pos_control_l1
modules/fw_att_control
modules/fw_pos_control_l1
modules/gnd_att_control
modules/gnd_pos_control
modules/mc_att_control
modules/mc_pos_control
modules/vtol_att_control
cmake/configs/nuttx_px4fmu-v3_default.cmake
+2
View File
@@ -118,6 +118,8 @@ set(config_module_list
#
modules/fw_att_control
modules/fw_pos_control_l1
modules/gnd_att_control
modules/gnd_pos_control
modules/mc_att_control
modules/mc_pos_control
modules/vtol_att_control
cmake/configs/nuttx_px4fmu-v4_default.cmake
+3 -2
View File
@@ -116,9 +116,10 @@ set(config_module_list
#
# Vehicle Control
#
# modules/segway # XXX Needs GCC 4.7 fix
modules/fw_pos_control_l1
modules/fw_att_control
modules/fw_pos_control_l1
modules/gnd_att_control
modules/gnd_pos_control
modules/mc_att_control
modules/mc_pos_control
modules/vtol_att_control
cmake/configs/nuttx_px4fmu-v4pro_default.cmake
+2
View File
@@ -119,6 +119,8 @@ set(config_module_list
#
modules/fw_att_control
modules/fw_pos_control_l1
modules/gnd_att_control
modules/gnd_pos_control
modules/mc_att_control
modules/mc_pos_control
modules/vtol_att_control
cmake/configs/nuttx_px4fmu-v5_default.cmake
+2
View File
@@ -117,6 +117,8 @@ set(config_module_list
#
modules/fw_att_control
modules/fw_pos_control_l1
modules/gnd_att_control
modules/gnd_pos_control
modules/mc_att_control
modules/mc_pos_control
modules/vtol_att_control
cmake/configs/posix_rpi_common.cmake
+4 -2
View File
@@ -47,10 +47,12 @@ set(config_module_list
#
# Vehicle Control
#
modules/mc_att_control
modules/mc_pos_control
modules/fw_att_control
modules/fw_pos_control_l1
modules/gnd_att_control
modules/gnd_pos_control
modules/mc_att_control
modules/mc_pos_control
modules/vtol_att_control
#
cmake/configs/posix_sitl_default.cmake
+2
View File
@@ -83,6 +83,8 @@ set(config_module_list
#
modules/fw_att_control
modules/fw_pos_control_l1
modules/gnd_att_control
modules/gnd_pos_control
modules/mc_att_control
modules/mc_pos_control
modules/vtol_att_control
posix-configs/SITL/init/ekf2/rover
+42 -31
View File
@@ -1,42 +1,52 @@
uorb start
param load
param set MAV_TYPE 1
param set SYS_AUTOSTART 3033
param set SYS_RESTART_TYPE 2
param set SYS_MC_EST_GROUP 2
dataman start
param set BAT_N_CELLS 3
param set CAL_GYRO0_ID 2293768
param set CAL_ACC0_ID 1376264
param set CAL_ACC1_ID 1310728
param set CAL_MAG0_ID 196616
param set CAL_GYRO0_XOFF 0.01
param set CAL_ACC0_XOFF 0.01
param set CAL_ACC0_YOFF -0.01
param set CAL_ACC0_ZOFF 0.01
param set CAL_ACC0_XSCALE 1.01
param set CAL_ACC0_YOFF -0.01
param set CAL_ACC0_YSCALE 1.01
param set CAL_ACC0_ZOFF 0.01
param set CAL_ACC0_ZSCALE 1.01
param set CAL_ACC1_ID 1310728
param set CAL_ACC1_XOFF 0.01
param set CAL_GYRO0_ID 2293768
param set CAL_GYRO0_XOFF 0.01
param set CAL_MAG0_ID 196616
param set CAL_MAG0_XOFF 0.01
param set SENS_BOARD_ROT 0
param set SENS_DPRES_OFF 0.001
param set SENS_BOARD_X_OFF 0.000001
param set COM_RC_IN_MODE 1
param set NAV_DLL_ACT 2
param set NAV_ACC_RAD 2.0
param set NAV_LOITER_RAD 10
param set EKF2_ANGERR_INIT 0.01
param set EKF2_GBIAS_INIT 0.01
param set EKF2_MAG_TYPE 1
param set GND_L1_DIST 5
param set GND_SP_CTRL_MODE 1
param set GND_SPEED_D 3
param set GND_SPEED_I 0.001
param set GND_SPEED_IMAX 0.125
param set GND_SPEED_P 0.25
param set GND_SPEED_THR_SC 1
param set GND_THR_CRUISE 0.3
param set GND_THR_IDLE 0
param set GND_THR_MAX 0.5
param set GND_THR_MIN 0
param set GND_SPEED_TRIM 4
param set GND_WR_D 1.2
param set GND_WR_I 0.9674
param set GND_WR_IMAX 0.1
param set GND_WR_P 2
param set MAV_TYPE 10
param set MIS_LTRMIN_ALT 0.01
param set MIS_TAKEOFF_ALT 0.01
param set FW_THR_IDLE 0.0
param set EKF2_GBIAS_INIT 0.01
param set EKF2_ANGERR_INIT 0.01
param set EKF2_MAG_TYPE 1
param set FW_AIRSPD_MIN 0
param set FW_AIRSPD_TRIM 6
param set FW_AIRSPD_MAX 8
param set FW_PR_I 0.0
param set FW_RR_I 0.0
param set NAV_ACC_RAD 0.5
param set NAV_DLL_ACT 0
param set NAV_LOITER_RAD 2
param set SENS_BOARD_ROT 0
param set SENS_BOARD_X_OFF 0.000001
param set SENS_DPRES_OFF 0.001
param set SYS_AUTOSTART 50002
param set SYS_MC_EST_GROUP 2
param set SYS_RESTART_TYPE 2
replay tryapplyparams
simulator start -s
tone_alarm start
@@ -50,23 +60,24 @@ pwm_out_sim mode_pwm
sleep 1
sensors start
commander start
land_detector start ugv
navigator start
ekf2 start
fw_pos_control_l1 start
fw_att_control start
land_detector start rover
gnd_pos_control start
gnd_att_control start
mixer load /dev/pwm_output0 ROMFS/sitl/mixers/rover_sitl.main.mix
mavlink start -u 14556 -r 2000000
mavlink start -u 14557 -r 2000000 -m onboard -o 14540
mavlink start -u 14556 -r 4000000
mavlink start -u 14557 -r 4000000 -m onboard -o 14540
mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556
mavlink stream -r 80 -s LOCAL_POSITION_NED -u 14556
mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556
mavlink stream -r 80 -s ATTITUDE -u 14556
mavlink stream -r 80 -s ATTITUDE_QUATERNION -u 14556
mavlink stream -r 80 -s ATTITUDE_TARGET -u 14556
mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 14556
mavlink stream -r 20 -s RC_CHANNELS -u 14556
mavlink stream -r 250 -s HIGHRES_IMU -u 14556
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556
logger start -e -t
sdlog2 start -r 200 -e -t -a
mavlink boot_complete
replay trystart
posix-configs/SITL/init/lpe/rover
+79
View File
@@ -0,0 +1,79 @@
uorb start
param load
dataman start
param set BAT_N_CELLS 3
param set CAL_ACC0_ID 1376264
param set CAL_ACC0_XOFF 0.01
param set CAL_ACC0_XSCALE 1.01
param set CAL_ACC0_YOFF -0.01
param set CAL_ACC0_YSCALE 1.01
param set CAL_ACC0_ZOFF 0.01
param set CAL_ACC0_ZSCALE 1.01
param set CAL_ACC1_ID 1310728
param set CAL_ACC1_XOFF 0.01
param set CAL_GYRO0_ID 2293768
param set CAL_GYRO0_XOFF 0.01
param set CAL_MAG0_ID 196616
param set CAL_MAG0_XOFF 0.01
param set COM_RC_IN_MODE 1
param set GND_L1_DIST 5
param set GND_SP_CTRL_MODE 0
param set GND_SPEED_D 3
param set GND_SPEED_I 0.001
param set GND_SPEED_IMAX 0.125
param set GND_SPEED_P 0.25
param set GND_SPEED_THR_SC 1
param set GND_THR_CRUISE 0.3
param set GND_THR_IDLE 0
param set GND_THR_MAX 1
param set GND_THR_MIN 0
param set GND_WR_D 1.2
param set GND_WR_I 0.9674
param set GND_WR_IMAX 0.1
param set GND_WR_P 2
param set MAV_TYPE 10
param set MIS_LTRMIN_ALT 0.01
param set MIS_TAKEOFF_ALT 0.01
param set NAV_ACC_RAD 0.5
param set NAV_DLL_ACT 0
param set NAV_LOITER_RAD 2
param set SENS_BOARD_ROT 0
param set SENS_BOARD_X_OFF 0.000001
param set SENS_DPRES_OFF 0.001
param set SYS_AUTOSTART 50002
param set SYS_MC_EST_GROUP 1
param set SYS_RESTART_TYPE 2
replay tryapplyparams
simulator start -s
tone_alarm start
gyrosim start
accelsim start
barosim start
adcsim start
gpssim start
measairspeedsim start
pwm_out_sim mode_pwm
sleep 1
sensors start
commander start
land_detector start ugv
navigator start
ekf2 start
gnd_pos_control start
gnd_att_control start
mixer load /dev/pwm_output0 ROMFS/sitl/mixers/rover_sitl.main.mix
mavlink start -u 14556 -r 4000000
mavlink start -u 14557 -r 4000000 -m onboard -o 14540
mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556
mavlink stream -r 80 -s LOCAL_POSITION_NED -u 14556
mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556
mavlink stream -r 80 -s ATTITUDE -u 14556
mavlink stream -r 80 -s ATTITUDE_QUATERNION -u 14556
mavlink stream -r 80 -s ATTITUDE_TARGET -u 14556
mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 14556
mavlink stream -r 20 -s RC_CHANNELS -u 14556
mavlink stream -r 250 -s HIGHRES_IMU -u 14556
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556
sdlog2 start -r 200 -e -t -a
mavlink boot_complete
replay trystart
src/modules/gnd_att_control/CMakeLists.txt
+45
View File
@@ -0,0 +1,45 @@
############################################################################
#
# Copyright (c) 2017 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.
#
############################################################################
px4_add_module(
MODULE modules__gnd_att_control
MAIN gnd_att_control
STACK_MAIN 1200
COMPILE_FLAGS
SRCS
GroundRoverAttitudeControl.cpp
DEPENDS
platforms__common
git_ecl
lib__ecl
)
# vim: set noet ft=cmake fenc=utf-8 ff=unix :
src/modules/gnd_att_control/GroundRoverAttitudeControl.cpp
+440
View File
@@ -0,0 +1,440 @@
/****************************************************************************
*
* Copyright (c) 2017 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.
*
****************************************************************************/
/**
*
* This module is a modification of the fixed wing module and it is designed for ground rovers.
* It has been developed starting from the fw module, simplified and improved with dedicated items.
*
* All the acknowledgments and credits for the fw wing app are reported in those files.
*
* @author Marco Zorzi <mzorzi@student.ethz.ch>
*/
#include "GroundRoverAttitudeControl.hpp"
/**
* GroundRover attitude control app start / stop handling function
*
* @ingroup apps
*/
extern "C" __EXPORT int gnd_att_control_main(int argc, char *argv[]);
namespace att_gnd_control
{
GroundRoverAttitudeControl *g_control = nullptr;
}
GroundRoverAttitudeControl::GroundRoverAttitudeControl() :
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "gnda_dt")),
_nonfinite_input_perf(perf_alloc(PC_COUNT, "gnda_nani")),
_nonfinite_output_perf(perf_alloc(PC_COUNT, "gnda_nano"))
{
_parameter_handles.w_p = param_find("GND_WR_P");
_parameter_handles.w_i = param_find("GND_WR_I");
_parameter_handles.w_d = param_find("GND_WR_D");
_parameter_handles.w_imax = param_find("GND_WR_IMAX");
_parameter_handles.trim_yaw = param_find("TRIM_YAW");
_parameter_handles.man_yaw_scale = param_find("GND_MAN_Y_SC");
_parameter_handles.bat_scale_en = param_find("GND_BAT_SCALE_EN");
/* fetch initial parameter values */
parameters_update();
}
GroundRoverAttitudeControl::~GroundRoverAttitudeControl()
{
if (_control_task != -1) {
/* task wakes up every 100ms or so at the longest */
_task_should_exit = true;
/* wait for a second for the task to quit at our request */
unsigned i = 0;
do {
/* wait 20ms */
usleep(20000);
/* if we have given up, kill it */
if (++i > 50) {
px4_task_delete(_control_task);
break;
}
} while (_control_task != -1);
}
perf_free(_loop_perf);
perf_free(_nonfinite_input_perf);
perf_free(_nonfinite_output_perf);
att_gnd_control::g_control = nullptr;
}
void
GroundRoverAttitudeControl::parameters_update()
{
param_get(_parameter_handles.w_p, &(_parameters.w_p));
param_get(_parameter_handles.w_i, &(_parameters.w_i));
param_get(_parameter_handles.w_d, &(_parameters.w_d));
param_get(_parameter_handles.w_imax, &(_parameters.w_imax));
param_get(_parameter_handles.trim_yaw, &(_parameters.trim_yaw));
param_get(_parameter_handles.man_yaw_scale, &(_parameters.man_yaw_scale));
param_get(_parameter_handles.bat_scale_en, &_parameters.bat_scale_en);
/* Steering pid parameters*/
pid_init(&_steering_ctrl, PID_MODE_DERIVATIV_SET, 0.01f);
pid_set_parameters(&_steering_ctrl,
_parameters.w_p,
_parameters.w_i,
_parameters.w_d,
_parameters.w_imax,
1.0f);
}
void
GroundRoverAttitudeControl::vehicle_control_mode_poll()
{
bool updated = false;
orb_check(_vcontrol_mode_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &_vcontrol_mode);
}
}
void
GroundRoverAttitudeControl::manual_control_setpoint_poll()
{
bool updated = false;
orb_check(_manual_sub, &updated);
if (updated) {
orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &_manual);
}
}
void
GroundRoverAttitudeControl::vehicle_attitude_setpoint_poll()
{
bool updated = false;
orb_check(_att_sp_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_attitude_setpoint), _att_sp_sub, &_att_sp);
}
}
void
GroundRoverAttitudeControl::battery_status_poll()
{
/* check if there is a new message */
bool updated;
orb_check(_battery_status_sub, &updated);
if (updated) {
orb_copy(ORB_ID(battery_status), _battery_status_sub, &_battery_status);
}
}
void
GroundRoverAttitudeControl::task_main_trampoline(int argc, char *argv[])
{
att_gnd_control::g_control->task_main();
}
void
GroundRoverAttitudeControl::task_main()
{
_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
_vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
_battery_status_sub = orb_subscribe(ORB_ID(battery_status));
parameters_update();
/* get an initial update for all sensor and status data */
vehicle_attitude_setpoint_poll();
vehicle_control_mode_poll();
manual_control_setpoint_poll();
battery_status_poll();
/* wakeup source */
px4_pollfd_struct_t fds[2];
/* Setup of loop */
fds[0].fd = _params_sub;
fds[0].events = POLLIN;
fds[1].fd = _ctrl_state_sub;
fds[1].events = POLLIN;
_task_running = true;
while (!_task_should_exit) {
static int loop_counter = 0;
/* wait for up to 500ms for data */
int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
/* timed out - periodic check for _task_should_exit, etc. */
if (pret == 0) {
continue;
}
/* this is undesirable but not much we can do - might want to flag unhappy status */
if (pret < 0) {
warn("poll error %d, %d", pret, errno);
continue;
}
perf_begin(_loop_perf);
/* only update parameters if they changed */
if (fds[0].revents & POLLIN) {
/* read from param to clear updated flag */
struct parameter_update_s update;
orb_copy(ORB_ID(parameter_update), _params_sub, &update);
/* update parameters from storage */
parameters_update();
}
/* only run controller if attitude changed */
if (fds[1].revents & POLLIN) {
static uint64_t last_run = 0;
float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
last_run = hrt_absolute_time();
/* guard against too large deltaT's */
if (deltaT > 1.0f ||
fabsf(deltaT) < 0.00001f ||
!PX4_ISFINITE(deltaT)) {
deltaT = 0.01f;
}
/* load local copies */
orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state);
vehicle_attitude_setpoint_poll();
vehicle_control_mode_poll();
manual_control_setpoint_poll();
battery_status_poll();
/* decide if in stabilized or full manual control */
if (_vcontrol_mode.flag_control_rates_enabled) {
/* Run attitude controllers */
if (_vcontrol_mode.flag_control_attitude_enabled) {
Eulerf euler_angles(matrix::Quatf(_ctrl_state.q));
/* Calculate the control output for the steering as yaw */
float yaw_u = pid_calculate(&_steering_ctrl, _att_sp.yaw_body, euler_angles.psi(), _ctrl_state.yaw_rate, deltaT);
float angle_diff = 0.0f;
if (_att_sp.yaw_body * euler_angles.psi() < 0.0f) {
if (_att_sp.yaw_body < 0.0f) {
angle_diff = euler_angles.psi() - _att_sp.yaw_body ;
} else {
angle_diff = _att_sp.yaw_body - euler_angles.psi();
}
// a switch might have happened
if ((double)angle_diff > M_PI) {
yaw_u = -yaw_u;
}
}
math::constrain(yaw_u, -1.0f, 1.0f);
if (PX4_ISFINITE(yaw_u)) {
_actuators.control[actuator_controls_s::INDEX_YAW] = yaw_u + _parameters.trim_yaw;
} else {
_actuators.control[actuator_controls_s::INDEX_YAW] = _parameters.trim_yaw;
perf_count(_nonfinite_output_perf);
if (_debug && loop_counter % 10 == 0) {
warnx("yaw_u %.4f", (double)yaw_u);
}
}
/* throttle passed through if it is finite and if no engine failure was detected */
_actuators.control[actuator_controls_s::INDEX_THROTTLE] = _att_sp.thrust;
/* scale effort by battery status */
if (_parameters.bat_scale_en && _battery_status.scale > 0.0f &&
_actuators.control[actuator_controls_s::INDEX_THROTTLE] > 0.1f) {
_actuators.control[actuator_controls_s::INDEX_THROTTLE] *= _battery_status.scale;
}
}
} else {
/* manual/direct control */
_actuators.control[actuator_controls_s::INDEX_ROLL] = _manual.y;
_actuators.control[actuator_controls_s::INDEX_PITCH] = -_manual.x;
_actuators.control[actuator_controls_s::INDEX_YAW] = _manual.r * _parameters.man_yaw_scale + _parameters.trim_yaw;
_actuators.control[actuator_controls_s::INDEX_THROTTLE] = _manual.z;
}
/* lazily publish the setpoint only once available */
_actuators.timestamp = hrt_absolute_time();
_actuators.timestamp_sample = _ctrl_state.timestamp;
/* Only publish if any of the proper modes are enabled */
if (_vcontrol_mode.flag_control_attitude_enabled ||
_vcontrol_mode.flag_control_manual_enabled) {
/* publish the actuator controls */
if (_actuators_0_pub != nullptr) {
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _actuators_0_pub, &_actuators);
} else {
_actuators_0_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &_actuators);
}
}
}
loop_counter++;
perf_end(_loop_perf);
}
warnx("exiting.\n");
_control_task = -1;
_task_running = false;
}
int
GroundRoverAttitudeControl::start()
{
ASSERT(_control_task == -1);
/* start the task */
_control_task = px4_task_spawn_cmd("gnd_att_control",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
1500,
(px4_main_t)&GroundRoverAttitudeControl::task_main_trampoline,
nullptr);
if (_control_task < 0) {
warn("task start failed");
return -errno;
}
return PX4_OK;
}
int gnd_att_control_main(int argc, char *argv[])
{
if (argc < 2) {
warnx("usage: gnd_att_control {start|stop|status}");
return 1;
}
if (!strcmp(argv[1], "start")) {
if (att_gnd_control::g_control != nullptr) {
warnx("already running");
return 1;
}
att_gnd_control::g_control = new GroundRoverAttitudeControl;
if (att_gnd_control::g_control == nullptr) {
warnx("alloc failed");
return 1;
}
if (PX4_OK != att_gnd_control::g_control->start()) {
delete att_gnd_control::g_control;
att_gnd_control::g_control = nullptr;
warn("start failed");
return 1;
}
/* check if the waiting is necessary at all */
if (att_gnd_control::g_control == nullptr || !att_gnd_control::g_control->task_running()) {
/* avoid memory fragmentation by not exiting start handler until the task has fully started */
while (att_gnd_control::g_control == nullptr || !att_gnd_control::g_control->task_running()) {
usleep(50000);
printf(".");
fflush(stdout);
}
printf("\n");
}
return 0;
}
if (!strcmp(argv[1], "stop")) {
if (att_gnd_control::g_control == nullptr) {
warnx("not running");
return 1;
}
delete att_gnd_control::g_control;
att_gnd_control::g_control = nullptr;
return 0;
}
if (!strcmp(argv[1], "status")) {
if (att_gnd_control::g_control) {
warnx("running");
return 0;
} else {
warnx("not running");
return 1;
}
}
warnx("unrecognized command");
return 1;
}
src/modules/gnd_att_control/GroundRoverAttitudeControl.hpp
+142
View File
@@ -0,0 +1,142 @@
/****************************************************************************
*
* Copyright (c) 2017 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.
*
****************************************************************************/
/**
*
* This module is a modification of the fixed wing module and it is designed for ground rovers.
* It has been developed starting from the fw module, simplified and improved with dedicated items.
*
* All the acknowledgments and credits for the fw wing app are reported in those files.
*
* @author Marco Zorzi <mzorzi@student.ethz.ch>
*/
#include <px4_config.h>
#include <px4_defines.h>
#include <px4_tasks.h>
#include <px4_posix.h>
#include <drivers/drv_hrt.h>
#include <mathlib/mathlib.h>
#include <systemlib/param/param.h>
#include <systemlib/pid/pid.h>
#include <systemlib/perf_counter.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/uORB.h>
using matrix::Eulerf;
using matrix::Quatf;
class GroundRoverAttitudeControl
{
public:
GroundRoverAttitudeControl();
~GroundRoverAttitudeControl();
int start();
bool task_running() { return _task_running; }
private:
bool _task_should_exit{false}; /**< if true, attitude control task should exit */
bool _task_running{false}; /**< if true, task is running in its mainloop */
int _control_task{-1}; /**< task handle */
int _att_sp_sub{-1}; /**< vehicle attitude setpoint */
int _battery_status_sub{-1}; /**< battery status subscription */
int _ctrl_state_sub{-1}; /**< control state subscription */
int _manual_sub{-1}; /**< notification of manual control updates */
int _params_sub{-1}; /**< notification of parameter updates */
int _vcontrol_mode_sub{-1}; /**< vehicle status subscription */
orb_advert_t _actuators_0_pub{nullptr}; /**< actuator control group 0 setpoint */
actuator_controls_s _actuators {}; /**< actuator control inputs */
battery_status_s _battery_status {}; /**< battery status */
control_state_s _ctrl_state {}; /**< control state */
manual_control_setpoint_s _manual {}; /**< r/c channel data */
vehicle_attitude_setpoint_s _att_sp {}; /**< vehicle attitude setpoint */
vehicle_control_mode_s _vcontrol_mode {}; /**< vehicle control mode */
perf_counter_t _loop_perf; /**< loop performance counter */
perf_counter_t _nonfinite_input_perf; /**< performance counter for non finite input */
perf_counter_t _nonfinite_output_perf; /**< performance counter for non finite output */
bool _debug{false}; /**< if set to true, print debug output */
struct {
float w_p; /**< Proportional gain of the steering controller */
float w_i; /**< Integral gain of the steering controller */
float w_d; /**< Derivative of the steering controller */
float w_imax; /**< maximum integrator level of the steering controller */
float trim_yaw;
float man_yaw_scale; /**< scale factor applied to yaw actuator control in pure manual mode */
int32_t bat_scale_en; /**< Battery scaling enabled */
} _parameters{}; /**< local copies of interesting parameters */
struct {
param_t w_p;
param_t w_i;
param_t w_d;
param_t w_imax;
param_t trim_yaw;
param_t man_yaw_scale;
param_t bat_scale_en;
} _parameter_handles{}; /**< handles for interesting parameters */
PID_t _steering_ctrl{};
void parameters_update();
void vehicle_control_mode_poll();
void manual_control_setpoint_poll();
void vehicle_attitude_setpoint_poll();
void battery_status_poll();
static void task_main_trampoline(int argc, char *argv[]);
void task_main();
};
src/modules/gnd_att_control/gnd_att_control_params.c
+195
View File
@@ -0,0 +1,195 @@
/****************************************************************************
*
* Copyright (c) 2013-2017 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 gnd_att_control_params.c
*
* Parameters defined by the attitude control task for ground rovers
*
* This is a modification of the fixed wing params and it is designed for ground rovers.
* It has been developed starting from the fw module, simplified and improved with dedicated items.
*
* All the ackowledgments and credits for the fw wing app are reported in those files.
*
* @author Marco Zorzi <mzorzi@student.ethz.ch>
*/
/*
* Controller parameters, accessible via MAVLink
*
*/
/**
* Attitude Wheel Time Constant
*
* This defines the latency between a steering step input and the achieved setpoint
* (inverse to a P gain). Half a second is a good start value and fits for
* most average systems. Smaller systems may require smaller values, but as
* this will wear out servos faster, the value should only be decreased as
* needed.
*
* @unit s
* @min 0.4
* @max 1.0
* @decimal 2
* @increment 0.05
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(GND_WR_TC, 0.4f);
/**
* Wheel steering rate proportional gain
*
* This defines how much the wheel steering input will be commanded depending on the
* current body angular rate error.
*
* @unit %/rad/s
* @min 0.005
* @max 1.0
* @decimal 3
* @increment 0.005
* @group GND Attitude Control
*/
PARAM_DEFINE_FLOAT(GND_WR_P, 1.0f);
/**
* Wheel steering rate integrator gain
*
* This gain defines how much control response will result out of a steady
* state error. It trims any constant error.
*
* @unit %/rad
* @min 0.00
* @max 0.5
* @decimal 3
* @increment 0.005
* @group GND Attitude Control
*/
PARAM_DEFINE_FLOAT(GND_WR_I, 0.00f);
/**
* Wheel steering rate integrator gain
*
*
* @unit %/rad
* @min 0.00
* @max 30
* @decimal 3
* @increment 0.005
* @group GND Attitude Control
*/
PARAM_DEFINE_FLOAT(GND_WR_D, 0.00f);
/**
* Wheel steering rate integrator limit
*
* The portion of the integrator part in the control surface deflection is
* limited to this value
*
* @min 0.0
* @max 1.0
* @decimal 2
* @increment 0.05
* @group GND Attitude Control
*/
PARAM_DEFINE_FLOAT(GND_WR_IMAX, 0.0f);
/**
* Maximum wheel steering rate
*
* This limits the maximum wheel steering rate the controller will output (in degrees per
* second). Setting a value of zero disables the limit.
*
* @unit deg/s
* @min 0.0
* @max 90.0
* @decimal 1
* @increment 0.5
* @group GND Attitude Control
*/
PARAM_DEFINE_FLOAT(GND_W_RMAX, 90.0f);
/**
* Wheel steering rate feed forward
*
* Direct feed forward from rate setpoint to control surface output
*
* @unit %/rad/s
* @min 0.0
* @max 10.0
* @decimal 2
* @increment 0.05
* @group GND Attitude Control
*/
PARAM_DEFINE_FLOAT(GND_WR_FF, 0.0f);
/**
* Manual yaw scale
*
* Scale factor applied to the desired yaw actuator command in full manual mode. This parameter allows
* to adjust the throws of the control surfaces.
*
* @unit norm
* @min 0.0
* @decimal 2
* @increment 0.01
* @group GND Attitude Control
*/
PARAM_DEFINE_FLOAT(GND_MAN_Y_SC, 1.0f);
/**
* Groundspeed speed trim
*
* This allows to scale the turning radius depending on the speed.
*
* @unit norm
* @min 0.0
* @decimal 2
* @increment 0.1
* @group GND Attitude Control
*/
PARAM_DEFINE_FLOAT(GND_GSPD_SP_TRIM, 1.0f);
/**
* Whether to scale throttle by battery power level
*
* This compensates for voltage drop of the battery over time by attempting to
* normalize performance across the operating range of the battery. The fixed wing
* should constantly behave as if it was fully charged with reduced max thrust
* at lower battery percentages. i.e. if cruise speed is at 0.5 throttle at 100% battery,
* it will still be 0.5 at 60% battery.
*
* @boolean
* @group GND Attitude Control
*/
PARAM_DEFINE_INT32(GND_BAT_SCALE_EN, 0);
src/modules/gnd_pos_control/CMakeLists.txt
+46
View File
@@ -0,0 +1,46 @@
############################################################################
#
# Copyright (c) 2017 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.
#
############################################################################
px4_add_module(
MODULE modules__gnd_pos_control
MAIN gnd_pos_control
STACK_MAIN 1200
COMPILE_FLAGS
SRCS
GroundRoverPositionControl.cpp
DEPENDS
platforms__common
git_ecl
lib__external_lgpl
lib__ecl
)
# vim: set noet ft=cmake fenc=utf-8 ff=unix :
src/modules/gnd_pos_control/GroundRoverPositionControl.cpp
+577
View File
File diff suppressed because it is too large Load Diff
src/modules/gnd_pos_control/GroundRoverPositionControl.hpp
+208
View File
@@ -0,0 +1,208 @@
/****************************************************************************
*
* Copyright (c) 2017 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.
*
****************************************************************************/
/**
*
* This module is a modification of the fixed wing module and it is designed for ground rovers.
* It has been developed starting from the fw module, simplified and improved with dedicated items.
*
* All the ackowledgments and credits for the fw wing app are reported in those files.
*
* @author Marco Zorzi <mzorzi@student.ethz.ch>
*/
#include <px4_config.h>
#include <px4_defines.h>
#include <px4_posix.h>
#include <px4_tasks.h>
#include <cfloat>
#include <drivers/drv_hrt.h>
#include <ecl/l1/ecl_l1_pos_controller.h>
#include <geo/geo.h>
#include <mathlib/mathlib.h>
#include <systemlib/perf_counter.h>
#include <systemlib/pid/pid.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/fw_pos_ctrl_status.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/uORB.h>
using matrix::Dcmf;
class GroundRoverPositionControl
{
public:
GroundRoverPositionControl();
~GroundRoverPositionControl();
GroundRoverPositionControl(const GroundRoverPositionControl &) = delete;
GroundRoverPositionControl operator=(const GroundRoverPositionControl &other) = delete;
/**
* Start the sensors task.
*
* @return OK on success.
*/
static int start();
/**
* Task status
*
* @return true if the mainloop is running
*/
bool task_running() { return _task_running; }
private:
orb_advert_t _attitude_sp_pub{nullptr}; /**< attitude setpoint */
orb_advert_t _gnd_pos_ctrl_status_pub{nullptr}; /**< navigation capabilities publication */
bool _task_should_exit{false}; /**< if true, sensor task should exit */
bool _task_running{false}; /**< if true, task is running in its mainloop */
int _control_mode_sub{-1}; /**< control mode subscription */
int _ctrl_state_sub{-1}; /**< control state subscription */
int _global_pos_sub{-1};
int _manual_control_sub{-1}; /**< notification of manual control updates */
int _params_sub{-1}; /**< notification of parameter updates */
int _pos_sp_triplet_sub{-1};
control_state_s _ctrl_state{}; /**< control state */
fw_pos_ctrl_status_s _gnd_pos_ctrl_status{}; /**< navigation capabilities */
manual_control_setpoint_s _manual{}; /**< r/c channel data */
position_setpoint_triplet_s _pos_sp_triplet{}; /**< triplet of mission items */
vehicle_attitude_setpoint_s _att_sp{}; /**< vehicle attitude setpoint */
vehicle_control_mode_s _control_mode{}; /**< control mode */
vehicle_global_position_s _global_pos{}; /**< global vehicle position */
perf_counter_t _loop_perf; /**< loop performance counter */
hrt_abstime _control_position_last_called{0}; /**<last call of control_position */
/* Pid controller for the speed. Here we assume we can control airspeed but the control variable is actually on
the throttle. For now just assuming a proportional scaler between controlled airspeed and throttle output.*/
PID_t _speed_ctrl{};
// estimator reset counters
uint8_t _pos_reset_counter{0}; // captures the number of times the estimator has reset the horizontal position
ECL_L1_Pos_Controller _gnd_control;
enum UGV_POSCTRL_MODE {
UGV_POSCTRL_MODE_AUTO,
UGV_POSCTRL_MODE_OTHER
} _control_mode_current{UGV_POSCTRL_MODE_OTHER}; ///< used to check the mode in the last control loop iteration. Use to check if the last iteration was in the same mode.
struct {
float l1_period;
float l1_damping;
float l1_distance;
float gndspeed_trim;
float gndspeed_max;
int32_t speed_control_mode;
float speed_p;
float speed_i;
float speed_d;
float speed_imax;
float throttle_speed_scaler;
float throttle_min;
float throttle_max;
float throttle_cruise;
float throttle_slew_max;
} _parameters{}; /**< local copies of interesting parameters */
struct {
param_t l1_period;
param_t l1_damping;
param_t l1_distance;
param_t gndspeed_trim;
param_t gndspeed_max;
param_t speed_control_mode;
param_t speed_p;
param_t speed_i;
param_t speed_d;
param_t speed_imax;
param_t throttle_speed_scaler;
param_t throttle_min;
param_t throttle_max;
param_t throttle_cruise;
param_t throttle_slew_max;
} _parameter_handles{}; /**< handles for interesting parameters */
/**
* Update our local parameter cache.
*/
int parameters_update();
void manual_control_setpoint_poll();
void control_state_poll();
void position_setpoint_triplet_poll();
void vehicle_control_mode_poll();
/**
* Publish navigation capabilities
*/
void gnd_pos_ctrl_status_publish();
/**
* Control position.
*/
bool control_position(const math::Vector<2> &global_pos, const math::Vector<3> &ground_speed,
const position_setpoint_triplet_s &_pos_sp_triplet);
/**
* Shim for calling task_main from task_create.
*/
static void task_main_trampoline(int argc, char *argv[]);
/**
* Main sensor collection task.
*/
void task_main();
};
src/modules/gnd_pos_control/gnd_pos_control_params.c
+262
View File
@@ -0,0 +1,262 @@
/****************************************************************************
*
* Copyright (c) 2013-2017 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 gnd_pos_control_params.c
*
* Parameters defined by the position control task for ground rovers
*
* This is a modification of the fixed wing params and it is designed for ground rovers.
* It has been developed starting from the fw module, simplified and improved with dedicated items.
*
* All the ackowledgments and credits for the fw wing app are reported in those files.
*
* @author Marco Zorzi <mzorzi@student.ethz.ch>
*/
/*
* Controller parameters, accessible via MAVLink
*/
/**
* L1 distance
*
* This is the waypoint radius
*
*
* @unit m
* @min 0.0
* @max 100.0
* @decimal 1
* @increment 0.1
* @group GND POS Control
*/
PARAM_DEFINE_FLOAT(GND_L1_DIST, 5.0f);
/**
* L1 period
*
* This is the L1 distance and defines the tracking
* point ahead of the rover it's following.
* Using values around 2-5 for a traxxas stampede. Shorten
* slowly during tuning until response is sharp without oscillation.
*
* @unit m
* @min 0.0
* @max 50.0
* @decimal 1
* @increment 0.5
* @group GND POS Control
*/
PARAM_DEFINE_FLOAT(GND_L1_PERIOD, 10.0f);
/**
* L1 damping
*
* Damping factor for L1 control.
*
* @min 0.6
* @max 0.9
* @decimal 2
* @increment 0.05
* @group GND POS Control
*/
PARAM_DEFINE_FLOAT(GND_L1_DAMPING, 0.75f);
/**
* Cruise throttle
*
* This is the throttle setting required to achieve the desired cruise speed.
* 10% is ok for a traxxas stampede vxl with ESC set to training mode
*
* @unit norm
* @min 0.0
* @max 1.0
* @decimal 2
* @increment 0.01
* @group GND POS Control
*/
PARAM_DEFINE_FLOAT(GND_THR_CRUISE, 0.1f);
/**
* Throttle limit max
*
* This is the maximum throttle % that can be used by the controller.
* For a Traxxas stampede vxl with the ESC set to training, 30 % is enough
*
* @unit norm
* @min 0.0
* @max 1.0
* @decimal 2
* @increment 0.01
* @group GND POS Control
*/
PARAM_DEFINE_FLOAT(GND_THR_MAX, 0.3f);
/**
* Throttle limit min
*
* This is the minimum throttle % that can be used by the controller.
* Set to 0 for rover
*
* @unit norm
* @min 0.0
* @max 1.0
* @decimal 2
* @increment 0.01
* @group GND POS Control
*/
PARAM_DEFINE_FLOAT(GND_THR_MIN, 0.0f);
/**
* Idle throttle
*
* This is the minimum throttle while on the ground, it should be 0 for a rover
*
*
* @unit norm
* @min 0.0
* @max 0.4
* @decimal 2
* @increment 0.01
* @group GND POS Control
*/
PARAM_DEFINE_FLOAT(GND_THR_IDLE, 0.0f);
/**
* Control mode for speed
*
* This allows the user to choose between closed loop gps speed or open loop cruise throttle speed
* @min 0
* @max 1
* @value 0 open loop control
* @value 1 close the loop with gps speed
* @group GND Attitude Control
*/
PARAM_DEFINE_INT32(GND_SP_CTRL_MODE, 1);
/**
* Speed proportional gain
*
* This is the proportional gain for the speed closed loop controller
*
* @unit %m/s
* @min 0.005
* @max 50.0
* @decimal 3
* @increment 0.005
* @group GND Attitude Control
*/
PARAM_DEFINE_FLOAT(GND_SPEED_P, 2.0f);
/**
* Speed Integral gain
*
* This is the integral gain for the speed closed loop controller
*
* @unit %m/s
* @min 0.00
* @max 50.0
* @decimal 3
* @increment 0.005
* @group GND Attitude Control
*/
PARAM_DEFINE_FLOAT(GND_SPEED_I, 0.1f);
/**
* Speed proportional gain
*
* This is the derivative gain for the speed closed loop controller
*
* @unit %m/s
* @min 0.00
* @max 50.0
* @decimal 3
* @increment 0.005
* @group GND Attitude Control
*/
PARAM_DEFINE_FLOAT(GND_SPEED_D, 0.0f);
/**
* Speed integral maximum value
*
* This is the maxim value the integral can reach to prevent wind-up.
*
* @unit %m/s
* @min 0.005
* @max 50.0
* @decimal 3
* @increment 0.005
* @group GND Attitude Control
*/
PARAM_DEFINE_FLOAT(GND_SPEED_IMAX, 1.0f);
/**
* Speed to throttle scaler
*
* This is a gain to map the speed control output to the throttle linearly.
*
* @unit %m/s
* @min 0.005
* @max 50.0
* @decimal 3
* @increment 0.005
* @group GND Attitude Control
*/
PARAM_DEFINE_FLOAT(GND_SPEED_THR_SC, 1.0f);
/**
* Trim ground speed
*
*
* @unit m/s
* @min 0.0
* @max 40
* @decimal 1
* @increment 0.5
* @group FW TECS
*/
PARAM_DEFINE_FLOAT(GND_SPEED_TRIM, 3.0f);
/**
* Maximum ground speed
*
*
* @unit m/s
* @min 0.0
* @max 40
* @decimal 1
* @increment 0.5
* @group FW TECS
*/
PARAM_DEFINE_FLOAT(GND_SPEED_MAX, 10.0f);
src/modules/land_detector/land_detector_main.cpp
+1 -1
View File
@@ -123,7 +123,7 @@ static int land_detector_start(const char *mode)
} else if (!strcmp(mode, "vtol")) {
land_detector_task = new VtolLandDetector();
} else if (!strcmp(mode, "rover")) {
} else if (!strcmp(mode, "ugv")) {
land_detector_task = new RoverLandDetector();
} else {