@ -713,8 +713,8 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
@@ -713,8 +713,8 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
break ;
}
const bool throttle_above_low = ( _sp_man . z > 0.1f ) ;
const bool throttle_above_center = ( _sp_man . z > 0.6f ) ;
const bool throttle_above_low = ( _manual_control_setpoint . z > 0.1f ) ;
const bool throttle_above_center = ( _manual_control_setpoint . z > 0.6f ) ;
if ( cmd_arms & & throttle_above_center & &
( status_local - > nav_state = = vehicle_status_s : : NAVIGATION_STATE_POSCTL | |
@ -1428,7 +1428,7 @@ Commander::run()
@@ -1428,7 +1428,7 @@ Commander::run()
# endif // BOARD_HAS_POWER_CONTROL
_sp_man _sub . update ( & _sp_man ) ;
_manual_control_setpoint _sub . update ( & _manual_control_setpoint ) ;
offboard_control_update ( ) ;
@ -1773,7 +1773,7 @@ Commander::run()
@@ -1773,7 +1773,7 @@ Commander::run()
// reset if no longer in LOITER or if manually switched to LOITER
const bool in_loiter_mode = _internal_state . main_state = = commander_state_s : : MAIN_STATE_AUTO_LOITER ;
const bool manual_loiter_switch_on = _sp_man . loiter_switch = = manual_control_setpoint_s : : SWITCH_POS_ON ;
const bool manual_loiter_switch_on = _manual_control_setpoint . loiter_switch = = manual_control_setpoint_s : : SWITCH_POS_ON ;
if ( ! in_loiter_mode | | manual_loiter_switch_on ) {
_geofence_loiter_on = false ;
@ -1782,7 +1782,7 @@ Commander::run()
@@ -1782,7 +1782,7 @@ Commander::run()
// reset if no longer in RTL or if manually switched to RTL
const bool in_rtl_mode = _internal_state . main_state = = commander_state_s : : MAIN_STATE_AUTO_RTL ;
const bool manual_return_switch_on = _sp_man . return_switch = = manual_control_setpoint_s : : SWITCH_POS_ON ;
const bool manual_return_switch_on = _manual_control_setpoint . return_switch = = manual_control_setpoint_s : : SWITCH_POS_ON ;
if ( ! in_rtl_mode | | manual_return_switch_on ) {
_geofence_rtl_on = false ;
@ -1819,11 +1819,11 @@ Commander::run()
@@ -1819,11 +1819,11 @@ Commander::run()
if ( ( override_auto_mode | | override_offboard_mode ) & & is_rotary_wing
& & ! in_low_battery_failsafe & & ! _geofence_warning_action_on ) {
// transition to previous state if sticks are touched
if ( ( _last_sp_man . timestamp ! = _sp_man . timestamp ) & &
( ( fabsf ( _sp_man . x - _last_sp_man . x ) > _min_stick_change ) | |
( fabsf ( _sp_man . y - _last_sp_man . y ) > _min_stick_change ) | |
( fabsf ( _sp_man . z - _last_sp_man . z ) > _min_stick_change ) | |
( fabsf ( _sp_man . r - _last_sp_man . r ) > _min_stick_change ) ) ) {
if ( ( _last_manual_control_setpoint . timestamp ! = _manual_control_setpoint . timestamp ) & &
( ( fabsf ( _manual_control_setpoint . x - _last_manual_control_setpoint . x ) > _min_stick_change ) | |
( fabsf ( _manual_control_setpoint . y - _last_manual_control_setpoint . y ) > _min_stick_change ) | |
( fabsf ( _manual_control_setpoint . z - _last_manual_control_setpoint . z ) > _min_stick_change ) | |
( fabsf ( _manual_control_setpoint . r - _last_manual_control_setpoint . r ) > _min_stick_change ) ) ) {
// revert to position control in any case
main_state_transition ( status , commander_state_s : : MAIN_STATE_POSCTL , status_flags , & _internal_state ) ;
@ -1849,8 +1849,8 @@ Commander::run()
@@ -1849,8 +1849,8 @@ Commander::run()
}
/* RC input check */
if ( ! status_flags . rc_input_blocked & & _sp_man . timestamp ! = 0 & &
( hrt_elapsed_time ( & _sp_man . timestamp ) < ( _param_com_rc_loss_t . get ( ) * 1 _s ) ) ) {
if ( ! status_flags . rc_input_blocked & & _manual_control_setpoint . timestamp ! = 0 & &
( hrt_elapsed_time ( & _manual_control_setpoint . timestamp ) < ( _param_com_rc_loss_t . get ( ) * 1 _s ) ) ) {
/* handle the case where RC signal was regained */
if ( ! status_flags . rc_signal_found_once ) {
@ -1873,19 +1873,22 @@ Commander::run()
@@ -1873,19 +1873,22 @@ Commander::run()
status . rc_signal_lost = false ;
const bool in_armed_state = ( status . arming_state = = vehicle_status_s : : ARMING_STATE_ARMED ) ;
const bool arm_switch_or_button_mapped = _sp_man . arm_switch ! = manual_control_setpoint_s : : SWITCH_POS_NONE ;
const bool arm_switch_or_button_mapped =
_manual_control_setpoint . arm_switch ! = manual_control_setpoint_s : : SWITCH_POS_NONE ;
const bool arm_button_pressed = _param_arm_switch_is_button . get ( )
& & ( _sp_man . arm_switch = = manual_control_setpoint_s : : SWITCH_POS_ON ) ;
& & ( _manual_control_setpoint . arm_switch = = manual_control_setpoint_s : : SWITCH_POS_ON ) ;
/* DISARM
* check if left stick is in lower left position or arm button is pushed or arm switch has transition from arm to disarm
* and we are in MANUAL , Rattitude , or AUTO_READY mode or ( ASSIST mode and landed )
* do it only for rotary wings in manual mode or fixed wing if landed .
* Disable stick - disarming if arming switch or button is mapped */
const bool stick_in_lower_left = _sp_man . r < - STICK_ON_OFF_LIMIT & & ( _sp_man . z < 0.1f ) & & ! arm_switch_or_button_mapped ;
const bool stick_in_lower_left = _manual_control_setpoint . r < - STICK_ON_OFF_LIMIT
& & ( _manual_control_setpoint . z < 0.1f )
& & ! arm_switch_or_button_mapped ;
const bool arm_switch_to_disarm_transition = ! _param_arm_switch_is_button . get ( ) & &
( _last_sp_man_arm_switch = = manual_control_setpoint_s : : SWITCH_POS_ON ) & &
( _sp_man . arm_switch = = manual_control_setpoint_s : : SWITCH_POS_OFF ) ;
( _last_manual_control_setpoint _arm_switch = = manual_control_setpoint_s : : SWITCH_POS_ON ) & &
( _manual_control_setpoint . arm_switch = = manual_control_setpoint_s : : SWITCH_POS_OFF ) ;
if ( in_armed_state & &
( status . rc_input_mode ! = vehicle_status_s : : RC_IN_MODE_OFF ) & &
@ -1907,7 +1910,8 @@ Commander::run()
@@ -1907,7 +1910,8 @@ Commander::run()
_stick_off_counter + + ;
} else if ( ! ( _param_arm_switch_is_button . get ( ) & & _sp_man . arm_switch = = manual_control_setpoint_s : : SWITCH_POS_ON ) ) {
} else if ( ! ( _param_arm_switch_is_button . get ( )
& & _manual_control_setpoint . arm_switch = = manual_control_setpoint_s : : SWITCH_POS_ON ) ) {
/* do not reset the counter when holding the arm button longer than needed */
_stick_off_counter = 0 ;
}
@ -1916,7 +1920,7 @@ Commander::run()
@@ -1916,7 +1920,7 @@ Commander::run()
* check if left stick is in lower right position or arm button is pushed or arm switch has transition from disarm to arm
* and we ' re in MANUAL mode .
* Disable stick - arming if arming switch or button is mapped */
const bool stick_in_lower_right = _sp_man . r > STICK_ON_OFF_LIMIT & & _sp_man . z < 0.1f
const bool stick_in_lower_right = _manual_control_setpoint . r > STICK_ON_OFF_LIMIT & & _manual_control_setpoint . z < 0.1f
& & ! arm_switch_or_button_mapped ;
/* allow a grace period for re-arming: preflight checks don't need to pass during that time,
* for example for accidential in - air disarming */
@ -1924,9 +1928,9 @@ Commander::run()
@@ -1924,9 +1928,9 @@ Commander::run()
& & ( hrt_elapsed_time ( & _last_disarmed_timestamp ) < 5 _s ) ;
const bool arm_switch_to_arm_transition = ! _param_arm_switch_is_button . get ( ) & &
( _last_sp_man _arm_switch = = manual_control_setpoint_s : : SWITCH_POS_OFF ) & &
( _sp_man . arm_switch = = manual_control_setpoint_s : : SWITCH_POS_ON ) & &
( _sp_man . z < 0.1f | | in_arming_grace_period ) ;
( _last_manual_control_setpoint _arm_switch = = manual_control_setpoint_s : : SWITCH_POS_OFF ) & &
( _manual_control_setpoint . arm_switch = = manual_control_setpoint_s : : SWITCH_POS_ON ) & &
( _manual_control_setpoint . z < 0.1f | | in_arming_grace_period ) ;
if ( ! in_armed_state & &
( status . rc_input_mode ! = vehicle_status_s : : RC_IN_MODE_OFF ) & &
@ -1966,12 +1970,13 @@ Commander::run()
@@ -1966,12 +1970,13 @@ Commander::run()
_stick_on_counter + + ;
} else if ( ! ( _param_arm_switch_is_button . get ( ) & & _sp_man . arm_switch = = manual_control_setpoint_s : : SWITCH_POS_ON ) ) {
} else if ( ! ( _param_arm_switch_is_button . get ( )
& & _manual_control_setpoint . arm_switch = = manual_control_setpoint_s : : SWITCH_POS_ON ) ) {
/* do not reset the counter when holding the arm button longer than needed */
_stick_on_counter = 0 ;
}
_last_sp_man_arm_switch = _sp_man . arm_switch ;
_last_manual_control_setpoint_arm_switch = _manual_control_setpoint . arm_switch ;
if ( arming_ret = = TRANSITION_DENIED ) {
/*
@ -1984,7 +1989,7 @@ Commander::run()
@@ -1984,7 +1989,7 @@ Commander::run()
}
/* evaluate the main state machine according to mode switches */
bool first_rc_eval = ( _last_sp_man . timestamp = = 0 ) & & ( _sp_man . timestamp > 0 ) ;
bool first_rc_eval = ( _last_manual_control_setpoint . timestamp = = 0 ) & & ( _manual_control_setpoint . timestamp > 0 ) ;
transition_result_t main_res = set_main_state ( status , & _status_changed ) ;
/* store last position lock state */
@ -2003,7 +2008,7 @@ Commander::run()
@@ -2003,7 +2008,7 @@ Commander::run()
}
/* check throttle kill switch */
if ( _sp_man . kill_switch = = manual_control_setpoint_s : : SWITCH_POS_ON ) {
if ( _manual_control_setpoint . kill_switch = = manual_control_setpoint_s : : SWITCH_POS_ON ) {
/* set lockdown flag */
if ( ! armed . manual_lockdown ) {
mavlink_log_emergency ( & mavlink_log_pub , " Manual kill-switch engaged " ) ;
@ -2011,7 +2016,7 @@ Commander::run()
@@ -2011,7 +2016,7 @@ Commander::run()
armed . manual_lockdown = true ;
}
} else if ( _sp_man . kill_switch = = manual_control_setpoint_s : : SWITCH_POS_OFF ) {
} else if ( _manual_control_setpoint . kill_switch = = manual_control_setpoint_s : : SWITCH_POS_OFF ) {
if ( armed . manual_lockdown ) {
mavlink_log_emergency ( & mavlink_log_pub , " Manual kill-switch disengaged " ) ;
_status_changed = true ;
@ -2025,7 +2030,7 @@ Commander::run()
@@ -2025,7 +2030,7 @@ Commander::run()
if ( ! status_flags . rc_input_blocked & & ! status . rc_signal_lost & & status_flags . rc_signal_found_once ) {
mavlink_log_critical ( & mavlink_log_pub , " Manual control lost " ) ;
status . rc_signal_lost = true ;
_rc_signal_lost_timestamp = _sp_man . timestamp ;
_rc_signal_lost_timestamp = _manual_control_setpoint . timestamp ;
set_health_flags ( subsystem_info_s : : SUBSYSTEM_TYPE_RCRECEIVER , true , true , false , status ) ;
_status_changed = true ;
}
@ -2618,19 +2623,19 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed
@@ -2618,19 +2623,19 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed
const bool altitude_got_valid = ( ! _last_condition_local_altitude_valid & & status_flags . condition_local_altitude_valid ) ;
const bool lpos_got_valid = ( ! _last_condition_local_position_valid & & status_flags . condition_local_position_valid ) ;
const bool gpos_got_valid = ( ! _last_condition_global_position_valid & & status_flags . condition_global_position_valid ) ;
const bool first_time_rc = ( _last_sp_man . timestamp = = 0 ) ;
const bool rc_values_updated = ( _last_sp_man . timestamp ! = _sp_man . timestamp ) ;
const bool first_time_rc = ( _last_manual_control_setpoint . timestamp = = 0 ) ;
const bool rc_values_updated = ( _last_manual_control_setpoint . timestamp ! = _manual_control_setpoint . timestamp ) ;
const bool some_switch_changed =
( _last_sp_man . offboard_switch ! = _sp_man . offboard_switch )
| | ( _last_sp_man . return_switch ! = _sp_man . return_switch )
| | ( _last_sp_man . mode_switch ! = _sp_man . mode_switch )
| | ( _last_sp_man . acro_switch ! = _sp_man . acro_switch )
| | ( _last_sp_man . rattitude_switch ! = _sp_man . rattitude_switch )
| | ( _last_sp_man . posctl_switch ! = _sp_man . posctl_switch )
| | ( _last_sp_man . loiter_switch ! = _sp_man . loiter_switch )
| | ( _last_sp_man . mode_slot ! = _sp_man . mode_slot )
| | ( _last_sp_man . stab_switch ! = _sp_man . stab_switch )
| | ( _last_sp_man . man_switch ! = _sp_man . man_switch ) ;
( _last_manual_control_setpoint . offboard_switch ! = _manual_control_setpoint . offboard_switch )
| | ( _last_manual_control_setpoint . return_switch ! = _manual_control_setpoint . return_switch )
| | ( _last_manual_control_setpoint . mode_switch ! = _manual_control_setpoint . mode_switch )
| | ( _last_manual_control_setpoint . acro_switch ! = _manual_control_setpoint . acro_switch )
| | ( _last_manual_control_setpoint . rattitude_switch ! = _manual_control_setpoint . rattitude_switch )
| | ( _last_manual_control_setpoint . posctl_switch ! = _manual_control_setpoint . posctl_switch )
| | ( _last_manual_control_setpoint . loiter_switch ! = _manual_control_setpoint . loiter_switch )
| | ( _last_manual_control_setpoint . mode_slot ! = _manual_control_setpoint . mode_slot )
| | ( _last_manual_control_setpoint . stab_switch ! = _manual_control_setpoint . stab_switch )
| | ( _last_manual_control_setpoint . man_switch ! = _manual_control_setpoint . man_switch ) ;
// only switch mode based on RC switch if necessary to also allow mode switching via MAVLink
const bool should_evaluate_rc_mode_switch = first_time_rc
@ -2653,25 +2658,25 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed
@@ -2653,25 +2658,25 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed
_internal_state . main_state = = commander_state_s : : MAIN_STATE_RATTITUDE | |
_internal_state . main_state = = commander_state_s : : MAIN_STATE_STAB ) ) {
_last_sp_man . timestamp = _sp_man . timestamp ;
_last_sp_man . x = _sp_man . x ;
_last_sp_man . y = _sp_man . y ;
_last_sp_man . z = _sp_man . z ;
_last_sp_man . r = _sp_man . r ;
_last_manual_control_setpoint . timestamp = _manual_control_setpoint . timestamp ;
_last_manual_control_setpoint . x = _manual_control_setpoint . x ;
_last_manual_control_setpoint . y = _manual_control_setpoint . y ;
_last_manual_control_setpoint . z = _manual_control_setpoint . z ;
_last_manual_control_setpoint . r = _manual_control_setpoint . r ;
}
/* no timestamp change or no switch change -> nothing changed */
return TRANSITION_NOT_CHANGED ;
}
_last_sp_man = _sp_man ;
_last_manual_control_setpoint = _manual_control_setpoint ;
// reset the position and velocity validity calculation to give the best change of being able to select
// the desired mode
reset_posvel_validity ( changed ) ;
/* offboard switch overrides main switch */
if ( _sp_man . offboard_switch = = manual_control_setpoint_s : : SWITCH_POS_ON ) {
if ( _manual_control_setpoint . offboard_switch = = manual_control_setpoint_s : : SWITCH_POS_ON ) {
res = main_state_transition ( status_local , commander_state_s : : MAIN_STATE_OFFBOARD , status_flags , & _internal_state ) ;
if ( res = = TRANSITION_DENIED ) {
@ -2685,7 +2690,7 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed
@@ -2685,7 +2690,7 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed
}
/* RTL switch overrides main switch */
if ( _sp_man . return_switch = = manual_control_setpoint_s : : SWITCH_POS_ON ) {
if ( _manual_control_setpoint . return_switch = = manual_control_setpoint_s : : SWITCH_POS_ON ) {
res = main_state_transition ( status_local , commander_state_s : : MAIN_STATE_AUTO_RTL , status_flags , & _internal_state ) ;
if ( res = = TRANSITION_DENIED ) {
@ -2704,7 +2709,7 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed
@@ -2704,7 +2709,7 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed
}
/* Loiter switch overrides main switch */
if ( _sp_man . loiter_switch = = manual_control_setpoint_s : : SWITCH_POS_ON ) {
if ( _manual_control_setpoint . loiter_switch = = manual_control_setpoint_s : : SWITCH_POS_ON ) {
res = main_state_transition ( status_local , commander_state_s : : MAIN_STATE_AUTO_LOITER , status_flags , & _internal_state ) ;
if ( res = = TRANSITION_DENIED ) {
@ -2716,14 +2721,14 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed
@@ -2716,14 +2721,14 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed
}
/* we know something has changed - check if we are in mode slot operation */
if ( _sp_man . mode_slot ! = manual_control_setpoint_s : : MODE_SLOT_NONE ) {
if ( _manual_control_setpoint . mode_slot ! = manual_control_setpoint_s : : MODE_SLOT_NONE ) {
if ( _sp_man . mode_slot > manual_control_setpoint_s : : MODE_SLOT_NUM ) {
if ( _manual_control_setpoint . mode_slot > manual_control_setpoint_s : : MODE_SLOT_NUM ) {
PX4_WARN ( " m slot overflow " ) ;
return TRANSITION_DENIED ;
}
int new_mode = _flight_mode_slots [ _sp_man . mode_slot - 1 ] ;
int new_mode = _flight_mode_slots [ _manual_control_setpoint . mode_slot - 1 ] ;
if ( new_mode < 0 ) {
/* slot is unused */
@ -2855,19 +2860,19 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed
@@ -2855,19 +2860,19 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed
}
/* offboard and RTL switches off or denied, check main mode switch */
switch ( _sp_man . mode_switch ) {
switch ( _manual_control_setpoint . mode_switch ) {
case manual_control_setpoint_s : : SWITCH_POS_NONE :
res = TRANSITION_NOT_CHANGED ;
break ;
case manual_control_setpoint_s : : SWITCH_POS_OFF : // MANUAL
if ( _sp_man . stab_switch = = manual_control_setpoint_s : : SWITCH_POS_NONE & &
_sp_man . man_switch = = manual_control_setpoint_s : : SWITCH_POS_NONE ) {
if ( _manual_control_setpoint . stab_switch = = manual_control_setpoint_s : : SWITCH_POS_NONE & &
_manual_control_setpoint . man_switch = = manual_control_setpoint_s : : SWITCH_POS_NONE ) {
/*
* Legacy mode :
* Acro switch being used as stabilized switch in FW .
*/
if ( _sp_man . acro_switch = = manual_control_setpoint_s : : SWITCH_POS_ON ) {
if ( _manual_control_setpoint . acro_switch = = manual_control_setpoint_s : : SWITCH_POS_ON ) {
/* manual mode is stabilized already for multirotors, so switch to acro
* for any non - manual mode
*/
@ -2881,7 +2886,7 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed
@@ -2881,7 +2886,7 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed
res = main_state_transition ( status_local , commander_state_s : : MAIN_STATE_MANUAL , status_flags , & _internal_state ) ;
}
} else if ( _sp_man . rattitude_switch = = manual_control_setpoint_s : : SWITCH_POS_ON ) {
} else if ( _manual_control_setpoint . rattitude_switch = = manual_control_setpoint_s : : SWITCH_POS_ON ) {
/* Similar to acro transitions for multirotors. FW aircraft don't need a
* rattitude mode . */
if ( status . vehicle_type = = vehicle_status_s : : VEHICLE_TYPE_ROTARY_WING ) {
@ -2900,19 +2905,19 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed
@@ -2900,19 +2905,19 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed
* - Acro is Acro
* - Manual is not default anymore when the manaul switch is assigned
*/
if ( _sp_man . man_switch = = manual_control_setpoint_s : : SWITCH_POS_ON ) {
if ( _manual_control_setpoint . man_switch = = manual_control_setpoint_s : : SWITCH_POS_ON ) {
res = main_state_transition ( status_local , commander_state_s : : MAIN_STATE_MANUAL , status_flags , & _internal_state ) ;
} else if ( _sp_man . acro_switch = = manual_control_setpoint_s : : SWITCH_POS_ON ) {
} else if ( _manual_control_setpoint . acro_switch = = manual_control_setpoint_s : : SWITCH_POS_ON ) {
res = main_state_transition ( status_local , commander_state_s : : MAIN_STATE_ACRO , status_flags , & _internal_state ) ;
} else if ( _sp_man . rattitude_switch = = manual_control_setpoint_s : : SWITCH_POS_ON ) {
} else if ( _manual_control_setpoint . rattitude_switch = = manual_control_setpoint_s : : SWITCH_POS_ON ) {
res = main_state_transition ( status_local , commander_state_s : : MAIN_STATE_RATTITUDE , status_flags , & _internal_state ) ;
} else if ( _sp_man . stab_switch = = manual_control_setpoint_s : : SWITCH_POS_ON ) {
} else if ( _manual_control_setpoint . stab_switch = = manual_control_setpoint_s : : SWITCH_POS_ON ) {
res = main_state_transition ( status_local , commander_state_s : : MAIN_STATE_STAB , status_flags , & _internal_state ) ;
} else if ( _sp_man . man_switch = = manual_control_setpoint_s : : SWITCH_POS_NONE ) {
} else if ( _manual_control_setpoint . man_switch = = manual_control_setpoint_s : : SWITCH_POS_NONE ) {
// default to MANUAL when no manual switch is set
res = main_state_transition ( status_local , commander_state_s : : MAIN_STATE_MANUAL , status_flags , & _internal_state ) ;
@ -2926,7 +2931,7 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed
@@ -2926,7 +2931,7 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed
break ;
case manual_control_setpoint_s : : SWITCH_POS_MIDDLE : // ASSIST
if ( _sp_man . posctl_switch = = manual_control_setpoint_s : : SWITCH_POS_ON ) {
if ( _manual_control_setpoint . posctl_switch = = manual_control_setpoint_s : : SWITCH_POS_ON ) {
res = main_state_transition ( status_local , commander_state_s : : MAIN_STATE_POSCTL , status_flags , & _internal_state ) ;
if ( res ! = TRANSITION_DENIED ) {
@ -2943,7 +2948,7 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed
@@ -2943,7 +2948,7 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed
break ; // changed successfully or already in this mode
}
if ( _sp_man . posctl_switch ! = manual_control_setpoint_s : : SWITCH_POS_ON ) {
if ( _manual_control_setpoint . posctl_switch ! = manual_control_setpoint_s : : SWITCH_POS_ON ) {
print_reject_mode ( " ALTITUDE CONTROL " ) ;
}