fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +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
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
5 changed files with 69 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -2881,6 +2881,7 @@ Commander::run()
_vehicle_land_detected.landed,
static_cast<link_loss_actions_t>(_param_nav_rcl_act.get()),
static_cast<offboard_loss_actions_t>(_param_com_obl_act.get()),
static_cast<quadchute_actions_t>(_param_com_qc_act.get()),
static_cast<offboard_loss_rc_actions_t>(_param_com_obl_rc_act.get()),
static_cast<position_nav_loss_actions_t>(_param_com_posctl_navl.get()),
_param_com_rcl_act_t.get(),

3
src/modules/commander/Commander.hpp
View File

@ -223,6 +223,9 @@ private:
(ParamInt<px4::params::RC_MAP_FLTMODE>) _param_rc_map_fltmode,
(ParamInt<px4::params::RC_MAP_MODE_SW>) _param_rc_map_mode_sw,
// Quadchute
(ParamInt<px4::params::COM_QC_ACT>) _param_com_qc_act,
// Offboard
(ParamFloat<px4::params::COM_OF_LOSS_T>) _param_com_of_loss_t,
(ParamInt<px4::params::COM_OBL_ACT>) _param_com_obl_act,

11
src/modules/commander/commander_params.c
View File

@ -416,6 +416,17 @@ PARAM_DEFINE_FLOAT(COM_OF_LOSS_T, 1.0f);
*/
PARAM_DEFINE_INT32(COM_OBL_ACT, 0);
/**
* Set command after a quadchute
*
* @value -1 No action: stay in current flight mode
* @value 0 Return mode
* @value 1 Land mode
* @value 2 Hold mode
* @group Commander
*/
PARAM_DEFINE_INT32(COM_QC_ACT, 0);
/**
* Set offboard loss failsafe mode when RC is available
*

49
src/modules/commander/state_machine_helper.cpp
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)

8
src/modules/commander/state_machine_helper.h
View File

@ -70,6 +70,13 @@ enum class link_loss_actions_t {
LOCKDOWN = 6, // Lock actuators (set actuator outputs to disarmed values)
};
enum class quadchute_actions_t {
NO_ACTION = -1,
AUTO_RTL = 0, // Return mode
AUTO_LAND = 1, // Land mode
AUTO_LOITER = 2, // Hold mode
};
enum class offboard_loss_actions_t {
DISABLED = -1,
AUTO_LAND = 0, // Land mode
@ -122,6 +129,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);