fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 00:33:34 +08: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-13 10:04:26 +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
src/modules/commander/Commander.cpp
+42 -19
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
src/modules/commander/PreflightCheck.cpp
+3 -2
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 {
src/modules/commander/commander_helper.cpp
+12
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
src/modules/commander/commander_helper.h
+2
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);
src/modules/commander/commander_tests/state_machine_helper_test.cpp
+2 -1
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;
src/modules/commander/state_machine_helper.cpp
+10 -9
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;