dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Rover: Rewrote gnd_pos_control and removed gnd_att_control (#12239)

This commit is contained in:
Timothy Scott 2019-07-11 15:39:13 +02:00 committed by Daniel Agar
parent 3743e6d8fb
commit 2ed00c9cb6
55 changed files with 297 additions and 1068 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
ROMFS/px4fmu_common/init.d-posix/1060_rover
View File

@ -19,10 +19,6 @@ then
param set GND_THR_IDLE 0
param set GND_THR_MAX 0.5
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 MIS_LTRMIN_ALT 0.01
param set MIS_TAKEOFF_ALT 0.01
@ -30,6 +26,10 @@ then
param set NAV_LOITER_RAD 2
param set CBRK_AIRSPD_CHK 162128
param set GND_MAX_ANG 0.6
param set GND_WHEEL_BASE 2.0
fi
set MAV_TYPE 10

20
ROMFS/px4fmu_common/init.d/airframes/50003_aion_robotics_r1_rover
View File

@ -15,7 +15,7 @@
# @board px4_fmu-v2 exclude
#
sh /etc/init.d/rc.ugv_defaults
sh /etc/init.d/rc.rover_defaults
if [ $AUTOCNF = yes ]
then
@ -29,15 +29,19 @@ then
param set FW_AIRSPD_TRIM 1
param set FW_AIRSPD_MAX 3
param set GND_WR_P 1
param set GND_WR_I 0.9674
param set GND_WR_IMAX 0.1
param set GND_WR_D 0.1
param set GND_SP_CTRL_MODE 1
param set GND_L1_DIST 10
param set GND_L1_DIST 5.0
param set GND_L1_PERIOD 3.0
param set GND_THR_IDLE 0
param set GND_THR_CRUISE 0
param set GND_THR_CRUISE 0.7
param set GND_THR_MAX 0.5
# Because this is differential drive, it can make a turn with radius 0.
# This corresponds to a turn angle of pi radians.
# If a special case is made for differential-drive, this will need to change.
param set GND_MAX_ANG 3.142
param set GND_WHEEL_BASE 0.3
# TODO: Set to -1.0, to allow reversing. This will require many changes in the codebase
# to support negative throttle.
param set GND_THR_MIN 0.0
@ -64,7 +68,7 @@ then
param set CBRK_AIRSPD_CHK 162128
fi
# Configure this as ugv
# Configure this as rover
set MAV_TYPE 10
# Set mixer

3
ROMFS/px4fmu_common/init.d/rc.rover_apps
View File

@ -16,8 +16,7 @@ ekf2 start
#
# Start attitude controllers.
#
gnd_att_control start
gnd_pos_control start
rover_pos_control start
#

3
boards/aerotenna/ocpoc/ubuntu.cmake
View File

@ -37,8 +37,7 @@ px4_add_board(
events
fw_att_control
fw_pos_control_l1
gnd_att_control
gnd_pos_control
rover_pos_control
land_detector
landing_target_estimator
load_mon

3
boards/airmind/mindpx-v2/default.cmake
View File

@ -58,8 +58,7 @@ px4_add_board(
events
fw_att_control
fw_pos_control_l1
gnd_att_control
gnd_pos_control
rover_pos_control
land_detector
landing_target_estimator
load_mon

3
boards/atlflight/eagle/default.cmake
View File

@ -72,8 +72,7 @@ px4_add_board(
events
fw_att_control
fw_pos_control_l1
gnd_att_control
gnd_pos_control
rover_pos_control
land_detector
landing_target_estimator
load_mon

3
boards/atlflight/eagle/qurt-default.cmake
View File

@ -66,8 +66,7 @@ px4_add_board(
ekf2
fw_att_control
fw_pos_control_l1
gnd_att_control
gnd_pos_control
rover_pos_control
land_detector
landing_target_estimator
local_position_estimator

3
boards/atlflight/excelsior/default.cmake
View File

@ -72,8 +72,7 @@ px4_add_board(
events
fw_att_control
fw_pos_control_l1
gnd_att_control
gnd_pos_control
rover_pos_control
land_detector
landing_target_estimator
load_mon

3
boards/atlflight/excelsior/qurt-default.cmake
View File

@ -66,8 +66,7 @@ px4_add_board(
ekf2
fw_att_control
fw_pos_control_l1
gnd_att_control
gnd_pos_control
rover_pos_control
land_detector
landing_target_estimator
local_position_estimator

3
boards/auav/x21/default.cmake
View File

@ -64,8 +64,7 @@ px4_add_board(
events
fw_att_control
fw_pos_control_l1
gnd_att_control
gnd_pos_control
rover_pos_control
land_detector
landing_target_estimator
load_mon

3
boards/av/x-v1/default.cmake
View File

@ -65,8 +65,7 @@ px4_add_board(
events
fw_att_control
fw_pos_control_l1
gnd_att_control
gnd_pos_control
rover_pos_control
land_detector
landing_target_estimator
load_mon

3
boards/beaglebone/blue/cross.cmake
View File

@ -36,8 +36,7 @@ px4_add_board(
events
fw_att_control
fw_pos_control_l1
gnd_att_control
gnd_pos_control
rover_pos_control
land_detector
landing_target_estimator
load_mon

3
boards/beaglebone/blue/native.cmake
View File

@ -34,8 +34,7 @@ px4_add_board(
events
fw_att_control
fw_pos_control_l1
gnd_att_control
gnd_pos_control
rover_pos_control
land_detector
landing_target_estimator
load_mon

3
boards/bitcraze/crazyflie/default.cmake
View File

@ -25,8 +25,7 @@ px4_add_board(
events
#fw_att_control
#fw_pos_control_l1
#gnd_att_control
#gnd_pos_control
#rover_pos_control
land_detector
landing_target_estimator
load_mon

3
boards/emlid/navio2/cross.cmake
View File

@ -40,8 +40,7 @@ px4_add_board(
events
fw_att_control
fw_pos_control_l1
gnd_att_control
gnd_pos_control
rover_pos_control
land_detector
landing_target_estimator
load_mon

3
boards/emlid/navio2/native.cmake
View File

@ -38,8 +38,7 @@ px4_add_board(
events
fw_att_control
fw_pos_control_l1
gnd_att_control
gnd_pos_control
rover_pos_control
land_detector
landing_target_estimator
load_mon

3
boards/intel/aerofc-v1/default.cmake
View File

@ -41,8 +41,7 @@ px4_add_board(
events
#fw_att_control
#fw_pos_control_l1
#gnd_att_control
#gnd_pos_control
#rover_pos_control
land_detector
landing_target_estimator
load_mon

3
boards/intel/aerofc-v1/rtps.cmake
View File

@ -44,8 +44,7 @@ px4_add_board(
events
#fw_att_control
#fw_pos_control_l1
#gnd_att_control
#gnd_pos_control
#rover_pos_control
land_detector
landing_target_estimator
load_mon

3
boards/nxp/fmuk66-v3/default.cmake
View File

@ -60,8 +60,7 @@ px4_add_board(
events
fw_att_control
fw_pos_control_l1
gnd_att_control
gnd_pos_control
rover_pos_control
land_detector
landing_target_estimator
load_mon

3
boards/omnibus/f4sd/default.cmake
View File

@ -53,8 +53,7 @@ px4_add_board(
events
#fw_att_control
#fw_pos_control_l1
#gnd_att_control
#gnd_pos_control
#rover_pos_control
land_detector
landing_target_estimator
load_mon

3
boards/parrot/bebop/default.cmake
View File

@ -28,8 +28,7 @@ px4_add_board(
events
#fw_att_control
#fw_pos_control_l1
#gnd_att_control
#gnd_pos_control
#rover_pos_control
land_detector
landing_target_estimator
load_mon

3
boards/px4/fmu-v2/default.cmake
View File

@ -73,8 +73,7 @@ px4_add_board(
events
fw_att_control
fw_pos_control_l1
#gnd_att_control
#gnd_pos_control
#rover_pos_control
land_detector
#landing_target_estimator
load_mon

3
boards/px4/fmu-v2/lpe.cmake
View File

@ -69,8 +69,7 @@ px4_add_board(
events
#fw_att_control
#fw_pos_control_l1
#gnd_att_control
#gnd_pos_control
#rover_pos_control
land_detector
landing_target_estimator
load_mon

3
boards/px4/fmu-v2/rover.cmake
View File

@ -42,8 +42,7 @@ px4_add_board(
dataman
ekf2
events
gnd_att_control
gnd_pos_control
rover_pos_control
land_detector
load_mon
logger

3
boards/px4/fmu-v2/test.cmake
View File

@ -69,8 +69,7 @@ px4_add_board(
events
#fw_att_control
#fw_pos_control_l1
#gnd_att_control
#gnd_pos_control
#rover_pos_control
land_detector
landing_target_estimator
load_mon

3
boards/px4/fmu-v3/default.cmake
View File

@ -72,8 +72,7 @@ px4_add_board(
events
fw_att_control
fw_pos_control_l1
gnd_att_control
gnd_pos_control
rover_pos_control
land_detector
landing_target_estimator
load_mon

3
boards/px4/fmu-v3/rtps.cmake
View File

@ -71,8 +71,7 @@ px4_add_board(
events
fw_att_control
fw_pos_control_l1
gnd_att_control
gnd_pos_control
rover_pos_control
land_detector
landing_target_estimator
load_mon

3
boards/px4/fmu-v3/stackcheck.cmake
View File

@ -71,8 +71,7 @@ px4_add_board(
events
fw_att_control
fw_pos_control_l1
gnd_att_control
gnd_pos_control
rover_pos_control
land_detector
landing_target_estimator
load_mon

3
boards/px4/fmu-v4/default.cmake
View File

@ -56,8 +56,7 @@ px4_add_board(
events
fw_att_control
fw_pos_control_l1
gnd_att_control
gnd_pos_control
rover_pos_control
land_detector
landing_target_estimator
load_mon

3
boards/px4/fmu-v4/rtps.cmake
View File

@ -58,8 +58,7 @@ px4_add_board(
events
fw_att_control
fw_pos_control_l1
gnd_att_control
gnd_pos_control
rover_pos_control
land_detector
landing_target_estimator
load_mon

3
boards/px4/fmu-v4/stackcheck.cmake
View File

@ -56,8 +56,7 @@ px4_add_board(
events
fw_att_control
fw_pos_control_l1
gnd_att_control
gnd_pos_control
rover_pos_control
land_detector
landing_target_estimator
load_mon

3
boards/px4/fmu-v4pro/default.cmake
View File

@ -70,8 +70,7 @@ px4_add_board(
events
fw_att_control
fw_pos_control_l1
gnd_att_control
gnd_pos_control
rover_pos_control
land_detector
landing_target_estimator
load_mon

3
boards/px4/fmu-v4pro/rtps.cmake
View File

@ -69,8 +69,7 @@ px4_add_board(
events
fw_att_control
fw_pos_control_l1
gnd_att_control
gnd_pos_control
rover_pos_control
land_detector
landing_target_estimator
load_mon

3
boards/px4/fmu-v5/default.cmake
View File

@ -70,8 +70,7 @@ px4_add_board(
events
fw_att_control
fw_pos_control_l1
gnd_att_control
gnd_pos_control
rover_pos_control
land_detector
landing_target_estimator
load_mon

3
boards/px4/fmu-v5/rover.cmake
View File

@ -53,8 +53,7 @@ px4_add_board(
dataman
ekf2
events
gnd_att_control
gnd_pos_control
rover_pos_control
land_detector
load_mon
logger

3
boards/px4/fmu-v5/rtps.cmake
View File

@ -68,8 +68,7 @@ px4_add_board(
events
fw_att_control
fw_pos_control_l1
gnd_att_control
gnd_pos_control
rover_pos_control
land_detector
landing_target_estimator
load_mon

3
boards/px4/fmu-v5/stackcheck.cmake
View File

@ -68,8 +68,7 @@ px4_add_board(
events
fw_att_control
fw_pos_control_l1
gnd_att_control
gnd_pos_control
rover_pos_control
land_detector
landing_target_estimator
load_mon

3
boards/px4/fmu-v5x/default.cmake
View File

@ -70,8 +70,6 @@ px4_add_board(
events
fw_att_control
fw_pos_control_l1
gnd_att_control
gnd_pos_control
land_detector
landing_target_estimator
load_mon
@ -81,6 +79,7 @@ px4_add_board(
mc_att_control
mc_pos_control
navigator
rover_pos_control
sensors
sih
vmount

3
boards/px4/fmu-v5x/rover.cmake
View File

@ -63,13 +63,12 @@ px4_add_board(
dataman
ekf2
events
gnd_att_control
gnd_pos_control
land_detector
load_mon
logger
mavlink
navigator
rover_pos_control
sensors
vmount

3
boards/px4/fmu-v5x/rtps.cmake
View File

@ -70,8 +70,6 @@ px4_add_board(
events
fw_att_control
fw_pos_control_l1
gnd_att_control
gnd_pos_control
land_detector
landing_target_estimator
load_mon
@ -82,6 +80,7 @@ px4_add_board(
mc_pos_control
micrortps_bridge
navigator
rover_pos_control
sensors
sih
vmount

3
boards/px4/fmu-v5x/stackcheck.cmake
View File

@ -70,8 +70,6 @@ px4_add_board(
events
fw_att_control
fw_pos_control_l1
gnd_att_control
gnd_pos_control
land_detector
landing_target_estimator
load_mon
@ -81,6 +79,7 @@ px4_add_board(
mc_att_control
mc_pos_control
navigator
rover_pos_control
sensors
sih
vmount

3
boards/px4/raspberrypi/cross.cmake
View File

@ -33,8 +33,7 @@ px4_add_board(
events
fw_att_control
fw_pos_control_l1
gnd_att_control
gnd_pos_control
rover_pos_control
land_detector
landing_target_estimator
load_mon

3
boards/px4/raspberrypi/native.cmake
View File

@ -31,8 +31,7 @@ px4_add_board(
events
fw_att_control
fw_pos_control_l1
gnd_att_control
gnd_pos_control
rover_pos_control
land_detector
landing_target_estimator
load_mon

3
boards/px4/sitl/default.cmake
View File

@ -30,8 +30,7 @@ px4_add_board(
events
fw_att_control
fw_pos_control_l1
gnd_att_control
gnd_pos_control
rover_pos_control
land_detector
landing_target_estimator
load_mon

3
boards/px4/sitl/rtps.cmake
View File

@ -30,8 +30,7 @@ px4_add_board(
events
fw_att_control
fw_pos_control_l1
gnd_att_control
gnd_pos_control
rover_pos_control
land_detector
landing_target_estimator
load_mon

3
boards/px4/sitl/test.cmake
View File

@ -30,8 +30,7 @@ px4_add_board(
events
fw_att_control
fw_pos_control_l1
gnd_att_control
gnd_pos_control
rover_pos_control
land_detector
landing_target_estimator
load_mon

40
src/modules/gnd_att_control/CMakeLists.txt
View File

@ -1,40 +0,0 @@
############################################################################
#
# 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
)

439
src/modules/gnd_att_control/GroundRoverAttitudeControl.cpp
View File

@ -1,439 +0,0 @@
/****************************************************************************
*
* 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 */
px4_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);
}
}
int
GroundRoverAttitudeControl::task_main_trampoline(int argc, char *argv[])
{
att_gnd_control::g_control->task_main();
return 0;
}
void
GroundRoverAttitudeControl::task_main()
{
_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
_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 = _att_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) {
PX4_ERR("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(vehicle_attitude), _att_sub, &_att);
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(_att.q));
/* Calculate the control output for the steering as yaw */
float yaw_u = pid_calculate(&_steering_ctrl, _att_sp.yaw_body, euler_angles.psi(), _att.yawspeed, 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) {
PX4_INFO("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_body[0];
/* 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 = _att.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);
}
PX4_INFO("exiting.");
_control_task = -1;
_task_running = false;
}
int
GroundRoverAttitudeControl::start()
{
/* 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) {
PX4_ERR("task start failed");
return -errno;
}
return PX4_OK;
}
int gnd_att_control_main(int argc, char *argv[])
{
if (argc < 2) {
PX4_INFO("usage: gnd_att_control {start|stop|status}");
return 1;
}
if (!strcmp(argv[1], "start")) {
if (att_gnd_control::g_control != nullptr) {
PX4_WARN("already running");
return 1;
}
att_gnd_control::g_control = new GroundRoverAttitudeControl;
if (att_gnd_control::g_control == nullptr) {
PX4_ERR("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;
PX4_ERR("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()) {
px4_usleep(50000);
printf(".");
fflush(stdout);
}
printf("\n");
}
return 0;
}
if (!strcmp(argv[1], "stop")) {
if (att_gnd_control::g_control == nullptr) {
PX4_WARN("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) {
PX4_INFO("running");
return 0;
} else {
PX4_INFO("not running");
return 1;
}
}
PX4_WARN("unrecognized command");
return 1;
}

143
src/modules/gnd_att_control/GroundRoverAttitudeControl.hpp
View File

@ -1,143 +0,0 @@
/****************************************************************************
*
* 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 <matrix/math.hpp>
#include <parameters/param.h>
#include <pid/pid.h>
#include <perf/perf_counter.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_attitude.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 _att_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 */
manual_control_setpoint_s _manual {}; /**< r/c channel data */
vehicle_attitude_s _att {}; /**< control state */
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 int task_main_trampoline(int argc, char *argv[]);
void task_main();
};

195
src/modules/gnd_att_control/gnd_att_control_params.c
View File

@ -1,195 +0,0 @@
/****************************************************************************
*
* 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 GND 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);

3
src/modules/navigator/navigator_main.cpp
View File

@ -753,6 +753,9 @@ Navigator::get_default_altitude_acceptance_radius()
if (get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
return _param_nav_fw_alt_rad.get();
} else if (get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROVER) {
return INFINITY;
} else {
float alt_acceptance_radius = _param_nav_mc_alt_rad.get();

6
src/modules/gnd_pos_control/CMakeLists.txt → src/modules/rover_pos_control/CMakeLists.txt
View File

@ -31,10 +31,10 @@
#
############################################################################
px4_add_module(
MODULE modules__gnd_pos_control
MAIN gnd_pos_control
MODULE modules__rover_pos_control
MAIN rover_pos_control
SRCS
GroundRoverPositionControl.cpp
RoverPositionControl.cpp
DEPENDS
git_ecl
ecl_l1

290
src/modules/gnd_pos_control/GroundRoverPositionControl.cpp → src/modules/rover_pos_control/RoverPositionControl.cpp
View File

@ -42,7 +42,10 @@
*/
#include "GroundRoverPositionControl.hpp"
#include "RoverPositionControl.hpp"
#include <lib/ecl/geo/geo.h>
#define ACTUATOR_PUBLISH_PERIOD_MS 4
static int _control_task = -1; /**< task handle for sensor task */
@ -55,17 +58,16 @@ using matrix::Vector3f;
*
* @ingroup apps
*/
extern "C" __EXPORT int gnd_pos_control_main(int argc, char *argv[]);
extern "C" __EXPORT int rover_pos_control_main(int argc, char *argv[]);
namespace gnd_control
{
GroundRoverPositionControl *g_control = nullptr;
RoverPositionControl *g_control = nullptr;
}
GroundRoverPositionControl::GroundRoverPositionControl() :
RoverPositionControl::RoverPositionControl() :
/* performance counters */
_sub_attitude(ORB_ID(vehicle_attitude)),
_sub_sensors(ORB_ID(sensor_bias)),
_loop_perf(perf_alloc(PC_ELAPSED, "rover position control")) // TODO : do we even need these perf counters
{
@ -87,11 +89,14 @@ GroundRoverPositionControl::GroundRoverPositionControl() :
_parameter_handles.throttle_max = param_find("GND_THR_MAX");
_parameter_handles.throttle_cruise = param_find("GND_THR_CRUISE");
_parameter_handles.wheel_base = param_find("GND_WHEEL_BASE");
_parameter_handles.max_turn_angle = param_find("GND_MAX_ANG");
/* fetch initial parameter values */
parameters_update();
}
GroundRoverPositionControl::~GroundRoverPositionControl()
RoverPositionControl::~RoverPositionControl()
{
if (_control_task != -1) {
@ -117,7 +122,7 @@ GroundRoverPositionControl::~GroundRoverPositionControl()
}
int
GroundRoverPositionControl::parameters_update()
RoverPositionControl::parameters_update()
{
/* L1 control parameters */
param_get(_parameter_handles.l1_damping, &(_parameters.l1_damping));
@ -138,6 +143,9 @@ GroundRoverPositionControl::parameters_update()
param_get(_parameter_handles.throttle_max, &(_parameters.throttle_max));
param_get(_parameter_handles.throttle_cruise, &(_parameters.throttle_cruise));
param_get(_parameter_handles.wheel_base, &(_parameters.wheel_base));
param_get(_parameter_handles.max_turn_angle, &(_parameters.max_turn_angle));
_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));
@ -154,7 +162,7 @@ GroundRoverPositionControl::parameters_update()
}
void
GroundRoverPositionControl::vehicle_control_mode_poll()
RoverPositionControl::vehicle_control_mode_poll()
{
bool updated;
orb_check(_control_mode_sub, &updated);
@ -165,7 +173,7 @@ GroundRoverPositionControl::vehicle_control_mode_poll()
}
void
GroundRoverPositionControl::manual_control_setpoint_poll()
RoverPositionControl::manual_control_setpoint_poll()
{
bool manual_updated;
orb_check(_manual_control_sub, &manual_updated);
@ -176,7 +184,7 @@ GroundRoverPositionControl::manual_control_setpoint_poll()
}
void
GroundRoverPositionControl::position_setpoint_triplet_poll()
RoverPositionControl::position_setpoint_triplet_poll()
{
bool pos_sp_triplet_updated;
orb_check(_pos_sp_triplet_sub, &pos_sp_triplet_updated);
@ -186,9 +194,20 @@ GroundRoverPositionControl::position_setpoint_triplet_poll()
}
}
void
RoverPositionControl::vehicle_attitude_poll()
{
bool att_updated;
orb_check(_vehicle_attitude_sub, &att_updated);
if (att_updated) {
orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &_vehicle_att);
}
}
bool
GroundRoverPositionControl::control_position(const matrix::Vector2f &current_position,
const matrix::Vector3f &ground_speed, const position_setpoint_triplet_s &pos_sp_triplet)
RoverPositionControl::control_position(const matrix::Vector2f &current_position,
const matrix::Vector3f &ground_speed, const position_setpoint_triplet_s &pos_sp_triplet)
{
float dt = 0.01; // Using non zero value to a avoid division by zero
@ -206,7 +225,7 @@ GroundRoverPositionControl::control_position(const matrix::Vector2f &current_pos
_control_mode_current = UGV_POSCTRL_MODE_AUTO;
/* get circle mode */
bool was_circle_mode = _gnd_control.circle_mode();
//bool was_circle_mode = _gnd_control.circle_mode();
/* current waypoint (the one currently heading for) */
matrix::Vector2f curr_wp((float)pos_sp_triplet.current.lat, (float)pos_sp_triplet.current.lon);
@ -235,7 +254,7 @@ GroundRoverPositionControl::control_position(const matrix::Vector2f &current_pos
}
// Velocity in body frame
const Dcmf R_to_body(Quatf(_sub_attitude.get().q).inversed());
const Dcmf R_to_body(Quatf(_vehicle_att.q).inversed());
const Vector3f vel = R_to_body * Vector3f(ground_speed(0), ground_speed(1), ground_speed(2));
const float x_vel = vel(0);
@ -257,51 +276,54 @@ GroundRoverPositionControl::control_position(const matrix::Vector2f &current_pos
}
}
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_body[0] = 0.0f;
float dist = get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon,
pos_sp_triplet.current.lat, pos_sp_triplet.current.lon);
} else if ((pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION)
|| (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF)) {
bool should_idle = true;
if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
// Because of noise in measurements, if the rover was always trying to reach an exact point, it would
// move around when it should be parked. So, I try to get the rover within loiter_radius/2, but then
// once I reach that point, I don't move until I'm outside of loiter_radius.
// TODO: Find out if there's a better measurement to use than loiter_radius.
if (dist > pos_sp_triplet.current.loiter_radius) {
_waypoint_reached = false;
} else if (dist <= pos_sp_triplet.current.loiter_radius / 2) {
_waypoint_reached = true;
}
should_idle = _waypoint_reached;
} else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION ||
pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF ||
pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
should_idle = false;
}
if (should_idle) {
_act_controls.control[actuator_controls_s::INDEX_YAW] = 0.0f;
_act_controls.control[actuator_controls_s::INDEX_THROTTLE] = 0.0f;
} else {
/* 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.get_roll_setpoint();
_att_sp.pitch_body = 0.0f;
_att_sp.yaw_body = _gnd_control.nav_bearing();
_att_sp.fw_control_yaw = true;
_att_sp.thrust_body[0] = mission_throttle;
} else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
_act_controls.control[actuator_controls_s::INDEX_THROTTLE] = mission_throttle;
/* 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);
float desired_r = ground_speed_2d.norm_squared() / math::abs_t(_gnd_control.nav_lateral_acceleration_demand());
float desired_theta = (0.5f * M_PI_F) - atan2f(desired_r, _parameters.wheel_base);
float control_effort = (desired_theta / _parameters.max_turn_angle) * math::sign(
_gnd_control.nav_lateral_acceleration_demand());
control_effort = math::constrain(control_effort, -1.0f, 1.0f);
_act_controls.control[actuator_controls_s::INDEX_YAW] = control_effort;
_att_sp.roll_body = _gnd_control.get_roll_setpoint();
_att_sp.pitch_body = 0.0f;
_att_sp.yaw_body = _gnd_control.nav_bearing();
_att_sp.fw_control_yaw = true;
_att_sp.thrust_body[0] = 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_body[0] = 0.0f;
/* do not publish the setpoint */
setpoint = false;
}
@ -309,13 +331,14 @@ GroundRoverPositionControl::control_position(const matrix::Vector2f &current_pos
}
void
GroundRoverPositionControl::task_main()
RoverPositionControl::task_main()
{
_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_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));
_vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
/* rate limit control mode updates to 5Hz */
orb_set_interval(_control_mode_sub, 200);
@ -331,25 +354,33 @@ GroundRoverPositionControl::task_main()
}
/* wakeup source(s) */
px4_pollfd_struct_t fds[2];
px4_pollfd_struct_t fds[3];
/* Setup of loop */
fds[0].fd = _params_sub;
fds[0].events = POLLIN;
fds[1].fd = _global_pos_sub;
fds[1].events = POLLIN;
fds[2].fd = _manual_control_sub;
fds[2].events = POLLIN;
_task_running = true;
// Absolute time (in us) at which the actuators were last published
long actuators_last_published = 0;
// Timeout for poll in ms
int timeout = 0;
while (!_task_should_exit) {
/* wait for up to 500ms for data */
int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
// The +500 is to round to the nearest millisecond, instead of to the smallest millisecond.
timeout = ACTUATOR_PUBLISH_PERIOD_MS - (hrt_absolute_time() - actuators_last_published) / 1000 - 1;
timeout = timeout > 0 ? timeout : 0;
/* timed out - periodic check for _task_should_exit, etc. */
if (pret == 0) {
continue;
}
//PX4_INFO("TIMEOUT: %d", timeout);
/* wait for up to 500ms for data */
int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), timeout);
/* this is undesirable but not much we can do - might want to flag unhappy status */
if (pret < 0) {
@ -359,11 +390,16 @@ GroundRoverPositionControl::task_main()
/* check vehicle control mode for changes to publication state */
vehicle_control_mode_poll();
//manual_control_setpoint_poll();
_sub_sensors.update();
bool manual_mode = _control_mode.flag_control_manual_enabled;
/* only update parameters if they changed */
if (fds[0].revents & POLLIN) {
/* read from param to clear updated flag */
struct parameter_update_s update;
parameter_update_s update;
orb_copy(ORB_ID(parameter_update), _params_sub, &update);
/* update parameters from storage */
@ -377,86 +413,96 @@ GroundRoverPositionControl::task_main()
/* 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) {
}
}
position_setpoint_triplet_poll();
vehicle_attitude_poll();
// update the reset counters in any case
_pos_reset_counter = _global_pos.lat_lon_reset_counter;
manual_control_setpoint_poll();
position_setpoint_triplet_poll();
_sub_attitude.update();
_sub_sensors.update();
matrix::Vector3f ground_speed(_global_pos.vel_n, _global_pos.vel_e, _global_pos.vel_d);
matrix::Vector2f 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();
// This if statement depends upon short-circuiting: If !manual_mode, then control_position(...)
// should not be called.
// It doesn't really matter if it is called, it will just be bad for performance.
if (!manual_mode && control_position(current_position, ground_speed, _pos_sp_triplet)) {
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;
/* XXX check if radius makes sense here */
float turn_distance = _parameters.l1_distance; //_gnd_control.switch_distance(100.0f);
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) {
// publish status
position_controller_status_s pos_ctrl_status = {};
/* 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);
pos_ctrl_status.nav_roll = 0.0f;
pos_ctrl_status.nav_pitch = 0.0f;
pos_ctrl_status.nav_bearing = _gnd_control.nav_bearing();
} else {
/* advertise and publish */
_attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
}
pos_ctrl_status.target_bearing = _gnd_control.target_bearing();
pos_ctrl_status.xtrack_error = _gnd_control.crosstrack_error();
/* XXX check if radius makes sense here */
float turn_distance = _parameters.l1_distance; //_gnd_control.switch_distance(100.0f);
pos_ctrl_status.wp_dist = get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon,
_pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon);
// publish status
position_controller_status_s pos_ctrl_status = {};
pos_ctrl_status.acceptance_radius = turn_distance;
pos_ctrl_status.yaw_acceptance = NAN;
pos_ctrl_status.nav_roll = _gnd_control.get_roll_setpoint();
pos_ctrl_status.nav_pitch = 0.0f;
pos_ctrl_status.nav_bearing = _gnd_control.nav_bearing();
pos_ctrl_status.timestamp = hrt_absolute_time();
pos_ctrl_status.target_bearing = _gnd_control.target_bearing();
pos_ctrl_status.xtrack_error = _gnd_control.crosstrack_error();
if (_pos_ctrl_status_pub != nullptr) {
orb_publish(ORB_ID(position_controller_status), _pos_ctrl_status_pub, &pos_ctrl_status);
pos_ctrl_status.wp_dist = get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon,
_pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon);
pos_ctrl_status.acceptance_radius = turn_distance;
pos_ctrl_status.yaw_acceptance = NAN;
pos_ctrl_status.timestamp = hrt_absolute_time();
if (_pos_ctrl_status_pub != nullptr) {
orb_publish(ORB_ID(position_controller_status), _pos_ctrl_status_pub, &pos_ctrl_status);
} else {
_pos_ctrl_status_pub = orb_advertise(ORB_ID(position_controller_status), &pos_ctrl_status);
}
} else {
_pos_ctrl_status_pub = orb_advertise(ORB_ID(position_controller_status), &pos_ctrl_status);
}
}
perf_end(_loop_perf);
}
if (fds[2].revents & POLLIN) {
// This should be copied even if not in manual mode. Otherwise, the poll(...) call will keep
// returning immediately and this loop will eat up resources.
orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sub, &_manual);
if (manual_mode) {
/* manual/direct control */
//PX4_INFO("Manual mode!");
_act_controls.control[actuator_controls_s::INDEX_ROLL] = _manual.y;
_act_controls.control[actuator_controls_s::INDEX_PITCH] = -_manual.x;
_act_controls.control[actuator_controls_s::INDEX_YAW] = _manual.r; //TODO: Readd yaw scale param
_act_controls.control[actuator_controls_s::INDEX_THROTTLE] = _manual.z;
}
}
if (pret == 0) {
//orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &_vehicle_att);
_act_controls.timestamp = hrt_absolute_time();
if (_actuator_controls_pub != nullptr) {
//PX4_INFO("Publishing actuator from pos control");
orb_publish(ORB_ID(actuator_controls_0), _actuator_controls_pub, &_act_controls);
} else {
_actuator_controls_pub = orb_advertise(ORB_ID(actuator_controls_0), &_act_controls);
}
actuators_last_published = hrt_absolute_time();
}
}
orb_unsubscribe(_control_mode_sub);
orb_unsubscribe(_global_pos_sub);
orb_unsubscribe(_manual_control_sub);
orb_unsubscribe(_params_sub);
orb_unsubscribe(_pos_sp_triplet_sub);
orb_unsubscribe(_vehicle_attitude_sub);
_task_running = false;
warnx("exiting.\n");
@ -465,9 +511,9 @@ GroundRoverPositionControl::task_main()
}
int
GroundRoverPositionControl::task_main_trampoline(int argc, char *argv[])
RoverPositionControl::task_main_trampoline(int argc, char *argv[])
{
gnd_control::g_control = new GroundRoverPositionControl();
gnd_control::g_control = new RoverPositionControl();
if (gnd_control::g_control == nullptr) {
warnx("OUT OF MEM");
@ -482,14 +528,14 @@ GroundRoverPositionControl::task_main_trampoline(int argc, char *argv[])
}
int
GroundRoverPositionControl::start()
RoverPositionControl::start()
{
/* start the task */
_control_task = px4_task_spawn_cmd("gnd_pos_ctrl",
_control_task = px4_task_spawn_cmd("rover_pos_ctrl",
SCHED_DEFAULT,
SCHED_PRIORITY_POSITION_CONTROL,
1700,
(px4_main_t)&GroundRoverPositionControl::task_main_trampoline,
(px4_main_t)&RoverPositionControl::task_main_trampoline,
nullptr);
if (_control_task < 0) {
@ -500,10 +546,10 @@ GroundRoverPositionControl::start()
return OK;
}
int gnd_pos_control_main(int argc, char *argv[])
int rover_pos_control_main(int argc, char *argv[])
{
if (argc < 2) {
warnx("usage: gnd_pos_control {start|stop|status}");
warnx("usage: rover_pos_control {start|stop|status}");
return 1;
}
@ -514,7 +560,7 @@ int gnd_pos_control_main(int argc, char *argv[])
return 1;
}
if (OK != GroundRoverPositionControl::start()) {
if (OK != RoverPositionControl::start()) {
warn("start failed");
return 1;
}

31
src/modules/gnd_pos_control/GroundRoverPositionControl.hpp → src/modules/rover_pos_control/RoverPositionControl.hpp
View File

@ -63,19 +63,21 @@
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/ekf2_timestamps.h>
#include <uORB/uORB.h>
using matrix::Dcmf;
using uORB::SubscriptionData;
class GroundRoverPositionControl
class RoverPositionControl
{
public:
GroundRoverPositionControl();
~GroundRoverPositionControl();
GroundRoverPositionControl(const GroundRoverPositionControl &) = delete;
GroundRoverPositionControl operator=(const GroundRoverPositionControl &other) = delete;
RoverPositionControl();
~RoverPositionControl();
RoverPositionControl(const RoverPositionControl &) = delete;
RoverPositionControl operator=(const RoverPositionControl &other) = delete;
/**
* Start the sensors task.
@ -92,8 +94,8 @@ public:
bool task_running() { return _task_running; }
private:
orb_advert_t _attitude_sp_pub{nullptr}; /**< attitude setpoint */
orb_advert_t _pos_ctrl_status_pub{nullptr}; /**< navigation capabilities publication */
orb_advert_t _actuator_controls_pub{nullptr}; /**< actuator controls publication */
bool _task_should_exit{false}; /**< if true, sensor task should exit */
bool _task_running{false}; /**< if true, task is running in its mainloop */
@ -103,14 +105,15 @@ private:
int _manual_control_sub{-1}; /**< notification of manual control updates */
int _params_sub{-1}; /**< notification of parameter updates */
int _pos_sp_triplet_sub{-1};
int _vehicle_attitude_sub{-1};
manual_control_setpoint_s _manual{}; /**< r/c channel data */
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_control_mode_s _control_mode{}; /**< control mode */
vehicle_global_position_s _global_pos{}; /**< global vehicle position */
actuator_controls_s _act_controls{}; /**< direct control of actuators */
vehicle_attitude_s _vehicle_att{};
SubscriptionData<vehicle_attitude_s> _sub_attitude;
SubscriptionData<sensor_bias_s> _sub_sensors;
perf_counter_t _loop_perf; /**< loop performance counter */
@ -126,6 +129,8 @@ private:
ECL_L1_Pos_Controller _gnd_control;
bool _waypoint_reached;
enum UGV_POSCTRL_MODE {
UGV_POSCTRL_MODE_AUTO,
UGV_POSCTRL_MODE_OTHER
@ -151,6 +156,9 @@ private:
float throttle_cruise;
float throttle_slew_max;
float wheel_base;
float max_turn_angle;
} _parameters{}; /**< local copies of interesting parameters */
struct {
@ -174,6 +182,8 @@ private:
param_t throttle_cruise;
param_t throttle_slew_max;
param_t wheel_base;
param_t max_turn_angle;
} _parameter_handles{}; /**< handles for interesting parameters */
@ -185,6 +195,7 @@ private:
void manual_control_setpoint_poll();
void position_setpoint_triplet_poll();
void vehicle_control_mode_poll();
void vehicle_attitude_poll();
/**
* Control position.

58
src/modules/gnd_pos_control/gnd_pos_control_params.c → src/modules/rover_pos_control/rover_pos_control_params.c
View File

@ -32,7 +32,7 @@
****************************************************************************/
/**
* @file gnd_pos_control_params.c
* @file rover_pos_control_params.c
*
* Parameters defined by the position control task for ground rovers
*
@ -59,7 +59,7 @@
* @max 100.0
* @decimal 1
* @increment 0.1
* @group GND POS Control
* @group Rover Position Control
*/
PARAM_DEFINE_FLOAT(GND_L1_DIST, 5.0f);
@ -76,7 +76,7 @@ PARAM_DEFINE_FLOAT(GND_L1_DIST, 5.0f);
* @max 50.0
* @decimal 1
* @increment 0.5
* @group GND POS Control
* @group Rover Position Control
*/
PARAM_DEFINE_FLOAT(GND_L1_PERIOD, 10.0f);
@ -89,7 +89,7 @@ PARAM_DEFINE_FLOAT(GND_L1_PERIOD, 10.0f);
* @max 0.9
* @decimal 2
* @increment 0.05
* @group GND POS Control
* @group Rover Position Control
*/
PARAM_DEFINE_FLOAT(GND_L1_DAMPING, 0.75f);
@ -104,7 +104,7 @@ PARAM_DEFINE_FLOAT(GND_L1_DAMPING, 0.75f);
* @max 1.0
* @decimal 2
* @increment 0.01
* @group GND POS Control
* @group Rover Position Control
*/
PARAM_DEFINE_FLOAT(GND_THR_CRUISE, 0.1f);
@ -119,7 +119,7 @@ PARAM_DEFINE_FLOAT(GND_THR_CRUISE, 0.1f);
* @max 1.0
* @decimal 2
* @increment 0.01
* @group GND POS Control
* @group Rover Position Control
*/
PARAM_DEFINE_FLOAT(GND_THR_MAX, 0.3f);
@ -134,7 +134,7 @@ PARAM_DEFINE_FLOAT(GND_THR_MAX, 0.3f);
* @max 1.0
* @decimal 2
* @increment 0.01
* @group GND POS Control
* @group Rover Position Control
*/
PARAM_DEFINE_FLOAT(GND_THR_MIN, 0.0f);
@ -149,7 +149,7 @@ PARAM_DEFINE_FLOAT(GND_THR_MIN, 0.0f);
* @max 0.4
* @decimal 2
* @increment 0.01
* @group GND POS Control
* @group Rover Position Control
*/
PARAM_DEFINE_FLOAT(GND_THR_IDLE, 0.0f);
@ -161,7 +161,7 @@ PARAM_DEFINE_FLOAT(GND_THR_IDLE, 0.0f);
* @max 1
* @value 0 open loop control
* @value 1 close the loop with gps speed
* @group GND Attitude Control
* @group Rover Position Control
*/
PARAM_DEFINE_INT32(GND_SP_CTRL_MODE, 1);
@ -175,7 +175,7 @@ PARAM_DEFINE_INT32(GND_SP_CTRL_MODE, 1);
* @max 50.0
* @decimal 3
* @increment 0.005
* @group GND Attitude Control
* @group Rover Position Control
*/
PARAM_DEFINE_FLOAT(GND_SPEED_P, 2.0f);
@ -189,7 +189,7 @@ PARAM_DEFINE_FLOAT(GND_SPEED_P, 2.0f);
* @max 50.0
* @decimal 3
* @increment 0.005
* @group GND Attitude Control
* @group Rover Position Control
*/
PARAM_DEFINE_FLOAT(GND_SPEED_I, 0.1f);
@ -203,7 +203,7 @@ PARAM_DEFINE_FLOAT(GND_SPEED_I, 0.1f);
* @max 50.0
* @decimal 3
* @increment 0.005
* @group GND Attitude Control
* @group Rover Position Control
*/
PARAM_DEFINE_FLOAT(GND_SPEED_D, 0.0f);
@ -217,7 +217,7 @@ PARAM_DEFINE_FLOAT(GND_SPEED_D, 0.0f);
* @max 50.0
* @decimal 3
* @increment 0.005
* @group GND Attitude Control
* @group Rover Position Control
*/
PARAM_DEFINE_FLOAT(GND_SPEED_IMAX, 1.0f);
@ -231,7 +231,7 @@ PARAM_DEFINE_FLOAT(GND_SPEED_IMAX, 1.0f);
* @max 50.0
* @decimal 3
* @increment 0.005
* @group GND Attitude Control
* @group Rover Position Control
*/
PARAM_DEFINE_FLOAT(GND_SPEED_THR_SC, 1.0f);
@ -244,7 +244,7 @@ PARAM_DEFINE_FLOAT(GND_SPEED_THR_SC, 1.0f);
* @max 40
* @decimal 1
* @increment 0.5
* @group FW TECS
* @group Rover Position Control
*/
PARAM_DEFINE_FLOAT(GND_SPEED_TRIM, 3.0f);
@ -257,6 +257,32 @@ PARAM_DEFINE_FLOAT(GND_SPEED_TRIM, 3.0f);
* @max 40
* @decimal 1
* @increment 0.5
* @group FW TECS
* @group Rover Position Control
*/
PARAM_DEFINE_FLOAT(GND_SPEED_MAX, 10.0f);
/**
* Distance from front axle to rear axle
*
*
* @unit m
* @min 0.0
* @decimal 3
* @increment 0.01
* @group Rover Position Control
*/
PARAM_DEFINE_FLOAT(GND_WHEEL_BASE, 2.0f);
/**
* Maximum turn angle for Ackerman steering.
* At a control output of 0, the steering wheels are at 0 radians.
* At a control output of 1, the steering wheels are at GND_MAX_ANG radians.
*
* @unit rad
* @min 0.0
* @max 3.14159
* @decimal 3
* @increment 0.01
* @group Rover Position Control
*/
PARAM_DEFINE_FLOAT(GND_MAX_ANG, 0.7854f);