yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

Removed is_rotor_wing, replaced with vehicle_type

Browse Source
This commit is contained in:
Timothy Scott
2019-06-11 12:54:22 +02:00
committed by Julian Oes
parent 2ca40bfc65
commit a134da6e12
21 changed files with 150 additions and 85 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
msg/vehicle_status.msg
View File

@@ -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_GROUND = 3
# state machine / state of vehicle.
# Encodes the complete system state and is set by the commander app.
@@ -61,7 +66,8 @@ 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
#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
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

3
src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp
View File

@@ -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,

61
src/modules/commander/Commander.cpp
View File

@@ -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_GROUND;
} else {
status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_UNKNOWN;
}
status.is_vtol = is_vtol(&status);
commander_boot_timestamp = hrt_absolute_time();
@@ -1395,10 +1409,13 @@ Commander::run()
/* 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;
status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
} else {
status.is_rotary_wing = false;
} else if (is_fixed_wing(&status) || is_vtol(&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_GROUND;
}
/* set vehicle_status.is_vtol flag */
@@ -1621,8 +1638,10 @@ 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;
if ((status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) != 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_FIXED_WING;
status_changed = true;
}
@@ -1641,8 +1660,8 @@ Commander::run()
status_changed = true;
}
if (armed.soft_stop != !status.is_rotary_wing) {
armed.soft_stop = !status.is_rotary_wing;
if (armed.soft_stop != (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING)) {
armed.soft_stop = status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING;
status_changed = true;
}
}
@@ -1989,7 +2008,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 &&
@@ -2137,7 +2156,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);
@@ -2924,10 +2943,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 {
@@ -2937,7 +2956,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 {
@@ -3133,7 +3152,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) {
@@ -3364,10 +3384,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
@@ -4034,9 +4055,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);
@@ -4332,7 +4355,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

5
src/modules/commander/PreflightCheck.cpp
View File

@@ -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 {

12
src/modules/commander/commander_helper.cpp
View File

@@ -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

2
src/modules/commander/commander_helper.h
View File

@@ -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);

3
src/modules/commander/commander_tests/state_machine_helper_test.cpp
View File

@@ -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;

19
src/modules/commander/state_machine_helper.cpp
View File

@@ -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,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 {
@@ -730,7 +730,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 +747,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 +788,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 +805,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 +870,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;

9
src/modules/ekf2/ekf2_main.cpp
View File

@@ -751,11 +751,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
@@ -1671,7 +1674,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;

20
src/modules/fw_att_control/FixedwingAttitudeControl.cpp
View File

@@ -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));

9
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
View File

@@ -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;
}

4
src/modules/land_detector/VtolLandDetector.cpp
View File

@@ -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;
}

2
src/modules/mavlink/mavlink_high_latency2.cpp
View File

@@ -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);
}

4
src/modules/mavlink/mavlink_messages.cpp
View File

@@ -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 @@ protected:
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) {

8
src/modules/mc_att_control/mc_att_control_main.cpp
View File

@@ -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();
}
@@ -644,7 +644,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;
@@ -678,6 +679,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(

2
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@@ -702,7 +702,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;
}

28
src/modules/navigator/mission.cpp
View File

@@ -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),

20
src/modules/navigator/mission_block.cpp
View File

@@ -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);
@@ -328,12 +329,14 @@ 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);
@@ -474,7 +477,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) {

3
src/modules/navigator/mission_feasibility_checker.cpp
View File

@@ -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 {

10
src/modules/navigator/navigator_main.cpp
View File

@@ -761,7 +761,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 {
@@ -781,7 +781,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) {
@@ -797,7 +797,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;
@@ -818,7 +818,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 {
@@ -1069,7 +1069,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();
}

3
src/modules/navigator/rtl.cpp
View File

@@ -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;
}