forked from Archive/PX4-Autopilot
Compare commits
8 Commits
main
...
rover_modu
| Author | SHA1 | Date |
|---|---|---|
PerFrivik
|
a471755dac | |
Matthias Grob
|
7cbc6241d7 | |
|
Matthias Grob
|
257a66d99d | |
|
Matthias Grob
|
a26be63f54 | |
|
PerFrivik
|
679a4e2d98 | |
|
PerFrivik
|
ddd201118d | |
|
PerFrivik
|
6e7ed49ad3 | |
|
PerFrivik
|
a33dccfcc2 |
|
|
@ -4,31 +4,9 @@
|
||||||
# @type Rover
|
# @type Rover
|
||||||
# @class Rover
|
# @class Rover
|
||||||
|
|
||||||
. ${R}etc/init.d/rc.rover_defaults
|
. ${R}etc/init.d/rc.rover_differential_defaults
|
||||||
|
|
||||||
param set-default GND_L1_DIST 5
|
|
||||||
param set-default GND_SP_CTRL_MODE 1
|
|
||||||
param set-default GND_SPEED_D 3
|
|
||||||
param set-default GND_SPEED_I 0.001
|
|
||||||
param set-default GND_SPEED_IMAX 0.125
|
|
||||||
param set-default GND_SPEED_P 0.25
|
|
||||||
param set-default GND_SPEED_THR_SC 1
|
|
||||||
param set-default GND_SPEED_TRIM 4
|
|
||||||
param set-default GND_THR_CRUISE 0.3
|
|
||||||
param set-default GND_THR_MAX 0.5
|
|
||||||
param set-default GND_THR_MIN 0
|
|
||||||
|
|
||||||
param set-default NAV_ACC_RAD 0.5
|
|
||||||
param set-default NAV_LOITER_RAD 2
|
|
||||||
|
|
||||||
param set-default GND_MAX_ANG 0.6
|
|
||||||
param set-default GND_WHEEL_BASE 2.0
|
|
||||||
|
|
||||||
param set-default CA_AIRFRAME 6
|
|
||||||
|
|
||||||
param set-default CA_R_REV 3
|
|
||||||
param set-default PWM_MAIN_FUNC1 101
|
param set-default PWM_MAIN_FUNC1 101
|
||||||
param set-default PWM_MAIN_FUNC2 101
|
param set-default PWM_MAIN_FUNC2 101
|
||||||
param set-default PWM_MAIN_FUNC6 102
|
param set-default PWM_MAIN_FUNC6 102
|
||||||
param set-default PWM_MAIN_FUNC7 102
|
param set-default PWM_MAIN_FUNC7 102
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -0,0 +1,31 @@
|
||||||
|
#!/bin/sh
|
||||||
|
# @name Aion Robotics R1 Rover
|
||||||
|
# @type Rover
|
||||||
|
# @class Rover
|
||||||
|
|
||||||
|
. ${R}etc/init.d/rc.rover_differential_defaults
|
||||||
|
|
||||||
|
PX4_SIMULATOR=${PX4_SIMULATOR:=gz}
|
||||||
|
PX4_GZ_WORLD=${PX4_GZ_WORLD:=default}
|
||||||
|
PX4_SIM_MODEL=${PX4_SIM_MODEL:=r1_rover}
|
||||||
|
|
||||||
|
param set-default SIM_GZ_EN 1 # Gazebo bridge
|
||||||
|
|
||||||
|
# Simulated sensors
|
||||||
|
param set-default SENS_EN_GPSSIM 1
|
||||||
|
param set-default SENS_EN_BAROSIM 0
|
||||||
|
param set-default SENS_EN_MAGSIM 1
|
||||||
|
param set-default SENS_EN_ARSPDSIM 1
|
||||||
|
|
||||||
|
# Actuator mapping
|
||||||
|
param set-default SIM_GZ_WH_FUNC1 101 # right wheel
|
||||||
|
param set-default SIM_GZ_WH_MIN1 0
|
||||||
|
param set-default SIM_GZ_WH_MAX1 200
|
||||||
|
param set-default SIM_GZ_WH_DIS1 100
|
||||||
|
|
||||||
|
param set-default SIM_GZ_WH_FUNC2 102 # left wheel
|
||||||
|
param set-default SIM_GZ_WH_MIN2 0
|
||||||
|
param set-default SIM_GZ_WH_MAX2 200
|
||||||
|
param set-default SIM_GZ_WH_DIS2 100
|
||||||
|
|
||||||
|
param set-default SIM_GZ_WH_REV 1 # reverse right wheel
|
||||||
|
|
@ -80,6 +80,7 @@ px4_add_romfs_files(
|
||||||
4005_gz_x500_vision
|
4005_gz_x500_vision
|
||||||
4006_gz_px4vision
|
4006_gz_px4vision
|
||||||
4008_gz_advanced_plane
|
4008_gz_advanced_plane
|
||||||
|
4009_gz_r1_rover
|
||||||
4010_gz_x500_mono_cam
|
4010_gz_x500_mono_cam
|
||||||
|
|
||||||
6011_gazebo-classic_typhoon_h480
|
6011_gazebo-classic_typhoon_h480
|
||||||
|
|
|
||||||
|
|
@ -51,6 +51,8 @@ px4_add_romfs_files(
|
||||||
rc.thermal_cal
|
rc.thermal_cal
|
||||||
rc.rover_apps
|
rc.rover_apps
|
||||||
rc.rover_defaults
|
rc.rover_defaults
|
||||||
|
rc.rover_differential_apps
|
||||||
|
rc.rover_differential_defaults
|
||||||
rc.uuv_apps
|
rc.uuv_apps
|
||||||
rc.uuv_defaults
|
rc.uuv_defaults
|
||||||
rc.vehicle_setup
|
rc.vehicle_setup
|
||||||
|
|
|
||||||
|
|
@ -11,52 +11,15 @@
|
||||||
# @board bitcraze_crazyflie exclude
|
# @board bitcraze_crazyflie exclude
|
||||||
#
|
#
|
||||||
|
|
||||||
. ${R}etc/init.d/rc.rover_defaults
|
. ${R}etc/init.d/rc.rover_differential_defaults
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
param set-default BAT1_N_CELLS 4
|
param set-default BAT1_N_CELLS 4
|
||||||
|
|
||||||
param set-default EKF2_GBIAS_INIT 0.01
|
param set-default EKF2_GBIAS_INIT 0.01
|
||||||
param set-default EKF2_ANGERR_INIT 0.01
|
param set-default EKF2_ANGERR_INIT 0.01
|
||||||
param set-default EKF2_MAG_TYPE 1
|
|
||||||
|
|
||||||
param set-default FW_AIRSPD_MIN 0
|
|
||||||
param set-default FW_AIRSPD_TRIM 1
|
|
||||||
param set-default FW_AIRSPD_MAX 3
|
|
||||||
|
|
||||||
param set-default GND_SP_CTRL_MODE 1
|
|
||||||
param set-default GND_L1_DIST 5
|
|
||||||
param set-default GND_L1_PERIOD 3
|
|
||||||
param set-default GND_THR_CRUISE 0.7
|
|
||||||
param set-default 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-default GND_MAX_ANG 3.142
|
|
||||||
param set-default 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-default GND_THR_MIN 0
|
|
||||||
param set-default GND_SPEED_P 0.25
|
|
||||||
param set-default GND_SPEED_I 3
|
|
||||||
param set-default GND_SPEED_D 0.001
|
|
||||||
param set-default GND_SPEED_IMAX 0.125
|
|
||||||
param set-default GND_SPEED_THR_SC 1
|
|
||||||
|
|
||||||
param set-default NAV_ACC_RAD 0.5
|
|
||||||
|
|
||||||
# Differential drive acts like ackermann steering with a maximum turn angle of 180 degrees, or pi radians
|
|
||||||
param set-default GND_MAX_ANG 3.1415
|
|
||||||
|
|
||||||
# Set geometry & output configration
|
# Set geometry & output configration
|
||||||
param set-default CA_AIRFRAME 6
|
|
||||||
param set-default CA_R_REV 3
|
|
||||||
|
|
||||||
|
|
||||||
param set-default RBCLW_ADDRESS 128
|
param set-default RBCLW_ADDRESS 128
|
||||||
param set-default RBCLW_FUNC1 101
|
param set-default RBCLW_FUNC1 101
|
||||||
param set-default RBCLW_FUNC2 102
|
param set-default RBCLW_FUNC2 102
|
||||||
param set-default RBCLW_REV 1
|
param set-default RBCLW_REV 1 # reverse right wheels
|
||||||
|
|
|
||||||
|
|
@ -9,8 +9,6 @@
|
||||||
# Start the attitude and position estimator.
|
# Start the attitude and position estimator.
|
||||||
#
|
#
|
||||||
ekf2 start &
|
ekf2 start &
|
||||||
#attitude_estimator_q start
|
|
||||||
#local_position_estimator start
|
|
||||||
|
|
||||||
#
|
#
|
||||||
# Start Control Allocator
|
# Start Control Allocator
|
||||||
|
|
@ -22,7 +20,6 @@ control_allocator start
|
||||||
#
|
#
|
||||||
rover_pos_control start
|
rover_pos_control start
|
||||||
|
|
||||||
|
|
||||||
#
|
#
|
||||||
# Start Land Detector.
|
# Start Land Detector.
|
||||||
#
|
#
|
||||||
|
|
|
||||||
|
|
@ -0,0 +1,11 @@
|
||||||
|
#!/bin/sh
|
||||||
|
# Standard apps for a differential drive rover.
|
||||||
|
|
||||||
|
# Start the attitude and position estimator.
|
||||||
|
ekf2 start &
|
||||||
|
|
||||||
|
# Start rover differential drive controller.
|
||||||
|
rover_control start
|
||||||
|
|
||||||
|
# Start Land Detector.
|
||||||
|
land_detector start rover
|
||||||
|
|
@ -0,0 +1,11 @@
|
||||||
|
#!/bin/sh
|
||||||
|
# Differential rover parameters.
|
||||||
|
|
||||||
|
set VEHICLE_TYPE rover_differential
|
||||||
|
|
||||||
|
param set-default MAV_TYPE 10 # MAV_TYPE_GROUND_ROVER
|
||||||
|
|
||||||
|
param set-default CA_AIRFRAME 6 # Rover (Differential)
|
||||||
|
param set-default CA_R_REV 3 # Right and left motors reversible
|
||||||
|
|
||||||
|
param set-default EKF2_MAG_TYPE 1 # make sure magnetometer is fused even when not flying
|
||||||
|
|
@ -32,6 +32,15 @@ then
|
||||||
. ${R}etc/init.d/rc.rover_apps
|
. ${R}etc/init.d/rc.rover_apps
|
||||||
fi
|
fi
|
||||||
|
|
||||||
|
#
|
||||||
|
# Differential Rover setup.
|
||||||
|
#
|
||||||
|
if [ $VEHICLE_TYPE = rover_differential ]
|
||||||
|
then
|
||||||
|
# Start differential drive rover apps.
|
||||||
|
. ${R}etc/init.d/rc.rover_differential_apps
|
||||||
|
fi
|
||||||
|
|
||||||
#
|
#
|
||||||
# VTOL setup.
|
# VTOL setup.
|
||||||
#
|
#
|
||||||
|
|
|
||||||
|
|
@ -0,0 +1,20 @@
|
||||||
|
{
|
||||||
|
"configurations": [
|
||||||
|
{
|
||||||
|
"browse": {
|
||||||
|
"databaseFilename": "${default}",
|
||||||
|
"limitSymbolsToIncludedHeaders": false
|
||||||
|
},
|
||||||
|
"includePath": [
|
||||||
|
"/opt/ros/humble/include/**",
|
||||||
|
"/usr/include/**"
|
||||||
|
],
|
||||||
|
"name": "ROS",
|
||||||
|
"intelliSenseMode": "gcc-x64",
|
||||||
|
"compilerPath": "/usr/bin/gcc",
|
||||||
|
"cStandard": "gnu11",
|
||||||
|
"cppStandard": "c++14"
|
||||||
|
}
|
||||||
|
],
|
||||||
|
"version": 4
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,10 @@
|
||||||
|
{
|
||||||
|
"python.autoComplete.extraPaths": [
|
||||||
|
"/opt/ros/humble/lib/python3.10/site-packages",
|
||||||
|
"/opt/ros/humble/local/lib/python3.10/dist-packages"
|
||||||
|
],
|
||||||
|
"python.analysis.extraPaths": [
|
||||||
|
"/opt/ros/humble/lib/python3.10/site-packages",
|
||||||
|
"/opt/ros/humble/local/lib/python3.10/dist-packages"
|
||||||
|
]
|
||||||
|
}
|
||||||
|
|
@ -39,6 +39,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
|
||||||
CONFIG_MODULES_COMMANDER=y
|
CONFIG_MODULES_COMMANDER=y
|
||||||
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
||||||
CONFIG_MODULES_DATAMAN=y
|
CONFIG_MODULES_DATAMAN=y
|
||||||
|
CONFIG_MODULES_ROVER_CONTROL=y
|
||||||
CONFIG_MODULES_EKF2=y
|
CONFIG_MODULES_EKF2=y
|
||||||
CONFIG_MODULES_ESC_BATTERY=y
|
CONFIG_MODULES_ESC_BATTERY=y
|
||||||
CONFIG_MODULES_EVENTS=y
|
CONFIG_MODULES_EVENTS=y
|
||||||
|
|
|
||||||
|
|
@ -39,6 +39,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
|
||||||
CONFIG_MODULES_COMMANDER=y
|
CONFIG_MODULES_COMMANDER=y
|
||||||
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
||||||
CONFIG_MODULES_DATAMAN=y
|
CONFIG_MODULES_DATAMAN=y
|
||||||
|
CONFIG_MODULES_ROVER_CONTROL=y
|
||||||
CONFIG_MODULES_EKF2=y
|
CONFIG_MODULES_EKF2=y
|
||||||
CONFIG_MODULES_ESC_BATTERY=y
|
CONFIG_MODULES_ESC_BATTERY=y
|
||||||
CONFIG_MODULES_EVENTS=y
|
CONFIG_MODULES_EVENTS=y
|
||||||
|
|
|
||||||
|
|
@ -36,6 +36,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
|
||||||
CONFIG_MODULES_COMMANDER=y
|
CONFIG_MODULES_COMMANDER=y
|
||||||
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
||||||
CONFIG_MODULES_DATAMAN=y
|
CONFIG_MODULES_DATAMAN=y
|
||||||
|
CONFIG_MODULES_ROVER_CONTROL=y
|
||||||
CONFIG_MODULES_EKF2=y
|
CONFIG_MODULES_EKF2=y
|
||||||
CONFIG_MODULES_ESC_BATTERY=y
|
CONFIG_MODULES_ESC_BATTERY=y
|
||||||
CONFIG_MODULES_EVENTS=y
|
CONFIG_MODULES_EVENTS=y
|
||||||
|
|
|
||||||
|
|
@ -25,6 +25,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
|
||||||
CONFIG_MODULES_COMMANDER=y
|
CONFIG_MODULES_COMMANDER=y
|
||||||
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
||||||
CONFIG_MODULES_DATAMAN=y
|
CONFIG_MODULES_DATAMAN=y
|
||||||
|
CONFIG_MODULES_ROVER_CONTROL=y
|
||||||
CONFIG_MODULES_EKF2=y
|
CONFIG_MODULES_EKF2=y
|
||||||
CONFIG_MODULES_ESC_BATTERY=y
|
CONFIG_MODULES_ESC_BATTERY=y
|
||||||
CONFIG_MODULES_EVENTS=y
|
CONFIG_MODULES_EVENTS=y
|
||||||
|
|
|
||||||
|
|
@ -43,6 +43,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
|
||||||
CONFIG_MODULES_COMMANDER=y
|
CONFIG_MODULES_COMMANDER=y
|
||||||
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
||||||
CONFIG_MODULES_DATAMAN=y
|
CONFIG_MODULES_DATAMAN=y
|
||||||
|
CONFIG_MODULES_ROVER_CONTROL=y
|
||||||
CONFIG_MODULES_EKF2=y
|
CONFIG_MODULES_EKF2=y
|
||||||
CONFIG_MODULES_ESC_BATTERY=y
|
CONFIG_MODULES_ESC_BATTERY=y
|
||||||
CONFIG_MODULES_EVENTS=y
|
CONFIG_MODULES_EVENTS=y
|
||||||
|
|
|
||||||
|
|
@ -44,6 +44,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
|
||||||
CONFIG_MODULES_COMMANDER=y
|
CONFIG_MODULES_COMMANDER=y
|
||||||
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
||||||
CONFIG_MODULES_DATAMAN=y
|
CONFIG_MODULES_DATAMAN=y
|
||||||
|
CONFIG_MODULES_ROVER_CONTROL=y
|
||||||
CONFIG_MODULES_EKF2=y
|
CONFIG_MODULES_EKF2=y
|
||||||
CONFIG_MODULES_ESC_BATTERY=y
|
CONFIG_MODULES_ESC_BATTERY=y
|
||||||
CONFIG_MODULES_EVENTS=y
|
CONFIG_MODULES_EVENTS=y
|
||||||
|
|
|
||||||
|
|
@ -39,6 +39,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
|
||||||
CONFIG_MODULES_COMMANDER=y
|
CONFIG_MODULES_COMMANDER=y
|
||||||
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
||||||
CONFIG_MODULES_DATAMAN=y
|
CONFIG_MODULES_DATAMAN=y
|
||||||
|
CONFIG_MODULES_ROVER_CONTROL=y
|
||||||
CONFIG_MODULES_EKF2=y
|
CONFIG_MODULES_EKF2=y
|
||||||
CONFIG_MODULES_ESC_BATTERY=y
|
CONFIG_MODULES_ESC_BATTERY=y
|
||||||
CONFIG_MODULES_EVENTS=y
|
CONFIG_MODULES_EVENTS=y
|
||||||
|
|
|
||||||
|
|
@ -27,6 +27,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
|
||||||
CONFIG_MODULES_COMMANDER=y
|
CONFIG_MODULES_COMMANDER=y
|
||||||
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
||||||
CONFIG_MODULES_DATAMAN=y
|
CONFIG_MODULES_DATAMAN=y
|
||||||
|
CONFIG_MODULES_ROVER_CONTROL=y
|
||||||
CONFIG_MODULES_EKF2=y
|
CONFIG_MODULES_EKF2=y
|
||||||
CONFIG_MODULES_ESC_BATTERY=y
|
CONFIG_MODULES_ESC_BATTERY=y
|
||||||
CONFIG_MODULES_EVENTS=y
|
CONFIG_MODULES_EVENTS=y
|
||||||
|
|
|
||||||
|
|
@ -37,6 +37,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
|
||||||
CONFIG_MODULES_COMMANDER=y
|
CONFIG_MODULES_COMMANDER=y
|
||||||
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
||||||
CONFIG_MODULES_DATAMAN=y
|
CONFIG_MODULES_DATAMAN=y
|
||||||
|
CONFIG_MODULES_ROVER_CONTROL=y
|
||||||
CONFIG_MODULES_EKF2=y
|
CONFIG_MODULES_EKF2=y
|
||||||
CONFIG_MODULES_ESC_BATTERY=y
|
CONFIG_MODULES_ESC_BATTERY=y
|
||||||
CONFIG_MODULES_EVENTS=y
|
CONFIG_MODULES_EVENTS=y
|
||||||
|
|
|
||||||
|
|
@ -32,6 +32,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
|
||||||
CONFIG_MODULES_COMMANDER=y
|
CONFIG_MODULES_COMMANDER=y
|
||||||
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
||||||
CONFIG_MODULES_DATAMAN=y
|
CONFIG_MODULES_DATAMAN=y
|
||||||
|
CONFIG_MODULES_ROVER_CONTROL=y
|
||||||
CONFIG_MODULES_EKF2=y
|
CONFIG_MODULES_EKF2=y
|
||||||
# CONFIG_EKF2_AUXVEL is not set
|
# CONFIG_EKF2_AUXVEL is not set
|
||||||
# CONFIG_EKF2_BARO_COMPENSATION is not set
|
# CONFIG_EKF2_BARO_COMPENSATION is not set
|
||||||
|
|
|
||||||
|
|
@ -31,6 +31,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
|
||||||
CONFIG_MODULES_COMMANDER=y
|
CONFIG_MODULES_COMMANDER=y
|
||||||
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
||||||
CONFIG_MODULES_DATAMAN=y
|
CONFIG_MODULES_DATAMAN=y
|
||||||
|
CONFIG_MODULES_ROVER_CONTROL=y
|
||||||
CONFIG_MODULES_EKF2=y
|
CONFIG_MODULES_EKF2=y
|
||||||
# CONFIG_EKF2_AUXVEL is not set
|
# CONFIG_EKF2_AUXVEL is not set
|
||||||
# CONFIG_EKF2_BARO_COMPENSATION is not set
|
# CONFIG_EKF2_BARO_COMPENSATION is not set
|
||||||
|
|
|
||||||
|
|
@ -32,6 +32,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
|
||||||
CONFIG_MODULES_COMMANDER=y
|
CONFIG_MODULES_COMMANDER=y
|
||||||
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
||||||
CONFIG_MODULES_DATAMAN=y
|
CONFIG_MODULES_DATAMAN=y
|
||||||
|
CONFIG_MODULES_ROVER_CONTROL=y
|
||||||
CONFIG_MODULES_EKF2=y
|
CONFIG_MODULES_EKF2=y
|
||||||
# CONFIG_EKF2_AUXVEL is not set
|
# CONFIG_EKF2_AUXVEL is not set
|
||||||
# CONFIG_EKF2_BARO_COMPENSATION is not set
|
# CONFIG_EKF2_BARO_COMPENSATION is not set
|
||||||
|
|
|
||||||
|
|
@ -43,6 +43,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
|
||||||
CONFIG_MODULES_COMMANDER=y
|
CONFIG_MODULES_COMMANDER=y
|
||||||
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
||||||
CONFIG_MODULES_DATAMAN=y
|
CONFIG_MODULES_DATAMAN=y
|
||||||
|
CONFIG_MODULES_ROVER_CONTROL=y
|
||||||
CONFIG_MODULES_EKF2=y
|
CONFIG_MODULES_EKF2=y
|
||||||
CONFIG_MODULES_ESC_BATTERY=y
|
CONFIG_MODULES_ESC_BATTERY=y
|
||||||
CONFIG_MODULES_EVENTS=y
|
CONFIG_MODULES_EVENTS=y
|
||||||
|
|
|
||||||
|
|
@ -41,6 +41,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
|
||||||
CONFIG_MODULES_COMMANDER=y
|
CONFIG_MODULES_COMMANDER=y
|
||||||
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
||||||
CONFIG_MODULES_DATAMAN=y
|
CONFIG_MODULES_DATAMAN=y
|
||||||
|
CONFIG_MODULES_ROVER_CONTROL=y
|
||||||
CONFIG_MODULES_EKF2=y
|
CONFIG_MODULES_EKF2=y
|
||||||
CONFIG_MODULES_ESC_BATTERY=y
|
CONFIG_MODULES_ESC_BATTERY=y
|
||||||
CONFIG_MODULES_EVENTS=y
|
CONFIG_MODULES_EVENTS=y
|
||||||
|
|
|
||||||
|
|
@ -36,6 +36,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
|
||||||
CONFIG_MODULES_COMMANDER=y
|
CONFIG_MODULES_COMMANDER=y
|
||||||
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
||||||
CONFIG_MODULES_DATAMAN=y
|
CONFIG_MODULES_DATAMAN=y
|
||||||
|
CONFIG_MODULES_ROVER_CONTROL=y
|
||||||
CONFIG_MODULES_EKF2=y
|
CONFIG_MODULES_EKF2=y
|
||||||
CONFIG_MODULES_ESC_BATTERY=y
|
CONFIG_MODULES_ESC_BATTERY=y
|
||||||
CONFIG_MODULES_EVENTS=y
|
CONFIG_MODULES_EVENTS=y
|
||||||
|
|
|
||||||
|
|
@ -37,6 +37,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
|
||||||
CONFIG_MODULES_COMMANDER=y
|
CONFIG_MODULES_COMMANDER=y
|
||||||
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
||||||
CONFIG_MODULES_DATAMAN=y
|
CONFIG_MODULES_DATAMAN=y
|
||||||
|
CONFIG_MODULES_ROVER_CONTROL=y
|
||||||
CONFIG_MODULES_EKF2=y
|
CONFIG_MODULES_EKF2=y
|
||||||
CONFIG_MODULES_ESC_BATTERY=y
|
CONFIG_MODULES_ESC_BATTERY=y
|
||||||
CONFIG_MODULES_EVENTS=y
|
CONFIG_MODULES_EVENTS=y
|
||||||
|
|
|
||||||
|
|
@ -37,6 +37,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
|
||||||
CONFIG_MODULES_COMMANDER=y
|
CONFIG_MODULES_COMMANDER=y
|
||||||
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
||||||
CONFIG_MODULES_DATAMAN=y
|
CONFIG_MODULES_DATAMAN=y
|
||||||
|
CONFIG_MODULES_ROVER_CONTROL=y
|
||||||
CONFIG_MODULES_EKF2=y
|
CONFIG_MODULES_EKF2=y
|
||||||
CONFIG_MODULES_ESC_BATTERY=y
|
CONFIG_MODULES_ESC_BATTERY=y
|
||||||
CONFIG_MODULES_EVENTS=y
|
CONFIG_MODULES_EVENTS=y
|
||||||
|
|
|
||||||
|
|
@ -35,6 +35,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
|
||||||
CONFIG_MODULES_COMMANDER=y
|
CONFIG_MODULES_COMMANDER=y
|
||||||
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
||||||
CONFIG_MODULES_DATAMAN=y
|
CONFIG_MODULES_DATAMAN=y
|
||||||
|
CONFIG_MODULES_ROVER_CONTROL=y
|
||||||
CONFIG_MODULES_EKF2=y
|
CONFIG_MODULES_EKF2=y
|
||||||
CONFIG_MODULES_ESC_BATTERY=y
|
CONFIG_MODULES_ESC_BATTERY=y
|
||||||
CONFIG_MODULES_EVENTS=y
|
CONFIG_MODULES_EVENTS=y
|
||||||
|
|
|
||||||
|
|
@ -36,6 +36,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
|
||||||
CONFIG_MODULES_COMMANDER=y
|
CONFIG_MODULES_COMMANDER=y
|
||||||
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
||||||
CONFIG_MODULES_DATAMAN=y
|
CONFIG_MODULES_DATAMAN=y
|
||||||
|
CONFIG_MODULES_ROVER_CONTROL=y
|
||||||
CONFIG_MODULES_EKF2=y
|
CONFIG_MODULES_EKF2=y
|
||||||
CONFIG_MODULES_ESC_BATTERY=y
|
CONFIG_MODULES_ESC_BATTERY=y
|
||||||
CONFIG_MODULES_EVENTS=y
|
CONFIG_MODULES_EVENTS=y
|
||||||
|
|
|
||||||
|
|
@ -35,6 +35,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
|
||||||
CONFIG_MODULES_COMMANDER=y
|
CONFIG_MODULES_COMMANDER=y
|
||||||
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
||||||
CONFIG_MODULES_DATAMAN=y
|
CONFIG_MODULES_DATAMAN=y
|
||||||
|
CONFIG_MODULES_ROVER_CONTROL=y
|
||||||
CONFIG_MODULES_EKF2=y
|
CONFIG_MODULES_EKF2=y
|
||||||
CONFIG_MODULES_ESC_BATTERY=y
|
CONFIG_MODULES_ESC_BATTERY=y
|
||||||
CONFIG_MODULES_EVENTS=y
|
CONFIG_MODULES_EVENTS=y
|
||||||
|
|
|
||||||
|
|
@ -38,6 +38,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
|
||||||
CONFIG_MODULES_COMMANDER=y
|
CONFIG_MODULES_COMMANDER=y
|
||||||
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
||||||
CONFIG_MODULES_DATAMAN=y
|
CONFIG_MODULES_DATAMAN=y
|
||||||
|
CONFIG_MODULES_ROVER_CONTROL=y
|
||||||
CONFIG_MODULES_EKF2=y
|
CONFIG_MODULES_EKF2=y
|
||||||
CONFIG_MODULES_ESC_BATTERY=y
|
CONFIG_MODULES_ESC_BATTERY=y
|
||||||
CONFIG_MODULES_EVENTS=y
|
CONFIG_MODULES_EVENTS=y
|
||||||
|
|
|
||||||
|
|
@ -39,6 +39,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
|
||||||
CONFIG_MODULES_COMMANDER=y
|
CONFIG_MODULES_COMMANDER=y
|
||||||
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
||||||
CONFIG_MODULES_DATAMAN=y
|
CONFIG_MODULES_DATAMAN=y
|
||||||
|
CONFIG_MODULES_ROVER_CONTROL=y
|
||||||
CONFIG_MODULES_EKF2=y
|
CONFIG_MODULES_EKF2=y
|
||||||
CONFIG_MODULES_ESC_BATTERY=y
|
CONFIG_MODULES_ESC_BATTERY=y
|
||||||
CONFIG_MODULES_EVENTS=y
|
CONFIG_MODULES_EVENTS=y
|
||||||
|
|
|
||||||
|
|
@ -40,6 +40,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
|
||||||
CONFIG_MODULES_COMMANDER=y
|
CONFIG_MODULES_COMMANDER=y
|
||||||
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
||||||
CONFIG_MODULES_DATAMAN=y
|
CONFIG_MODULES_DATAMAN=y
|
||||||
|
CONFIG_MODULES_ROVER_CONTROL=y
|
||||||
CONFIG_MODULES_EKF2=y
|
CONFIG_MODULES_EKF2=y
|
||||||
CONFIG_MODULES_ESC_BATTERY=y
|
CONFIG_MODULES_ESC_BATTERY=y
|
||||||
CONFIG_MODULES_EVENTS=y
|
CONFIG_MODULES_EVENTS=y
|
||||||
|
|
|
||||||
|
|
@ -41,6 +41,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
|
||||||
CONFIG_MODULES_COMMANDER=y
|
CONFIG_MODULES_COMMANDER=y
|
||||||
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
||||||
CONFIG_MODULES_DATAMAN=y
|
CONFIG_MODULES_DATAMAN=y
|
||||||
|
CONFIG_MODULES_ROVER_CONTROL=y
|
||||||
CONFIG_MODULES_EKF2=y
|
CONFIG_MODULES_EKF2=y
|
||||||
CONFIG_MODULES_ESC_BATTERY=y
|
CONFIG_MODULES_ESC_BATTERY=y
|
||||||
CONFIG_MODULES_EVENTS=y
|
CONFIG_MODULES_EVENTS=y
|
||||||
|
|
|
||||||
|
|
@ -28,6 +28,7 @@ CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
|
||||||
CONFIG_MODULES_BATTERY_STATUS=y
|
CONFIG_MODULES_BATTERY_STATUS=y
|
||||||
CONFIG_MODULES_CAMERA_FEEDBACK=y
|
CONFIG_MODULES_CAMERA_FEEDBACK=y
|
||||||
CONFIG_MODULES_COMMANDER=y
|
CONFIG_MODULES_COMMANDER=y
|
||||||
|
CONFIG_MODULES_ROVER_CONTROL=y
|
||||||
CONFIG_MODULES_ESC_BATTERY=y
|
CONFIG_MODULES_ESC_BATTERY=y
|
||||||
CONFIG_MODULES_EVENTS=y
|
CONFIG_MODULES_EVENTS=y
|
||||||
CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
|
CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
|
||||||
|
|
|
||||||
|
|
@ -15,6 +15,7 @@ CONFIG_MODULES_AIRSPEED_SELECTOR=y
|
||||||
CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
|
CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
|
||||||
CONFIG_MODULES_CAMERA_FEEDBACK=y
|
CONFIG_MODULES_CAMERA_FEEDBACK=y
|
||||||
CONFIG_MODULES_COMMANDER=y
|
CONFIG_MODULES_COMMANDER=y
|
||||||
|
CONFIG_MODULES_ROVER_CONTROL=y
|
||||||
CONFIG_MODULES_ESC_BATTERY=y
|
CONFIG_MODULES_ESC_BATTERY=y
|
||||||
CONFIG_MODULES_EVENTS=y
|
CONFIG_MODULES_EVENTS=y
|
||||||
CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
|
CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
|
||||||
|
|
|
||||||
|
|
@ -20,6 +20,7 @@ CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
|
||||||
CONFIG_MODULES_BATTERY_STATUS=y
|
CONFIG_MODULES_BATTERY_STATUS=y
|
||||||
CONFIG_MODULES_CAMERA_FEEDBACK=y
|
CONFIG_MODULES_CAMERA_FEEDBACK=y
|
||||||
CONFIG_MODULES_COMMANDER=y
|
CONFIG_MODULES_COMMANDER=y
|
||||||
|
CONFIG_MODULES_ROVER_CONTROL=y
|
||||||
CONFIG_MODULES_ESC_BATTERY=y
|
CONFIG_MODULES_ESC_BATTERY=y
|
||||||
CONFIG_MODULES_EVENTS=y
|
CONFIG_MODULES_EVENTS=y
|
||||||
CONFIG_MODULES_FW_ATT_CONTROL=y
|
CONFIG_MODULES_FW_ATT_CONTROL=y
|
||||||
|
|
|
||||||
|
|
@ -1,3 +1,4 @@
|
||||||
|
CONFIG_MODULES_ROVER_CONTROL=y
|
||||||
CONFIG_MODULES_FLIGHT_MODE_MANAGER=n
|
CONFIG_MODULES_FLIGHT_MODE_MANAGER=n
|
||||||
CONFIG_MODULES_MC_ATT_CONTROL=n
|
CONFIG_MODULES_MC_ATT_CONTROL=n
|
||||||
CONFIG_MODULES_MC_POS_CONTROL=n
|
CONFIG_MODULES_MC_POS_CONTROL=n
|
||||||
|
|
|
||||||
|
|
@ -43,6 +43,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
|
||||||
CONFIG_MODULES_COMMANDER=y
|
CONFIG_MODULES_COMMANDER=y
|
||||||
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
||||||
CONFIG_MODULES_DATAMAN=y
|
CONFIG_MODULES_DATAMAN=y
|
||||||
|
CONFIG_MODULES_ROVER_CONTROL=y
|
||||||
CONFIG_MODULES_EKF2=y
|
CONFIG_MODULES_EKF2=y
|
||||||
CONFIG_MODULES_ESC_BATTERY=y
|
CONFIG_MODULES_ESC_BATTERY=y
|
||||||
CONFIG_MODULES_EVENTS=y
|
CONFIG_MODULES_EVENTS=y
|
||||||
|
|
|
||||||
|
|
@ -44,6 +44,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
|
||||||
CONFIG_MODULES_COMMANDER=y
|
CONFIG_MODULES_COMMANDER=y
|
||||||
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
||||||
CONFIG_MODULES_DATAMAN=y
|
CONFIG_MODULES_DATAMAN=y
|
||||||
|
CONFIG_MODULES_ROVER_CONTROL=y
|
||||||
CONFIG_MODULES_EKF2=y
|
CONFIG_MODULES_EKF2=y
|
||||||
CONFIG_MODULES_ESC_BATTERY=y
|
CONFIG_MODULES_ESC_BATTERY=y
|
||||||
CONFIG_MODULES_EVENTS=y
|
CONFIG_MODULES_EVENTS=y
|
||||||
|
|
|
||||||
|
|
@ -40,6 +40,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
|
||||||
CONFIG_MODULES_COMMANDER=y
|
CONFIG_MODULES_COMMANDER=y
|
||||||
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
||||||
CONFIG_MODULES_DATAMAN=y
|
CONFIG_MODULES_DATAMAN=y
|
||||||
|
CONFIG_MODULES_ROVER_CONTROL=y
|
||||||
CONFIG_MODULES_EKF2=y
|
CONFIG_MODULES_EKF2=y
|
||||||
CONFIG_MODULES_ESC_BATTERY=y
|
CONFIG_MODULES_ESC_BATTERY=y
|
||||||
CONFIG_MODULES_EVENTS=y
|
CONFIG_MODULES_EVENTS=y
|
||||||
|
|
|
||||||
|
|
@ -48,6 +48,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
|
||||||
CONFIG_MODULES_COMMANDER=y
|
CONFIG_MODULES_COMMANDER=y
|
||||||
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
||||||
CONFIG_MODULES_DATAMAN=y
|
CONFIG_MODULES_DATAMAN=y
|
||||||
|
CONFIG_MODULES_ROVER_CONTROL=y
|
||||||
CONFIG_MODULES_EKF2=y
|
CONFIG_MODULES_EKF2=y
|
||||||
CONFIG_MODULES_ESC_BATTERY=y
|
CONFIG_MODULES_ESC_BATTERY=y
|
||||||
CONFIG_MODULES_EVENTS=y
|
CONFIG_MODULES_EVENTS=y
|
||||||
|
|
|
||||||
|
|
@ -53,6 +53,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
|
||||||
CONFIG_MODULES_COMMANDER=y
|
CONFIG_MODULES_COMMANDER=y
|
||||||
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
||||||
CONFIG_MODULES_DATAMAN=y
|
CONFIG_MODULES_DATAMAN=y
|
||||||
|
CONFIG_MODULES_ROVER_CONTROL=y
|
||||||
CONFIG_MODULES_EKF2=y
|
CONFIG_MODULES_EKF2=y
|
||||||
CONFIG_MODULES_ESC_BATTERY=y
|
CONFIG_MODULES_ESC_BATTERY=y
|
||||||
CONFIG_MODULES_EVENTS=y
|
CONFIG_MODULES_EVENTS=y
|
||||||
|
|
|
||||||
|
|
@ -37,6 +37,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
|
||||||
CONFIG_MODULES_COMMANDER=y
|
CONFIG_MODULES_COMMANDER=y
|
||||||
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
||||||
CONFIG_MODULES_DATAMAN=y
|
CONFIG_MODULES_DATAMAN=y
|
||||||
|
CONFIG_MODULES_ROVER_CONTROL=y
|
||||||
CONFIG_MODULES_EKF2=y
|
CONFIG_MODULES_EKF2=y
|
||||||
CONFIG_MODULES_ESC_BATTERY=y
|
CONFIG_MODULES_ESC_BATTERY=y
|
||||||
CONFIG_MODULES_EVENTS=y
|
CONFIG_MODULES_EVENTS=y
|
||||||
|
|
|
||||||
|
|
@ -40,6 +40,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
|
||||||
CONFIG_MODULES_COMMANDER=y
|
CONFIG_MODULES_COMMANDER=y
|
||||||
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
||||||
CONFIG_MODULES_DATAMAN=y
|
CONFIG_MODULES_DATAMAN=y
|
||||||
|
CONFIG_MODULES_ROVER_CONTROL=y
|
||||||
CONFIG_MODULES_EKF2=y
|
CONFIG_MODULES_EKF2=y
|
||||||
CONFIG_MODULES_ESC_BATTERY=y
|
CONFIG_MODULES_ESC_BATTERY=y
|
||||||
CONFIG_MODULES_EVENTS=y
|
CONFIG_MODULES_EVENTS=y
|
||||||
|
|
|
||||||
|
|
@ -37,6 +37,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
|
||||||
CONFIG_MODULES_COMMANDER=y
|
CONFIG_MODULES_COMMANDER=y
|
||||||
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
||||||
CONFIG_MODULES_DATAMAN=y
|
CONFIG_MODULES_DATAMAN=y
|
||||||
|
CONFIG_MODULES_ROVER_CONTROL=y
|
||||||
CONFIG_MODULES_EKF2=y
|
CONFIG_MODULES_EKF2=y
|
||||||
CONFIG_MODULES_EVENTS=y
|
CONFIG_MODULES_EVENTS=y
|
||||||
CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
|
CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
|
||||||
|
|
|
||||||
|
|
@ -24,6 +24,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
|
||||||
CONFIG_MODULES_COMMANDER=y
|
CONFIG_MODULES_COMMANDER=y
|
||||||
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
||||||
CONFIG_MODULES_DATAMAN=y
|
CONFIG_MODULES_DATAMAN=y
|
||||||
|
CONFIG_MODULES_ROVER_CONTROL=y
|
||||||
CONFIG_MODULES_EKF2=y
|
CONFIG_MODULES_EKF2=y
|
||||||
CONFIG_MODULES_ESC_BATTERY=y
|
CONFIG_MODULES_ESC_BATTERY=y
|
||||||
CONFIG_MODULES_EVENTS=y
|
CONFIG_MODULES_EVENTS=y
|
||||||
|
|
|
||||||
|
|
@ -12,6 +12,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
|
||||||
CONFIG_MODULES_COMMANDER=y
|
CONFIG_MODULES_COMMANDER=y
|
||||||
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
||||||
CONFIG_MODULES_DATAMAN=y
|
CONFIG_MODULES_DATAMAN=y
|
||||||
|
CONFIG_MODULES_ROVER_CONTROL=y
|
||||||
CONFIG_MODULES_EKF2=y
|
CONFIG_MODULES_EKF2=y
|
||||||
CONFIG_EKF2_AUX_GLOBAL_POSITION=y
|
CONFIG_EKF2_AUX_GLOBAL_POSITION=y
|
||||||
CONFIG_MODULES_EVENTS=y
|
CONFIG_MODULES_EVENTS=y
|
||||||
|
|
|
||||||
|
|
@ -27,6 +27,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
|
||||||
CONFIG_MODULES_COMMANDER=y
|
CONFIG_MODULES_COMMANDER=y
|
||||||
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
||||||
CONFIG_MODULES_DATAMAN=y
|
CONFIG_MODULES_DATAMAN=y
|
||||||
|
CONFIG_MODULES_ROVER_CONTROL=y
|
||||||
CONFIG_MODULES_EKF2=y
|
CONFIG_MODULES_EKF2=y
|
||||||
CONFIG_MODULES_EVENTS=y
|
CONFIG_MODULES_EVENTS=y
|
||||||
CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
|
CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
|
||||||
|
|
|
||||||
|
|
@ -31,6 +31,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
|
||||||
CONFIG_MODULES_COMMANDER=y
|
CONFIG_MODULES_COMMANDER=y
|
||||||
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
||||||
CONFIG_MODULES_DATAMAN=y
|
CONFIG_MODULES_DATAMAN=y
|
||||||
|
CONFIG_MODULES_ROVER_CONTROL=y
|
||||||
CONFIG_MODULES_EKF2=y
|
CONFIG_MODULES_EKF2=y
|
||||||
CONFIG_MODULES_ESC_BATTERY=y
|
CONFIG_MODULES_ESC_BATTERY=y
|
||||||
CONFIG_MODULES_EVENTS=y
|
CONFIG_MODULES_EVENTS=y
|
||||||
|
|
|
||||||
|
|
@ -37,6 +37,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
|
||||||
CONFIG_MODULES_COMMANDER=y
|
CONFIG_MODULES_COMMANDER=y
|
||||||
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
||||||
CONFIG_MODULES_DATAMAN=y
|
CONFIG_MODULES_DATAMAN=y
|
||||||
|
CONFIG_MODULES_ROVER_CONTROL=y
|
||||||
CONFIG_MODULES_EKF2=y
|
CONFIG_MODULES_EKF2=y
|
||||||
CONFIG_MODULES_ESC_BATTERY=y
|
CONFIG_MODULES_ESC_BATTERY=y
|
||||||
CONFIG_MODULES_EVENTS=y
|
CONFIG_MODULES_EVENTS=y
|
||||||
|
|
|
||||||
|
|
@ -37,6 +37,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
|
||||||
CONFIG_MODULES_COMMANDER=y
|
CONFIG_MODULES_COMMANDER=y
|
||||||
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
||||||
CONFIG_MODULES_DATAMAN=y
|
CONFIG_MODULES_DATAMAN=y
|
||||||
|
CONFIG_MODULES_ROVER_CONTROL=y
|
||||||
CONFIG_MODULES_EKF2=y
|
CONFIG_MODULES_EKF2=y
|
||||||
CONFIG_MODULES_ESC_BATTERY=y
|
CONFIG_MODULES_ESC_BATTERY=y
|
||||||
CONFIG_MODULES_EVENTS=y
|
CONFIG_MODULES_EVENTS=y
|
||||||
|
|
|
||||||
|
|
@ -69,6 +69,7 @@ set(msg_files
|
||||||
DebugKeyValue.msg
|
DebugKeyValue.msg
|
||||||
DebugValue.msg
|
DebugValue.msg
|
||||||
DebugVect.msg
|
DebugVect.msg
|
||||||
|
DifferentialDriveSetpoint.msg
|
||||||
DifferentialPressure.msg
|
DifferentialPressure.msg
|
||||||
DistanceSensor.msg
|
DistanceSensor.msg
|
||||||
Ekf2Timestamps.msg
|
Ekf2Timestamps.msg
|
||||||
|
|
|
||||||
|
|
@ -0,0 +1,4 @@
|
||||||
|
uint64 timestamp # time since system start (microseconds)
|
||||||
|
|
||||||
|
float32 speed # [m/s] collective roll-off speed in body x-axis
|
||||||
|
float32 yaw_rate # [rad/s] yaw rate
|
||||||
|
|
@ -91,7 +91,7 @@ extern pthread_mutex_t px4_modules_mutex;
|
||||||
* static int custom_command(int argc, char *argv[])
|
* static int custom_command(int argc, char *argv[])
|
||||||
* {
|
* {
|
||||||
* // support for custom commands
|
* // support for custom commands
|
||||||
* // it none are supported, just do:
|
* // if none are supported, just do:
|
||||||
* return print_usage("unrecognized command");
|
* return print_usage("unrecognized command");
|
||||||
* }
|
* }
|
||||||
*
|
*
|
||||||
|
|
|
||||||
|
|
@ -75,7 +75,7 @@
|
||||||
* static int custom_command(int argc, char *argv[])
|
* static int custom_command(int argc, char *argv[])
|
||||||
* {
|
* {
|
||||||
* // support for custom commands
|
* // support for custom commands
|
||||||
* // it none are supported, just do:
|
* // if none are supported, just do:
|
||||||
* return print_usage("unrecognized command");
|
* return print_usage("unrecognized command");
|
||||||
* }
|
* }
|
||||||
*
|
*
|
||||||
|
|
|
||||||
|
|
@ -62,8 +62,6 @@ px4_add_library(ActuatorEffectiveness
|
||||||
ActuatorEffectivenessTailsitterVTOL.hpp
|
ActuatorEffectivenessTailsitterVTOL.hpp
|
||||||
ActuatorEffectivenessRoverAckermann.hpp
|
ActuatorEffectivenessRoverAckermann.hpp
|
||||||
ActuatorEffectivenessRoverAckermann.cpp
|
ActuatorEffectivenessRoverAckermann.cpp
|
||||||
ActuatorEffectivenessRoverDifferential.hpp
|
|
||||||
ActuatorEffectivenessRoverDifferential.cpp
|
|
||||||
)
|
)
|
||||||
|
|
||||||
target_compile_options(ActuatorEffectiveness PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
|
target_compile_options(ActuatorEffectiveness PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
|
||||||
|
|
|
||||||
|
|
@ -239,7 +239,7 @@ ControlAllocator::update_effectiveness_source()
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case EffectivenessSource::ROVER_DIFFERENTIAL:
|
case EffectivenessSource::ROVER_DIFFERENTIAL:
|
||||||
tmp = new ActuatorEffectivenessRoverDifferential();
|
// rover_control does allocation and publishes directly to actuator_motors topic
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case EffectivenessSource::FIXED_WING:
|
case EffectivenessSource::FIXED_WING:
|
||||||
|
|
|
||||||
|
|
@ -47,7 +47,6 @@
|
||||||
#include <ActuatorEffectivenessTiltrotorVTOL.hpp>
|
#include <ActuatorEffectivenessTiltrotorVTOL.hpp>
|
||||||
#include <ActuatorEffectivenessTailsitterVTOL.hpp>
|
#include <ActuatorEffectivenessTailsitterVTOL.hpp>
|
||||||
#include <ActuatorEffectivenessRoverAckermann.hpp>
|
#include <ActuatorEffectivenessRoverAckermann.hpp>
|
||||||
#include <ActuatorEffectivenessRoverDifferential.hpp>
|
|
||||||
#include <ActuatorEffectivenessFixedWing.hpp>
|
#include <ActuatorEffectivenessFixedWing.hpp>
|
||||||
#include <ActuatorEffectivenessMCTilt.hpp>
|
#include <ActuatorEffectivenessMCTilt.hpp>
|
||||||
#include <ActuatorEffectivenessCustom.hpp>
|
#include <ActuatorEffectivenessCustom.hpp>
|
||||||
|
|
|
||||||
|
|
@ -57,6 +57,7 @@ void LoggedTopics::add_default_topics()
|
||||||
add_topic("commander_state");
|
add_topic("commander_state");
|
||||||
add_topic("config_overrides");
|
add_topic("config_overrides");
|
||||||
add_topic("cpuload");
|
add_topic("cpuload");
|
||||||
|
add_optional_topic("differential_drive_setpoint", 100);
|
||||||
add_optional_topic("external_ins_attitude");
|
add_optional_topic("external_ins_attitude");
|
||||||
add_optional_topic("external_ins_global_position");
|
add_optional_topic("external_ins_global_position");
|
||||||
add_optional_topic("external_ins_local_position");
|
add_optional_topic("external_ins_local_position");
|
||||||
|
|
|
||||||
|
|
@ -0,0 +1,49 @@
|
||||||
|
############################################################################
|
||||||
|
#
|
||||||
|
# Copyright (c) 2023 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.
|
||||||
|
#
|
||||||
|
############################################################################
|
||||||
|
|
||||||
|
add_subdirectory(DifferentialDriveKinematics)
|
||||||
|
|
||||||
|
px4_add_module(
|
||||||
|
MODULE modules__rover_control
|
||||||
|
MAIN rover_control
|
||||||
|
SRCS
|
||||||
|
DifferentialDriveControl.cpp
|
||||||
|
DifferentialDriveControl.hpp
|
||||||
|
RoverControl.cpp
|
||||||
|
RoverControl.hpp
|
||||||
|
DEPENDS
|
||||||
|
DifferentialDriveKinematics
|
||||||
|
px4_work_queue
|
||||||
|
MODULE_CONFIG
|
||||||
|
module.yaml
|
||||||
|
)
|
||||||
|
|
@ -0,0 +1,188 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (c) 2023 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.
|
||||||
|
*
|
||||||
|
****************************************************************************/
|
||||||
|
|
||||||
|
#include "DifferentialDriveControl.hpp"
|
||||||
|
|
||||||
|
using namespace time_literals;
|
||||||
|
using namespace matrix;
|
||||||
|
namespace differential_drive_control
|
||||||
|
{
|
||||||
|
|
||||||
|
DifferentialDriveControl::DifferentialDriveControl(ModuleParams *parent) :
|
||||||
|
ModuleParams(parent),
|
||||||
|
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl)
|
||||||
|
{
|
||||||
|
updateParams();
|
||||||
|
}
|
||||||
|
|
||||||
|
bool DifferentialDriveControl::init()
|
||||||
|
{
|
||||||
|
// ScheduleOnInterval(10_ms); // 100 Hz
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
void DifferentialDriveControl::updateParams()
|
||||||
|
{
|
||||||
|
ModuleParams::updateParams();
|
||||||
|
_max_speed = _param_rdd_max_wheel_speed.get() * _param_rdd_wheel_radius.get();
|
||||||
|
_max_angular_velocity = _max_speed / (_param_rdd_wheel_base.get() / 2.f);
|
||||||
|
|
||||||
|
_differential_drive_kinematics.setWheelBase(_param_rdd_wheel_base.get());
|
||||||
|
_differential_drive_kinematics.setMaxSpeed(_max_speed);
|
||||||
|
_differential_drive_kinematics.setMaxAngularVelocity(_max_angular_velocity);
|
||||||
|
}
|
||||||
|
|
||||||
|
void DifferentialDriveControl::Run()
|
||||||
|
{
|
||||||
|
if (should_exit()) {
|
||||||
|
ScheduleClear();
|
||||||
|
exit_and_cleanup();
|
||||||
|
}
|
||||||
|
|
||||||
|
hrt_abstime now = hrt_absolute_time();
|
||||||
|
|
||||||
|
if (_parameter_update_sub.updated()) {
|
||||||
|
parameter_update_s pupdate;
|
||||||
|
_parameter_update_sub.copy(&pupdate);
|
||||||
|
|
||||||
|
updateParams();
|
||||||
|
}
|
||||||
|
|
||||||
|
if (_vehicle_control_mode_sub.updated()) {
|
||||||
|
vehicle_control_mode_s vehicle_control_mode;
|
||||||
|
|
||||||
|
if (_vehicle_control_mode_sub.copy(&vehicle_control_mode)) {
|
||||||
|
_armed = vehicle_control_mode.flag_armed;
|
||||||
|
_manual_driving = vehicle_control_mode.flag_control_manual_enabled; // change this when more modes are supported
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (_manual_driving) {
|
||||||
|
// Manual mode
|
||||||
|
// directly produce setpoints from the manual control setpoint (joystick)
|
||||||
|
if (_manual_control_setpoint_sub.updated()) {
|
||||||
|
manual_control_setpoint_s manual_control_setpoint{};
|
||||||
|
|
||||||
|
if (_manual_control_setpoint_sub.copy(&manual_control_setpoint)) {
|
||||||
|
_differential_drive_setpoint.timestamp = now;
|
||||||
|
_differential_drive_setpoint.speed = manual_control_setpoint.throttle * _param_rdd_speed_scale.get() * _max_speed;
|
||||||
|
_differential_drive_setpoint.yaw_rate = manual_control_setpoint.roll * _param_rdd_ang_velocity_scale.get() *
|
||||||
|
_max_angular_velocity;
|
||||||
|
_differential_drive_setpoint_pub.publish(_differential_drive_setpoint);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
_differential_drive_setpoint_sub.update(&_differential_drive_setpoint);
|
||||||
|
|
||||||
|
// publish data to actuator_motors (output module)
|
||||||
|
// get the wheel speeds from the inverse kinematics class (DifferentialDriveKinematics)
|
||||||
|
Vector2f wheel_speeds = _differential_drive_kinematics.computeInverseKinematics(
|
||||||
|
_differential_drive_setpoint.speed,
|
||||||
|
_differential_drive_setpoint.yaw_rate);
|
||||||
|
|
||||||
|
// Check if max_angular_wheel_speed is zero
|
||||||
|
const bool setpoint_timeout = (_differential_drive_setpoint.timestamp + 100_ms) < now;
|
||||||
|
const bool valid_max_speed = _param_rdd_speed_scale.get() > FLT_EPSILON;
|
||||||
|
|
||||||
|
if (!_armed || setpoint_timeout || !valid_max_speed) {
|
||||||
|
wheel_speeds = {}; // stop
|
||||||
|
}
|
||||||
|
|
||||||
|
wheel_speeds = matrix::constrain(wheel_speeds, -1.f, 1.f);
|
||||||
|
|
||||||
|
actuator_motors_s actuator_motors{};
|
||||||
|
actuator_motors.reversible_flags = _param_r_rev.get(); // should be 3 see rc.rover_differential_defaults
|
||||||
|
wheel_speeds.copyTo(actuator_motors.control);
|
||||||
|
actuator_motors.timestamp = now;
|
||||||
|
_actuator_motors_pub.publish(actuator_motors);
|
||||||
|
}
|
||||||
|
|
||||||
|
void DifferentialDriveControl::Update()
|
||||||
|
{
|
||||||
|
Run();
|
||||||
|
}
|
||||||
|
|
||||||
|
int DifferentialDriveControl::task_spawn(int argc, char *argv[])
|
||||||
|
{
|
||||||
|
// DifferentialDriveControl *instance = new DifferentialDriveControl();
|
||||||
|
|
||||||
|
// if (instance) {
|
||||||
|
// _object.store(instance);
|
||||||
|
// _task_id = task_id_is_work_queue;
|
||||||
|
|
||||||
|
// if (instance->init()) {
|
||||||
|
// return PX4_OK;
|
||||||
|
// }
|
||||||
|
|
||||||
|
// } else {
|
||||||
|
// PX4_ERR("alloc failed");
|
||||||
|
// }
|
||||||
|
|
||||||
|
// delete instance;
|
||||||
|
// _object.store(nullptr);
|
||||||
|
// _task_id = -1;
|
||||||
|
|
||||||
|
// return PX4_ERROR;
|
||||||
|
return PX4_OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
int DifferentialDriveControl::custom_command(int argc, char *argv[])
|
||||||
|
{
|
||||||
|
return print_usage("unknown command");
|
||||||
|
}
|
||||||
|
|
||||||
|
int DifferentialDriveControl::print_usage(const char *reason)
|
||||||
|
{
|
||||||
|
if (reason) {
|
||||||
|
PX4_ERR("%s\n", reason);
|
||||||
|
}
|
||||||
|
|
||||||
|
PRINT_MODULE_DESCRIPTION(
|
||||||
|
R"DESCR_STR(
|
||||||
|
### Description
|
||||||
|
Rover Differential Drive controller.
|
||||||
|
)DESCR_STR");
|
||||||
|
|
||||||
|
PRINT_MODULE_USAGE_NAME("differential_drive_control", "controller");
|
||||||
|
PRINT_MODULE_USAGE_COMMAND("start");
|
||||||
|
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
extern "C" __EXPORT int differential_drive_control_main(int argc, char *argv[])
|
||||||
|
{
|
||||||
|
return DifferentialDriveControl::main(argc, argv);
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace differential_drive_control
|
||||||
|
|
@ -0,0 +1,118 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (c) 2023 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.
|
||||||
|
*
|
||||||
|
****************************************************************************/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
// PX4 includes
|
||||||
|
#include <px4_platform_common/px4_config.h>
|
||||||
|
#include <px4_platform_common/defines.h>
|
||||||
|
#include <px4_platform_common/module.h>
|
||||||
|
#include <px4_platform_common/module_params.h>
|
||||||
|
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
|
||||||
|
|
||||||
|
// uORB includes
|
||||||
|
#include <uORB/topics/actuator_outputs.h>
|
||||||
|
#include <uORB/topics/actuator_motors.h>
|
||||||
|
#include <uORB/PublicationMulti.hpp>
|
||||||
|
#include <uORB/topics/manual_control_setpoint.h>
|
||||||
|
#include <uORB/topics/parameter_update.h>
|
||||||
|
#include <uORB/topics/vehicle_status.h>
|
||||||
|
#include <uORB/topics/vehicle_control_mode.h>
|
||||||
|
#include <uORB/Publication.hpp>
|
||||||
|
#include <uORB/Subscription.hpp>
|
||||||
|
#include <uORB/SubscriptionMultiArray.hpp>
|
||||||
|
#include <uORB/topics/differential_drive_setpoint.h>
|
||||||
|
|
||||||
|
// Standard library includes
|
||||||
|
#include <math.h>
|
||||||
|
|
||||||
|
// Local includes
|
||||||
|
#include <DifferentialDriveKinematics.hpp>
|
||||||
|
|
||||||
|
namespace differential_drive_control
|
||||||
|
{
|
||||||
|
|
||||||
|
class DifferentialDriveControl : public ModuleBase<DifferentialDriveControl>, public ModuleParams,
|
||||||
|
public px4::ScheduledWorkItem
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
DifferentialDriveControl(ModuleParams *parent);
|
||||||
|
~DifferentialDriveControl() override = default;
|
||||||
|
|
||||||
|
/** @see ModuleBase */
|
||||||
|
static int task_spawn(int argc, char *argv[]);
|
||||||
|
|
||||||
|
/** @see ModuleBase */
|
||||||
|
static int custom_command(int argc, char *argv[]);
|
||||||
|
|
||||||
|
/** @see ModuleBase */
|
||||||
|
static int print_usage(const char *reason = nullptr);
|
||||||
|
|
||||||
|
bool init();
|
||||||
|
|
||||||
|
void Update();
|
||||||
|
|
||||||
|
protected:
|
||||||
|
void updateParams() override;
|
||||||
|
|
||||||
|
private:
|
||||||
|
void Run() override;
|
||||||
|
|
||||||
|
uORB::Subscription _differential_drive_setpoint_sub{ORB_ID(differential_drive_setpoint)};
|
||||||
|
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
|
||||||
|
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
|
||||||
|
uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
|
||||||
|
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
|
||||||
|
|
||||||
|
uORB::PublicationMulti<actuator_motors_s> _actuator_motors_pub{ORB_ID(actuator_motors)};
|
||||||
|
uORB::Publication<differential_drive_setpoint_s> _differential_drive_setpoint_pub{ORB_ID(differential_drive_setpoint)};
|
||||||
|
|
||||||
|
differential_drive_setpoint_s _differential_drive_setpoint{};
|
||||||
|
DifferentialDriveKinematics _differential_drive_kinematics{};
|
||||||
|
|
||||||
|
bool _armed = false;
|
||||||
|
bool _manual_driving = false;
|
||||||
|
float _max_speed{0.f};
|
||||||
|
float _max_angular_velocity{0.f};
|
||||||
|
|
||||||
|
DEFINE_PARAMETERS(
|
||||||
|
(ParamFloat<px4::params::RDD_SPEED_SCALE>) _param_rdd_speed_scale,
|
||||||
|
(ParamFloat<px4::params::RDD_ANG_SCALE>) _param_rdd_ang_velocity_scale,
|
||||||
|
(ParamFloat<px4::params::RDD_WHL_SPEED>) _param_rdd_max_wheel_speed,
|
||||||
|
(ParamFloat<px4::params::RDD_WHEEL_BASE>) _param_rdd_wheel_base,
|
||||||
|
(ParamFloat<px4::params::RDD_WHEEL_RADIUS>) _param_rdd_wheel_radius,
|
||||||
|
(ParamInt<px4::params::CA_R_REV>) _param_r_rev
|
||||||
|
)
|
||||||
|
};
|
||||||
|
|
||||||
|
} // namespace differential_drive_control
|
||||||
|
|
@ -0,0 +1,40 @@
|
||||||
|
############################################################################
|
||||||
|
#
|
||||||
|
# Copyright (c) 2023 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_library(DifferentialDriveKinematics
|
||||||
|
DifferentialDriveKinematics.cpp
|
||||||
|
)
|
||||||
|
|
||||||
|
target_include_directories(DifferentialDriveKinematics PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
||||||
|
|
||||||
|
px4_add_unit_gtest(SRC DifferentialDriveKinematicsTest.cpp LINKLIBS DifferentialDriveKinematics)
|
||||||
|
|
@ -1,6 +1,6 @@
|
||||||
/****************************************************************************
|
/****************************************************************************
|
||||||
*
|
*
|
||||||
* Copyright (c) 2021 PX4 Development Team. All rights reserved.
|
* Copyright (C) 2023 PX4 Development Team. All rights reserved.
|
||||||
*
|
*
|
||||||
* Redistribution and use in source and binary forms, with or without
|
* Redistribution and use in source and binary forms, with or without
|
||||||
* modification, are permitted provided that the following conditions
|
* modification, are permitted provided that the following conditions
|
||||||
|
|
@ -31,29 +31,37 @@
|
||||||
*
|
*
|
||||||
****************************************************************************/
|
****************************************************************************/
|
||||||
|
|
||||||
#include "ActuatorEffectivenessRoverDifferential.hpp"
|
#include "DifferentialDriveKinematics.hpp"
|
||||||
#include <ControlAllocation/ControlAllocation.hpp>
|
|
||||||
|
#include <mathlib/mathlib.h>
|
||||||
|
|
||||||
using namespace matrix;
|
using namespace matrix;
|
||||||
|
|
||||||
bool
|
matrix::Vector2f DifferentialDriveKinematics::computeInverseKinematics(float linear_velocity_x, float yaw_rate)
|
||||||
ActuatorEffectivenessRoverDifferential::getEffectivenessMatrix(Configuration &configuration,
|
|
||||||
EffectivenessUpdateReason external_update)
|
|
||||||
{
|
{
|
||||||
if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) {
|
if (_max_speed < FLT_EPSILON) {
|
||||||
return false;
|
return Vector2f();
|
||||||
}
|
}
|
||||||
|
|
||||||
configuration.addActuator(ActuatorType::MOTORS, Vector3f{0.f, 0.f, 0.5f}, Vector3f{0.5f, 0.f, 0.f});
|
linear_velocity_x = math::constrain(linear_velocity_x, -_max_speed, _max_speed);
|
||||||
configuration.addActuator(ActuatorType::MOTORS, Vector3f{0.f, 0.f, -0.5f}, Vector3f{0.5f, 0.f, 0.f});
|
yaw_rate = math::constrain(yaw_rate, -_max_angular_velocity, _max_angular_velocity);
|
||||||
_motors_mask = (1u << 0) | (1u << 1);
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
|
|
||||||
void ActuatorEffectivenessRoverDifferential::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
|
const float rotational_velocity = (_wheel_base / 2.f) * yaw_rate;
|
||||||
int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
|
float combined_velocity = fabsf(linear_velocity_x) + fabsf(rotational_velocity);
|
||||||
const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
|
|
||||||
{
|
|
||||||
stopMaskedMotorsWithZeroThrust(_motors_mask, actuator_sp);
|
|
||||||
}
|
|
||||||
|
|
||||||
|
// Compute an initial gain
|
||||||
|
float gain = 1.0f;
|
||||||
|
|
||||||
|
if (combined_velocity > _max_speed) {
|
||||||
|
float excess_velocity = fabsf(combined_velocity - _max_speed);
|
||||||
|
float adjusted_linear_velocity = fabsf(linear_velocity_x) - excess_velocity;
|
||||||
|
gain = adjusted_linear_velocity / fabsf(linear_velocity_x);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Apply the gain
|
||||||
|
linear_velocity_x *= gain;
|
||||||
|
|
||||||
|
// Calculate the left and right wheel speeds
|
||||||
|
return Vector2f(linear_velocity_x - rotational_velocity,
|
||||||
|
linear_velocity_x + rotational_velocity) / _max_speed;
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,84 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (c) 2023 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.
|
||||||
|
*
|
||||||
|
****************************************************************************/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <matrix/matrix/math.hpp>
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Differential Drive Kinematics class for computing the kinematics of a differential drive robot.
|
||||||
|
*
|
||||||
|
* This class provides functions to set the wheel base and radius, and to compute the inverse kinematics
|
||||||
|
* given linear velocity and yaw rate.
|
||||||
|
*/
|
||||||
|
class DifferentialDriveKinematics
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
DifferentialDriveKinematics() = default;
|
||||||
|
~DifferentialDriveKinematics() = default;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Sets the wheel base of the robot.
|
||||||
|
*
|
||||||
|
* @param wheel_base The distance between the centers of the wheels.
|
||||||
|
*/
|
||||||
|
void setWheelBase(const float wheel_base) { _wheel_base = wheel_base; };
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Sets the maximum speed of the robot.
|
||||||
|
*
|
||||||
|
* @param max_speed The maximum speed of the robot.
|
||||||
|
*/
|
||||||
|
void setMaxSpeed(const float max_speed) { _max_speed = max_speed; };
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Sets the maximum angular speed of the robot.
|
||||||
|
*
|
||||||
|
* @param max_angular_speed The maximum angular speed of the robot.
|
||||||
|
*/
|
||||||
|
void setMaxAngularVelocity(const float max_angular_velocity) { _max_angular_velocity = max_angular_velocity; };
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Computes the inverse kinematics for differential drive.
|
||||||
|
*
|
||||||
|
* @param linear_velocity_x Linear velocity along the x-axis.
|
||||||
|
* @param yaw_rate Yaw rate of the robot.
|
||||||
|
* @return matrix::Vector2f Motor velocities for the right and left motors.
|
||||||
|
*/
|
||||||
|
matrix::Vector2f computeInverseKinematics(float linear_velocity_x, float yaw_rate);
|
||||||
|
|
||||||
|
private:
|
||||||
|
float _wheel_base{0.f};
|
||||||
|
float _max_speed{0.f};
|
||||||
|
float _max_angular_velocity{0.f};
|
||||||
|
};
|
||||||
|
|
@ -0,0 +1,174 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (C) 2023 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.
|
||||||
|
*
|
||||||
|
****************************************************************************/
|
||||||
|
|
||||||
|
#include <gtest/gtest.h>
|
||||||
|
#include "DifferentialDriveKinematics.hpp"
|
||||||
|
#include <mathlib/math/Functions.hpp>
|
||||||
|
|
||||||
|
using namespace matrix;
|
||||||
|
|
||||||
|
TEST(DifferentialDriveKinematicsTest, AllZeroInputCase)
|
||||||
|
{
|
||||||
|
DifferentialDriveKinematics kinematics;
|
||||||
|
kinematics.setWheelBase(1.f);
|
||||||
|
kinematics.setMaxSpeed(10.f);
|
||||||
|
kinematics.setMaxAngularVelocity(10.f);
|
||||||
|
|
||||||
|
// Test with zero linear velocity and zero yaw rate (stationary vehicle)
|
||||||
|
EXPECT_EQ(kinematics.computeInverseKinematics(0.f, 0.f), Vector2f());
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
TEST(DifferentialDriveKinematicsTest, InvalidParameterCase)
|
||||||
|
{
|
||||||
|
DifferentialDriveKinematics kinematics;
|
||||||
|
kinematics.setWheelBase(0.f);
|
||||||
|
kinematics.setMaxSpeed(10.f);
|
||||||
|
kinematics.setMaxAngularVelocity(10.f);
|
||||||
|
|
||||||
|
// Test with invalid parameters (zero wheel base and wheel radius)
|
||||||
|
EXPECT_EQ(kinematics.computeInverseKinematics(0.f, .1f), Vector2f());
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
TEST(DifferentialDriveKinematicsTest, UnitCase)
|
||||||
|
{
|
||||||
|
DifferentialDriveKinematics kinematics;
|
||||||
|
kinematics.setWheelBase(1.f);
|
||||||
|
kinematics.setMaxSpeed(10.f);
|
||||||
|
kinematics.setMaxAngularVelocity(10.f);
|
||||||
|
|
||||||
|
// Test with unit values for linear velocity and yaw rate
|
||||||
|
EXPECT_EQ(kinematics.computeInverseKinematics(1.f, 1.f), Vector2f(0.05f, 0.15f));
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
TEST(DifferentialDriveKinematicsTest, UnitSaturationCase)
|
||||||
|
{
|
||||||
|
DifferentialDriveKinematics kinematics;
|
||||||
|
kinematics.setWheelBase(1.f);
|
||||||
|
kinematics.setMaxSpeed(1.f);
|
||||||
|
kinematics.setMaxAngularVelocity(1.f);
|
||||||
|
|
||||||
|
// Test with unit values for linear velocity and yaw rate, but with max speed that requires saturation
|
||||||
|
EXPECT_EQ(kinematics.computeInverseKinematics(1.f, 1.f), Vector2f(0, 1));
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
TEST(DifferentialDriveKinematicsTest, OppositeUnitSaturationCase)
|
||||||
|
{
|
||||||
|
DifferentialDriveKinematics kinematics;
|
||||||
|
kinematics.setWheelBase(1.f);
|
||||||
|
kinematics.setMaxSpeed(1.f);
|
||||||
|
kinematics.setMaxAngularVelocity(1.f);
|
||||||
|
|
||||||
|
// Negative linear velocity for backward motion and positive yaw rate for turning right
|
||||||
|
EXPECT_EQ(kinematics.computeInverseKinematics(-1.f, 1.f), Vector2f(-1, 0));
|
||||||
|
}
|
||||||
|
|
||||||
|
TEST(DifferentialDriveKinematicsTest, RandomCase)
|
||||||
|
{
|
||||||
|
DifferentialDriveKinematics kinematics;
|
||||||
|
kinematics.setWheelBase(2.f);
|
||||||
|
kinematics.setMaxSpeed(1.f);
|
||||||
|
kinematics.setMaxAngularVelocity(1.f);
|
||||||
|
|
||||||
|
// Negative linear velocity for backward motion and positive yaw rate for turning right
|
||||||
|
EXPECT_EQ(kinematics.computeInverseKinematics(0.5f, 0.7f), Vector2f(-0.4f, 1.0f));
|
||||||
|
}
|
||||||
|
|
||||||
|
TEST(DifferentialDriveKinematicsTest, RotateInPlaceCase)
|
||||||
|
{
|
||||||
|
DifferentialDriveKinematics kinematics;
|
||||||
|
kinematics.setWheelBase(1.f);
|
||||||
|
kinematics.setMaxSpeed(1.f);
|
||||||
|
kinematics.setMaxAngularVelocity(1.f);
|
||||||
|
|
||||||
|
// Test rotating in place (zero linear velocity, non-zero yaw rate)
|
||||||
|
EXPECT_EQ(kinematics.computeInverseKinematics(0.f, 1.f), Vector2f(-0.5f, 0.5f));
|
||||||
|
}
|
||||||
|
|
||||||
|
TEST(DifferentialDriveKinematicsTest, StraightMovementCase)
|
||||||
|
{
|
||||||
|
DifferentialDriveKinematics kinematics;
|
||||||
|
kinematics.setWheelBase(1.f);
|
||||||
|
kinematics.setMaxSpeed(1.f);
|
||||||
|
kinematics.setMaxAngularVelocity(1.f);
|
||||||
|
|
||||||
|
// Test moving straight (non-zero linear velocity, zero yaw rate)
|
||||||
|
EXPECT_EQ(kinematics.computeInverseKinematics(1.f, 0.f), Vector2f(1.f, 1.f));
|
||||||
|
}
|
||||||
|
|
||||||
|
TEST(DifferentialDriveKinematicsTest, MinInputValuesCase)
|
||||||
|
{
|
||||||
|
DifferentialDriveKinematics kinematics;
|
||||||
|
kinematics.setWheelBase(FLT_MIN);
|
||||||
|
kinematics.setMaxSpeed(FLT_MIN);
|
||||||
|
kinematics.setMaxAngularVelocity(FLT_MIN);
|
||||||
|
|
||||||
|
// Test with minimum possible input values
|
||||||
|
EXPECT_EQ(kinematics.computeInverseKinematics(FLT_MIN, FLT_MIN), Vector2f(0.f, 0.f));
|
||||||
|
}
|
||||||
|
|
||||||
|
TEST(DifferentialDriveKinematicsTest, MaxSpeedLimitCase)
|
||||||
|
{
|
||||||
|
DifferentialDriveKinematics kinematics;
|
||||||
|
kinematics.setWheelBase(1.f);
|
||||||
|
kinematics.setMaxSpeed(1.f);
|
||||||
|
kinematics.setMaxAngularVelocity(1.f);
|
||||||
|
|
||||||
|
// Test with high linear velocity and yaw rate, expecting speeds to be scaled down to fit the max speed
|
||||||
|
EXPECT_EQ(kinematics.computeInverseKinematics(10.f, 10.f), Vector2f(0.f, 1.f));
|
||||||
|
}
|
||||||
|
|
||||||
|
TEST(DifferentialDriveKinematicsTest, MaxSpeedForwardsCase)
|
||||||
|
{
|
||||||
|
DifferentialDriveKinematics kinematics;
|
||||||
|
kinematics.setWheelBase(1.f);
|
||||||
|
kinematics.setMaxSpeed(1.f);
|
||||||
|
kinematics.setMaxAngularVelocity(1.f);
|
||||||
|
|
||||||
|
// Test with high linear velocity and yaw rate, expecting speeds to be scaled down to fit the max speed
|
||||||
|
EXPECT_EQ(kinematics.computeInverseKinematics(10.f, 0.f), Vector2f(1.f, 1.f));
|
||||||
|
}
|
||||||
|
|
||||||
|
TEST(DifferentialDriveKinematicsTest, MaxAngularCase)
|
||||||
|
{
|
||||||
|
DifferentialDriveKinematics kinematics;
|
||||||
|
kinematics.setWheelBase(2.f);
|
||||||
|
kinematics.setMaxSpeed(1.f);
|
||||||
|
kinematics.setMaxAngularVelocity(1.f);
|
||||||
|
|
||||||
|
// Test with high linear velocity and yaw rate, expecting speeds to be scaled down to fit the max speed
|
||||||
|
EXPECT_EQ(kinematics.computeInverseKinematics(0.f, 10.f), Vector2f(-1.f, 1.f));
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,6 @@
|
||||||
|
menuconfig MODULES_ROVER_CONTROL
|
||||||
|
bool "ROVER_CONTROL"
|
||||||
|
default n
|
||||||
|
depends on MODULES_CONTROL_ALLOCATOR
|
||||||
|
---help---
|
||||||
|
Enable support for control of differential drive rovers
|
||||||
|
|
@ -0,0 +1,122 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (c) 2023 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.
|
||||||
|
*
|
||||||
|
****************************************************************************/
|
||||||
|
|
||||||
|
#include "RoverControl.hpp"
|
||||||
|
|
||||||
|
using namespace time_literals;
|
||||||
|
using namespace matrix;
|
||||||
|
namespace rover_control
|
||||||
|
{
|
||||||
|
|
||||||
|
RoverControl::RoverControl() :
|
||||||
|
ModuleParams(nullptr),
|
||||||
|
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl),
|
||||||
|
_differential_drive_control(this)
|
||||||
|
{
|
||||||
|
updateParams();
|
||||||
|
}
|
||||||
|
|
||||||
|
bool RoverControl::init()
|
||||||
|
{
|
||||||
|
ScheduleOnInterval(10_ms); // 100 Hz
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
void RoverControl::updateParams()
|
||||||
|
{
|
||||||
|
ModuleParams::updateParams();
|
||||||
|
}
|
||||||
|
|
||||||
|
void RoverControl::Run()
|
||||||
|
{
|
||||||
|
if (should_exit()) {
|
||||||
|
ScheduleClear();
|
||||||
|
exit_and_cleanup();
|
||||||
|
}
|
||||||
|
|
||||||
|
_differential_drive_control.Update();
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
int RoverControl::task_spawn(int argc, char *argv[])
|
||||||
|
{
|
||||||
|
RoverControl *instance = new RoverControl();
|
||||||
|
|
||||||
|
if (instance) {
|
||||||
|
_object.store(instance);
|
||||||
|
_task_id = task_id_is_work_queue;
|
||||||
|
|
||||||
|
if (instance->init()) {
|
||||||
|
return PX4_OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
} else {
|
||||||
|
PX4_ERR("alloc failed");
|
||||||
|
}
|
||||||
|
|
||||||
|
delete instance;
|
||||||
|
_object.store(nullptr);
|
||||||
|
_task_id = -1;
|
||||||
|
|
||||||
|
return PX4_ERROR;
|
||||||
|
}
|
||||||
|
|
||||||
|
int RoverControl::custom_command(int argc, char *argv[])
|
||||||
|
{
|
||||||
|
return print_usage("unknown command");
|
||||||
|
}
|
||||||
|
|
||||||
|
int RoverControl::print_usage(const char *reason)
|
||||||
|
{
|
||||||
|
if (reason) {
|
||||||
|
PX4_ERR("%s\n", reason);
|
||||||
|
}
|
||||||
|
|
||||||
|
PRINT_MODULE_DESCRIPTION(
|
||||||
|
R"DESCR_STR(
|
||||||
|
### Description
|
||||||
|
Rover state machine.
|
||||||
|
)DESCR_STR");
|
||||||
|
|
||||||
|
PRINT_MODULE_USAGE_NAME("rover_control", "controller");
|
||||||
|
PRINT_MODULE_USAGE_COMMAND("start");
|
||||||
|
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
extern "C" __EXPORT int rover_control_main(int argc, char *argv[])
|
||||||
|
{
|
||||||
|
return RoverControl::main(argc, argv);
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace rover_control
|
||||||
|
|
@ -0,0 +1,118 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (c) 2023 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.
|
||||||
|
*
|
||||||
|
****************************************************************************/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
// PX4 includes
|
||||||
|
#include <px4_platform_common/px4_config.h>
|
||||||
|
#include <px4_platform_common/defines.h>
|
||||||
|
#include <px4_platform_common/module.h>
|
||||||
|
#include <px4_platform_common/module_params.h>
|
||||||
|
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
|
||||||
|
|
||||||
|
// uORB includes
|
||||||
|
#include <uORB/topics/actuator_outputs.h>
|
||||||
|
#include <uORB/topics/actuator_motors.h>
|
||||||
|
#include <uORB/PublicationMulti.hpp>
|
||||||
|
#include <uORB/topics/manual_control_setpoint.h>
|
||||||
|
#include <uORB/topics/parameter_update.h>
|
||||||
|
#include <uORB/topics/vehicle_status.h>
|
||||||
|
#include <uORB/topics/vehicle_control_mode.h>
|
||||||
|
#include <uORB/Publication.hpp>
|
||||||
|
#include <uORB/Subscription.hpp>
|
||||||
|
#include <uORB/SubscriptionMultiArray.hpp>
|
||||||
|
#include <uORB/topics/differential_drive_setpoint.h>
|
||||||
|
|
||||||
|
// Standard library includes
|
||||||
|
#include <math.h>
|
||||||
|
|
||||||
|
// Local includes
|
||||||
|
#include <DifferentialDriveKinematics.hpp>
|
||||||
|
#include "DifferentialDriveControl.hpp"
|
||||||
|
|
||||||
|
namespace rover_control
|
||||||
|
{
|
||||||
|
|
||||||
|
class RoverControl : public ModuleBase<RoverControl>, public ModuleParams,
|
||||||
|
public px4::ScheduledWorkItem
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
RoverControl();
|
||||||
|
~RoverControl() override = default;
|
||||||
|
|
||||||
|
/** @see ModuleBase */
|
||||||
|
static int task_spawn(int argc, char *argv[]);
|
||||||
|
|
||||||
|
/** @see ModuleBase */
|
||||||
|
static int custom_command(int argc, char *argv[]);
|
||||||
|
|
||||||
|
/** @see ModuleBase */
|
||||||
|
static int print_usage(const char *reason = nullptr);
|
||||||
|
|
||||||
|
bool init();
|
||||||
|
|
||||||
|
protected:
|
||||||
|
void updateParams() override;
|
||||||
|
|
||||||
|
private:
|
||||||
|
void Run() override;
|
||||||
|
|
||||||
|
uORB::Subscription _differential_drive_setpoint_sub{ORB_ID(differential_drive_setpoint)};
|
||||||
|
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
|
||||||
|
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
|
||||||
|
uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
|
||||||
|
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
|
||||||
|
|
||||||
|
uORB::PublicationMulti<actuator_motors_s> _actuator_motors_pub{ORB_ID(actuator_motors)};
|
||||||
|
uORB::Publication<differential_drive_setpoint_s> _differential_drive_setpoint_pub{ORB_ID(differential_drive_setpoint)};
|
||||||
|
|
||||||
|
// differential_drive_setpoint_s _differential_drive_setpoint{};
|
||||||
|
// DifferentialDriveKinematics _differential_drive_kinematics{};
|
||||||
|
differential_drive_control::DifferentialDriveControl _differential_drive_control;
|
||||||
|
|
||||||
|
bool _armed = false;
|
||||||
|
bool _manual_driving = false;
|
||||||
|
float _max_speed{0.f};
|
||||||
|
float _max_angular_velocity{0.f};
|
||||||
|
|
||||||
|
DEFINE_PARAMETERS(
|
||||||
|
(ParamFloat<px4::params::RDD_SPEED_SCALE>) _param_rdd_speed_scale,
|
||||||
|
(ParamFloat<px4::params::RDD_ANG_SCALE>) _param_rdd_ang_velocity_scale,
|
||||||
|
(ParamFloat<px4::params::RDD_WHL_SPEED>) _param_rdd_max_wheel_speed,
|
||||||
|
(ParamFloat<px4::params::RDD_WHEEL_BASE>) _param_rdd_wheel_base,
|
||||||
|
(ParamFloat<px4::params::RDD_WHEEL_RADIUS>) _param_rdd_wheel_radius,
|
||||||
|
(ParamInt<px4::params::CA_R_REV>) _param_r_rev
|
||||||
|
)
|
||||||
|
};
|
||||||
|
|
||||||
|
} // namespace rover_control
|
||||||
|
|
@ -0,0 +1,55 @@
|
||||||
|
module_name: Differential Drive Control
|
||||||
|
|
||||||
|
parameters:
|
||||||
|
- group: Rover Differential Drive
|
||||||
|
definitions:
|
||||||
|
RDD_WHEEL_BASE:
|
||||||
|
description:
|
||||||
|
short: Wheel base
|
||||||
|
long: Distance from the center of the right wheel to the center of the left wheel
|
||||||
|
type: float
|
||||||
|
unit: m
|
||||||
|
min: 0.001
|
||||||
|
max: 100
|
||||||
|
increment: 0.001
|
||||||
|
decimal: 3
|
||||||
|
default: 0.5
|
||||||
|
RDD_WHEEL_RADIUS:
|
||||||
|
description:
|
||||||
|
short: Wheel radius
|
||||||
|
long: Size of the wheel, half the diameter of the wheel
|
||||||
|
type: float
|
||||||
|
unit: m
|
||||||
|
min: 0.001
|
||||||
|
max: 100
|
||||||
|
increment: 0.001
|
||||||
|
decimal: 3
|
||||||
|
default: 0.1
|
||||||
|
RDD_SPEED_SCALE:
|
||||||
|
description:
|
||||||
|
short: Manual speed scale
|
||||||
|
type: float
|
||||||
|
min: 0
|
||||||
|
max: 1.0
|
||||||
|
increment: 0.01
|
||||||
|
decimal: 2
|
||||||
|
default: 1.0
|
||||||
|
RDD_ANG_SCALE:
|
||||||
|
description:
|
||||||
|
short: Manual angular velocity scale
|
||||||
|
type: float
|
||||||
|
min: 0
|
||||||
|
max: 1.0
|
||||||
|
increment: 0.01
|
||||||
|
decimal: 2
|
||||||
|
default: 1.0
|
||||||
|
RDD_WHL_SPEED:
|
||||||
|
description:
|
||||||
|
short: Maximum wheel speed
|
||||||
|
type: float
|
||||||
|
unit: rad/s
|
||||||
|
min: 0
|
||||||
|
max: 100
|
||||||
|
increment: 0.01
|
||||||
|
decimal: 2
|
||||||
|
default: 10
|
||||||
|
|
@ -1,6 +1,6 @@
|
||||||
############################################################################
|
############################################################################
|
||||||
#
|
#
|
||||||
# Copyright (c) 2022 PX4 Development Team. All rights reserved.
|
# Copyright (c) 2022-2023 PX4 Development Team. All rights reserved.
|
||||||
#
|
#
|
||||||
# Redistribution and use in source and binary forms, with or without
|
# Redistribution and use in source and binary forms, with or without
|
||||||
# modification, are permitted provided that the following conditions
|
# modification, are permitted provided that the following conditions
|
||||||
|
|
@ -64,6 +64,8 @@ if(gz-transport_FOUND)
|
||||||
GZMixingInterfaceESC.hpp
|
GZMixingInterfaceESC.hpp
|
||||||
GZMixingInterfaceServo.cpp
|
GZMixingInterfaceServo.cpp
|
||||||
GZMixingInterfaceServo.hpp
|
GZMixingInterfaceServo.hpp
|
||||||
|
GZMixingInterfaceWheel.cpp
|
||||||
|
GZMixingInterfaceWheel.hpp
|
||||||
DEPENDS
|
DEPENDS
|
||||||
mixer_module
|
mixer_module
|
||||||
px4_work_queue
|
px4_work_queue
|
||||||
|
|
|
||||||
|
|
@ -226,6 +226,11 @@ int GZBridge::init()
|
||||||
return PX4_ERROR;
|
return PX4_ERROR;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
if (!_mixing_interface_wheel.init(_model_name)) {
|
||||||
|
PX4_ERR("failed to init motor output");
|
||||||
|
return PX4_ERROR;
|
||||||
|
}
|
||||||
|
|
||||||
ScheduleNow();
|
ScheduleNow();
|
||||||
return OK;
|
return OK;
|
||||||
}
|
}
|
||||||
|
|
@ -724,6 +729,7 @@ void GZBridge::Run()
|
||||||
|
|
||||||
_mixing_interface_esc.stop();
|
_mixing_interface_esc.stop();
|
||||||
_mixing_interface_servo.stop();
|
_mixing_interface_servo.stop();
|
||||||
|
_mixing_interface_wheel.stop();
|
||||||
|
|
||||||
exit_and_cleanup();
|
exit_and_cleanup();
|
||||||
return;
|
return;
|
||||||
|
|
@ -739,6 +745,7 @@ void GZBridge::Run()
|
||||||
|
|
||||||
_mixing_interface_esc.updateParams();
|
_mixing_interface_esc.updateParams();
|
||||||
_mixing_interface_servo.updateParams();
|
_mixing_interface_servo.updateParams();
|
||||||
|
_mixing_interface_wheel.updateParams();
|
||||||
}
|
}
|
||||||
|
|
||||||
ScheduleDelayed(10_ms);
|
ScheduleDelayed(10_ms);
|
||||||
|
|
@ -754,6 +761,9 @@ int GZBridge::print_status()
|
||||||
PX4_INFO_RAW("Servo outputs:\n");
|
PX4_INFO_RAW("Servo outputs:\n");
|
||||||
_mixing_interface_servo.mixingOutput().printStatus();
|
_mixing_interface_servo.mixingOutput().printStatus();
|
||||||
|
|
||||||
|
PX4_INFO_RAW("Wheel outputs:\n");
|
||||||
|
_mixing_interface_wheel.mixingOutput().printStatus();
|
||||||
|
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -35,6 +35,7 @@
|
||||||
|
|
||||||
#include "GZMixingInterfaceESC.hpp"
|
#include "GZMixingInterfaceESC.hpp"
|
||||||
#include "GZMixingInterfaceServo.hpp"
|
#include "GZMixingInterfaceServo.hpp"
|
||||||
|
#include "GZMixingInterfaceWheel.hpp"
|
||||||
|
|
||||||
#include <px4_platform_common/atomic.h>
|
#include <px4_platform_common/atomic.h>
|
||||||
#include <px4_platform_common/defines.h>
|
#include <px4_platform_common/defines.h>
|
||||||
|
|
@ -56,6 +57,7 @@
|
||||||
#include <uORB/topics/vehicle_local_position.h>
|
#include <uORB/topics/vehicle_local_position.h>
|
||||||
#include <uORB/topics/sensor_baro.h>
|
#include <uORB/topics/sensor_baro.h>
|
||||||
#include <uORB/topics/vehicle_odometry.h>
|
#include <uORB/topics/vehicle_odometry.h>
|
||||||
|
#include <uORB/topics/wheel_encoders.h>
|
||||||
|
|
||||||
#include <gz/math.hh>
|
#include <gz/math.hh>
|
||||||
#include <gz/msgs.hh>
|
#include <gz/msgs.hh>
|
||||||
|
|
@ -63,6 +65,7 @@
|
||||||
|
|
||||||
#include <gz/msgs/imu.pb.h>
|
#include <gz/msgs/imu.pb.h>
|
||||||
#include <gz/msgs/fluid_pressure.pb.h>
|
#include <gz/msgs/fluid_pressure.pb.h>
|
||||||
|
#include <gz/msgs/model.pb.h>
|
||||||
#include <gz/msgs/odometry_with_covariance.pb.h>
|
#include <gz/msgs/odometry_with_covariance.pb.h>
|
||||||
|
|
||||||
using namespace time_literals;
|
using namespace time_literals;
|
||||||
|
|
@ -129,6 +132,7 @@ private:
|
||||||
|
|
||||||
GZMixingInterfaceESC _mixing_interface_esc{_node, _node_mutex};
|
GZMixingInterfaceESC _mixing_interface_esc{_node, _node_mutex};
|
||||||
GZMixingInterfaceServo _mixing_interface_servo{_node, _node_mutex};
|
GZMixingInterfaceServo _mixing_interface_servo{_node, _node_mutex};
|
||||||
|
GZMixingInterfaceWheel _mixing_interface_wheel{_node, _node_mutex};
|
||||||
|
|
||||||
px4::atomic<uint64_t> _world_time_us{0};
|
px4::atomic<uint64_t> _world_time_us{0};
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -0,0 +1,124 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (c) 2023 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.
|
||||||
|
*
|
||||||
|
****************************************************************************/
|
||||||
|
|
||||||
|
#include "GZMixingInterfaceWheel.hpp"
|
||||||
|
|
||||||
|
bool GZMixingInterfaceWheel::init(const std::string &model_name)
|
||||||
|
{
|
||||||
|
|
||||||
|
std::string wheel_speed_topic = "/model/" + model_name + "/command/motor_speed";
|
||||||
|
|
||||||
|
if (!_node.Subscribe(wheel_speed_topic, &GZMixingInterfaceWheel::wheelSpeedCallback, this)) {
|
||||||
|
PX4_ERR("failed to subscribe to %s", wheel_speed_topic.c_str());
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::string wheel_topic = "/model/" + model_name + "/command/motor_speed";
|
||||||
|
_actuators_pub = _node.Advertise<gz::msgs::Actuators>(wheel_topic);
|
||||||
|
|
||||||
|
if (!_actuators_pub.Valid()) {
|
||||||
|
PX4_ERR("failed to advertise %s", wheel_topic.c_str());
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
_wheel_encoders_pub.advertise();
|
||||||
|
|
||||||
|
ScheduleNow();
|
||||||
|
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool GZMixingInterfaceWheel::updateOutputs(bool stop_wheels, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
|
||||||
|
unsigned num_control_groups_updated)
|
||||||
|
{
|
||||||
|
unsigned active_output_count = 0;
|
||||||
|
|
||||||
|
for (unsigned i = 0; i < num_outputs; i++) {
|
||||||
|
if (_mixing_output.isFunctionSet(i)) {
|
||||||
|
active_output_count++;
|
||||||
|
|
||||||
|
} else {
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (active_output_count > 0) {
|
||||||
|
gz::msgs::Actuators wheel_velocity_message;
|
||||||
|
wheel_velocity_message.mutable_velocity()->Resize(active_output_count, 0);
|
||||||
|
|
||||||
|
for (unsigned i = 0; i < active_output_count; i++) {
|
||||||
|
// Offsetting the output allows for negative values despite unsigned integer to reverse the wheels
|
||||||
|
static constexpr float output_offset = 100.0f;
|
||||||
|
float scaled_output = (float)outputs[i] - output_offset;
|
||||||
|
wheel_velocity_message.set_velocity(i, scaled_output);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
if (_actuators_pub.Valid()) {
|
||||||
|
return _actuators_pub.Publish(wheel_velocity_message);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
void GZMixingInterfaceWheel::Run()
|
||||||
|
{
|
||||||
|
pthread_mutex_lock(&_node_mutex);
|
||||||
|
_mixing_output.update();
|
||||||
|
_mixing_output.updateSubscriptions(false);
|
||||||
|
pthread_mutex_unlock(&_node_mutex);
|
||||||
|
}
|
||||||
|
|
||||||
|
void GZMixingInterfaceWheel::wheelSpeedCallback(const gz::msgs::Actuators &actuators)
|
||||||
|
{
|
||||||
|
if (hrt_absolute_time() == 0) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
pthread_mutex_lock(&_node_mutex);
|
||||||
|
|
||||||
|
wheel_encoders_s wheel_encoders{};
|
||||||
|
|
||||||
|
for (int i = 0; i < actuators.velocity_size(); i++) {
|
||||||
|
// Convert from RPM to rad/s
|
||||||
|
wheel_encoders.wheel_speed[i] = (float)actuators.velocity(i) * (2.0f * M_PI_F / 60.0f);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (actuators.velocity_size() > 0) {
|
||||||
|
wheel_encoders.timestamp = hrt_absolute_time();
|
||||||
|
_wheel_encoders_pub.publish(wheel_encoders);
|
||||||
|
}
|
||||||
|
|
||||||
|
pthread_mutex_unlock(&_node_mutex);
|
||||||
|
}
|
||||||
|
|
@ -1,6 +1,6 @@
|
||||||
/****************************************************************************
|
/****************************************************************************
|
||||||
*
|
*
|
||||||
* Copyright (c) 2021 PX4 Development Team. All rights reserved.
|
* Copyright (c) 2023 PX4 Development Team. All rights reserved.
|
||||||
*
|
*
|
||||||
* Redistribution and use in source and binary forms, with or without
|
* Redistribution and use in source and binary forms, with or without
|
||||||
* modification, are permitted provided that the following conditions
|
* modification, are permitted provided that the following conditions
|
||||||
|
|
@ -33,21 +33,56 @@
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include "ActuatorEffectiveness.hpp"
|
#include <lib/mixer_module/mixer_module.hpp>
|
||||||
|
|
||||||
class ActuatorEffectivenessRoverDifferential: public ActuatorEffectiveness
|
#include <gz/msgs.hh>
|
||||||
|
#include <gz/transport.hh>
|
||||||
|
|
||||||
|
|
||||||
|
#include <uORB/PublicationMulti.hpp>
|
||||||
|
#include <uORB/topics/wheel_encoders.h>
|
||||||
|
|
||||||
|
|
||||||
|
// GZBridge mixing class for Wheels.
|
||||||
|
// It is separate from GZBridge to have separate WorkItems and therefore allowing independent scheduling
|
||||||
|
// All work items are expected to run on the same work queue.
|
||||||
|
class GZMixingInterfaceWheel : public OutputModuleInterface
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
ActuatorEffectivenessRoverDifferential() = default;
|
static constexpr int MAX_ACTUATORS = MixingOutput::MAX_ACTUATORS;
|
||||||
virtual ~ActuatorEffectivenessRoverDifferential() = default;
|
|
||||||
|
|
||||||
bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override;
|
GZMixingInterfaceWheel(gz::transport::Node &node, pthread_mutex_t &node_mutex) :
|
||||||
|
OutputModuleInterface(MODULE_NAME "-actuators-wheel", px4::wq_configurations::rate_ctrl),
|
||||||
|
_node(node),
|
||||||
|
_node_mutex(node_mutex)
|
||||||
|
{}
|
||||||
|
|
||||||
void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
|
bool updateOutputs(bool stop_wheels, uint16_t outputs[MAX_ACTUATORS],
|
||||||
ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
|
unsigned num_outputs, unsigned num_control_groups_updated) override;
|
||||||
const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
|
|
||||||
|
MixingOutput &mixingOutput() { return _mixing_output; }
|
||||||
|
|
||||||
|
bool init(const std::string &model_name);
|
||||||
|
|
||||||
|
void stop()
|
||||||
|
{
|
||||||
|
_mixing_output.unregister();
|
||||||
|
ScheduleClear();
|
||||||
|
}
|
||||||
|
|
||||||
const char *name() const override { return "Rover (Differential)"; }
|
|
||||||
private:
|
private:
|
||||||
uint32_t _motors_mask{};
|
friend class GZBridge;
|
||||||
|
|
||||||
|
void Run() override;
|
||||||
|
|
||||||
|
void wheelSpeedCallback(const gz::msgs::Actuators &actuators);
|
||||||
|
|
||||||
|
gz::transport::Node &_node;
|
||||||
|
pthread_mutex_t &_node_mutex;
|
||||||
|
|
||||||
|
MixingOutput _mixing_output{"SIM_GZ_WH", MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
|
||||||
|
|
||||||
|
gz::transport::Node::Publisher _actuators_pub;
|
||||||
|
|
||||||
|
uORB::Publication<wheel_encoders_s> _wheel_encoders_pub{ORB_ID(wheel_encoders)};
|
||||||
};
|
};
|
||||||
|
|
@ -25,3 +25,12 @@ actuator_output:
|
||||||
max: { min: 0, max: 1000, default: 1000 }
|
max: { min: 0, max: 1000, default: 1000 }
|
||||||
failsafe: { min: 0, max: 1000 }
|
failsafe: { min: 0, max: 1000 }
|
||||||
num_channels: 8
|
num_channels: 8
|
||||||
|
- param_prefix: SIM_GZ_WH
|
||||||
|
group_label: 'Wheels'
|
||||||
|
channel_label: 'Wheels'
|
||||||
|
standard_params:
|
||||||
|
disarmed: { min: 0, max: 200, default: 100 }
|
||||||
|
min: { min: 0, max: 200, default: 0 }
|
||||||
|
max: { min: 0, max: 200, default: 200 }
|
||||||
|
failsafe: { min: 0, max: 200 }
|
||||||
|
num_channels: 2
|
||||||
|
|
|
||||||
|
|
@ -127,6 +127,9 @@ subscriptions:
|
||||||
- topic: /fmu/in/vehicle_rates_setpoint
|
- topic: /fmu/in/vehicle_rates_setpoint
|
||||||
type: px4_msgs::msg::VehicleRatesSetpoint
|
type: px4_msgs::msg::VehicleRatesSetpoint
|
||||||
|
|
||||||
|
- topic: /fmu/in/differential_drive_setpoint
|
||||||
|
type: px4_msgs::msg::DifferentialDriveSetpoint
|
||||||
|
|
||||||
- topic: /fmu/in/vehicle_visual_odometry
|
- topic: /fmu/in/vehicle_visual_odometry
|
||||||
type: px4_msgs::msg::VehicleOdometry
|
type: px4_msgs::msg::VehicleOdometry
|
||||||
|
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue