mirror of
https://gitee.com/xiaohuolufeihua/bizhang_-obav.git
synced 2026-05-22 09:28:58 +00:00
Implement RC and DL failsafe action handling for multirotors
Move RC and DL failsafe actions handling from navigator to commander (credits to @AndreasAntener) Separate manual kill switch handling via manual_lockdown to prevent override and release of software lockdown by RC switch Other changes: Add failsafe tune Fix LED blinking for Pixracer Return back support for rc inputs in simulator but now it is configurable via cmake
This commit is contained in:
Anton Matosov
committed by
Lorenz Meier
Lorenz Meier
parent
964dabe179
commit
3a17c07b1e
@@ -118,15 +118,25 @@ static const char *const state_names[vehicle_status_s::ARMING_STATE_MAX] = {
|
||||
static hrt_abstime last_preflight_check = 0; ///< initialize so it gets checked immediately
|
||||
static int last_prearm_ret = 1; ///< initialize to fail
|
||||
|
||||
void set_link_loss_nav_state(struct vehicle_status_s *status,
|
||||
struct actuator_armed_s *armed,
|
||||
status_flags_s *status_flags,
|
||||
const link_loss_actions_t link_loss_act,
|
||||
uint8_t auto_recovery_nav_state);
|
||||
|
||||
void reset_link_loss_globals(struct actuator_armed_s *armed,
|
||||
const bool old_failsafe,
|
||||
const link_loss_actions_t link_loss_act);
|
||||
|
||||
transition_result_t arming_state_transition(struct vehicle_status_s *status,
|
||||
struct battery_status_s *battery,
|
||||
const struct safety_s *safety,
|
||||
arming_state_t new_arming_state,
|
||||
struct actuator_armed_s *armed,
|
||||
bool fRunPreArmChecks,
|
||||
orb_advert_t *mavlink_log_pub, ///< uORB handle for mavlink log
|
||||
struct battery_status_s *battery,
|
||||
const struct safety_s *safety,
|
||||
arming_state_t new_arming_state,
|
||||
struct actuator_armed_s *armed,
|
||||
bool fRunPreArmChecks,
|
||||
orb_advert_t *mavlink_log_pub, ///< uORB handle for mavlink log
|
||||
status_flags_s *status_flags,
|
||||
float avionics_power_rail_voltage,
|
||||
float avionics_power_rail_voltage,
|
||||
bool can_arm_without_gps,
|
||||
hrt_abstime time_since_boot)
|
||||
{
|
||||
@@ -356,7 +366,8 @@ bool is_safe(const struct vehicle_status_s *status, const struct safety_s *safet
|
||||
// 1) Not armed
|
||||
// 2) Armed, but in software lockdown (HIL)
|
||||
// 3) Safety switch is present AND engaged -> actuators locked
|
||||
if (!armed->armed || (armed->armed && armed->lockdown) || (safety->safety_switch_available && !safety->safety_off)) {
|
||||
const bool lockdown = (armed->lockdown || armed->manual_lockdown);
|
||||
if (!armed->armed || (armed->armed && lockdown) || (safety->safety_switch_available && !safety->safety_off)) {
|
||||
return true;
|
||||
|
||||
} else {
|
||||
@@ -640,7 +651,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta
|
||||
*/
|
||||
void enable_failsafe(struct vehicle_status_s *status,
|
||||
bool old_failsafe,
|
||||
orb_advert_t *mavlink_log_pub, const char * reason) {
|
||||
orb_advert_t *mavlink_log_pub, const char *reason) {
|
||||
if (old_failsafe == false) {
|
||||
mavlink_and_console_log_info(mavlink_log_pub, reason);
|
||||
}
|
||||
@@ -650,19 +661,34 @@ void enable_failsafe(struct vehicle_status_s *status,
|
||||
/**
|
||||
* Check failsafe and main status and set navigation status for navigator accordingly
|
||||
*/
|
||||
bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *internal_state,
|
||||
bool set_nav_state(struct vehicle_status_s *status,
|
||||
struct actuator_armed_s *armed,
|
||||
struct commander_state_s *internal_state,
|
||||
orb_advert_t *mavlink_log_pub,
|
||||
const bool data_link_loss_enabled, const bool mission_finished,
|
||||
const bool stay_in_failsafe, status_flags_s *status_flags, bool landed,
|
||||
const bool rc_loss_enabled, const int offb_loss_act, const int offb_loss_rc_act)
|
||||
const link_loss_actions_t data_link_loss_act,
|
||||
const bool mission_finished,
|
||||
const bool stay_in_failsafe,
|
||||
status_flags_s *status_flags,
|
||||
bool landed,
|
||||
const link_loss_actions_t rc_loss_act,
|
||||
const int offb_loss_act,
|
||||
const int offb_loss_rc_act)
|
||||
{
|
||||
navigation_state_t nav_state_old = status->nav_state;
|
||||
|
||||
bool armed = (status->arming_state == vehicle_status_s::ARMING_STATE_ARMED
|
||||
const bool data_link_loss_act_configured = data_link_loss_act > link_loss_actions_t::DISABLED;
|
||||
const bool rc_loss_act_configured = rc_loss_act > link_loss_actions_t::DISABLED;
|
||||
const bool rc_lost = rc_loss_act_configured && (status->rc_signal_lost || status_flags->rc_signal_lost_cmd);
|
||||
|
||||
bool is_armed = (status->arming_state == vehicle_status_s::ARMING_STATE_ARMED
|
||||
|| status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR);
|
||||
bool old_failsafe = status->failsafe;
|
||||
status->failsafe = false;
|
||||
|
||||
// Safe to do reset flags here, as if loss state persists flags will be restored in the code below
|
||||
reset_link_loss_globals(armed, old_failsafe, rc_loss_act);
|
||||
reset_link_loss_globals(armed, old_failsafe, data_link_loss_act);
|
||||
|
||||
/* evaluate main state to decide in normal (non-failsafe) mode */
|
||||
switch (internal_state->main_state) {
|
||||
case commander_state_s::MAIN_STATE_ACRO:
|
||||
@@ -672,21 +698,10 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in
|
||||
case commander_state_s::MAIN_STATE_ALTCTL:
|
||||
|
||||
/* require RC for all manual modes */
|
||||
if (rc_loss_enabled && (status->rc_signal_lost || status_flags->rc_signal_lost_cmd) && armed) {
|
||||
if (rc_lost && is_armed) {
|
||||
enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc);
|
||||
|
||||
if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) {
|
||||
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER;
|
||||
|
||||
} else if (status_flags->condition_local_position_valid) {
|
||||
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
|
||||
|
||||
} else if (status_flags->condition_local_altitude_valid) {
|
||||
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
|
||||
|
||||
} else {
|
||||
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
|
||||
}
|
||||
set_rc_loss_nav_state(status, armed, status_flags, rc_loss_act);
|
||||
|
||||
} else {
|
||||
switch (internal_state->main_state) {
|
||||
@@ -719,26 +734,11 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in
|
||||
break;
|
||||
|
||||
case commander_state_s::MAIN_STATE_POSCTL: {
|
||||
const bool rc_lost = rc_loss_enabled && (status->rc_signal_lost || status_flags->rc_signal_lost_cmd);
|
||||
|
||||
if (rc_lost && armed) {
|
||||
if (rc_lost && is_armed) {
|
||||
enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc);
|
||||
|
||||
if (status_flags->condition_global_position_valid &&
|
||||
status_flags->condition_home_position_valid &&
|
||||
!status_flags->gps_failure) {
|
||||
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER;
|
||||
|
||||
} else if (status_flags->condition_local_position_valid &&
|
||||
!status_flags->gps_failure) {
|
||||
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
|
||||
|
||||
} else if (status_flags->condition_local_altitude_valid) {
|
||||
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
|
||||
|
||||
} else {
|
||||
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
|
||||
}
|
||||
set_rc_loss_nav_state(status, armed, status_flags, rc_loss_act);
|
||||
|
||||
/* As long as there is RC, we can fallback to ALTCTL, or STAB. */
|
||||
/* A local position estimate is enough for POSCTL for multirotors,
|
||||
@@ -747,7 +747,7 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in
|
||||
|
||||
} else if (((status->is_rotary_wing && !status_flags->condition_local_position_valid) ||
|
||||
(!status->is_rotary_wing && !status_flags->condition_global_position_valid))
|
||||
&& armed) {
|
||||
&& is_armed) {
|
||||
enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc);
|
||||
|
||||
if (status_flags->condition_local_altitude_valid) {
|
||||
@@ -806,41 +806,19 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in
|
||||
/* datalink loss enabled:
|
||||
* check for datalink lost: this should always trigger RTGS */
|
||||
|
||||
} else if (data_link_loss_enabled && status->data_link_lost) {
|
||||
} else if (data_link_loss_act_configured && status->data_link_lost) {
|
||||
enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_datalink);
|
||||
|
||||
if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) {
|
||||
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;
|
||||
set_data_link_loss_nav_state(status, armed, status_flags, data_link_loss_act);
|
||||
|
||||
} else if (status_flags->condition_local_position_valid) {
|
||||
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
|
||||
|
||||
} else if (status_flags->condition_local_altitude_valid) {
|
||||
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
|
||||
|
||||
} else {
|
||||
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
|
||||
}
|
||||
|
||||
/* datalink loss disabled:
|
||||
/* datalink loss DISABLED:
|
||||
* check if both, RC and datalink are lost during the mission
|
||||
* or all links are lost after the mission finishes in air: this should always trigger RCRECOVER */
|
||||
|
||||
} else if (!data_link_loss_enabled && status->rc_signal_lost && status->data_link_lost && !landed && mission_finished) {
|
||||
} else if (!data_link_loss_act_configured && status->rc_signal_lost && status->data_link_lost && !landed && mission_finished) {
|
||||
enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_datalink);
|
||||
|
||||
if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) {
|
||||
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER;
|
||||
|
||||
} else if (status_flags->condition_local_position_valid) {
|
||||
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
|
||||
|
||||
} else if (status_flags->condition_local_altitude_valid) {
|
||||
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
|
||||
|
||||
} else {
|
||||
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
|
||||
}
|
||||
set_rc_loss_nav_state(status, armed, status_flags, rc_loss_act);
|
||||
|
||||
/* stay where you are if you should stay in failsafe, otherwise everything is perfect */
|
||||
|
||||
@@ -862,44 +840,23 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in
|
||||
|
||||
/* also go into failsafe if just datalink is lost */
|
||||
|
||||
} else if (status->data_link_lost && data_link_loss_enabled) {
|
||||
} else if (status->data_link_lost && data_link_loss_act_configured) {
|
||||
enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_datalink);
|
||||
|
||||
if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) {
|
||||
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;
|
||||
set_data_link_loss_nav_state(status, armed, status_flags, data_link_loss_act);
|
||||
|
||||
} else if (status_flags->condition_local_position_valid) {
|
||||
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
|
||||
/* go into failsafe if RC is lost and datalink loss is not set up and rc loss is not DISABLED */
|
||||
|
||||
} else if (status_flags->condition_local_altitude_valid) {
|
||||
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
|
||||
|
||||
} else {
|
||||
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
|
||||
}
|
||||
|
||||
/* go into failsafe if RC is lost and datalink loss is not set up and rc loss is not disabled */
|
||||
|
||||
} else if (status->rc_signal_lost && rc_loss_enabled && !data_link_loss_enabled) {
|
||||
} else if (rc_lost && !data_link_loss_act_configured) {
|
||||
enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc);
|
||||
if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) {
|
||||
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;
|
||||
|
||||
} else if (status_flags->condition_local_position_valid) {
|
||||
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
|
||||
|
||||
} else if (status_flags->condition_local_altitude_valid) {
|
||||
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
|
||||
|
||||
} else {
|
||||
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
|
||||
}
|
||||
set_rc_loss_nav_state(status, armed, status_flags, rc_loss_act);
|
||||
|
||||
/* don't bother if RC is lost if datalink is connected */
|
||||
|
||||
} else if (status->rc_signal_lost) {
|
||||
|
||||
/* this mode is ok, we don't need RC for loitering */
|
||||
/* this mode is ok, we don't need RC for LOITERing */
|
||||
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
|
||||
|
||||
} else {
|
||||
@@ -1103,6 +1060,93 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in
|
||||
return status->nav_state != nav_state_old;
|
||||
}
|
||||
|
||||
void set_rc_loss_nav_state(struct vehicle_status_s *status,
|
||||
struct actuator_armed_s *armed,
|
||||
status_flags_s *status_flags,
|
||||
const link_loss_actions_t link_loss_act)
|
||||
{
|
||||
set_link_loss_nav_state(status, armed, status_flags, link_loss_act, vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER);
|
||||
}
|
||||
|
||||
void set_data_link_loss_nav_state(struct vehicle_status_s *status,
|
||||
struct actuator_armed_s *armed,
|
||||
status_flags_s *status_flags,
|
||||
const link_loss_actions_t link_loss_act)
|
||||
{
|
||||
set_link_loss_nav_state(status, armed, status_flags, link_loss_act, vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS);
|
||||
}
|
||||
|
||||
void set_link_loss_nav_state(struct vehicle_status_s *status,
|
||||
struct actuator_armed_s *armed,
|
||||
status_flags_s *status_flags,
|
||||
const link_loss_actions_t link_loss_act,
|
||||
uint8_t auto_recovery_nav_state)
|
||||
{
|
||||
// do the best you can according to the action set
|
||||
if (link_loss_act == link_loss_actions_t::AUTO_RECOVER
|
||||
&& status_flags->condition_global_position_valid && status_flags->condition_home_position_valid)
|
||||
{
|
||||
status->nav_state = auto_recovery_nav_state;
|
||||
}
|
||||
else if (link_loss_act == link_loss_actions_t::AUTO_LOITER && status_flags->condition_global_position_valid)
|
||||
{
|
||||
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
|
||||
}
|
||||
else if (link_loss_act == link_loss_actions_t::AUTO_RTL
|
||||
&& status_flags->condition_global_position_valid && status_flags->condition_home_position_valid)
|
||||
{
|
||||
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
|
||||
}
|
||||
else if (link_loss_act == link_loss_actions_t::AUTO_LAND && status_flags->condition_local_position_valid)
|
||||
{
|
||||
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
|
||||
}
|
||||
else if (link_loss_act == link_loss_actions_t::TERMINATE)
|
||||
{
|
||||
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
|
||||
armed->force_failsafe = true;
|
||||
}
|
||||
else if (link_loss_act == link_loss_actions_t::LOCKDOWN)
|
||||
{
|
||||
armed->lockdown = true;
|
||||
|
||||
// do the best you can according to the current state
|
||||
}
|
||||
else if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid)
|
||||
{
|
||||
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
|
||||
}
|
||||
else if (status_flags->condition_local_position_valid)
|
||||
{
|
||||
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
|
||||
}
|
||||
else if (status_flags->condition_local_altitude_valid)
|
||||
{
|
||||
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
|
||||
}
|
||||
else
|
||||
{
|
||||
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
|
||||
}
|
||||
}
|
||||
|
||||
void reset_link_loss_globals(struct actuator_armed_s *armed,
|
||||
const bool old_failsafe,
|
||||
const link_loss_actions_t link_loss_act)
|
||||
{
|
||||
if (old_failsafe)
|
||||
{
|
||||
if (link_loss_act == link_loss_actions_t::TERMINATE)
|
||||
{
|
||||
armed->force_failsafe = false;
|
||||
}
|
||||
else if (link_loss_act == link_loss_actions_t::LOCKDOWN)
|
||||
{
|
||||
armed->lockdown = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_pub, bool prearm, bool force_report,
|
||||
status_flags_s *status_flags, battery_status_s *battery, bool can_arm_without_gps, hrt_abstime time_since_boot)
|
||||
{
|
||||
|
||||
Reference in New Issue
Block a user