fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-29 18:50:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Quadchute param update (#19351)

Browse Source 
New parameter for actions after a quadchute (COM_QC_ACT)

Co-authored-by: Jonas <jonas.perolini@rigi.tech>
Co-authored-by: Matthias Grob <maetugr@gmail.com>
This commit is contained in:
Jonas Perolini
2022-03-24 10:27:32 +01:00
committed by GitHub
parent 5ae4cae073
commit e31304d7d5
5 changed files with 69 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/state_machine_helper.cpp
+46 -3
View File
@@ -147,6 +147,10 @@ void set_offboard_loss_nav_state(vehicle_status_s &status, actuator_armed_s &arm
const vehicle_status_flags_s &status_flags,
const offboard_loss_actions_t offboard_loss_act);
void set_quadchute_nav_state(vehicle_status_s &status, actuator_armed_s &armed,
const vehicle_status_flags_s &status_flags,
const quadchute_actions_t quadchute_act);
void set_offboard_loss_rc_nav_state(vehicle_status_s &status, actuator_armed_s &armed,
const vehicle_status_flags_s &status_flags,
const offboard_loss_rc_actions_t offboard_loss_rc_act);
@@ -497,6 +501,7 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_
orb_advert_t *mavlink_log_pub, const link_loss_actions_t data_link_loss_act, const bool mission_finished,
const bool stay_in_failsafe, const vehicle_status_flags_s &status_flags, bool landed,
const link_loss_actions_t rc_loss_act, const offboard_loss_actions_t offb_loss_act,
const quadchute_actions_t quadchute_act,
const offboard_loss_rc_actions_t offb_loss_rc_act,
const position_nav_loss_actions_t posctl_nav_loss_act,
const float param_com_rcl_act_t, const int param_com_rcl_except)
@@ -588,7 +593,8 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
} else if (status_flags.vtol_transition_failure) {
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
set_quadchute_nav_state(status, armed, status_flags, quadchute_act);
} else if (status.mission_failure) {
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
@@ -641,7 +647,8 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_
// nothing to do - everything done in check_invalid_pos_nav_state
} else if (status_flags.vtol_transition_failure) {
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
set_quadchute_nav_state(status, armed, status_flags, quadchute_act);
} else if (status.data_link_lost && data_link_loss_act_configured && !landed && is_armed) {
// Data link lost, data link loss reaction configured -> do configured reaction
@@ -756,7 +763,8 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_
// nothing to do - everything done in check_invalid_pos_nav_state
} else if (status_flags.vtol_transition_failure) {
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
set_quadchute_nav_state(status, armed, status_flags, quadchute_act);
} else if (status.data_link_lost && data_link_loss_act_configured && !landed && is_armed) {
// Data link lost, data link loss reaction configured -> do configured reaction
@@ -985,6 +993,41 @@ void reset_link_loss_globals(actuator_armed_s &armed, const bool old_failsafe, c
}
}
void set_quadchute_nav_state(vehicle_status_s &status, actuator_armed_s &armed,
const vehicle_status_flags_s &status_flags,
const quadchute_actions_t quadchute_act)
{
switch (quadchute_act) {
case quadchute_actions_t::NO_ACTION:
// If quadchute action is disabled then no action must be taken.
break;
default:
case quadchute_actions_t::AUTO_RTL:
if (status_flags.condition_global_position_valid && status_flags.condition_home_position_valid) {
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
}
break;
// FALLTHROUGH
case quadchute_actions_t::AUTO_LAND:
if (status_flags.condition_global_position_valid) {
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
}
break;
// FALLTHROUGH
case quadchute_actions_t::AUTO_LOITER:
if (status_flags.condition_global_position_valid) {
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
}
break;
}
}
void set_offboard_loss_nav_state(vehicle_status_s &status, actuator_armed_s &armed,
const vehicle_status_flags_s &status_flags,
const offboard_loss_actions_t offboard_loss_act)