From aef522553ec6ded61d2d8afc0ad5f88027dfd010 Mon Sep 17 00:00:00 2001 From: Marco Zorzi Date: Tue, 6 Jun 2017 19:26:51 +0200 Subject: [PATCH] unmanned ground vehicle (UGV) controllers and Traxxas Stampede configuration (#7175) --- .../init.d/50002_traxxas_stampede_2wd | 61 ++ .../init.d/rc.axialracing_ax10_defaults | 2 +- ROMFS/px4fmu_common/init.d/rc.gnd_apps | 24 + ROMFS/px4fmu_common/init.d/rc.gnd_defaults | 32 + ROMFS/px4fmu_common/init.d/rcS | 25 +- ROMFS/px4fmu_common/mixers/stampede.main.mix | 37 ++ ROMFS/sitl/mixers/rover_sitl.main.mix | 53 +- cmake/configs/nuttx_aerocore2_default.cmake | 4 +- cmake/configs/nuttx_aerofc-v1_default.cmake | 4 +- cmake/configs/nuttx_auav-x21_default.cmake | 4 +- cmake/configs/nuttx_mindpx-v2_default.cmake | 4 +- cmake/configs/nuttx_px4fmu-v2_default.cmake | 4 +- cmake/configs/nuttx_px4fmu-v3_default.cmake | 2 + cmake/configs/nuttx_px4fmu-v4_default.cmake | 5 +- .../configs/nuttx_px4fmu-v4pro_default.cmake | 2 + cmake/configs/nuttx_px4fmu-v5_default.cmake | 2 + cmake/configs/posix_rpi_common.cmake | 6 +- cmake/configs/posix_sitl_default.cmake | 2 + posix-configs/SITL/init/ekf2/rover | 73 ++- posix-configs/SITL/init/lpe/rover | 79 +++ src/modules/gnd_att_control/CMakeLists.txt | 45 ++ .../GroundRoverAttitudeControl.cpp | 440 +++++++++++++ .../GroundRoverAttitudeControl.hpp | 142 +++++ .../gnd_att_control/gnd_att_control_params.c | 195 ++++++ src/modules/gnd_pos_control/CMakeLists.txt | 46 ++ .../GroundRoverPositionControl.cpp | 577 ++++++++++++++++++ .../GroundRoverPositionControl.hpp | 208 +++++++ .../gnd_pos_control/gnd_pos_control_params.c | 262 ++++++++ .../land_detector/land_detector_main.cpp | 2 +- 29 files changed, 2270 insertions(+), 72 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d/50002_traxxas_stampede_2wd create mode 100644 ROMFS/px4fmu_common/init.d/rc.gnd_apps create mode 100644 ROMFS/px4fmu_common/init.d/rc.gnd_defaults create mode 100644 ROMFS/px4fmu_common/mixers/stampede.main.mix create mode 100644 posix-configs/SITL/init/lpe/rover create mode 100644 src/modules/gnd_att_control/CMakeLists.txt create mode 100644 src/modules/gnd_att_control/GroundRoverAttitudeControl.cpp create mode 100644 src/modules/gnd_att_control/GroundRoverAttitudeControl.hpp create mode 100644 src/modules/gnd_att_control/gnd_att_control_params.c create mode 100644 src/modules/gnd_pos_control/CMakeLists.txt create mode 100644 src/modules/gnd_pos_control/GroundRoverPositionControl.cpp create mode 100644 src/modules/gnd_pos_control/GroundRoverPositionControl.hpp create mode 100644 src/modules/gnd_pos_control/gnd_pos_control_params.c diff --git a/ROMFS/px4fmu_common/init.d/50002_traxxas_stampede_2wd b/ROMFS/px4fmu_common/init.d/50002_traxxas_stampede_2wd new file mode 100644 index 0000000000..baa5b87b68 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/50002_traxxas_stampede_2wd @@ -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 diff --git a/ROMFS/px4fmu_common/init.d/rc.axialracing_ax10_defaults b/ROMFS/px4fmu_common/init.d/rc.axialracing_ax10_defaults index 743ca358ba..7caac82b54 100644 --- a/ROMFS/px4fmu_common/init.d/rc.axialracing_ax10_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.axialracing_ax10_defaults @@ -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 diff --git a/ROMFS/px4fmu_common/init.d/rc.gnd_apps b/ROMFS/px4fmu_common/init.d/rc.gnd_apps new file mode 100644 index 0000000000..17c566ee04 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.gnd_apps @@ -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 diff --git a/ROMFS/px4fmu_common/init.d/rc.gnd_defaults b/ROMFS/px4fmu_common/init.d/rc.gnd_defaults new file mode 100644 index 0000000000..556098f924 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.gnd_defaults @@ -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 diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 6c0367f33b..6b86362326 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -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 # diff --git a/ROMFS/px4fmu_common/mixers/stampede.main.mix b/ROMFS/px4fmu_common/mixers/stampede.main.mix new file mode 100644 index 0000000000..6ab22d8300 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/stampede.main.mix @@ -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 diff --git a/ROMFS/sitl/mixers/rover_sitl.main.mix b/ROMFS/sitl/mixers/rover_sitl.main.mix index 8895aee3a7..d56d5a067d 100644 --- a/ROMFS/sitl/mixers/rover_sitl.main.mix +++ b/ROMFS/sitl/mixers/rover_sitl.main.mix @@ -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: diff --git a/cmake/configs/nuttx_aerocore2_default.cmake b/cmake/configs/nuttx_aerocore2_default.cmake index 75b70dcdba..ea73e86ed1 100644 --- a/cmake/configs/nuttx_aerocore2_default.cmake +++ b/cmake/configs/nuttx_aerocore2_default.cmake @@ -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 diff --git a/cmake/configs/nuttx_aerofc-v1_default.cmake b/cmake/configs/nuttx_aerofc-v1_default.cmake index 5357688cc3..5bdb509015 100644 --- a/cmake/configs/nuttx_aerofc-v1_default.cmake +++ b/cmake/configs/nuttx_aerofc-v1_default.cmake @@ -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 diff --git a/cmake/configs/nuttx_auav-x21_default.cmake b/cmake/configs/nuttx_auav-x21_default.cmake index becbfc40b1..c60e67f4dc 100644 --- a/cmake/configs/nuttx_auav-x21_default.cmake +++ b/cmake/configs/nuttx_auav-x21_default.cmake @@ -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 diff --git a/cmake/configs/nuttx_mindpx-v2_default.cmake b/cmake/configs/nuttx_mindpx-v2_default.cmake index aadad97e93..a7d51b4ec4 100644 --- a/cmake/configs/nuttx_mindpx-v2_default.cmake +++ b/cmake/configs/nuttx_mindpx-v2_default.cmake @@ -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 diff --git a/cmake/configs/nuttx_px4fmu-v2_default.cmake b/cmake/configs/nuttx_px4fmu-v2_default.cmake index 76d74530eb..6de846f567 100644 --- a/cmake/configs/nuttx_px4fmu-v2_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v2_default.cmake @@ -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 diff --git a/cmake/configs/nuttx_px4fmu-v3_default.cmake b/cmake/configs/nuttx_px4fmu-v3_default.cmake index 9f039e5c1a..d47e21be70 100644 --- a/cmake/configs/nuttx_px4fmu-v3_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v3_default.cmake @@ -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 diff --git a/cmake/configs/nuttx_px4fmu-v4_default.cmake b/cmake/configs/nuttx_px4fmu-v4_default.cmake index 0490c2bdf1..5d1a2939cd 100644 --- a/cmake/configs/nuttx_px4fmu-v4_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v4_default.cmake @@ -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 diff --git a/cmake/configs/nuttx_px4fmu-v4pro_default.cmake b/cmake/configs/nuttx_px4fmu-v4pro_default.cmake index f1b5751458..2775814cb5 100644 --- a/cmake/configs/nuttx_px4fmu-v4pro_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v4pro_default.cmake @@ -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 diff --git a/cmake/configs/nuttx_px4fmu-v5_default.cmake b/cmake/configs/nuttx_px4fmu-v5_default.cmake index 4bf9cea879..15ee985b38 100644 --- a/cmake/configs/nuttx_px4fmu-v5_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v5_default.cmake @@ -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 diff --git a/cmake/configs/posix_rpi_common.cmake b/cmake/configs/posix_rpi_common.cmake index b57f509dd0..3dca20173b 100644 --- a/cmake/configs/posix_rpi_common.cmake +++ b/cmake/configs/posix_rpi_common.cmake @@ -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 # diff --git a/cmake/configs/posix_sitl_default.cmake b/cmake/configs/posix_sitl_default.cmake index 2bf991ca5b..c7dd8ef2a5 100644 --- a/cmake/configs/posix_sitl_default.cmake +++ b/cmake/configs/posix_sitl_default.cmake @@ -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 diff --git a/posix-configs/SITL/init/ekf2/rover b/posix-configs/SITL/init/ekf2/rover index a5fd02702c..4bb5a018ed 100644 --- a/posix-configs/SITL/init/ekf2/rover +++ b/posix-configs/SITL/init/ekf2/rover @@ -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 diff --git a/posix-configs/SITL/init/lpe/rover b/posix-configs/SITL/init/lpe/rover new file mode 100644 index 0000000000..d3a3dc3aa9 --- /dev/null +++ b/posix-configs/SITL/init/lpe/rover @@ -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 diff --git a/src/modules/gnd_att_control/CMakeLists.txt b/src/modules/gnd_att_control/CMakeLists.txt new file mode 100644 index 0000000000..dca07b4705 --- /dev/null +++ b/src/modules/gnd_att_control/CMakeLists.txt @@ -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 : diff --git a/src/modules/gnd_att_control/GroundRoverAttitudeControl.cpp b/src/modules/gnd_att_control/GroundRoverAttitudeControl.cpp new file mode 100644 index 0000000000..14bdc58c14 --- /dev/null +++ b/src/modules/gnd_att_control/GroundRoverAttitudeControl.cpp @@ -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 + */ + +#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; +} diff --git a/src/modules/gnd_att_control/GroundRoverAttitudeControl.hpp b/src/modules/gnd_att_control/GroundRoverAttitudeControl.hpp new file mode 100644 index 0000000000..c61e2a740c --- /dev/null +++ b/src/modules/gnd_att_control/GroundRoverAttitudeControl.hpp @@ -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 + */ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +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(); + +}; diff --git a/src/modules/gnd_att_control/gnd_att_control_params.c b/src/modules/gnd_att_control/gnd_att_control_params.c new file mode 100644 index 0000000000..dab349e2f4 --- /dev/null +++ b/src/modules/gnd_att_control/gnd_att_control_params.c @@ -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 + */ + +/* + * 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); diff --git a/src/modules/gnd_pos_control/CMakeLists.txt b/src/modules/gnd_pos_control/CMakeLists.txt new file mode 100644 index 0000000000..89a10ebf5e --- /dev/null +++ b/src/modules/gnd_pos_control/CMakeLists.txt @@ -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 : diff --git a/src/modules/gnd_pos_control/GroundRoverPositionControl.cpp b/src/modules/gnd_pos_control/GroundRoverPositionControl.cpp new file mode 100644 index 0000000000..78ffbfef9c --- /dev/null +++ b/src/modules/gnd_pos_control/GroundRoverPositionControl.cpp @@ -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 + */ + + +#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> ¤t_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; +} diff --git a/src/modules/gnd_pos_control/GroundRoverPositionControl.hpp b/src/modules/gnd_pos_control/GroundRoverPositionControl.hpp new file mode 100644 index 0000000000..901cf9cf43 --- /dev/null +++ b/src/modules/gnd_pos_control/GroundRoverPositionControl.hpp @@ -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 + */ + +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +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}; /** &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(); + +}; diff --git a/src/modules/gnd_pos_control/gnd_pos_control_params.c b/src/modules/gnd_pos_control/gnd_pos_control_params.c new file mode 100644 index 0000000000..ef9f607138 --- /dev/null +++ b/src/modules/gnd_pos_control/gnd_pos_control_params.c @@ -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 + */ + +/* + * 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); diff --git a/src/modules/land_detector/land_detector_main.cpp b/src/modules/land_detector/land_detector_main.cpp index 5bf2d348e8..a28a6e0bf4 100644 --- a/src/modules/land_detector/land_detector_main.cpp +++ b/src/modules/land_detector/land_detector_main.cpp @@ -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 {