dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

commander: automatically initialize to assist mode if using mavlink manual control

This commit is contained in:
Daniel Agar 2021-01-22 10:31:32 -05:00
parent fa5a6cf712
commit 849f4531a3
3 changed files with 17 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
msg/commander_state.msg
View File

@ -19,3 +19,5 @@ uint8 MAIN_STATE_ORBIT = 14
uint8 MAIN_STATE_MAX = 15
uint8 main_state # main state machine
uint16 main_state_changes

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

@ -1950,7 +1950,15 @@ Commander::run()
}
// update manual_control_setpoint before geofence (which might check sticks or switches)
_manual_control_setpoint_sub.update(&_manual_control_setpoint);
if (_manual_control_setpoint_sub.update(&_manual_control_setpoint)) {
// manual control from mavlink
if (_manual_control_setpoint.data_source > manual_control_setpoint_s::SOURCE_RC) {
// if there's never been a mode change force position control as initial state
if (!_armed.armed && (_internal_state.main_state_changes == 0)) {
_internal_state.main_state = commander_state_s::MAIN_STATE_POSCTL;
}
}
}
/* start geofence result check */
_geofence_result_sub.update(&_geofence_result);

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

@ -369,6 +369,7 @@ main_state_transition(const vehicle_status_s &status, const main_state_t new_mai
if (ret == TRANSITION_CHANGED) {
if (internal_state->main_state != new_main_state) {
internal_state->main_state = new_main_state;
internal_state->main_state_changes++;
internal_state->timestamp = hrt_absolute_time();
} else {
@ -462,6 +463,8 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_
case commander_state_s::MAIN_STATE_POSCTL: {
const bool rc_fallback_allowed = (posctl_nav_loss_act != position_nav_loss_actions_t::LAND_TERMINATE) || !is_armed;
if (rc_lost && is_armed) {
enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc);
@ -472,10 +475,8 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_
* this enables POSCTL using e.g. flow.
* For fixedwing, a global position is needed. */
} else if (is_armed
&& check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags,
!(posctl_nav_loss_act == position_nav_loss_actions_t::LAND_TERMINATE),
status->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING)) {
} else if (check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags,
rc_fallback_allowed, status->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING)) {
// nothing to do - everything done in check_invalid_pos_nav_state
} else {
@ -739,7 +740,7 @@ bool check_invalid_pos_nav_state(vehicle_status_s *status, bool old_failsafe, or
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) {
} else if (status_flags.condition_local_altitude_valid) {
if (status->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;