Changed constant name to UNMANNED_GROUND_VEHICLE

This commit is contained in:
Timothy Scott
2019-05-02 10:26:42 +02:00
committed by Julian Oes
parent 714c90b9db
commit 21760a5856
19 changed files with 39 additions and 32 deletions

View File

@@ -3,7 +3,7 @@
# @name Rover # @name Rover
# #
sh /etc/init.d/rc.ugv_defaults sh /etc/init.d/rc.rover_defaults
if [ $AUTOCNF = yes ] if [ $AUTOCNF = yes ]
then then

View File

@@ -229,7 +229,7 @@ fi
# Configure vehicle type specific parameters. # Configure vehicle type specific parameters.
# Note: rc.vehicle_setup is the entry point for rc.interface, # Note: rc.vehicle_setup is the entry point for rc.interface,
# rc.fw_apps, rc.mc_apps, rc.ugv_apps, and rc.vtol_apps. # rc.fw_apps, rc.mc_apps, rc.rover_apps, and rc.vtol_apps.
# #
sh etc/init.d/rc.vehicle_setup sh etc/init.d/rc.vehicle_setup

View File

@@ -43,8 +43,8 @@ px4_add_romfs_files(
rcS rcS
rc.sensors rc.sensors
rc.thermal_cal rc.thermal_cal
rc.ugv_apps rc.rover_apps
rc.ugv_defaults rc.rover_defaults
rc.vehicle_setup rc.vehicle_setup
rc.vtol_apps rc.vtol_apps
rc.vtol_defaults rc.vtol_defaults

View File

@@ -13,7 +13,7 @@
# @board px4_fmu-v2 exclude # @board px4_fmu-v2 exclude
# #
sh /etc/init.d/rc.ugv_defaults sh /etc/init.d/rc.rover_defaults
if [ $AUTOCNF = yes ] if [ $AUTOCNF = yes ]
then then
@@ -54,10 +54,10 @@ then
param set PWM_MIN 1000 param set PWM_MIN 1000
fi fi
# Configure this as ugv # Configure this as rover
set MAV_TYPE 10 set MAV_TYPE 10
# Set mixer # Set mixer
set MIXER ugv_generic set MIXER rover_generic
set PWM_MAIN_REV2 1 set PWM_MAIN_REV2 1

View File

@@ -17,7 +17,7 @@
# @board px4_fmu-v2 exclude # @board px4_fmu-v2 exclude
# #
sh /etc/init.d/rc.ugv_defaults sh /etc/init.d/rc.rover_defaults
# #
# This section can be enabled once tuning parameters for this particular # This section can be enabled once tuning parameters for this particular

View File

@@ -15,7 +15,7 @@
# @board px4_fmu-v2 exclude # @board px4_fmu-v2 exclude
# #
sh /etc/init.d/rc.ugv_defaults sh /etc/init.d/rc.rover_defaults
if [ $AUTOCNF = yes ] if [ $AUTOCNF = yes ]
then then
@@ -56,7 +56,7 @@ then
param set PWM_MIN 1000 param set PWM_MIN 1000
fi fi
# Configure this as ugv # Configure this as rover
set MAV_TYPE 10 set MAV_TYPE 10
# Set mixer # Set mixer

View File

@@ -23,4 +23,4 @@ gnd_pos_control start
# #
# Start Land Detector. # Start Land Detector.
# #
land_detector start ugv land_detector start rover

View File

@@ -5,7 +5,7 @@
# NOTE: Script variables are declared/initialized/unset in the rcS script. # NOTE: Script variables are declared/initialized/unset in the rcS script.
# #
set VEHICLE_TYPE ugv set VEHICLE_TYPE rover
if [ $AUTOCNF = yes ] if [ $AUTOCNF = yes ]
then then

View File

@@ -87,12 +87,12 @@ fi
# #
# UGV setup. # UGV setup.
# #
if [ $VEHICLE_TYPE = ugv ] if [ $VEHICLE_TYPE = rover ]
then then
if [ $MIXER = none ] if [ $MIXER = none ]
then then
# Set default mixer for UGV if not defined. # Set default mixer for UGV if not defined.
set MIXER ugv_generic set MIXER rover_generic
fi fi
if [ $MAV_TYPE = none ] if [ $MAV_TYPE = none ]
@@ -108,7 +108,7 @@ then
sh /etc/init.d/rc.interface sh /etc/init.d/rc.interface
# Start standard UGV apps. # Start standard UGV apps.
sh /etc/init.d/rc.ugv_apps sh /etc/init.d/rc.rover_apps
fi fi
# #

View File

@@ -421,7 +421,7 @@ else
# #
# Configure vehicle type specific parameters. # Configure vehicle type specific parameters.
# Note: rc.vehicle_setup is the entry point for rc.interface, # Note: rc.vehicle_setup is the entry point for rc.interface,
# rc.fw_apps, rc.mc_apps, rc.ugv_apps, and rc.vtol_apps. # rc.fw_apps, rc.mc_apps, rc.rover_apps, and rc.vtol_apps.
# #
sh /etc/init.d/rc.vehicle_setup sh /etc/init.d/rc.vehicle_setup

View File

@@ -78,7 +78,7 @@ px4_add_romfs_files(
stampede.main.mix stampede.main.mix
tri_y_yaw-.main.mix tri_y_yaw-.main.mix
tri_y_yaw+.main.mix tri_y_yaw+.main.mix
ugv_generic.main.mix rover_generic.main.mix
Viper.main.mix Viper.main.mix
vtol_AAERT.aux.mix vtol_AAERT.aux.mix
vtol_AAVVT.aux.mix vtol_AAVVT.aux.mix

View File

@@ -44,7 +44,7 @@ px4_add_romfs_files(
quad_+.main.mix quad_+.main.mix
quad_test.mix quad_test.mix
quad_+_vtol.main.mix quad_+_vtol.main.mix
ugv_generic.main.mix rover_generic.main.mix
vtol1_test.mix vtol1_test.mix
vtol2_test.mix vtol2_test.mix
vtol_AAERT.aux.mix vtol_AAERT.aux.mix

View File

@@ -52,7 +52,7 @@ uint8 RC_IN_MODE_GENERATED = 2
uint8 VEHICLE_TYPE_UNKNOWN = 0 uint8 VEHICLE_TYPE_UNKNOWN = 0
uint8 VEHICLE_TYPE_ROTARY_WING = 1 uint8 VEHICLE_TYPE_ROTARY_WING = 1
uint8 VEHICLE_TYPE_FIXED_WING = 2 uint8 VEHICLE_TYPE_FIXED_WING = 2
uint8 VEHICLE_TYPE_GROUND = 3 uint8 VEHICLE_TYPE_ROVER = 3
# state machine / state of vehicle. # state machine / state of vehicle.
# Encodes the complete system state and is set by the commander app. # Encodes the complete system state and is set by the commander app.
@@ -67,6 +67,8 @@ uint8 system_id # system id, contains MAVLink's system ID field
uint8 component_id # subsystem / component id, contains MAVLink's component ID field uint8 component_id # subsystem / component id, contains MAVLink's component ID field
uint8 vehicle_type # Type of vehicle (fixed-wing, rotary wing, ground) uint8 vehicle_type # Type of vehicle (fixed-wing, rotary wing, ground)
# If the vehicle is a VTOL, then this value will be VEHICLE_TYPE_ROTARY_WING while flying as a multicopter,
# and VEHICLE_TYPE_FIXED_WING when flying as a fixed-wing
bool is_vtol # True if the system is VTOL capable bool is_vtol # True if the system is VTOL capable
bool vtol_fw_permanent_stab # True if VTOL should stabilize attitude for fw in manual mode bool vtol_fw_permanent_stab # True if VTOL should stabilize attitude for fw in manual mode

View File

@@ -1297,7 +1297,7 @@ Commander::run()
status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_FIXED_WING; status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_FIXED_WING;
} else if (is_ground_rover(&status)) { } else if (is_ground_rover(&status)) {
status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_GROUND; status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROVER;
} else { } else {
status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_UNKNOWN; status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_UNKNOWN;
@@ -1407,9 +1407,9 @@ Commander::run()
status.system_type = (uint8_t)system_type; status.system_type = (uint8_t)system_type;
} }
bool is_rotary = is_rotary_wing(&status) || (is_vtol(&status) && vtol_status.vtol_in_rw_mode); const bool is_rotary = is_rotary_wing(&status) || (is_vtol(&status) && vtol_status.vtol_in_rw_mode);
bool is_fixed = is_fixed_wing(&status) || (is_vtol(&status) && !vtol_status.vtol_in_rw_mode); const bool is_fixed = is_fixed_wing(&status) || (is_vtol(&status) && !vtol_status.vtol_in_rw_mode);
bool is_ground = is_ground_rover(&status); const bool is_ground = is_ground_rover(&status);
/* disable manual override for all systems that rely on electronic stabilization */ /* disable manual override for all systems that rely on electronic stabilization */
if (is_rotary) { if (is_rotary) {
@@ -1419,7 +1419,7 @@ Commander::run()
status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_FIXED_WING; status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_FIXED_WING;
} else if (is_ground) { } else if (is_ground) {
status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_GROUND; status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROVER;
} }
/* set vehicle_status.is_vtol flag */ /* set vehicle_status.is_vtol flag */
@@ -1642,9 +1642,11 @@ Commander::run()
if (is_vtol(&status)) { if (is_vtol(&status)) {
// Check if there has been any change while updating the flags // Check if there has been any change while updating the flags
bool is_rotary = status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; const auto new_vehicle_type = vtol_status.vtol_in_rw_mode ?
vehicle_status_s::VEHICLE_TYPE_ROTARY_WING :
vehicle_status_s::VEHICLE_TYPE_FIXED_WING;
if (is_rotary != vtol_status.vtol_in_rw_mode) { if (new_vehicle_type != status.vehicle_type) {
status.vehicle_type = vtol_status.vtol_in_rw_mode ? status.vehicle_type = vtol_status.vtol_in_rw_mode ?
vehicle_status_s::VEHICLE_TYPE_ROTARY_WING : vehicle_status_s::VEHICLE_TYPE_ROTARY_WING :
vehicle_status_s::VEHICLE_TYPE_FIXED_WING; vehicle_status_s::VEHICLE_TYPE_FIXED_WING;

View File

@@ -507,7 +507,7 @@ bool StateMachineHelperTest::mainStateTransitionTest()
struct vehicle_status_flags_s current_status_flags = {}; struct vehicle_status_flags_s current_status_flags = {};
current_commander_state.main_state = test->from_state; current_commander_state.main_state = test->from_state;
current_vehicle_status.vehicle_type = test->condition_bits & MTT_ROTARY_WING ? current_vehicle_status.vehicle_type = (test->condition_bits & MTT_ROTARY_WING) ?
vehicle_status_s::VEHICLE_TYPE_ROTARY_WING : vehicle_status_s::VEHICLE_TYPE_FIXED_WING; vehicle_status_s::VEHICLE_TYPE_ROTARY_WING : vehicle_status_s::VEHICLE_TYPE_FIXED_WING;
current_status_flags.condition_local_altitude_valid = test->condition_bits & MTT_LOC_ALT_VALID; current_status_flags.condition_local_altitude_valid = test->condition_bits & MTT_LOC_ALT_VALID;
current_status_flags.condition_local_position_valid = test->condition_bits & MTT_LOC_POS_VALID; current_status_flags.condition_local_position_valid = test->condition_bits & MTT_LOC_POS_VALID;

View File

@@ -691,6 +691,7 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER; status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
} else if (status_flags.condition_local_altitude_valid) { } else if (status_flags.condition_local_altitude_valid) {
//TODO: Add case for rover
if (status->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { if (status->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;

View File

@@ -76,7 +76,7 @@ int LandDetector::task_spawn(int argc, char *argv[])
} else if (strcmp(argv[1], "vtol") == 0) { } else if (strcmp(argv[1], "vtol") == 0) {
obj = new VtolLandDetector(); obj = new VtolLandDetector();
} else if (strcmp(argv[1], "ugv") == 0) { } else if (strcmp(argv[1], "rover") == 0) {
obj = new RoverLandDetector(); obj = new RoverLandDetector();
} else { } else {
@@ -165,7 +165,7 @@ The module runs periodically on the HP work queue.
PRINT_MODULE_USAGE_NAME("land_detector", "system"); PRINT_MODULE_USAGE_NAME("land_detector", "system");
PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start the background task"); PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start the background task");
PRINT_MODULE_USAGE_ARG("fixedwing|multicopter|vtol|ugv", "Select vehicle type", false); PRINT_MODULE_USAGE_ARG("fixedwing|multicopter|vtol|rover", "Select vehicle type", false);
PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
return 0; return 0;
} }

View File

@@ -335,10 +335,12 @@ MissionBlock::is_mission_item_reached()
&& PX4_ISFINITE(_mission_item.yaw))) { && PX4_ISFINITE(_mission_item.yaw))) {
/* check course if defined only for rotary wing except takeoff */ /* check course if defined only for rotary wing except takeoff */
float cog = _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING float cog = (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) ?
? _navigator->get_global_position()->yaw : atan2f( _navigator->get_global_position()->yaw :
atan2f(
_navigator->get_global_position()->vel_e, _navigator->get_global_position()->vel_e,
_navigator->get_global_position()->vel_n); _navigator->get_global_position()->vel_n
);
float yaw_err = wrap_pi(_mission_item.yaw - cog); float yaw_err = wrap_pi(_mission_item.yaw - cog);