diff --git a/msg/manual_control_setpoint.msg b/msg/manual_control_setpoint.msg index d3cfb078be..dd1f7d0b5e 100644 --- a/msg/manual_control_setpoint.msg +++ b/msg/manual_control_setpoint.msg @@ -38,6 +38,7 @@ float32 aux5 # default function: payload drop uint8 mode_switch # main mode 3 position switch (mandatory): _MANUAL_, ASSIST, AUTO uint8 return_switch # return to launch 2 position switch (mandatory): _NORMAL_, RTL +uint8 rattitude_switch # rattitude control 2 position switch (optional): _MANUAL, RATTITUDE uint8 posctl_switch # position control 2 position switch (optional): _ALTCTL_, POSCTL uint8 loiter_switch # loiter 2 position switch (optional): _MISSION_, LOITER uint8 acro_switch # acro 2 position switch (optional): _MANUAL_, ACRO diff --git a/msg/rc_channels.msg b/msg/rc_channels.msg index 0fa5ed2fc4..b2e08d864a 100644 --- a/msg/rc_channels.msg +++ b/msg/rc_channels.msg @@ -1,4 +1,4 @@ -int32 RC_CHANNELS_FUNCTION_MAX=19 +int32 RC_CHANNELS_FUNCTION_MAX=20 uint8 RC_CHANNELS_FUNCTION_THROTTLE=0 uint8 RC_CHANNELS_FUNCTION_ROLL=1 uint8 RC_CHANNELS_FUNCTION_PITCH=2 @@ -18,10 +18,11 @@ uint8 RC_CHANNELS_FUNCTION_AUX_5=15 uint8 RC_CHANNELS_FUNCTION_PARAM_1=16 uint8 RC_CHANNELS_FUNCTION_PARAM_2=17 uint8 RC_CHANNELS_FUNCTION_PARAM_3_5=18 +uint8 RC_CHANNELS_FUNCTION_RATTITUDE=19 uint64 timestamp # Timestamp in microseconds since boot time uint64 timestamp_last_valid # Timestamp of last valid RC signal -float32[19] channels # Scaled to -1..1 (throttle: 0..1) +float32[20] channels # Scaled to -1..1 (throttle: 0..1) uint8 channel_count # Number of valid channels -int8[19] function # Functions mapping +int8[20] function # Functions mapping uint8 rssi # Receive signal strength index bool signal_lost # Control signal lost, should be checked together with topic timeout diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index c0c6dc7f0f..e575b9d5e3 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -8,7 +8,8 @@ uint8 MAIN_STATE_AUTO_RTL = 5 uint8 MAIN_STATE_ACRO = 6 uint8 MAIN_STATE_OFFBOARD = 7 uint8 MAIN_STATE_STAB = 8 -uint8 MAIN_STATE_MAX = 9 +uint8 MAIN_STATE_RATTITUDE = 9 +uint8 MAIN_STATE_MAX = 10 # If you change the order, add or remove arming_state_t states make sure to update the arrays # in state_machine_helper.cpp as well. @@ -41,7 +42,8 @@ uint8 NAVIGATION_STATE_DESCEND = 12 # Descend mode (no position control) uint8 NAVIGATION_STATE_TERMINATION = 13 # Termination mode uint8 NAVIGATION_STATE_OFFBOARD = 14 uint8 NAVIGATION_STATE_STAB = 15 # Stabilized mode -uint8 NAVIGATION_STATE_MAX = 16 +uint8 NAVIGATION_STATE_RATTITUDE = 16 # Rattitude (aka "flip") mode +uint8 NAVIGATION_STATE_MAX = 17 # VEHICLE_MODE_FLAG, same as MAV_MODE_FLAG of MAVLink 1.0 protocol uint8 VEHICLE_MODE_FLAG_SAFETY_ARMED = 128 diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 3395b8c069..ee0888033a 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -246,6 +246,9 @@ void print_reject_arm(const char *msg); void print_status(); +transition_result_t check_navigation_state_machine(struct vehicle_status_s *status, + struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos); + transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char *armedBy); /** @@ -941,6 +944,7 @@ int commander_thread_main(int argc, char *argv[]) const char *nav_states_str[vehicle_status_s::NAVIGATION_STATE_MAX]; nav_states_str[vehicle_status_s::NAVIGATION_STATE_MANUAL] = "MANUAL"; nav_states_str[vehicle_status_s::NAVIGATION_STATE_STAB] = "STAB"; + nav_states_str[vehicle_status_s::NAVIGATION_STATE_RATTITUDE] = "RATTITUDE"; nav_states_str[vehicle_status_s::NAVIGATION_STATE_ALTCTL] = "ALTCTL"; nav_states_str[vehicle_status_s::NAVIGATION_STATE_POSCTL] = "POSCTL"; nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION] = "AUTO_MISSION"; @@ -1978,13 +1982,14 @@ int commander_thread_main(int argc, char *argv[]) status.rc_signal_lost = false; - /* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSIST mode and landed) -> disarm + /* check if left stick is in lower left position and we are in MANUAL, Rattitude, or AUTO_READY mode or (ASSIST mode and landed) -> disarm * do it only for rotary wings */ if (status.is_rotary_wing && status.rc_input_mode != vehicle_status_s::RC_IN_MODE_OFF && (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED || status.arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) && (status.main_state == vehicle_status_s::MAIN_STATE_MANUAL || status.main_state == vehicle_status_s::MAIN_STATE_ACRO || status.main_state == vehicle_status_s::MAIN_STATE_STAB || + status.main_state == vehicle_status_s::MAIN_STATE_RATTITUDE || status.condition_landed) && sp_man.r < -STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) { @@ -2196,6 +2201,7 @@ int commander_thread_main(int argc, char *argv[]) * and both failed we want to terminate the flight */ if (status.main_state !=vehicle_status_s::MAIN_STATE_MANUAL && status.main_state !=vehicle_status_s::MAIN_STATE_ACRO && + status.main_state !=vehicle_status_s::MAIN_STATE_RATTITUDE && status.main_state !=vehicle_status_s::MAIN_STATE_STAB && status.main_state !=vehicle_status_s::MAIN_STATE_ALTCTL && status.main_state !=vehicle_status_s::MAIN_STATE_POSCTL && @@ -2220,6 +2226,7 @@ int commander_thread_main(int argc, char *argv[]) * If we are in manual (controlled with RC): * if both failed we want to terminate the flight */ if ((status.main_state ==vehicle_status_s::MAIN_STATE_ACRO || + status.main_state ==vehicle_status_s::MAIN_STATE_RATTITUDE || status.main_state ==vehicle_status_s::MAIN_STATE_MANUAL || status.main_state !=vehicle_status_s::MAIN_STATE_STAB || status.main_state ==vehicle_status_s::MAIN_STATE_ALTCTL || @@ -2513,13 +2520,14 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s } /* manual setpoint has not updated, do not re-evaluate it */ - if ((_last_sp_man.timestamp == sp_man->timestamp) || - ((_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.posctl_switch == sp_man->posctl_switch) && - (_last_sp_man.loiter_switch == sp_man->loiter_switch))) { + if ((last_manual_input == sp_man->timestamp) || + ((last_offboard_switch == sp_man->offboard_switch) && + (last_return_switch == sp_man->return_switch) && + (last_mode_switch == sp_man->mode_switch) && + (last_acro_switch == sp_man->acro_switch) && + (last_rattitude_switch == sp_man->rattitude_switch) && + (last_posctl_switch == sp_man->posctl_switch) && + (last_loiter_switch == sp_man->loiter_switch))) { // update these fields for the geofence system _last_sp_man.timestamp = sp_man->timestamp; @@ -2585,7 +2593,16 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_STAB); } - } else { + } + if(sp_man->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.is_rotary_wing) { + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_RATTITUDE); + } else { + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_STAB); + } + }else { res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_MANUAL); } @@ -2708,6 +2725,18 @@ set_control_mode() control_mode.flag_external_manual_override_ok = false; break; + case vehicle_status_s::NAVIGATION_STATE_RATTITUDE: + control_mode.flag_control_manual_enabled = true; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + control_mode.flag_control_termination_enabled = false; + break; + case vehicle_status_s::NAVIGATION_STATE_ALTCTL: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index d44f1f3506..ea3cab257c 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -304,6 +304,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta switch (new_main_state) { case vehicle_status_s::MAIN_STATE_MANUAL: case vehicle_status_s::MAIN_STATE_ACRO: + case vehicle_status_s::MAIN_STATE_RATTITUDE: case vehicle_status_s::MAIN_STATE_STAB: ret = TRANSITION_CHANGED; break; @@ -528,6 +529,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en switch (status->main_state) { case vehicle_status_s::MAIN_STATE_ACRO: case vehicle_status_s::MAIN_STATE_MANUAL: + case vehicle_status_s::MAIN_STATE_RATTITUDE: case vehicle_status_s::MAIN_STATE_STAB: case vehicle_status_s::MAIN_STATE_ALTCTL: case vehicle_status_s::MAIN_STATE_POSCTL: @@ -555,6 +557,10 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL; break; + case vehicle_status_s::MAIN_STATE_RATTITUDE: + status->nav_state = vehicle_status_s::NAVIGATION_STATE_RATTITUDE; + break; + case vehicle_status_s::MAIN_STATE_STAB: status->nav_state = vehicle_status_s::NAVIGATION_STATE_STAB; break; diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 393cd63a06..3d4dfd62c1 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -1969,6 +1969,15 @@ PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 0); */ PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0); +/** + * Rattitude switch channel mapping. + * + * @min 0 + * @max 18 + * @group Radio Switches + */ +PARAM_DEFINE_INT32(RC_MAP_RATT_SW, 0); + /** * Posctl switch channel mapping. * @@ -2132,6 +2141,23 @@ PARAM_DEFINE_FLOAT(RC_ASSIST_TH, 0.25f); */ PARAM_DEFINE_FLOAT(RC_AUTO_TH, 0.75f); +/** + * Threshold for selecting rattitude mode + * + * 0-1 indicate where in the full channel range the threshold sits + * 0 : min + * 1 : max + * sign indicates polarity of comparison + * positive : true when channel>th + * negative : true when channel