diff --git a/ROMFS/px4fmu_common/init.d-posix/1060_rover b/ROMFS/px4fmu_common/init.d-posix/1060_rover index ba5fdc3d4da3..aecd1c3b4a31 100644 --- a/ROMFS/px4fmu_common/init.d-posix/1060_rover +++ b/ROMFS/px4fmu_common/init.d-posix/1060_rover @@ -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 diff --git a/ROMFS/px4fmu_common/init.d/airframes/50003_aion_robotics_r1_rover b/ROMFS/px4fmu_common/init.d/airframes/50003_aion_robotics_r1_rover index fb22d2f86050..7992a906eec0 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/50003_aion_robotics_r1_rover +++ b/ROMFS/px4fmu_common/init.d/airframes/50003_aion_robotics_r1_rover @@ -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 diff --git a/ROMFS/px4fmu_common/init.d/rc.rover_apps b/ROMFS/px4fmu_common/init.d/rc.rover_apps index ccb7ce815940..dfef3e4eeb8f 100644 --- a/ROMFS/px4fmu_common/init.d/rc.rover_apps +++ b/ROMFS/px4fmu_common/init.d/rc.rover_apps @@ -16,8 +16,7 @@ ekf2 start # # Start attitude controllers. # -gnd_att_control start -gnd_pos_control start +rover_pos_control start # diff --git a/boards/aerotenna/ocpoc/ubuntu.cmake b/boards/aerotenna/ocpoc/ubuntu.cmake index 12f8a093e603..ee06da570d60 100644 --- a/boards/aerotenna/ocpoc/ubuntu.cmake +++ b/boards/aerotenna/ocpoc/ubuntu.cmake @@ -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 diff --git a/boards/airmind/mindpx-v2/default.cmake b/boards/airmind/mindpx-v2/default.cmake index 94eeae9c05e4..2b2a037f4a67 100644 --- a/boards/airmind/mindpx-v2/default.cmake +++ b/boards/airmind/mindpx-v2/default.cmake @@ -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 diff --git a/boards/atlflight/eagle/default.cmake b/boards/atlflight/eagle/default.cmake index 13e235d36717..b6093a1fa89f 100644 --- a/boards/atlflight/eagle/default.cmake +++ b/boards/atlflight/eagle/default.cmake @@ -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 diff --git a/boards/atlflight/eagle/qurt-default.cmake b/boards/atlflight/eagle/qurt-default.cmake index f399ecb13c57..5e25f06291aa 100644 --- a/boards/atlflight/eagle/qurt-default.cmake +++ b/boards/atlflight/eagle/qurt-default.cmake @@ -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 diff --git a/boards/atlflight/excelsior/default.cmake b/boards/atlflight/excelsior/default.cmake index 4ad315e86c42..37aeb921828b 100644 --- a/boards/atlflight/excelsior/default.cmake +++ b/boards/atlflight/excelsior/default.cmake @@ -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 diff --git a/boards/atlflight/excelsior/qurt-default.cmake b/boards/atlflight/excelsior/qurt-default.cmake index 7934fece4d32..a31a0b7523ee 100644 --- a/boards/atlflight/excelsior/qurt-default.cmake +++ b/boards/atlflight/excelsior/qurt-default.cmake @@ -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 diff --git a/boards/auav/x21/default.cmake b/boards/auav/x21/default.cmake index 017c6e7e2479..6ec2ea23d862 100644 --- a/boards/auav/x21/default.cmake +++ b/boards/auav/x21/default.cmake @@ -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 diff --git a/boards/av/x-v1/default.cmake b/boards/av/x-v1/default.cmake index 510fa1e6a427..e99d54ef840f 100644 --- a/boards/av/x-v1/default.cmake +++ b/boards/av/x-v1/default.cmake @@ -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 diff --git a/boards/beaglebone/blue/cross.cmake b/boards/beaglebone/blue/cross.cmake index 0ceebaf32c0b..7c15ea1435ad 100644 --- a/boards/beaglebone/blue/cross.cmake +++ b/boards/beaglebone/blue/cross.cmake @@ -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 diff --git a/boards/beaglebone/blue/native.cmake b/boards/beaglebone/blue/native.cmake index a6e921156fae..747dc206acc0 100644 --- a/boards/beaglebone/blue/native.cmake +++ b/boards/beaglebone/blue/native.cmake @@ -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 diff --git a/boards/bitcraze/crazyflie/default.cmake b/boards/bitcraze/crazyflie/default.cmake index 0773684cb6b1..11fcde6b786b 100644 --- a/boards/bitcraze/crazyflie/default.cmake +++ b/boards/bitcraze/crazyflie/default.cmake @@ -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 diff --git a/boards/emlid/navio2/cross.cmake b/boards/emlid/navio2/cross.cmake index 8a66ae5fa92f..2a6e9657a59a 100644 --- a/boards/emlid/navio2/cross.cmake +++ b/boards/emlid/navio2/cross.cmake @@ -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 diff --git a/boards/emlid/navio2/native.cmake b/boards/emlid/navio2/native.cmake index 96340c0e7b7f..20addc3f0f32 100644 --- a/boards/emlid/navio2/native.cmake +++ b/boards/emlid/navio2/native.cmake @@ -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 diff --git a/boards/intel/aerofc-v1/default.cmake b/boards/intel/aerofc-v1/default.cmake index b5c4be7e5c51..ce0d07d9cc33 100644 --- a/boards/intel/aerofc-v1/default.cmake +++ b/boards/intel/aerofc-v1/default.cmake @@ -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 diff --git a/boards/intel/aerofc-v1/rtps.cmake b/boards/intel/aerofc-v1/rtps.cmake index ae13699ae953..123cadfd8dde 100644 --- a/boards/intel/aerofc-v1/rtps.cmake +++ b/boards/intel/aerofc-v1/rtps.cmake @@ -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 diff --git a/boards/nxp/fmuk66-v3/default.cmake b/boards/nxp/fmuk66-v3/default.cmake index e1f886191440..78fa57dca0ee 100644 --- a/boards/nxp/fmuk66-v3/default.cmake +++ b/boards/nxp/fmuk66-v3/default.cmake @@ -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 diff --git a/boards/omnibus/f4sd/default.cmake b/boards/omnibus/f4sd/default.cmake index 31cd2f7a7773..3a6f44902385 100644 --- a/boards/omnibus/f4sd/default.cmake +++ b/boards/omnibus/f4sd/default.cmake @@ -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 diff --git a/boards/parrot/bebop/default.cmake b/boards/parrot/bebop/default.cmake index 525277ae283a..4b8d01ee68ac 100644 --- a/boards/parrot/bebop/default.cmake +++ b/boards/parrot/bebop/default.cmake @@ -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 diff --git a/boards/px4/fmu-v2/default.cmake b/boards/px4/fmu-v2/default.cmake index 6403c594fa31..675f43bf4386 100644 --- a/boards/px4/fmu-v2/default.cmake +++ b/boards/px4/fmu-v2/default.cmake @@ -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 diff --git a/boards/px4/fmu-v2/lpe.cmake b/boards/px4/fmu-v2/lpe.cmake index 04c2bc394e13..56b916812363 100644 --- a/boards/px4/fmu-v2/lpe.cmake +++ b/boards/px4/fmu-v2/lpe.cmake @@ -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 diff --git a/boards/px4/fmu-v2/rover.cmake b/boards/px4/fmu-v2/rover.cmake index 6f22280d0abf..36290de0cff4 100644 --- a/boards/px4/fmu-v2/rover.cmake +++ b/boards/px4/fmu-v2/rover.cmake @@ -42,8 +42,7 @@ px4_add_board( dataman ekf2 events - gnd_att_control - gnd_pos_control + rover_pos_control land_detector load_mon logger diff --git a/boards/px4/fmu-v2/test.cmake b/boards/px4/fmu-v2/test.cmake index 845d6cea7908..52f18422298f 100644 --- a/boards/px4/fmu-v2/test.cmake +++ b/boards/px4/fmu-v2/test.cmake @@ -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 diff --git a/boards/px4/fmu-v3/default.cmake b/boards/px4/fmu-v3/default.cmake index 167e9b3e29e8..c2d195ca14ba 100644 --- a/boards/px4/fmu-v3/default.cmake +++ b/boards/px4/fmu-v3/default.cmake @@ -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 diff --git a/boards/px4/fmu-v3/rtps.cmake b/boards/px4/fmu-v3/rtps.cmake index a462e77c2b91..e26982fb63f2 100644 --- a/boards/px4/fmu-v3/rtps.cmake +++ b/boards/px4/fmu-v3/rtps.cmake @@ -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 diff --git a/boards/px4/fmu-v3/stackcheck.cmake b/boards/px4/fmu-v3/stackcheck.cmake index 6d5d29b970d9..9f10338263db 100644 --- a/boards/px4/fmu-v3/stackcheck.cmake +++ b/boards/px4/fmu-v3/stackcheck.cmake @@ -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 diff --git a/boards/px4/fmu-v4/default.cmake b/boards/px4/fmu-v4/default.cmake index cf5ffec43728..848ac86b23eb 100644 --- a/boards/px4/fmu-v4/default.cmake +++ b/boards/px4/fmu-v4/default.cmake @@ -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 diff --git a/boards/px4/fmu-v4/rtps.cmake b/boards/px4/fmu-v4/rtps.cmake index 199ddacc24ee..6e52ef65fc33 100644 --- a/boards/px4/fmu-v4/rtps.cmake +++ b/boards/px4/fmu-v4/rtps.cmake @@ -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 diff --git a/boards/px4/fmu-v4/stackcheck.cmake b/boards/px4/fmu-v4/stackcheck.cmake index ca09533b2671..c06ffe17cd53 100644 --- a/boards/px4/fmu-v4/stackcheck.cmake +++ b/boards/px4/fmu-v4/stackcheck.cmake @@ -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 diff --git a/boards/px4/fmu-v4pro/default.cmake b/boards/px4/fmu-v4pro/default.cmake index 53eda0328915..116fccbe3db9 100644 --- a/boards/px4/fmu-v4pro/default.cmake +++ b/boards/px4/fmu-v4pro/default.cmake @@ -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 diff --git a/boards/px4/fmu-v4pro/rtps.cmake b/boards/px4/fmu-v4pro/rtps.cmake index 84e5ca78879f..6fa41515faca 100644 --- a/boards/px4/fmu-v4pro/rtps.cmake +++ b/boards/px4/fmu-v4pro/rtps.cmake @@ -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 diff --git a/boards/px4/fmu-v5/default.cmake b/boards/px4/fmu-v5/default.cmake index 582405e8f666..ab8e819d627d 100644 --- a/boards/px4/fmu-v5/default.cmake +++ b/boards/px4/fmu-v5/default.cmake @@ -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 diff --git a/boards/px4/fmu-v5/rover.cmake b/boards/px4/fmu-v5/rover.cmake index b0e53d95818e..03b0fef0eaae 100644 --- a/boards/px4/fmu-v5/rover.cmake +++ b/boards/px4/fmu-v5/rover.cmake @@ -53,8 +53,7 @@ px4_add_board( dataman ekf2 events - gnd_att_control - gnd_pos_control + rover_pos_control land_detector load_mon logger diff --git a/boards/px4/fmu-v5/rtps.cmake b/boards/px4/fmu-v5/rtps.cmake index 11cf2bbb9c5c..11624bb479b2 100644 --- a/boards/px4/fmu-v5/rtps.cmake +++ b/boards/px4/fmu-v5/rtps.cmake @@ -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 diff --git a/boards/px4/fmu-v5/stackcheck.cmake b/boards/px4/fmu-v5/stackcheck.cmake index 13911da55af0..6ff26f50231c 100644 --- a/boards/px4/fmu-v5/stackcheck.cmake +++ b/boards/px4/fmu-v5/stackcheck.cmake @@ -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 diff --git a/boards/px4/fmu-v5x/default.cmake b/boards/px4/fmu-v5x/default.cmake index de5c22703961..e67006cc2533 100644 --- a/boards/px4/fmu-v5x/default.cmake +++ b/boards/px4/fmu-v5x/default.cmake @@ -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 diff --git a/boards/px4/fmu-v5x/rover.cmake b/boards/px4/fmu-v5x/rover.cmake index d6541cd70832..d695514e9795 100644 --- a/boards/px4/fmu-v5x/rover.cmake +++ b/boards/px4/fmu-v5x/rover.cmake @@ -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 diff --git a/boards/px4/fmu-v5x/rtps.cmake b/boards/px4/fmu-v5x/rtps.cmake index 3f7a1ddabd87..25bad9755412 100644 --- a/boards/px4/fmu-v5x/rtps.cmake +++ b/boards/px4/fmu-v5x/rtps.cmake @@ -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 diff --git a/boards/px4/fmu-v5x/stackcheck.cmake b/boards/px4/fmu-v5x/stackcheck.cmake index affa459bedf5..08554209076b 100644 --- a/boards/px4/fmu-v5x/stackcheck.cmake +++ b/boards/px4/fmu-v5x/stackcheck.cmake @@ -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 diff --git a/boards/px4/raspberrypi/cross.cmake b/boards/px4/raspberrypi/cross.cmake index 606e2f89d3f6..38d29ddde8af 100644 --- a/boards/px4/raspberrypi/cross.cmake +++ b/boards/px4/raspberrypi/cross.cmake @@ -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 diff --git a/boards/px4/raspberrypi/native.cmake b/boards/px4/raspberrypi/native.cmake index 548b2fc27f8a..f317e5e3733c 100644 --- a/boards/px4/raspberrypi/native.cmake +++ b/boards/px4/raspberrypi/native.cmake @@ -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 diff --git a/boards/px4/sitl/default.cmake b/boards/px4/sitl/default.cmake index 9c01209b5b08..3c9c66564f53 100644 --- a/boards/px4/sitl/default.cmake +++ b/boards/px4/sitl/default.cmake @@ -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 diff --git a/boards/px4/sitl/rtps.cmake b/boards/px4/sitl/rtps.cmake index edb5bffaba2c..a14e6c0d668b 100644 --- a/boards/px4/sitl/rtps.cmake +++ b/boards/px4/sitl/rtps.cmake @@ -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 diff --git a/boards/px4/sitl/test.cmake b/boards/px4/sitl/test.cmake index ebf7e878e750..cdf693b794c1 100644 --- a/boards/px4/sitl/test.cmake +++ b/boards/px4/sitl/test.cmake @@ -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 diff --git a/src/modules/gnd_att_control/CMakeLists.txt b/src/modules/gnd_att_control/CMakeLists.txt deleted file mode 100644 index f5864fe0f7d2..000000000000 --- a/src/modules/gnd_att_control/CMakeLists.txt +++ /dev/null @@ -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 - ) diff --git a/src/modules/gnd_att_control/GroundRoverAttitudeControl.cpp b/src/modules/gnd_att_control/GroundRoverAttitudeControl.cpp deleted file mode 100644 index 95d1b8e5554f..000000000000 --- a/src/modules/gnd_att_control/GroundRoverAttitudeControl.cpp +++ /dev/null @@ -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 - */ - -#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; -} diff --git a/src/modules/gnd_att_control/GroundRoverAttitudeControl.hpp b/src/modules/gnd_att_control/GroundRoverAttitudeControl.hpp deleted file mode 100644 index bb11a36f8d79..000000000000 --- a/src/modules/gnd_att_control/GroundRoverAttitudeControl.hpp +++ /dev/null @@ -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 - */ - -#include -#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 _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(); - -}; diff --git a/src/modules/gnd_att_control/gnd_att_control_params.c b/src/modules/gnd_att_control/gnd_att_control_params.c deleted file mode 100644 index 3c23fc92c102..000000000000 --- a/src/modules/gnd_att_control/gnd_att_control_params.c +++ /dev/null @@ -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 - */ - -/* - * 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); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 45ce495ce65a..8aae5510aede 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -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(); diff --git a/src/modules/gnd_pos_control/CMakeLists.txt b/src/modules/rover_pos_control/CMakeLists.txt similarity index 95% rename from src/modules/gnd_pos_control/CMakeLists.txt rename to src/modules/rover_pos_control/CMakeLists.txt index 1c52a1be8791..f63908a25674 100644 --- a/src/modules/gnd_pos_control/CMakeLists.txt +++ b/src/modules/rover_pos_control/CMakeLists.txt @@ -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 diff --git a/src/modules/gnd_pos_control/GroundRoverPositionControl.cpp b/src/modules/rover_pos_control/RoverPositionControl.cpp similarity index 63% rename from src/modules/gnd_pos_control/GroundRoverPositionControl.cpp rename to src/modules/rover_pos_control/RoverPositionControl.cpp index e5aaf392a1a7..1c742a7e379e 100644 --- a/src/modules/gnd_pos_control/GroundRoverPositionControl.cpp +++ b/src/modules/rover_pos_control/RoverPositionControl.cpp @@ -42,7 +42,10 @@ */ -#include "GroundRoverPositionControl.hpp" +#include "RoverPositionControl.hpp" +#include + +#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 ¤t_position, - const matrix::Vector3f &ground_speed, const position_setpoint_triplet_s &pos_sp_triplet) +RoverPositionControl::control_position(const matrix::Vector2f ¤t_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 ¤t_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 ¤t_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 ¤t_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); + + bool should_idle = true; - } else if ((pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) - || (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF)) { + 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) { - - /* 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.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; + _act_controls.control[actuator_controls_s::INDEX_THROTTLE] = mission_throttle; + + 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; + } } 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 ¤t_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)) { + + /* XXX check if radius makes sense here */ + float turn_distance = _parameters.l1_distance; //_gnd_control.switch_distance(100.0f); + + // publish status + position_controller_status_s pos_ctrl_status = {}; + + pos_ctrl_status.nav_roll = 0.0f; + pos_ctrl_status.nav_pitch = 0.0f; + pos_ctrl_status.nav_bearing = _gnd_control.nav_bearing(); + + pos_ctrl_status.target_bearing = _gnd_control.target_bearing(); + pos_ctrl_status.xtrack_error = _gnd_control.crosstrack_error(); - 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; + 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); - 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) { + pos_ctrl_status.acceptance_radius = turn_distance; + pos_ctrl_status.yaw_acceptance = NAN; - /* 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.timestamp = hrt_absolute_time(); - } else { - /* advertise and publish */ - _attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); - } + if (_pos_ctrl_status_pub != nullptr) { + orb_publish(ORB_ID(position_controller_status), _pos_ctrl_status_pub, &pos_ctrl_status); - /* XXX check if radius makes sense here */ - float turn_distance = _parameters.l1_distance; //_gnd_control.switch_distance(100.0f); + } else { + _pos_ctrl_status_pub = orb_advertise(ORB_ID(position_controller_status), &pos_ctrl_status); + } + + } + + + perf_end(_loop_perf); + } - // publish status - position_controller_status_s pos_ctrl_status = {}; + if (fds[2].revents & POLLIN) { - 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(); + // 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); - pos_ctrl_status.target_bearing = _gnd_control.target_bearing(); - pos_ctrl_status.xtrack_error = _gnd_control.crosstrack_error(); + 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; + } + } - 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); + if (pret == 0) { - pos_ctrl_status.acceptance_radius = turn_distance; - pos_ctrl_status.yaw_acceptance = NAN; + //orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &_vehicle_att); + _act_controls.timestamp = hrt_absolute_time(); - pos_ctrl_status.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); - if (_pos_ctrl_status_pub != nullptr) { - orb_publish(ORB_ID(position_controller_status), _pos_ctrl_status_pub, &pos_ctrl_status); + } else { - } else { - _pos_ctrl_status_pub = orb_advertise(ORB_ID(position_controller_status), &pos_ctrl_status); - } - } + _actuator_controls_pub = orb_advertise(ORB_ID(actuator_controls_0), &_act_controls); } - perf_end(_loop_perf); + 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; } diff --git a/src/modules/gnd_pos_control/GroundRoverPositionControl.hpp b/src/modules/rover_pos_control/RoverPositionControl.hpp similarity index 87% rename from src/modules/gnd_pos_control/GroundRoverPositionControl.hpp rename to src/modules/rover_pos_control/RoverPositionControl.hpp index d1b6b60e9380..137698fa19fe 100644 --- a/src/modules/gnd_pos_control/GroundRoverPositionControl.hpp +++ b/src/modules/rover_pos_control/RoverPositionControl.hpp @@ -63,19 +63,21 @@ #include #include #include +#include +#include #include 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 @@ class GroundRoverPositionControl 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 @@ class GroundRoverPositionControl 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 _sub_attitude; SubscriptionData _sub_sensors; perf_counter_t _loop_perf; /**< loop performance counter */ @@ -126,6 +129,8 @@ class GroundRoverPositionControl 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 @@ class GroundRoverPositionControl 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 @@ class GroundRoverPositionControl 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 @@ class GroundRoverPositionControl void manual_control_setpoint_poll(); void position_setpoint_triplet_poll(); void vehicle_control_mode_poll(); + void vehicle_attitude_poll(); /** * Control position. diff --git a/src/modules/gnd_pos_control/gnd_pos_control_params.c b/src/modules/rover_pos_control/rover_pos_control_params.c similarity index 84% rename from src/modules/gnd_pos_control/gnd_pos_control_params.c rename to src/modules/rover_pos_control/rover_pos_control_params.c index ef9f607138db..bdeb6de3b0d9 100644 --- a/src/modules/gnd_pos_control/gnd_pos_control_params.c +++ b/src/modules/rover_pos_control/rover_pos_control_params.c @@ -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); \ No newline at end of file