forked from Archive/PX4-Autopilot
commander: automatically initialize to assist mode if using mavlink manual control
This commit is contained in:
parent
fa5a6cf712
commit
849f4531a3
|
@ -19,3 +19,5 @@ uint8 MAIN_STATE_ORBIT = 14
|
||||||
uint8 MAIN_STATE_MAX = 15
|
uint8 MAIN_STATE_MAX = 15
|
||||||
|
|
||||||
uint8 main_state # main state machine
|
uint8 main_state # main state machine
|
||||||
|
|
||||||
|
uint16 main_state_changes
|
||||||
|
|
|
@ -1950,7 +1950,15 @@ Commander::run()
|
||||||
}
|
}
|
||||||
|
|
||||||
// update manual_control_setpoint before geofence (which might check sticks or switches)
|
// 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 */
|
/* start geofence result check */
|
||||||
_geofence_result_sub.update(&_geofence_result);
|
_geofence_result_sub.update(&_geofence_result);
|
||||||
|
|
|
@ -369,6 +369,7 @@ main_state_transition(const vehicle_status_s &status, const main_state_t new_mai
|
||||||
if (ret == TRANSITION_CHANGED) {
|
if (ret == TRANSITION_CHANGED) {
|
||||||
if (internal_state->main_state != new_main_state) {
|
if (internal_state->main_state != new_main_state) {
|
||||||
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();
|
internal_state->timestamp = hrt_absolute_time();
|
||||||
|
|
||||||
} else {
|
} 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: {
|
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) {
|
if (rc_lost && is_armed) {
|
||||||
enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc);
|
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.
|
* this enables POSCTL using e.g. flow.
|
||||||
* For fixedwing, a global position is needed. */
|
* For fixedwing, a global position is needed. */
|
||||||
|
|
||||||
} else if (is_armed
|
} else if (check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags,
|
||||||
&& 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)) {
|
||||||
!(posctl_nav_loss_act == position_nav_loss_actions_t::LAND_TERMINATE),
|
|
||||||
status->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING)) {
|
|
||||||
// nothing to do - everything done in check_invalid_pos_nav_state
|
// nothing to do - everything done in check_invalid_pos_nav_state
|
||||||
|
|
||||||
} else {
|
} 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) {
|
if (status_flags.condition_local_position_valid) {
|
||||||
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
|
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) {
|
if (status->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
|
||||||
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
|
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue