fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues 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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

61
ROMFS/px4fmu_common/init.d/50002_traxxas_stampede_2wd Normal file
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

2
ROMFS/px4fmu_common/init.d/rc.axialracing_ax10_defaults
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

24
ROMFS/px4fmu_common/init.d/rc.gnd_apps Normal file
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

32
ROMFS/px4fmu_common/init.d/rc.gnd_defaults Normal file
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

25
ROMFS/px4fmu_common/init.d/rcS
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
#

37
ROMFS/px4fmu_common/mixers/stampede.main.mix Normal file
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

53
ROMFS/sitl/mixers/rover_sitl.main.mix
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:

4
cmake/configs/nuttx_aerocore2_default.cmake
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

4
cmake/configs/nuttx_aerofc-v1_default.cmake
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

4
cmake/configs/nuttx_auav-x21_default.cmake
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

4
cmake/configs/nuttx_mindpx-v2_default.cmake
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

4
cmake/configs/nuttx_px4fmu-v2_default.cmake
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

2
cmake/configs/nuttx_px4fmu-v3_default.cmake
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

5
cmake/configs/nuttx_px4fmu-v4_default.cmake
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

2
cmake/configs/nuttx_px4fmu-v4pro_default.cmake
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

2
cmake/configs/nuttx_px4fmu-v5_default.cmake
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

6
cmake/configs/posix_rpi_common.cmake
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
#

2
cmake/configs/posix_sitl_default.cmake
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

73
posix-configs/SITL/init/ekf2/rover
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

79
posix-configs/SITL/init/lpe/rover Normal file
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

45
src/modules/gnd_att_control/CMakeLists.txt Normal file
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 :

440
src/modules/gnd_att_control/GroundRoverAttitudeControl.cpp Normal file
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;
}

142
src/modules/gnd_att_control/GroundRoverAttitudeControl.hpp Normal file
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();
};

195
src/modules/gnd_att_control/gnd_att_control_params.c Normal file
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);

46
src/modules/gnd_pos_control/CMakeLists.txt Normal file
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 :

577
src/modules/gnd_pos_control/GroundRoverPositionControl.cpp Normal file
View File

@ -0,0 +1,577 @@
/****************************************************************************
*
* 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 "GroundRoverPositionControl.hpp"
static int _control_task = -1; /**< task handle for sensor task */
using matrix::Eulerf;
using matrix::Quatf;
/**
* L1 control app start / stop handling function
*
* @ingroup apps
*/
extern "C" __EXPORT int gnd_pos_control_main(int argc, char *argv[]);
namespace gnd_control
{
GroundRoverPositionControl *g_control = nullptr;
}
GroundRoverPositionControl::GroundRoverPositionControl() :
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control"))
{
_parameter_handles.l1_period = param_find("GND_L1_PERIOD");
_parameter_handles.l1_damping = param_find("GND_L1_DAMPING");
_parameter_handles.l1_distance = param_find("GND_L1_DIST");
_parameter_handles.gndspeed_trim = param_find("GND_SPEED_TRIM");
_parameter_handles.gndspeed_max = param_find("GND_SPEED_MAX");
_parameter_handles.speed_control_mode = param_find("GND_SP_CTRL_MODE");
_parameter_handles.speed_p = param_find("GND_SPEED_P");
_parameter_handles.speed_i = param_find("GND_SPEED_I");
_parameter_handles.speed_d = param_find("GND_SPEED_D");
_parameter_handles.speed_imax = param_find("GND_SPEED_IMAX");
_parameter_handles.throttle_speed_scaler = param_find("GND_SPEED_THR_SC");
_parameter_handles.throttle_min = param_find("GND_THR_MIN");
_parameter_handles.throttle_max = param_find("GND_THR_MAX");
_parameter_handles.throttle_cruise = param_find("GND_THR_CRUISE");
/* fetch initial parameter values */
parameters_update();
}
GroundRoverPositionControl::~GroundRoverPositionControl()
{
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);
}
gnd_control::g_control = nullptr;
}
int
GroundRoverPositionControl::parameters_update()
{
/* L1 control parameters */
param_get(_parameter_handles.l1_damping, &(_parameters.l1_damping));
param_get(_parameter_handles.l1_period, &(_parameters.l1_period));
param_get(_parameter_handles.l1_distance, &(_parameters.l1_distance));
param_get(_parameter_handles.gndspeed_trim, &(_parameters.gndspeed_trim));
param_get(_parameter_handles.gndspeed_max, &(_parameters.gndspeed_max));
param_get(_parameter_handles.speed_control_mode, &(_parameters.speed_control_mode));
param_get(_parameter_handles.speed_p, &(_parameters.speed_p));
param_get(_parameter_handles.speed_i, &(_parameters.speed_i));
param_get(_parameter_handles.speed_d, &(_parameters.speed_d));
param_get(_parameter_handles.speed_imax, &(_parameters.speed_imax));
param_get(_parameter_handles.throttle_speed_scaler, &(_parameters.throttle_speed_scaler));
param_get(_parameter_handles.throttle_min, &(_parameters.throttle_min));
param_get(_parameter_handles.throttle_max, &(_parameters.throttle_max));
param_get(_parameter_handles.throttle_cruise, &(_parameters.throttle_cruise));
_gnd_control.set_l1_damping(_parameters.l1_damping);
_gnd_control.set_l1_period(_parameters.l1_period);
_gnd_control.set_l1_roll_limit(math::radians(0.0f));
pid_init(&_speed_ctrl, PID_MODE_DERIVATIV_CALC, 0.01f);
pid_set_parameters(&_speed_ctrl,
_parameters.speed_p,
_parameters.speed_d,
_parameters.speed_i,
_parameters.speed_imax,
_parameters.gndspeed_max);
/* Update and publish the navigation capabilities */
_gnd_pos_ctrl_status.landing_slope_angle_rad = 0;
_gnd_pos_ctrl_status.landing_horizontal_slope_displacement = 0;
_gnd_pos_ctrl_status.landing_flare_length = 0;
gnd_pos_ctrl_status_publish();
return OK;
}
void
GroundRoverPositionControl::vehicle_control_mode_poll()
{
bool updated;
orb_check(_control_mode_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);
}
}
void
GroundRoverPositionControl::manual_control_setpoint_poll()
{
bool manual_updated;
orb_check(_manual_control_sub, &manual_updated);
if (manual_updated) {
orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sub, &_manual);
}
}
void
GroundRoverPositionControl::control_state_poll()
{
bool ctrl_state_updated;
orb_check(_ctrl_state_sub, &ctrl_state_updated);
if (ctrl_state_updated) {
orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state);
}
}
void
GroundRoverPositionControl::position_setpoint_triplet_poll()
{
bool pos_sp_triplet_updated;
orb_check(_pos_sp_triplet_sub, &pos_sp_triplet_updated);
if (pos_sp_triplet_updated) {
orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
}
}
void GroundRoverPositionControl::gnd_pos_ctrl_status_publish()
{
_gnd_pos_ctrl_status.timestamp = hrt_absolute_time();
if (_gnd_pos_ctrl_status_pub != nullptr) {
orb_publish(ORB_ID(fw_pos_ctrl_status), _gnd_pos_ctrl_status_pub, &_gnd_pos_ctrl_status);
} else {
_gnd_pos_ctrl_status_pub = orb_advertise(ORB_ID(fw_pos_ctrl_status), &_gnd_pos_ctrl_status);
}
}
bool
GroundRoverPositionControl::control_position(const math::Vector<2> &current_position,
const math::Vector<3> &ground_speed, const position_setpoint_triplet_s &pos_sp_triplet)
{
float dt = 0.01; // Using non zero value to a avoid division by zero
if (_control_position_last_called > 0) {
dt = hrt_elapsed_time(&_control_position_last_called) * 1e-6f;
}
_control_position_last_called = hrt_absolute_time();
bool setpoint = true;
if (_control_mode.flag_control_auto_enabled && pos_sp_triplet.current.valid) {
/* AUTONOMOUS FLIGHT */
_control_mode_current = UGV_POSCTRL_MODE_AUTO;
/* get circle mode */
bool was_circle_mode = _gnd_control.circle_mode();
/* current waypoint (the one currently heading for) */
math::Vector<2> curr_wp((float)pos_sp_triplet.current.lat, (float)pos_sp_triplet.current.lon);
/* previous waypoint */
math::Vector<2> prev_wp = curr_wp;
if (pos_sp_triplet.previous.valid) {
prev_wp(0) = (float)pos_sp_triplet.previous.lat;
prev_wp(1) = (float)pos_sp_triplet.previous.lon;
}
math::Vector<2> ground_speed_2d = {ground_speed(0), ground_speed(1)};
float mission_throttle = _parameters.throttle_cruise;
/* Just control the throttle */
if (_parameters.speed_control_mode == 1) {
/* control the speed in closed loop */
float mission_target_speed = _parameters.gndspeed_trim;
if (PX4_ISFINITE(_pos_sp_triplet.current.cruising_speed) &&
_pos_sp_triplet.current.cruising_speed > 0.1f) {
mission_target_speed = _pos_sp_triplet.current.cruising_speed;
}
//Compute airspeed control out and just scale it as a constant
mission_throttle = _parameters.throttle_speed_scaler
* pid_calculate(&_speed_ctrl, mission_target_speed, _ctrl_state.x_vel, _ctrl_state.x_acc, dt);
// Constrain throttle between min and max
mission_throttle = math::constrain(mission_throttle, _parameters.throttle_min, _parameters.throttle_max);
} else {
/* Just control throttle in open loop */
if (PX4_ISFINITE(_pos_sp_triplet.current.cruising_throttle) &&
_pos_sp_triplet.current.cruising_throttle > 0.01f) {
mission_throttle = _pos_sp_triplet.current.cruising_throttle;
}
}
if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
_att_sp.roll_body = 0.0f;
_att_sp.pitch_body = 0.0f;
_att_sp.yaw_body = 0.0f;
_att_sp.thrust = 0.0f;
} else if ((pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION)
|| (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF)) {
/* waypoint is a plain navigation waypoint or the takeoff waypoint, does not matter */
_gnd_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d);
_att_sp.roll_body = _gnd_control.nav_roll();
_att_sp.pitch_body = 0.0f;
_att_sp.yaw_body = _gnd_control.nav_bearing();
_att_sp.fw_control_yaw = true;
_att_sp.thrust = mission_throttle;
} else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
/* waypoint is a loiter waypoint so we want to stop*/
_gnd_control.navigate_loiter(curr_wp, current_position, pos_sp_triplet.current.loiter_radius,
pos_sp_triplet.current.loiter_direction, ground_speed_2d);
_att_sp.roll_body = _gnd_control.nav_roll();
_att_sp.pitch_body = 0.0f;
_att_sp.yaw_body = _gnd_control.nav_bearing();
_att_sp.fw_control_yaw = true;
_att_sp.thrust = 0.0f;
}
if (was_circle_mode && !_gnd_control.circle_mode()) {
/* just kicked out of loiter, reset integrals */
_att_sp.yaw_reset_integral = true;
}
} else {
_control_mode_current = UGV_POSCTRL_MODE_OTHER;
_att_sp.roll_body = 0.0f;
_att_sp.pitch_body = 0.0f;
_att_sp.yaw_body = 0.0f;
_att_sp.fw_control_yaw = true;
_att_sp.thrust = 0.0f;
/* do not publish the setpoint */
setpoint = false;
}
return setpoint;
}
void
GroundRoverPositionControl::task_main()
{
_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
_manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
/* rate limit control mode updates to 5Hz */
orb_set_interval(_control_mode_sub, 200);
/* rate limit position updates to 50 Hz */
orb_set_interval(_global_pos_sub, 20);
/* abort on a nonzero return value from the parameter init */
if (parameters_update()) {
/* parameter setup went wrong, abort */
warnx("aborting startup due to errors.");
_task_should_exit = true;
}
/* wakeup source(s) */
px4_pollfd_struct_t fds[2];
/* Setup of loop */
fds[0].fd = _params_sub;
fds[0].events = POLLIN;
fds[1].fd = _global_pos_sub;
fds[1].events = POLLIN;
_task_running = true;
while (!_task_should_exit) {
/* 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;
}
/* check vehicle control mode for changes to publication state */
vehicle_control_mode_poll();
/* 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 position changed */
if (fds[1].revents & POLLIN) {
perf_begin(_loop_perf);
/* load local copies */
orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos);
// handle estimator reset events. we only adjust setpoins for manual modes
if (_control_mode.flag_control_manual_enabled) {
// adjust navigation waypoints in position control mode
if (_control_mode.flag_control_altitude_enabled && _control_mode.flag_control_velocity_enabled
&& _global_pos.lat_lon_reset_counter != _pos_reset_counter) {
}
}
// update the reset counters in any case
_pos_reset_counter = _global_pos.lat_lon_reset_counter;
control_state_poll();
manual_control_setpoint_poll();
position_setpoint_triplet_poll();
math::Vector<3> ground_speed(_global_pos.vel_n, _global_pos.vel_e, _global_pos.vel_d);
math::Vector<2> current_position((float)_global_pos.lat, (float)_global_pos.lon);
/*
* Attempt to control position, on success (= sensors present and not in manual mode),
* publish setpoint.
*/
if (control_position(current_position, ground_speed, _pos_sp_triplet)) {
_att_sp.timestamp = hrt_absolute_time();
Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body));
q.copyTo(_att_sp.q_d);
_att_sp.q_d_valid = true;
if (!_control_mode.flag_control_offboard_enabled ||
_control_mode.flag_control_position_enabled ||
_control_mode.flag_control_velocity_enabled ||
_control_mode.flag_control_acceleration_enabled) {
/* lazily publish the setpoint only once available */
if (_attitude_sp_pub != nullptr) {
/* publish the attitude setpoint */
orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &_att_sp);
} else {
/* advertise and publish */
_attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
}
}
/* XXX check if radius makes sense here */
float turn_distance = _parameters.l1_distance; //_gnd_control.switch_distance(100.0f);
/* lazily publish navigation capabilities */
if ((hrt_elapsed_time(&_gnd_pos_ctrl_status.timestamp) > 1000000)
|| (fabsf(turn_distance - _gnd_pos_ctrl_status.turn_distance) > FLT_EPSILON
&& turn_distance > 0)) {
/* set new turn distance */
_gnd_pos_ctrl_status.turn_distance = turn_distance;
_gnd_pos_ctrl_status.nav_roll = _gnd_control.nav_roll();
_gnd_pos_ctrl_status.nav_pitch = 0.0f;
_gnd_pos_ctrl_status.nav_bearing = _gnd_control.nav_bearing();
_gnd_pos_ctrl_status.target_bearing = _gnd_control.target_bearing();
_gnd_pos_ctrl_status.xtrack_error = _gnd_control.crosstrack_error();
math::Vector<2> curr_wp((float)_pos_sp_triplet.current.lat, (float)_pos_sp_triplet.current.lon);
_gnd_pos_ctrl_status.wp_dist = get_distance_to_next_waypoint(current_position(0), current_position(1), curr_wp(0),
curr_wp(1));
gnd_pos_ctrl_status_publish();
}
}
perf_end(_loop_perf);
}
}
_task_running = false;
warnx("exiting.\n");
_control_task = -1;
}
void
GroundRoverPositionControl::task_main_trampoline(int argc, char *argv[])
{
gnd_control::g_control = new GroundRoverPositionControl();
if (gnd_control::g_control == nullptr) {
warnx("OUT OF MEM");
return;
}
/* only returns on exit */
gnd_control::g_control->task_main();
delete gnd_control::g_control;
gnd_control::g_control = nullptr;
}
int
GroundRoverPositionControl::start()
{
ASSERT(_control_task == -1);
warn("Starting by marco");
/* start the task */
_control_task = px4_task_spawn_cmd("gnd_pos_ctrl",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
1700,
(px4_main_t)&GroundRoverPositionControl::task_main_trampoline,
nullptr);
warn("done");
if (_control_task < 0) {
warn("task start failed");
return -errno;
}
return OK;
}
int gnd_pos_control_main(int argc, char *argv[])
{
if (argc < 2) {
warnx("usage: gnd_pos_control {start|stop|status}");
return 1;
}
if (!strcmp(argv[1], "start")) {
if (gnd_control::g_control != nullptr) {
warnx("already running");
return 1;
}
if (OK != GroundRoverPositionControl::start()) {
warn("start failed");
return 1;
}
/* avoid memory fragmentation by not exiting start handler until the task has fully started */
while (gnd_control::g_control == nullptr || !gnd_control::g_control->task_running()) {
usleep(50000);
printf(".");
fflush(stdout);
}
printf("\n");
return 0;
}
if (!strcmp(argv[1], "stop")) {
if (gnd_control::g_control == nullptr) {
warnx("not running");
return 1;
}
delete gnd_control::g_control;
gnd_control::g_control = nullptr;
return 0;
}
if (!strcmp(argv[1], "status")) {
if (gnd_control::g_control) {
warnx("running");
return 0;
} else {
warnx("not running");
return 1;
}
}
warnx("unrecognized command");
return 1;
}

208
src/modules/gnd_pos_control/GroundRoverPositionControl.hpp Normal file
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();
};

262
src/modules/gnd_pos_control/gnd_pos_control_params.c Normal file
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);

2
src/modules/land_detector/land_detector_main.cpp
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 {