diff --git a/ROMFS/px4fmu_common/init.d-posix/1060_rover b/ROMFS/px4fmu_common/init.d-posix/1060_rover index 5461d68144be..ba5fdc3d4da3 100644 --- a/ROMFS/px4fmu_common/init.d-posix/1060_rover +++ b/ROMFS/px4fmu_common/init.d-posix/1060_rover @@ -3,7 +3,7 @@ # @name Rover # -sh /etc/init.d/rc.ugv_defaults +sh /etc/init.d/rc.rover_defaults if [ $AUTOCNF = yes ] then diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index f7afafc143da..4bd7df91ed68 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/ROMFS/px4fmu_common/init.d-posix/rcS @@ -229,7 +229,7 @@ fi # Configure vehicle type specific parameters. # 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 diff --git a/ROMFS/px4fmu_common/init.d/CMakeLists.txt b/ROMFS/px4fmu_common/init.d/CMakeLists.txt index 773c71eb0aa0..925a560bf7a2 100644 --- a/ROMFS/px4fmu_common/init.d/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d/CMakeLists.txt @@ -43,8 +43,8 @@ px4_add_romfs_files( rcS rc.sensors rc.thermal_cal - rc.ugv_apps - rc.ugv_defaults + rc.rover_apps + rc.rover_defaults rc.vehicle_setup rc.vtol_apps rc.vtol_defaults diff --git a/ROMFS/px4fmu_common/init.d/airframes/50000_generic_ground_vehicle b/ROMFS/px4fmu_common/init.d/airframes/50000_generic_ground_vehicle index 780740a26951..6168778371e5 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/50000_generic_ground_vehicle +++ b/ROMFS/px4fmu_common/init.d/airframes/50000_generic_ground_vehicle @@ -13,7 +13,7 @@ # @board px4_fmu-v2 exclude # -sh /etc/init.d/rc.ugv_defaults +sh /etc/init.d/rc.rover_defaults if [ $AUTOCNF = yes ] then @@ -54,10 +54,10 @@ then param set PWM_MIN 1000 fi -# Configure this as ugv +# Configure this as rover set MAV_TYPE 10 # Set mixer -set MIXER ugv_generic +set MIXER rover_generic set PWM_MAIN_REV2 1 diff --git a/ROMFS/px4fmu_common/init.d/airframes/50001_axialracing_ax10 b/ROMFS/px4fmu_common/init.d/airframes/50001_axialracing_ax10 index 3b45445d7574..e8467251a983 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/50001_axialracing_ax10 +++ b/ROMFS/px4fmu_common/init.d/airframes/50001_axialracing_ax10 @@ -17,7 +17,7 @@ # @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 diff --git a/ROMFS/px4fmu_common/init.d/airframes/50002_traxxas_stampede_2wd b/ROMFS/px4fmu_common/init.d/airframes/50002_traxxas_stampede_2wd index d9965500c1be..81a0013a294c 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/50002_traxxas_stampede_2wd +++ b/ROMFS/px4fmu_common/init.d/airframes/50002_traxxas_stampede_2wd @@ -15,7 +15,7 @@ # @board px4_fmu-v2 exclude # -sh /etc/init.d/rc.ugv_defaults +sh /etc/init.d/rc.rover_defaults if [ $AUTOCNF = yes ] then @@ -56,7 +56,7 @@ then param set PWM_MIN 1000 fi -# Configure this as ugv +# Configure this as rover set MAV_TYPE 10 # Set mixer diff --git a/ROMFS/px4fmu_common/init.d/rc.ugv_apps b/ROMFS/px4fmu_common/init.d/rc.rover_apps similarity index 93% rename from ROMFS/px4fmu_common/init.d/rc.ugv_apps rename to ROMFS/px4fmu_common/init.d/rc.rover_apps index cd54785ca88f..ccb7ce815940 100644 --- a/ROMFS/px4fmu_common/init.d/rc.ugv_apps +++ b/ROMFS/px4fmu_common/init.d/rc.rover_apps @@ -23,4 +23,4 @@ gnd_pos_control start # # Start Land Detector. # -land_detector start ugv +land_detector start rover diff --git a/ROMFS/px4fmu_common/init.d/rc.ugv_defaults b/ROMFS/px4fmu_common/init.d/rc.rover_defaults similarity index 96% rename from ROMFS/px4fmu_common/init.d/rc.ugv_defaults rename to ROMFS/px4fmu_common/init.d/rc.rover_defaults index 15757ba12b40..9f93fd61aa3f 100644 --- a/ROMFS/px4fmu_common/init.d/rc.ugv_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.rover_defaults @@ -5,7 +5,7 @@ # NOTE: Script variables are declared/initialized/unset in the rcS script. # -set VEHICLE_TYPE ugv +set VEHICLE_TYPE rover if [ $AUTOCNF = yes ] then diff --git a/ROMFS/px4fmu_common/init.d/rc.vehicle_setup b/ROMFS/px4fmu_common/init.d/rc.vehicle_setup index af875820d820..20a60edf1bc0 100644 --- a/ROMFS/px4fmu_common/init.d/rc.vehicle_setup +++ b/ROMFS/px4fmu_common/init.d/rc.vehicle_setup @@ -87,12 +87,12 @@ fi # # UGV setup. # -if [ $VEHICLE_TYPE = ugv ] +if [ $VEHICLE_TYPE = rover ] then if [ $MIXER = none ] then # Set default mixer for UGV if not defined. - set MIXER ugv_generic + set MIXER rover_generic fi if [ $MAV_TYPE = none ] @@ -108,7 +108,7 @@ then sh /etc/init.d/rc.interface # Start standard UGV apps. - sh /etc/init.d/rc.ugv_apps + sh /etc/init.d/rc.rover_apps fi # diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 8d2deb77bd73..24aa4ed41f1c 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -421,7 +421,7 @@ else # # Configure vehicle type specific parameters. # 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 diff --git a/ROMFS/px4fmu_common/mixers/CMakeLists.txt b/ROMFS/px4fmu_common/mixers/CMakeLists.txt index 77634fdd86b9..0b277340a79c 100644 --- a/ROMFS/px4fmu_common/mixers/CMakeLists.txt +++ b/ROMFS/px4fmu_common/mixers/CMakeLists.txt @@ -78,7 +78,7 @@ px4_add_romfs_files( stampede.main.mix tri_y_yaw-.main.mix tri_y_yaw+.main.mix - ugv_generic.main.mix + rover_generic.main.mix Viper.main.mix vtol_AAERT.aux.mix vtol_AAVVT.aux.mix diff --git a/ROMFS/px4fmu_common/mixers/ugv_generic.main.mix b/ROMFS/px4fmu_common/mixers/rover_generic.main.mix similarity index 100% rename from ROMFS/px4fmu_common/mixers/ugv_generic.main.mix rename to ROMFS/px4fmu_common/mixers/rover_generic.main.mix diff --git a/ROMFS/px4fmu_test/mixers/CMakeLists.txt b/ROMFS/px4fmu_test/mixers/CMakeLists.txt index f5d6d2de3060..287652a9b818 100644 --- a/ROMFS/px4fmu_test/mixers/CMakeLists.txt +++ b/ROMFS/px4fmu_test/mixers/CMakeLists.txt @@ -44,7 +44,7 @@ px4_add_romfs_files( quad_+.main.mix quad_test.mix quad_+_vtol.main.mix - ugv_generic.main.mix + rover_generic.main.mix vtol1_test.mix vtol2_test.mix vtol_AAERT.aux.mix diff --git a/ROMFS/px4fmu_test/mixers/ugv_generic.main.mix b/ROMFS/px4fmu_test/mixers/rover_generic.main.mix similarity index 100% rename from ROMFS/px4fmu_test/mixers/ugv_generic.main.mix rename to ROMFS/px4fmu_test/mixers/rover_generic.main.mix diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index 9eaabebc0494..5a61f7296e6d 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -49,6 +49,11 @@ uint8 RC_IN_MODE_DEFAULT = 0 uint8 RC_IN_MODE_OFF = 1 uint8 RC_IN_MODE_GENERATED = 2 +uint8 VEHICLE_TYPE_UNKNOWN = 0 +uint8 VEHICLE_TYPE_ROTARY_WING = 1 +uint8 VEHICLE_TYPE_FIXED_WING = 2 +uint8 VEHICLE_TYPE_ROVER = 3 + # state machine / state of vehicle. # Encodes the complete system state and is set by the commander app. @@ -61,7 +66,9 @@ uint8 system_type # system type, contains mavlink MAV_TYPE uint8 system_id # system id, contains MAVLink's system ID field uint8 component_id # subsystem / component id, contains MAVLink's component ID field -bool is_rotary_wing # True if system is in rotary wing configuration, so for a VTOL this is only true while flying as a multicopter +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 vtol_fw_permanent_stab # True if VTOL should stabilize attitude for fw in manual mode diff --git a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp index dde59f91161d..fb716373781f 100644 --- a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp +++ b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp @@ -274,7 +274,8 @@ bool FlightTaskAuto::_evaluateTriplets() _mission_gear = _sub_triplet_setpoint->get().current.landing_gear; } - if (_param_com_obs_avoid.get() && _sub_vehicle_status->get().is_rotary_wing) { + if (_param_com_obs_avoid.get() + && _sub_vehicle_status->get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { _obstacle_avoidance.updateAvoidanceDesiredWaypoints(_triplet_target, _yaw_setpoint, _yawspeed_setpoint, _triplet_next_wp, _sub_triplet_setpoint->get().next.yaw, diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 380ea75352fe..f68689319677 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -431,7 +431,8 @@ int commander_main(int argc, char *argv[]) if (!strcmp(argv[1], "transition")) { bool ret = send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION, - (float)(status.is_rotary_wing ? vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW : + (float)(status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING ? + vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW : vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC)); return (ret ? 0 : 1); @@ -1288,7 +1289,20 @@ Commander::run() int32_t system_type; param_get(_param_sys_type, &system_type); // get system type status.system_type = (uint8_t)system_type; - status.is_rotary_wing = is_rotary_wing(&status) || is_vtol(&status); + + if (is_rotary_wing(&status) || is_vtol(&status)) { + status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; + + } else if (is_fixed_wing(&status)) { + status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_FIXED_WING; + + } else if (is_ground_rover(&status)) { + status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROVER; + + } else { + status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_UNKNOWN; + } + status.is_vtol = is_vtol(&status); commander_boot_timestamp = hrt_absolute_time(); @@ -1393,12 +1407,19 @@ Commander::run() status.system_type = (uint8_t)system_type; } + const bool is_rotary = is_rotary_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); + const bool is_ground = is_ground_rover(&status); + /* disable manual override for all systems that rely on electronic stabilization */ - if (is_rotary_wing(&status) || (is_vtol(&status) && vtol_status.vtol_in_rw_mode)) { - status.is_rotary_wing = true; + if (is_rotary) { + status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; - } else { - status.is_rotary_wing = false; + } else if (is_fixed) { + status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_FIXED_WING; + + } else if (is_ground) { + status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROVER; } /* set vehicle_status.is_vtol flag */ @@ -1607,8 +1628,14 @@ Commander::run() if (is_vtol(&status)) { // Check if there has been any change while updating the flags - if (status.is_rotary_wing != vtol_status.vtol_in_rw_mode) { - status.is_rotary_wing = vtol_status.vtol_in_rw_mode; + 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 (new_vehicle_type != status.vehicle_type) { + status.vehicle_type = vtol_status.vtol_in_rw_mode ? + vehicle_status_s::VEHICLE_TYPE_ROTARY_WING : + vehicle_status_s::VEHICLE_TYPE_FIXED_WING; status_changed = true; } @@ -1627,8 +1654,10 @@ Commander::run() status_changed = true; } - if (armed.soft_stop != !status.is_rotary_wing) { - armed.soft_stop = !status.is_rotary_wing; + const bool should_soft_stop = (status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING); + + if (armed.soft_stop != should_soft_stop) { + armed.soft_stop = should_soft_stop; status_changed = true; } } @@ -1975,7 +2004,7 @@ Commander::run() if (in_armed_state && status.rc_input_mode != vehicle_status_s::RC_IN_MODE_OFF && - (status.is_rotary_wing || (!status.is_rotary_wing && land_detector.landed)) && + (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING || land_detector.landed) && (stick_in_lower_left || arm_button_pressed || arm_switch_to_disarm_transition)) { if (internal_state.main_state != commander_state_s::MAIN_STATE_MANUAL && @@ -2123,7 +2152,7 @@ Commander::run() * only for fixed wing for now */ if (!status_flags.circuit_breaker_engaged_enginefailure_check && - !status.is_rotary_wing && !status.is_vtol && armed.armed) { + status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING && !status.is_vtol && armed.armed) { actuator_controls_s actuator_controls = {}; actuator_controls_sub.copy(&actuator_controls); @@ -2910,10 +2939,10 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed /* manual mode is stabilized already for multirotors, so switch to acro * for any non-manual mode */ - if (status.is_rotary_wing && !status.is_vtol) { + if (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && !status.is_vtol) { res = main_state_transition(status_local, commander_state_s::MAIN_STATE_ACRO, status_flags, &internal_state); - } else if (!status.is_rotary_wing) { + } else if (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { res = main_state_transition(status_local, commander_state_s::MAIN_STATE_STAB, status_flags, &internal_state); } else { @@ -2923,7 +2952,7 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed } else if (sp_man.rattitude_switch == manual_control_setpoint_s::SWITCH_POS_ON) { /* Similar to acro transitions for multirotors. FW aircraft don't need a * rattitude mode.*/ - if (status.is_rotary_wing) { + if (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { res = main_state_transition(status_local, commander_state_s::MAIN_STATE_RATTITUDE, status_flags, &internal_state); } else { @@ -3119,7 +3148,8 @@ set_control_mode() { /* set vehicle_control_mode according to set_navigation_state */ control_mode.flag_armed = armed.armed; - control_mode.flag_external_manual_override_ok = (!status.is_rotary_wing && !status.is_vtol); + control_mode.flag_external_manual_override_ok = (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING + && !status.is_vtol); control_mode.flag_control_offboard_enabled = false; switch (status.nav_state) { @@ -3350,10 +3380,11 @@ set_control_mode() bool stabilization_required() { - return (status.is_rotary_wing || // is a rotary wing, or + return (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING || // is a rotary wing, or status.vtol_fw_permanent_stab || // is a VTOL in fixed wing mode and stabilisation is on, or (vtol_status.vtol_in_trans_mode && // is currently a VTOL transitioning AND - !status.is_rotary_wing)); // is a fixed wing, ie: transitioning back to rotary wing mode + status.vehicle_type == + vehicle_status_s::VEHICLE_TYPE_FIXED_WING)); // is a fixed wing, ie: transitioning back to rotary wing mode } void @@ -4020,9 +4051,11 @@ void Commander::airspeed_use_check() _sensor_bias_sub.update(); const sensor_bias_s &sensors = _sensor_bias_sub.get(); + bool is_fixed_wing = status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING; + // assume airspeed sensor is good before starting FW flight bool valid_flight_condition = (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) && - !status.is_rotary_wing && !land_detector.landed; + is_fixed_wing && !land_detector.landed; bool fault_declared = false; bool fault_cleared = false; bool bad_number_fail = !PX4_ISFINITE(airspeed.indicated_airspeed_m_s) || !PX4_ISFINITE(airspeed.true_airspeed_m_s); @@ -4318,7 +4351,7 @@ void Commander::estimator_check(bool *status_changed) * for a short time interval after takeoff. Fixed wing vehicles can recover using GPS heading, * but rotary wing vehicles cannot so the position and velocity validity needs to be latched * to false after failure to prevent flyaway crashes */ - if (run_quality_checks && status.is_rotary_wing) { + if (run_quality_checks && status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { if (status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) { // reset flags and timer diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index 738581d3508c..4bab7c8e512c 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -727,7 +727,8 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, /* Perform airspeed check only if circuit breaker is not * engaged and it's not a rotary wing */ - if (!status_flags.circuit_breaker_engaged_airspd_check && (!status.is_rotary_wing || status.is_vtol)) { + if (!status_flags.circuit_breaker_engaged_airspd_check && + (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING || status.is_vtol)) { checkAirspeed = true; } @@ -960,7 +961,7 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, // only check EKF2 data if EKF2 is selected as the estimator and GNSS checking is enabled int32_t estimator_type = -1; - if (status.is_rotary_wing && !status.is_vtol) { + if (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && !status.is_vtol) { param_get(param_find("SYS_MC_EST_GROUP"), &estimator_type); } else { diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 26e5bdfafe55..8ab34a04c024 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -67,9 +67,11 @@ using namespace DriverFramework; +#define VEHICLE_TYPE_FIXED_WING 1 #define VEHICLE_TYPE_QUADROTOR 2 #define VEHICLE_TYPE_COAXIAL 3 #define VEHICLE_TYPE_HELICOPTER 4 +#define VEHICLE_TYPE_GROUND_ROVER 10 #define VEHICLE_TYPE_HEXAROTOR 13 #define VEHICLE_TYPE_OCTOROTOR 14 #define VEHICLE_TYPE_TRICOPTER 15 @@ -108,6 +110,16 @@ bool is_vtol(const struct vehicle_status_s *current_status) current_status->system_type == VEHICLE_TYPE_VTOL_RESERVED5); } +bool is_fixed_wing(const struct vehicle_status_s *current_status) +{ + return current_status->system_type == VEHICLE_TYPE_FIXED_WING; +} + +bool is_ground_rover(const struct vehicle_status_s *current_status) +{ + return current_status->system_type == VEHICLE_TYPE_GROUND_ROVER; +} + static hrt_abstime blink_msg_end = 0; // end time for currently blinking LED message, 0 if no blink message static hrt_abstime tune_end = 0; // end time of currently played tune, 0 for repeating tunes or silence static int tune_current = TONE_STOP_TUNE; // currently playing tune, can be interrupted after tune_end diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h index 78fb7d88e978..52528c3a66d3 100644 --- a/src/modules/commander/commander_helper.h +++ b/src/modules/commander/commander_helper.h @@ -53,6 +53,8 @@ bool is_multirotor(const struct vehicle_status_s *current_status); bool is_rotary_wing(const struct vehicle_status_s *current_status); bool is_vtol(const struct vehicle_status_s *current_status); +bool is_fixed_wing(const struct vehicle_status_s *current_status); +bool is_ground_rover(const struct vehicle_status_s *current_status); int buzzer_init(void); void buzzer_deinit(void); diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp index e52eff9fba9d..2231c3092322 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp +++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp @@ -507,7 +507,8 @@ bool StateMachineHelperTest::mainStateTransitionTest() struct vehicle_status_flags_s current_status_flags = {}; current_commander_state.main_state = test->from_state; - current_vehicle_status.is_rotary_wing = 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; 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_home_position_valid = test->condition_bits & MTT_HOME_POS_VALID; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index a372372e33b8..f189fbaacd7b 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -298,7 +298,7 @@ main_state_transition(const vehicle_status_s &status, const main_state_t new_mai case commander_state_s::MAIN_STATE_ORBIT: /* Follow and orbit only implemented for multicopter */ - if (status.is_rotary_wing) { + if (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { ret = TRANSITION_CHANGED; } @@ -339,7 +339,7 @@ main_state_transition(const vehicle_status_s &status, const main_state_t new_mai /* need local and global position, and precision land only implemented for multicopters */ if (status_flags.condition_local_position_valid && status_flags.condition_global_position_valid - && status.is_rotary_wing) { + && status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { ret = TRANSITION_CHANGED; } @@ -469,7 +469,7 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_ } else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, !(posctl_nav_loss_act == 1), - !status->is_rotary_wing)) { + status->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING)) { // nothing to do - everything done in check_invalid_pos_nav_state } else { @@ -691,7 +691,8 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_ status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER; } else if (status_flags.condition_local_altitude_valid) { - if (status->is_rotary_wing) { + //TODO: Add case for rover + if (status->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; } else { @@ -730,7 +731,7 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_ status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND; } else if (status_flags.condition_local_altitude_valid) { - if (status->is_rotary_wing) { + if (status->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; } else { @@ -747,7 +748,7 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_ status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER; } else if (status_flags.condition_local_altitude_valid) { - if (status->is_rotary_wing) { + if (status->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; } else { @@ -788,7 +789,8 @@ bool check_invalid_pos_nav_state(vehicle_status_s *status, bool old_failsafe, or if (fallback_required) { if (use_rc) { // fallback to a mode that gives the operator stick control - if (status->is_rotary_wing && status_flags.condition_local_position_valid) { + if (status->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING + && status_flags.condition_local_position_valid) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL; } else if (status_flags.condition_local_altitude_valid) { @@ -804,7 +806,7 @@ bool check_invalid_pos_nav_state(vehicle_status_s *status, bool old_failsafe, or status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND; } else if (status_flags.condition_local_altitude_valid) { - if (status->is_rotary_wing) { + if (status->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; } else { @@ -869,7 +871,7 @@ void set_link_loss_nav_state(vehicle_status_s *status, actuator_armed_s *armed, return; } else { - if (status->is_rotary_wing) { + if (status->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { if (status_flags.condition_local_position_valid) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND; return; diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 55e4712d3156..0bf6950e1c83 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -755,11 +755,14 @@ void Ekf2::run() // update all other topics if they have new data if (_status_sub.update(&vehicle_status)) { + + bool is_fixed_wing = vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING; + // only fuse synthetic sideslip measurements if conditions are met - _ekf.set_fuse_beta_flag(!vehicle_status.is_rotary_wing && (_param_ekf2_fuse_beta.get() == 1)); + _ekf.set_fuse_beta_flag(is_fixed_wing && (_param_ekf2_fuse_beta.get() == 1)); // let the EKF know if the vehicle motion is that of a fixed wing (forward flight only relative to wind) - _ekf.set_is_fixed_wing(!vehicle_status.is_rotary_wing); + _ekf.set_is_fixed_wing(is_fixed_wing); } // Always update sensor selction first time through if time stamp is non zero @@ -1690,7 +1693,7 @@ void Ekf2::run() float yaw_test_limit; - if (doing_ne_aiding && vehicle_status.is_rotary_wing) { + if (doing_ne_aiding && vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { // use a smaller tolerance when doing NE inertial frame aiding as a rotary wing // vehicle which cannot use GPS course to realign heading in flight yaw_test_limit = _nav_yaw_innov_test_lim; diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index 9782ee6ef7aa..bf9df5844b25 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -262,7 +262,8 @@ FixedwingAttitudeControl::vehicle_control_mode_poll() _vcontrol_mode_sub.update(&_vcontrol_mode); if (_vehicle_status.is_vtol) { - const bool is_hovering = _vehicle_status.is_rotary_wing && !_vehicle_status.in_transition_mode; + const bool is_hovering = _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING + && !_vehicle_status.in_transition_mode; const bool is_tailsitter_transition = _vehicle_status.in_transition_mode && _is_tailsitter; if (is_hovering || is_tailsitter_transition) { @@ -276,10 +277,9 @@ void FixedwingAttitudeControl::vehicle_manual_poll() { const bool is_tailsitter_transition = _is_tailsitter && _vehicle_status.in_transition_mode; - const bool is_fixed_wing = !_vehicle_status.is_rotary_wing; + const bool is_fixed_wing = _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING; if (_vcontrol_mode.flag_control_manual_enabled && (!is_tailsitter_transition || is_fixed_wing)) { - // Always copy the new manual setpoint, even if it wasn't updated, to fill the _actuators with valid values if (_manual_sub.copy(&_manual)) { @@ -428,7 +428,8 @@ float FixedwingAttitudeControl::get_airspeed_and_update_scaling() // VTOL: if we have no airspeed available and we are in hover mode then assume the lowest airspeed possible // this assumption is good as long as the vehicle is not hovering in a headwind which is much larger // than the minimum airspeed - if (_vehicle_status.is_vtol && _vehicle_status.is_rotary_wing && !_vehicle_status.in_transition_mode) { + if (_vehicle_status.is_vtol && _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING + && !_vehicle_status.in_transition_mode) { airspeed = _parameters.airspeed_min; } } @@ -560,7 +561,8 @@ void FixedwingAttitudeControl::run() _att_sp.fw_control_yaw = _att_sp.fw_control_yaw && _vcontrol_mode.flag_control_auto_enabled; /* lock integrator until control is started */ - bool lock_integrator = !_vcontrol_mode.flag_control_rates_enabled || _vehicle_status.is_rotary_wing; + bool lock_integrator = !_vcontrol_mode.flag_control_rates_enabled + || _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; /* Simple handling of failsafe: deploy parachute if failsafe is on */ if (_vcontrol_mode.flag_control_termination_enabled) { @@ -571,7 +573,7 @@ void FixedwingAttitudeControl::run() } /* if we are in rotary wing mode, do nothing */ - if (_vehicle_status.is_rotary_wing && !_vehicle_status.is_vtol) { + if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && !_vehicle_status.is_vtol) { continue; } @@ -608,7 +610,8 @@ void FixedwingAttitudeControl::run() * or a multicopter (but not transitioning VTOL) */ if (_landed - || (_vehicle_status.is_rotary_wing && !_vehicle_status.in_transition_mode)) { + || (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING + && !_vehicle_status.in_transition_mode)) { _roll_ctrl.reset_integrator(); _pitch_ctrl.reset_integrator(); @@ -637,7 +640,8 @@ void FixedwingAttitudeControl::run() /* reset body angular rate limits on mode change */ if ((_vcontrol_mode.flag_control_attitude_enabled != _flag_control_attitude_enabled_last) || params_updated) { - if (_vcontrol_mode.flag_control_attitude_enabled || _vehicle_status.is_rotary_wing) { + if (_vcontrol_mode.flag_control_attitude_enabled + || _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { _roll_ctrl.set_max_rate(math::radians(_parameters.r_rmax)); _pitch_ctrl.set_max_rate_pos(math::radians(_parameters.p_rmax_pos)); _pitch_ctrl.set_max_rate_neg(math::radians(_parameters.p_rmax_neg)); diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index ca0d121b8d85..c8ff1209bdfc 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -716,7 +716,7 @@ FixedwingPositionControl::update_desired_altitude(float dt) } if (_vehicle_status.is_vtol) { - if (_vehicle_status.is_rotary_wing || _vehicle_status.in_transition_mode) { + if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING || _vehicle_status.in_transition_mode) { _hold_alt = _global_pos.alt; } } @@ -764,7 +764,7 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto _l1_control.set_dt(dt); /* only run position controller in fixed-wing mode and during transitions for VTOL */ - if (_vehicle_status.is_rotary_wing && !_vehicle_status.in_transition_mode) { + if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && !_vehicle_status.in_transition_mode) { _control_mode_current = FW_POSCTRL_MODE_OTHER; return false; } @@ -1799,7 +1799,8 @@ FixedwingPositionControl::run() } // only publish status in full FW mode - if (!_vehicle_status.is_rotary_wing && !_vehicle_status.in_transition_mode) { + if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING + && !_vehicle_status.in_transition_mode) { status_publish(); } } @@ -1869,7 +1870,7 @@ FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float airspee // do not run TECS if vehicle is a VTOL and we are in rotary wing mode or in transition // (it should also not run during VTOL blending because airspeed is too low still) if (_vehicle_status.is_vtol) { - if (_vehicle_status.is_rotary_wing || _vehicle_status.in_transition_mode) { + if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING || _vehicle_status.in_transition_mode) { run_tecs = false; } diff --git a/src/modules/land_detector/VtolLandDetector.cpp b/src/modules/land_detector/VtolLandDetector.cpp index fc1596ee2fd6..b1877a9b846f 100644 --- a/src/modules/land_detector/VtolLandDetector.cpp +++ b/src/modules/land_detector/VtolLandDetector.cpp @@ -62,7 +62,7 @@ void VtolLandDetector::_update_topics() bool VtolLandDetector::_get_maybe_landed_state() { // Only trigger in RW mode - if ((_vehicle_status.timestamp != 0) && !_vehicle_status.is_rotary_wing) { + if ((_vehicle_status.timestamp != 0) && _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { return false; } @@ -72,7 +72,7 @@ bool VtolLandDetector::_get_maybe_landed_state() bool VtolLandDetector::_get_landed_state() { // Only trigger in RW mode - if ((_vehicle_status.timestamp != 0) && !_vehicle_status.is_rotary_wing) { + if ((_vehicle_status.timestamp != 0) && _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { return false; } diff --git a/src/modules/land_detector/land_detector_main.cpp b/src/modules/land_detector/land_detector_main.cpp index 71e36bdabd91..f9550004431d 100644 --- a/src/modules/land_detector/land_detector_main.cpp +++ b/src/modules/land_detector/land_detector_main.cpp @@ -76,7 +76,7 @@ int LandDetector::task_spawn(int argc, char *argv[]) } else if (strcmp(argv[1], "vtol") == 0) { obj = new VtolLandDetector(); - } else if (strcmp(argv[1], "ugv") == 0) { + } else if (strcmp(argv[1], "rover") == 0) { obj = new RoverLandDetector(); } else { @@ -165,7 +165,7 @@ The module runs periodically on the HP work queue. PRINT_MODULE_USAGE_NAME("land_detector", "system"); 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(); return 0; } diff --git a/src/modules/mavlink/mavlink_high_latency2.cpp b/src/modules/mavlink/mavlink_high_latency2.cpp index 7b9074f1dc19..1506cf9aecf8 100644 --- a/src/modules/mavlink/mavlink_high_latency2.cpp +++ b/src/modules/mavlink/mavlink_high_latency2.cpp @@ -538,7 +538,7 @@ void MavlinkStreamHighLatency2::update_vehicle_status() if (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) { struct actuator_controls_s actuator = {}; - if (status.is_vtol && !status.is_rotary_wing) { + if (status.is_vtol && status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { if (_actuator_sub_1->update(&actuator)) { _throttle.add_value(actuator.control[actuator_controls_s::INDEX_THROTTLE], _update_rate_filtered); } diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 28149ef82070..2eba396d7090 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -153,7 +153,7 @@ void get_mavlink_navigation_mode(const struct vehicle_status_s *const status, ui switch (status->nav_state) { case vehicle_status_s::NAVIGATION_STATE_MANUAL: *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED - | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); + | (status->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); custom_mode->main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; break; @@ -4284,7 +4284,7 @@ class MavlinkStreamExtendedSysState : public MavlinkStream updated = true; if (status.is_vtol) { - if (!status.in_transition_mode && status.is_rotary_wing) { + if (!status.in_transition_mode && status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { _msg.vtol_state = MAV_VTOL_STATE_MC; } else if (!status.in_transition_mode) { diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 013877a6c38d..6e3ca2885697 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -69,7 +69,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _sensor_gyro_sub[i] = -1; } - _vehicle_status.is_rotary_wing = true; + _vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; /* initialize quaternions in messages to be valid */ _v_att.q[0] = 1.f; @@ -400,7 +400,7 @@ void MulticopterAttitudeControl::control_attitude_rates(float dt) { /* reset integral if disarmed */ - if (!_v_control_mode.flag_armed || !_vehicle_status.is_rotary_wing) { + if (!_v_control_mode.flag_armed || _vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { _rates_int.zero(); } @@ -642,7 +642,8 @@ MulticopterAttitudeControl::run() bool attitude_setpoint_generated = false; - const bool is_hovering = _vehicle_status.is_rotary_wing && !_vehicle_status.in_transition_mode; + const bool is_hovering = _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING + && !_vehicle_status.in_transition_mode; // vehicle is a tailsitter in transition mode const bool is_tailsitter_transition = _vehicle_status.in_transition_mode && _is_tailsitter; @@ -676,6 +677,7 @@ MulticopterAttitudeControl::run() } else { /* attitude controller disabled, poll rates setpoint topic */ if (_v_control_mode.flag_control_manual_enabled && is_hovering) { + if (manual_control_updated) { /* manual rates control - ACRO mode */ Vector3f man_rate_sp( diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 443b7f82a127..e56c856c1485 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -715,7 +715,7 @@ MulticopterPositionControl::start_flight_task() int prev_failure_count = _task_failure_count; // Do not run any flight task for VTOLs in fixed-wing mode - if (!_vehicle_status.is_rotary_wing) { + if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { _flight_tasks.switchTask(FlightTaskIndex::None); return; } diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 527b338a93b1..47b1d0b7656a 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -239,7 +239,8 @@ Mission::on_active() } /* see if we need to update the current yaw heading */ - if (!_param_mis_mnt_yaw_ctl.get() && (_navigator->get_vstatus()->is_rotary_wing) + if (!_param_mis_mnt_yaw_ctl.get() + && (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) && (_navigator->get_vroi().mode != vehicle_roi_s::ROI_NONE) && !(_mission_item.nav_cmd == NAV_CMD_TAKEOFF || _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF @@ -321,7 +322,7 @@ Mission::set_execution_mode(const uint8_t mode) case mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD: if (mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE) { // command a transition if in vtol mc mode - if (_navigator->get_vstatus()->is_rotary_wing && + if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && _navigator->get_vstatus()->is_vtol && !_navigator->get_land_detected()->landed) { @@ -703,7 +704,7 @@ Mission::set_mission_items() } else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF && _work_item_type == WORK_ITEM_TYPE_DEFAULT && new_work_item_type == WORK_ITEM_TYPE_DEFAULT - && _navigator->get_vstatus()->is_rotary_wing) { + && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { /* if there is no need to do a takeoff but we have a takeoff item, treat is as waypoint */ _mission_item.nav_cmd = NAV_CMD_WAYPOINT; @@ -719,7 +720,7 @@ Mission::set_mission_items() && _work_item_type == WORK_ITEM_TYPE_DEFAULT && new_work_item_type == WORK_ITEM_TYPE_DEFAULT) { - if (_navigator->get_vstatus()->is_rotary_wing) { + if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { /* haven't transitioned yet, trigger vtol takeoff logic below */ _work_item_type = WORK_ITEM_TYPE_TAKEOFF; @@ -750,7 +751,7 @@ Mission::set_mission_items() if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF && _work_item_type == WORK_ITEM_TYPE_TAKEOFF && new_work_item_type == WORK_ITEM_TYPE_DEFAULT && - _navigator->get_vstatus()->is_rotary_wing && + _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && !_navigator->get_land_detected()->landed) { /* check if the vtol_takeoff waypoint is on top of us */ @@ -806,7 +807,7 @@ Mission::set_mission_items() if (_mission_item.nav_cmd == NAV_CMD_VTOL_LAND && _work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND && new_work_item_type == WORK_ITEM_TYPE_DEFAULT - && !_navigator->get_vstatus()->is_rotary_wing + && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING && !_navigator->get_land_detected()->landed) { set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC); @@ -929,7 +930,7 @@ Mission::set_mission_items() if (_mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION && _work_item_type == WORK_ITEM_TYPE_DEFAULT && new_work_item_type == WORK_ITEM_TYPE_DEFAULT - && _navigator->get_vstatus()->is_rotary_wing + && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && !_navigator->get_land_detected()->landed && has_next_position_item) { @@ -957,7 +958,7 @@ Mission::set_mission_items() if (_mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION && _work_item_type == WORK_ITEM_TYPE_DEFAULT && new_work_item_type == WORK_ITEM_TYPE_DEFAULT - && !_navigator->get_vstatus()->is_rotary_wing + && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING && !_navigator->get_land_detected()->landed && pos_sp_triplet->current.valid) { @@ -1061,7 +1062,7 @@ Mission::set_mission_items() bool Mission::do_need_vertical_takeoff() { - if (_navigator->get_vstatus()->is_rotary_wing) { + if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { float takeoff_alt = calculate_takeoff_altitude(&_mission_item); @@ -1099,7 +1100,7 @@ Mission::do_need_vertical_takeoff() bool Mission::do_need_move_to_land() { - if (_navigator->get_vstatus()->is_rotary_wing + if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && (_mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_VTOL_LAND)) { float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon, @@ -1114,7 +1115,8 @@ Mission::do_need_move_to_land() bool Mission::do_need_move_to_takeoff() { - if (_navigator->get_vstatus()->is_rotary_wing && _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF) { + if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING + && _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF) { float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon, _navigator->get_global_position()->lat, _navigator->get_global_position()->lon); @@ -1253,7 +1255,7 @@ Mission::altitude_sp_foh_update() if (!pos_sp_triplet->previous.valid || !pos_sp_triplet->current.valid || !PX4_ISFINITE(pos_sp_triplet->previous.alt) || !(pos_sp_triplet->previous.type == position_setpoint_s::SETPOINT_TYPE_POSITION || pos_sp_triplet->previous.type == position_setpoint_s::SETPOINT_TYPE_LOITER) || - _navigator->get_vstatus()->is_rotary_wing) { + _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { return; } @@ -1720,7 +1722,7 @@ Mission::index_closest_mission_item() const if (item_contains_position(missionitem)) { // do not consider land waypoints for a fw if (!((missionitem.nav_cmd == NAV_CMD_LAND) && - (!_navigator->get_vstatus()->is_rotary_wing) && + (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) && (!_navigator->get_vstatus()->is_vtol))) { float dist = get_distance_to_point_global_wgs84(missionitem.lat, missionitem.lon, get_absolute_altitude_for_item(missionitem), diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index fa986af6d7e7..e9c819c9d947 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -149,7 +149,7 @@ MissionBlock::is_mission_item_reached() &dist_xy, &dist_z); /* FW special case for NAV_CMD_WAYPOINT to achieve altitude via loiter */ - if (!_navigator->get_vstatus()->is_rotary_wing && + if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING && (_mission_item.nav_cmd == NAV_CMD_WAYPOINT)) { struct position_setpoint_s *curr_sp = &_navigator->get_position_setpoint_triplet()->current; @@ -183,7 +183,7 @@ MissionBlock::is_mission_item_reached() } if ((_mission_item.nav_cmd == NAV_CMD_TAKEOFF || _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF) - && _navigator->get_vstatus()->is_rotary_wing) { + && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { /* We want to avoid the edge case where the acceptance radius is bigger or equal than * the altitude of the takeoff waypoint above home. Otherwise, we do not really follow @@ -213,7 +213,7 @@ MissionBlock::is_mission_item_reached() _waypoint_position_reached = true; } - } else if (!_navigator->get_vstatus()->is_rotary_wing && + } else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING && (_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED || _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT)) { /* Loiter mission item on a non rotary wing: the aircraft is going to circle the @@ -231,7 +231,7 @@ MissionBlock::is_mission_item_reached() _time_first_inside_orbit = 0; } - } else if (!_navigator->get_vstatus()->is_rotary_wing && + } else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING && (_mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT)) { @@ -297,7 +297,8 @@ MissionBlock::is_mission_item_reached() } /* for vtol back transition calculate acceptance radius based on time and ground speed */ - if (_mission_item.vtol_back_transition && !_navigator->get_vstatus()->is_rotary_wing) { + if (_mission_item.vtol_back_transition + && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { float velocity = sqrtf(_navigator->get_local_position()->vx * _navigator->get_local_position()->vx + _navigator->get_local_position()->vy * _navigator->get_local_position()->vy); @@ -312,7 +313,7 @@ MissionBlock::is_mission_item_reached() } - if (dist >= 0.0f && dist <= mission_acceptance_radius + if (dist_xy >= 0.0f && dist_xy <= mission_acceptance_radius && dist_z <= _navigator->get_altitude_acceptance_radius()) { _waypoint_position_reached = true; } @@ -328,14 +329,18 @@ MissionBlock::is_mission_item_reached() if (_waypoint_position_reached && !_waypoint_yaw_reached) { - if ((_navigator->get_vstatus()->is_rotary_wing && PX4_ISFINITE(_navigator->get_yaw_acceptance(_mission_item.yaw))) + if ((_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING + && PX4_ISFINITE(_navigator->get_yaw_acceptance(_mission_item.yaw))) || ((_mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT) && _mission_item.force_heading && PX4_ISFINITE(_mission_item.yaw))) { /* check course if defined only for rotary wing except takeoff */ - float cog = _navigator->get_vstatus()->is_rotary_wing ? _navigator->get_global_position()->yaw : atan2f( + float cog = (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) ? + _navigator->get_global_position()->yaw : + atan2f( _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); @@ -479,7 +484,8 @@ MissionBlock::issue_command(const mission_item_s &item) float MissionBlock::get_time_inside(const mission_item_s &item) const { - if ((item.nav_cmd == NAV_CMD_WAYPOINT && _navigator->get_vstatus()->is_rotary_wing) || + if ((item.nav_cmd == NAV_CMD_WAYPOINT + && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) || item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || item.nav_cmd == NAV_CMD_DELAY) { diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index c2b748f17512..456db59fa9b1 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -83,7 +83,8 @@ MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission, failed = failed || !checkHomePositionAltitude(mission, home_alt, home_alt_valid, warned); // VTOL always respects rotary wing feasibility - if (_navigator->get_vstatus()->is_rotary_wing || _navigator->get_vstatus()->is_vtol) { + if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING + || _navigator->get_vstatus()->is_vtol) { failed = failed || !checkRotarywing(mission, home_alt); } else { diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 65ae550c5216..2219746e1e0b 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -771,7 +771,7 @@ Navigator::get_acceptance_radius() float Navigator::get_default_altitude_acceptance_radius() { - if (!get_vstatus()->is_rotary_wing) { + if (get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { return _param_nav_fw_alt_rad.get(); } else { @@ -791,7 +791,7 @@ Navigator::get_default_altitude_acceptance_radius() float Navigator::get_altitude_acceptance_radius() { - if (!get_vstatus()->is_rotary_wing) { + if (get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { const position_setpoint_s &next_sp = get_position_setpoint_triplet()->next; if (next_sp.type == position_setpoint_s::SETPOINT_TYPE_LAND && next_sp.valid) { @@ -807,7 +807,7 @@ float Navigator::get_cruising_speed() { /* there are three options: The mission-requested cruise speed, or the current hover / plane speed */ - if (_vstatus.is_rotary_wing) { + if (_vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { if (is_planned_mission() && _mission_cruising_speed_mc > 0.0f) { return _mission_cruising_speed_mc; @@ -828,7 +828,7 @@ Navigator::get_cruising_speed() void Navigator::set_cruising_speed(float speed) { - if (_vstatus.is_rotary_wing) { + if (_vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { _mission_cruising_speed_mc = speed; } else { @@ -1079,7 +1079,7 @@ bool Navigator::force_vtol() { return _vstatus.is_vtol && - (!_vstatus.is_rotary_wing || _vstatus.in_transition_to_fw) + (_vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING || _vstatus.in_transition_to_fw) && _param_nav_force_vt.get(); } diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index cb57b79bed37..bd06a9a09a68 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -318,7 +318,8 @@ RTL::advance_rtl() _rtl_state = RTL_STATE_LAND; } - if (_navigator->get_vstatus()->is_vtol && !_navigator->get_vstatus()->is_rotary_wing) { + if (_navigator->get_vstatus()->is_vtol + && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { _rtl_state = RTL_STATE_TRANSITION_TO_MC; }