diff --git a/src/examples/fixedwing_control/main.cpp b/src/examples/fixedwing_control/main.cpp index ddb6fa4ca6..08895a11ec 100644 --- a/src/examples/fixedwing_control/main.cpp +++ b/src/examples/fixedwing_control/main.cpp @@ -284,8 +284,8 @@ int fixedwing_control_thread_main(int argc, char *argv[]) memset(&rates_sp, 0, sizeof(rates_sp)); struct vehicle_global_position_s global_pos; memset(&global_pos, 0, sizeof(global_pos)); - struct manual_control_setpoint_s manual_sp; - memset(&manual_sp, 0, sizeof(manual_sp)); + struct manual_control_setpoint_s manual_control_setpoint; + memset(&manual_control_setpoint, 0, sizeof(manual_control_setpoint)); struct vehicle_status_s vstatus; memset(&vstatus, 0, sizeof(vstatus)); struct position_setpoint_s global_sp; @@ -311,7 +311,7 @@ int fixedwing_control_thread_main(int argc, char *argv[]) /* subscribe to topics. */ int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - int manual_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + int manual_control_setpoint_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); int global_sp_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); @@ -367,8 +367,8 @@ int fixedwing_control_thread_main(int argc, char *argv[]) orb_check(global_pos_sub, &pos_updated); bool global_sp_updated; orb_check(global_sp_sub, &global_sp_updated); - bool manual_sp_updated; - orb_check(manual_sp_sub, &manual_sp_updated); + bool manual_control_setpoint_updated; + orb_check(manual_control_setpoint_sub, &manual_control_setpoint_updated); /* get a local copy of attitude */ orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); @@ -379,16 +379,16 @@ int fixedwing_control_thread_main(int argc, char *argv[]) memcpy(&global_sp, &triplet.current, sizeof(global_sp)); } - if (manual_sp_updated) + if (manual_control_setpoint_updated) /* get the RC (or otherwise user based) input */ { - orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp); + orb_copy(ORB_ID(manual_control_setpoint), manual_control_setpoint_sub, &manual_control_setpoint); } /* check if the throttle was ever more than 50% - go later only to failsafe if yes */ - if (PX4_ISFINITE(manual_sp.z) && - (manual_sp.z >= 0.6f) && - (manual_sp.z <= 1.0f)) { + if (PX4_ISFINITE(manual_control_setpoint.z) && + (manual_control_setpoint.z >= 0.6f) && + (manual_control_setpoint.z <= 1.0f)) { } /* get the system status and the flight mode we're in */ diff --git a/src/examples/rover_steering_control/main.cpp b/src/examples/rover_steering_control/main.cpp index c07b2981be..df51eb94b4 100644 --- a/src/examples/rover_steering_control/main.cpp +++ b/src/examples/rover_steering_control/main.cpp @@ -235,8 +235,8 @@ int rover_steering_control_thread_main(int argc, char *argv[]) memset(&att_sp, 0, sizeof(att_sp)); struct vehicle_global_position_s global_pos; memset(&global_pos, 0, sizeof(global_pos)); - struct manual_control_setpoint_s manual_sp; - memset(&manual_sp, 0, sizeof(manual_sp)); + struct manual_control_setpoint_s manual_control_setpoint; + memset(&manual_control_setpoint, 0, sizeof(manual_control_setpoint)); struct vehicle_status_s vstatus; memset(&vstatus, 0, sizeof(vstatus)); struct position_setpoint_s global_sp; @@ -265,7 +265,7 @@ int rover_steering_control_thread_main(int argc, char *argv[]) int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - int manual_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + int manual_control_setpoint_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); @@ -325,8 +325,8 @@ int rover_steering_control_thread_main(int argc, char *argv[]) orb_check(global_pos_sub, &pos_updated); bool att_sp_updated; orb_check(att_sp_sub, &att_sp_updated); - bool manual_sp_updated; - orb_check(manual_sp_sub, &manual_sp_updated); + bool manual_control_setpoint_updated; + orb_check(manual_control_setpoint_sub, &manual_control_setpoint_updated); /* get a local copy of attitude */ orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); @@ -338,10 +338,10 @@ int rover_steering_control_thread_main(int argc, char *argv[]) /* control attitude / heading */ control_attitude(&_att_sp, &att, &actuators); - if (manual_sp_updated) + if (manual_control_setpoint_updated) /* get the RC (or otherwise user based) input */ { - orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp); + orb_copy(ORB_ID(manual_control_setpoint), manual_control_setpoint_sub, &manual_control_setpoint); } // XXX copy from manual depending on flight / usage mode to override diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/manualControlCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/manualControlCheck.cpp index ae82669a21..fecfa35d33 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/manualControlCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/manualControlCheck.cpp @@ -45,12 +45,12 @@ bool PreFlightCheck::manualControlCheck(orb_advert_t *mavlink_log_pub, const boo uORB::SubscriptionData manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; manual_control_setpoint_sub.update(); - const manual_control_setpoint_s &manual_control = manual_control_setpoint_sub.get(); + const manual_control_setpoint_s &manual_control_setpoint = manual_control_setpoint_sub.get(); - if (hrt_elapsed_time(&manual_control.timestamp) < 1_s) { + if (hrt_elapsed_time(&manual_control_setpoint.timestamp) < 1_s) { //check action switches - if (manual_control.return_switch == manual_control_setpoint_s::SWITCH_POS_ON) { + if (manual_control_setpoint.return_switch == manual_control_setpoint_s::SWITCH_POS_ON) { success = false; if (report_fail) { @@ -58,7 +58,7 @@ bool PreFlightCheck::manualControlCheck(orb_advert_t *mavlink_log_pub, const boo } } - if (manual_control.kill_switch == manual_control_setpoint_s::SWITCH_POS_ON) { + if (manual_control_setpoint.kill_switch == manual_control_setpoint_s::SWITCH_POS_ON) { success = false; if (report_fail) { @@ -66,7 +66,7 @@ bool PreFlightCheck::manualControlCheck(orb_advert_t *mavlink_log_pub, const boo } } - if (manual_control.gear_switch == manual_control_setpoint_s::SWITCH_POS_ON) { + if (manual_control_setpoint.gear_switch == manual_control_setpoint_s::SWITCH_POS_ON) { success = false; if (report_fail) { @@ -74,7 +74,7 @@ bool PreFlightCheck::manualControlCheck(orb_advert_t *mavlink_log_pub, const boo } } - if (manual_control.transition_switch == manual_control_setpoint_s::SWITCH_POS_ON) { + if (manual_control_setpoint.transition_switch == manual_control_setpoint_s::SWITCH_POS_ON) { success = false; if (report_fail) { diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 76116783d4..b815ac71e8 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -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() #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() // 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() // 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() 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() } /* 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() 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() _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() * 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() && (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() _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() } /* 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() } /* 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() 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() 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 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 _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 } /* 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 } /* 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 } /* 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 } /* 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 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 * - 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 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 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"); } diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index ccc1b0341e..008a7d2f2d 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -343,11 +343,11 @@ private: unsigned int _leds_counter{0}; - manual_control_setpoint_s _sp_man{}; ///< the current manual control setpoint - manual_control_setpoint_s _last_sp_man{}; ///< the manual control setpoint valid at the last mode switch + manual_control_setpoint_s _manual_control_setpoint{}; ///< the current manual control setpoint + manual_control_setpoint_s _last_manual_control_setpoint{}; ///< the manual control setpoint valid at the last mode switch hrt_abstime _rc_signal_lost_timestamp{0}; ///< Time at which the RC reception was lost int32_t _flight_mode_slots[manual_control_setpoint_s::MODE_SLOT_NUM] {}; - uint8_t _last_sp_man_arm_switch{0}; + uint8_t _last_manual_control_setpoint_arm_switch{0}; float _min_stick_change{}; uint32_t _stick_off_counter{0}; uint32_t _stick_on_counter{0}; @@ -400,7 +400,7 @@ private: uORB::Subscription _land_detector_sub{ORB_ID(vehicle_land_detected)}; uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; uORB::Subscription _safety_sub{ORB_ID(safety)}; - uORB::Subscription _sp_man_sub{ORB_ID(manual_control_setpoint)}; + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; uORB::Subscription _subsys_sub{ORB_ID(subsystem_info)}; uORB::Subscription _system_power_sub{ORB_ID(system_power)}; uORB::Subscription _telemetry_status_sub{ORB_ID(telemetry_status)}; diff --git a/src/modules/commander/rc_calibration.cpp b/src/modules/commander/rc_calibration.cpp index 6a0a5ae7dc..20266bb2fd 100644 --- a/src/modules/commander/rc_calibration.cpp +++ b/src/modules/commander/rc_calibration.cpp @@ -52,17 +52,17 @@ int do_trim_calibration(orb_advert_t *mavlink_log_pub) { - uORB::Subscription sub_man{ORB_ID(manual_control_setpoint)}; + uORB::Subscription manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; px4_usleep(400000); - manual_control_setpoint_s sp{}; - bool changed = sub_man.updated(); + manual_control_setpoint_s manual_control_setpoint{}; + bool changed = manual_control_setpoint_sub.updated(); if (!changed) { mavlink_log_critical(mavlink_log_pub, "no inputs, aborting"); return PX4_ERROR; } - sub_man.copy(&sp); + manual_control_setpoint_sub.copy(&manual_control_setpoint); /* load trim values which are active */ float roll_trim_active; @@ -83,15 +83,15 @@ int do_trim_calibration(orb_advert_t *mavlink_log_pub) /* set parameters: the new trim values are the combination of active trim values and the values coming from the remote control of the user */ - float p = sp.y * roll_scale + roll_trim_active; + float p = manual_control_setpoint.y * roll_scale + roll_trim_active; int p1r = param_set(param_find("TRIM_ROLL"), &p); /* we explicitly swap sign here because the trim is added to the actuator controls which are moving in an inverse sense to manual pitch inputs */ - p = -sp.x * pitch_scale + pitch_trim_active; + p = -manual_control_setpoint.x * pitch_scale + pitch_trim_active; int p2r = param_set(param_find("TRIM_PITCH"), &p); - p = sp.r * yaw_scale + yaw_trim_active; + p = manual_control_setpoint.r * yaw_scale + yaw_trim_active; int p3r = param_set(param_find("TRIM_YAW"), &p); if (p1r != 0 || p2r != 0 || p3r != 0) { diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index 1d139c8c9f..cadd8b5144 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -141,11 +141,12 @@ FixedwingAttitudeControl::vehicle_manual_poll() if (_vcontrol_mode.flag_control_manual_enabled && (!is_tailsitter_transition || is_fixed_wing)) { // Always copy the new manual setpoint, even if it wasn't updated, to fill the _actuators with valid values - if (_manual_sub.copy(&_manual)) { + if (_manual_control_setpoint_sub.copy(&_manual_control_setpoint)) { // Check if we are in rattitude mode and the pilot is above the threshold on pitch if (_vcontrol_mode.flag_control_rattitude_enabled) { - if (fabsf(_manual.y) > _param_fw_ratt_th.get() || fabsf(_manual.x) > _param_fw_ratt_th.get()) { + if (fabsf(_manual_control_setpoint.y) > _param_fw_ratt_th.get() + || fabsf(_manual_control_setpoint.x) > _param_fw_ratt_th.get()) { _vcontrol_mode.flag_control_attitude_enabled = false; } } @@ -156,16 +157,17 @@ FixedwingAttitudeControl::vehicle_manual_poll() if (_vcontrol_mode.flag_control_attitude_enabled) { // STABILIZED mode generate the attitude setpoint from manual user inputs - _att_sp.roll_body = _manual.y * radians(_param_fw_man_r_max.get()) + radians(_param_fw_rsp_off.get()); + _att_sp.roll_body = _manual_control_setpoint.y * radians(_param_fw_man_r_max.get()) + radians(_param_fw_rsp_off.get()); _att_sp.roll_body = constrain(_att_sp.roll_body, -radians(_param_fw_man_r_max.get()), radians(_param_fw_man_r_max.get())); - _att_sp.pitch_body = -_manual.x * radians(_param_fw_man_p_max.get()) + radians(_param_fw_psp_off.get()); + _att_sp.pitch_body = -_manual_control_setpoint.x * radians(_param_fw_man_p_max.get()) + + radians(_param_fw_psp_off.get()); _att_sp.pitch_body = constrain(_att_sp.pitch_body, -radians(_param_fw_man_p_max.get()), radians(_param_fw_man_p_max.get())); _att_sp.yaw_body = 0.0f; - _att_sp.thrust_body[0] = _manual.z; + _att_sp.thrust_body[0] = _manual_control_setpoint.z; Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body)); q.copyTo(_att_sp.q_d); @@ -179,19 +181,22 @@ FixedwingAttitudeControl::vehicle_manual_poll() // RATE mode we need to generate the rate setpoint from manual user inputs _rates_sp.timestamp = hrt_absolute_time(); - _rates_sp.roll = _manual.y * radians(_param_fw_acro_x_max.get()); - _rates_sp.pitch = -_manual.x * radians(_param_fw_acro_y_max.get()); - _rates_sp.yaw = _manual.r * radians(_param_fw_acro_z_max.get()); - _rates_sp.thrust_body[0] = _manual.z; + _rates_sp.roll = _manual_control_setpoint.y * radians(_param_fw_acro_x_max.get()); + _rates_sp.pitch = -_manual_control_setpoint.x * radians(_param_fw_acro_y_max.get()); + _rates_sp.yaw = _manual_control_setpoint.r * radians(_param_fw_acro_z_max.get()); + _rates_sp.thrust_body[0] = _manual_control_setpoint.z; _rate_sp_pub.publish(_rates_sp); } else { /* manual/direct control */ - _actuators.control[actuator_controls_s::INDEX_ROLL] = _manual.y * _param_fw_man_r_sc.get() + _param_trim_roll.get(); - _actuators.control[actuator_controls_s::INDEX_PITCH] = -_manual.x * _param_fw_man_p_sc.get() + _param_trim_pitch.get(); - _actuators.control[actuator_controls_s::INDEX_YAW] = _manual.r * _param_fw_man_y_sc.get() + _param_trim_yaw.get(); - _actuators.control[actuator_controls_s::INDEX_THROTTLE] = _manual.z; + _actuators.control[actuator_controls_s::INDEX_ROLL] = + _manual_control_setpoint.y * _param_fw_man_r_sc.get() + _param_trim_roll.get(); + _actuators.control[actuator_controls_s::INDEX_PITCH] = + -_manual_control_setpoint.x * _param_fw_man_p_sc.get() + _param_trim_pitch.get(); + _actuators.control[actuator_controls_s::INDEX_YAW] = + _manual_control_setpoint.r * _param_fw_man_y_sc.get() + _param_trim_yaw.get(); + _actuators.control[actuator_controls_s::INDEX_THROTTLE] = _manual_control_setpoint.z; } } } @@ -543,7 +548,7 @@ void FixedwingAttitudeControl::Run() /* add in manual rudder control in manual modes */ if (_vcontrol_mode.flag_control_manual_enabled) { - _actuators.control[actuator_controls_s::INDEX_YAW] += _manual.r; + _actuators.control[actuator_controls_s::INDEX_YAW] += _manual_control_setpoint.r; } if (!PX4_ISFINITE(yaw_u)) { @@ -626,10 +631,10 @@ void FixedwingAttitudeControl::Run() * constrain(_actuators.control[actuator_controls_s::INDEX_ROLL], -1.0f, 1.0f); _actuators.control[actuator_controls_s::INDEX_FLAPS] = _flaps_applied; - _actuators.control[5] = _manual.aux1; + _actuators.control[5] = _manual_control_setpoint.aux1; _actuators.control[actuator_controls_s::INDEX_AIRBRAKES] = _flaperons_applied; // FIXME: this should use _vcontrol_mode.landing_gear_pos in the future - _actuators.control[7] = _manual.aux3; + _actuators.control[7] = _manual_control_setpoint.aux3; /* lazily publish the setpoint only once available */ _actuators.timestamp = hrt_absolute_time(); @@ -652,9 +657,9 @@ void FixedwingAttitudeControl::control_flaps(const float dt) float flap_control = 0.0f; /* map flaps by default to manual if valid */ - if (PX4_ISFINITE(_manual.flaps) && _vcontrol_mode.flag_control_manual_enabled + if (PX4_ISFINITE(_manual_control_setpoint.flaps) && _vcontrol_mode.flag_control_manual_enabled && fabsf(_param_fw_flaps_scl.get()) > 0.01f) { - flap_control = 0.5f * (_manual.flaps + 1.0f) * _param_fw_flaps_scl.get(); + flap_control = 0.5f * (_manual_control_setpoint.flaps + 1.0f) * _param_fw_flaps_scl.get(); } else if (_vcontrol_mode.flag_control_auto_enabled && fabsf(_param_fw_flaps_scl.get()) > 0.01f) { @@ -686,10 +691,10 @@ void FixedwingAttitudeControl::control_flaps(const float dt) float flaperon_control = 0.0f; /* map flaperons by default to manual if valid */ - if (PX4_ISFINITE(_manual.aux2) && _vcontrol_mode.flag_control_manual_enabled + if (PX4_ISFINITE(_manual_control_setpoint.aux2) && _vcontrol_mode.flag_control_manual_enabled && fabsf(_param_fw_flaperon_scl.get()) > 0.01f) { - flaperon_control = 0.5f * (_manual.aux2 + 1.0f) * _param_fw_flaperon_scl.get(); + flaperon_control = 0.5f * (_manual_control_setpoint.aux2 + 1.0f) * _param_fw_flaperon_scl.get(); } else if (_vcontrol_mode.flag_control_auto_enabled && fabsf(_param_fw_flaperon_scl.get()) > 0.01f) { diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp index 95439400de..0837b142d4 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp @@ -98,7 +98,7 @@ private: uORB::Subscription _att_sp_sub{ORB_ID(vehicle_attitude_setpoint)}; /**< vehicle attitude setpoint */ uORB::Subscription _battery_status_sub{ORB_ID(battery_status)}; /**< battery status subscription */ uORB::Subscription _local_pos_sub{ORB_ID(vehicle_local_position)}; /**< local position subscription */ - uORB::Subscription _manual_sub{ORB_ID(manual_control_setpoint)}; /**< notification of manual control updates */ + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; /**< notification of manual control updates */ uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; /**< notification of parameter updates */ uORB::Subscription _rates_sp_sub{ORB_ID(vehicle_rates_setpoint)}; /**< vehicle rates setpoint */ uORB::Subscription _vcontrol_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle status subscription */ @@ -114,7 +114,7 @@ private: uORB::PublicationMulti _rate_ctrl_status_pub{ORB_ID(rate_ctrl_status)}; actuator_controls_s _actuators {}; /**< actuator control inputs */ - manual_control_setpoint_s _manual {}; /**< r/c channel data */ + manual_control_setpoint_s _manual_control_setpoint {}; /**< r/c channel data */ vehicle_attitude_s _att {}; /**< vehicle attitude setpoint */ vehicle_attitude_setpoint_s _att_sp {}; /**< vehicle attitude setpoint */ vehicle_control_mode_s _vcontrol_mode {}; /**< vehicle control mode */ diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 355d04ec8a..e920aa4f0d 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -229,17 +229,17 @@ FixedwingPositionControl::get_demanded_airspeed() float altctrl_airspeed = 0; // neutral throttle corresponds to trim airspeed - if (_manual.z < 0.5f) { + if (_manual_control_setpoint.z < 0.5f) { // lower half of throttle is min to trim airspeed altctrl_airspeed = _param_fw_airspd_min.get() + (_param_fw_airspd_trim.get() - _param_fw_airspd_min.get()) * - _manual.z * 2; + _manual_control_setpoint.z * 2; } else { // upper half of throttle is trim to max airspeed altctrl_airspeed = _param_fw_airspd_trim.get() + (_param_fw_airspd_max.get() - _param_fw_airspd_trim.get()) * - (_manual.z * 2 - 1); + (_manual_control_setpoint.z * 2 - 1); } return altctrl_airspeed; @@ -481,15 +481,15 @@ FixedwingPositionControl::update_desired_altitude(float dt) * an axis. Positive X means to rotate positively around * the X axis in NED frame, which is pitching down */ - if (_manual.x > deadBand) { + if (_manual_control_setpoint.x > deadBand) { /* pitching down */ - float pitch = -(_manual.x - deadBand) / factor; + float pitch = -(_manual_control_setpoint.x - deadBand) / factor; _hold_alt += (_param_fw_t_sink_max.get() * dt) * pitch; _was_in_deadband = false; - } else if (_manual.x < - deadBand) { + } else if (_manual_control_setpoint.x < - deadBand) { /* pitching up */ - float pitch = -(_manual.x + deadBand) / factor; + float pitch = -(_manual_control_setpoint.x + deadBand) / factor; _hold_alt += (_param_fw_t_clmb_max.get() * dt) * pitch; _was_in_deadband = false; climbout_mode = (pitch > MANUAL_THROTTLE_CLIMBOUT_THRESH); @@ -791,7 +791,7 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto /* reset setpoints from other modes (auto) otherwise we won't * level out without new manual input */ - _att_sp.roll_body = _manual.y * radians(_param_fw_man_r_max.get()); + _att_sp.roll_body = _manual_control_setpoint.y * radians(_param_fw_man_r_max.get()); _att_sp.yaw_body = 0; } @@ -811,7 +811,7 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto /* throttle limiting */ throttle_max = _param_fw_thr_max.get(); - if (_vehicle_land_detected.landed && (fabsf(_manual.z) < THROTTLE_THRESH)) { + if (_vehicle_land_detected.landed && (fabsf(_manual_control_setpoint.z) < THROTTLE_THRESH)) { throttle_max = 0.0f; } @@ -827,8 +827,8 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto tecs_status_s::TECS_MODE_NORMAL); /* heading control */ - if (fabsf(_manual.y) < HDG_HOLD_MAN_INPUT_THRESH && - fabsf(_manual.r) < HDG_HOLD_MAN_INPUT_THRESH) { + if (fabsf(_manual_control_setpoint.y) < HDG_HOLD_MAN_INPUT_THRESH && + fabsf(_manual_control_setpoint.r) < HDG_HOLD_MAN_INPUT_THRESH) { /* heading / roll is zero, lock onto current heading */ if (fabsf(_vehicle_rates_sub.get().xyz[2]) < HDG_HOLD_YAWRATE_THRESH && !_yaw_lock_engaged) { @@ -879,12 +879,12 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto } } - if (!_yaw_lock_engaged || fabsf(_manual.y) >= HDG_HOLD_MAN_INPUT_THRESH || - fabsf(_manual.r) >= HDG_HOLD_MAN_INPUT_THRESH) { + if (!_yaw_lock_engaged || fabsf(_manual_control_setpoint.y) >= HDG_HOLD_MAN_INPUT_THRESH || + fabsf(_manual_control_setpoint.r) >= HDG_HOLD_MAN_INPUT_THRESH) { _hdg_hold_enabled = false; _yaw_lock_engaged = false; - _att_sp.roll_body = _manual.y * radians(_param_fw_man_r_max.get()); + _att_sp.roll_body = _manual_control_setpoint.y * radians(_param_fw_man_r_max.get()); _att_sp.yaw_body = 0; } @@ -913,7 +913,7 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto /* throttle limiting */ throttle_max = _param_fw_thr_max.get(); - if (_vehicle_land_detected.landed && (fabsf(_manual.z) < THROTTLE_THRESH)) { + if (_vehicle_land_detected.landed && (fabsf(_manual_control_setpoint.z) < THROTTLE_THRESH)) { throttle_max = 0.0f; } @@ -928,7 +928,7 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto climbout_requested ? radians(10.0f) : pitch_limit_min, tecs_status_s::TECS_MODE_NORMAL); - _att_sp.roll_body = _manual.y * radians(_param_fw_man_r_max.get()); + _att_sp.roll_body = _manual_control_setpoint.y * radians(_param_fw_man_r_max.get()); _att_sp.yaw_body = 0; } else { @@ -1544,7 +1544,7 @@ FixedwingPositionControl::Run() _pos_reset_counter = _local_pos.vxy_reset_counter; airspeed_poll(); - _manual_control_sub.update(&_manual); + _manual_control_setpoint_sub.update(&_manual_control_setpoint); _pos_sp_triplet_sub.update(&_pos_sp_triplet); vehicle_attitude_poll(); vehicle_command_poll(); diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index 7d4fadb393..e06099c361 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -152,7 +152,7 @@ private: uORB::Subscription _control_mode_sub{ORB_ID(vehicle_control_mode)}; ///< control mode subscription uORB::Subscription _global_pos_sub{ORB_ID(vehicle_global_position)}; - uORB::Subscription _manual_control_sub{ORB_ID(manual_control_setpoint)}; ///< notification of manual control updates + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; ///< notification of manual control updates uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; ///< notification of parameter updates uORB::Subscription _pos_sp_triplet_sub{ORB_ID(position_setpoint_triplet)}; uORB::Subscription _sensor_baro_sub{ORB_ID(sensor_baro)}; @@ -167,7 +167,7 @@ private: uORB::Publication _pos_ctrl_landing_status_pub{ORB_ID(position_controller_landing_status)}; ///< landing status publication uORB::Publication _tecs_status_pub{ORB_ID(tecs_status)}; ///< TECS status publication - manual_control_setpoint_s _manual {}; ///< r/c channel data + manual_control_setpoint_s _manual_control_setpoint {}; ///< r/c channel data position_setpoint_triplet_s _pos_sp_triplet {}; ///< triplet of mission items vehicle_attitude_s _att {}; ///< vehicle attitude setpoint vehicle_attitude_setpoint_s _att_sp {}; ///< vehicle attitude setpoint diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp index f93360e8b3..48ce1ae9df 100644 --- a/src/modules/logger/logger.cpp +++ b/src/modules/logger/logger.cpp @@ -965,11 +965,11 @@ bool Logger::start_stop_logging() if (_log_mode == LogMode::rc_aux1) { // aux1-based logging - manual_control_setpoint_s manual_sp; + manual_control_setpoint_s manual_control_setpoint; - if (_manual_control_sp_sub.update(&manual_sp)) { + if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { - desired_state = (manual_sp.aux1 > 0.3f); + desired_state = (manual_control_setpoint.aux1 > 0.3f); updated = true; } diff --git a/src/modules/logger/logger.h b/src/modules/logger/logger.h index 75359efed3..af0fd10227 100644 --- a/src/modules/logger/logger.h +++ b/src/modules/logger/logger.h @@ -347,7 +347,7 @@ private: hrt_abstime _logger_status_last {0}; #endif - uORB::Subscription _manual_control_sp_sub{ORB_ID(manual_control_setpoint)}; + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; uORB::SubscriptionInterval _log_message_sub{ORB_ID(log_message), 20}; diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 60dfa22314..4267339ce8 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -3776,11 +3776,12 @@ public: unsigned get_size() override { - return _manual_sub.advertised() ? (MAVLINK_MSG_ID_MANUAL_CONTROL_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; + return _manual_control_setpoint_sub.advertised() ? + (MAVLINK_MSG_ID_MANUAL_CONTROL_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; } private: - uORB::Subscription _manual_sub{ORB_ID(manual_control_setpoint)}; + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; /* do not allow top copying this class */ MavlinkStreamManualControl(MavlinkStreamManualControl &) = delete; @@ -3792,24 +3793,24 @@ protected: bool send(const hrt_abstime t) override { - manual_control_setpoint_s manual; + manual_control_setpoint_s manual_control_setpoint; - if (_manual_sub.update(&manual)) { + if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { mavlink_manual_control_t msg{}; msg.target = mavlink_system.sysid; - msg.x = manual.x * 1000; - msg.y = manual.y * 1000; - msg.z = manual.z * 1000; - msg.r = manual.r * 1000; + msg.x = manual_control_setpoint.x * 1000; + msg.y = manual_control_setpoint.y * 1000; + msg.z = manual_control_setpoint.z * 1000; + msg.r = manual_control_setpoint.r * 1000; unsigned shift = 2; msg.buttons = 0; - msg.buttons |= (manual.mode_switch << (shift * 0)); - msg.buttons |= (manual.return_switch << (shift * 1)); - msg.buttons |= (manual.posctl_switch << (shift * 2)); - msg.buttons |= (manual.loiter_switch << (shift * 3)); - msg.buttons |= (manual.acro_switch << (shift * 4)); - msg.buttons |= (manual.offboard_switch << (shift * 5)); + msg.buttons |= (manual_control_setpoint.mode_switch << (shift * 0)); + msg.buttons |= (manual_control_setpoint.return_switch << (shift * 1)); + msg.buttons |= (manual_control_setpoint.posctl_switch << (shift * 2)); + msg.buttons |= (manual_control_setpoint.loiter_switch << (shift * 3)); + msg.buttons |= (manual_control_setpoint.acro_switch << (shift * 4)); + msg.buttons |= (manual_control_setpoint.offboard_switch << (shift * 5)); mavlink_msg_manual_control_send_struct(_mavlink->get_channel(), &msg); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index c4f9f870fb..758001f272 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -2036,7 +2036,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) manual.z = man.z / 1000.0f; manual.data_source = manual_control_setpoint_s::SOURCE_MAVLINK_0 + _mavlink->get_instance_id(); - _manual_pub.publish(manual); + _manual_control_setpoint_pub.publish(manual); } } diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index dd3debc5c0..709b76beee 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -265,7 +265,7 @@ private: uORB::PublicationMulti _distance_sensor_pub{ORB_ID(distance_sensor), ORB_PRIO_LOW}; uORB::PublicationMulti _flow_distance_sensor_pub{ORB_ID(distance_sensor), ORB_PRIO_LOW}; uORB::PublicationMulti _rc_pub{ORB_ID(input_rc), ORB_PRIO_LOW}; - uORB::PublicationMulti _manual_pub{ORB_ID(manual_control_setpoint), ORB_PRIO_LOW}; + uORB::PublicationMulti _manual_control_setpoint_pub{ORB_ID(manual_control_setpoint), ORB_PRIO_LOW}; uORB::PublicationMulti _ping_pub{ORB_ID(ping), ORB_PRIO_LOW}; uORB::PublicationMulti _radio_status_pub{ORB_ID(radio_status), ORB_PRIO_LOW}; diff --git a/src/modules/mc_att_control/mc_att_control.hpp b/src/modules/mc_att_control/mc_att_control.hpp index fa85d6100d..7a2f56d323 100644 --- a/src/modules/mc_att_control/mc_att_control.hpp +++ b/src/modules/mc_att_control/mc_att_control.hpp @@ -109,7 +109,7 @@ private: uORB::Subscription _v_rates_sp_sub{ORB_ID(vehicle_rates_setpoint)}; /**< vehicle rates setpoint subscription */ uORB::Subscription _v_control_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle control mode subscription */ uORB::Subscription _params_sub{ORB_ID(parameter_update)}; /**< parameter updates subscription */ - uORB::Subscription _manual_control_sp_sub{ORB_ID(manual_control_setpoint)}; /**< manual control setpoint subscription */ + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; /**< manual control setpoint subscription */ uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */ uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */ @@ -120,7 +120,7 @@ private: struct vehicle_attitude_s _v_att {}; /**< vehicle attitude */ struct vehicle_rates_setpoint_s _v_rates_sp {}; /**< vehicle rates setpoint */ - struct manual_control_setpoint_s _manual_control_sp {}; /**< manual control setpoint */ + struct manual_control_setpoint_s _manual_control_setpoint {}; /**< manual control setpoint */ struct vehicle_control_mode_s _v_control_mode {}; /**< vehicle control mode */ struct vehicle_status_s _vehicle_status {}; /**< vehicle status */ struct vehicle_land_detected_s _vehicle_land_detected {}; diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 47feabe23a..0dfa87f38c 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -136,10 +136,10 @@ MulticopterAttitudeControl::generate_attitude_setpoint(float dt, bool reset_yaw_ if (reset_yaw_sp) { _man_yaw_sp = yaw; - } else if (_manual_control_sp.z > 0.05f || _param_mc_airmode.get() == (int32_t)Mixer::Airmode::roll_pitch_yaw) { + } else if (_manual_control_setpoint.z > 0.05f || _param_mc_airmode.get() == (int32_t)Mixer::Airmode::roll_pitch_yaw) { const float yaw_rate = math::radians(_param_mpc_man_y_max.get()); - attitude_setpoint.yaw_sp_move_rate = _manual_control_sp.r * yaw_rate; + attitude_setpoint.yaw_sp_move_rate = _manual_control_setpoint.r * yaw_rate; _man_yaw_sp = wrap_pi(_man_yaw_sp + attitude_setpoint.yaw_sp_move_rate * dt); } @@ -155,8 +155,8 @@ MulticopterAttitudeControl::generate_attitude_setpoint(float dt, bool reset_yaw_ */ _man_x_input_filter.setParameters(dt, _param_mc_man_tilt_tau.get()); _man_y_input_filter.setParameters(dt, _param_mc_man_tilt_tau.get()); - _man_x_input_filter.update(_manual_control_sp.x * _man_tilt_max); - _man_y_input_filter.update(_manual_control_sp.y * _man_tilt_max); + _man_x_input_filter.update(_manual_control_setpoint.x * _man_tilt_max); + _man_y_input_filter.update(_manual_control_setpoint.y * _man_tilt_max); const float x = _man_x_input_filter.getState(); const float y = _man_y_input_filter.getState(); @@ -219,7 +219,7 @@ MulticopterAttitudeControl::generate_attitude_setpoint(float dt, bool reset_yaw_ Quatf q_sp = Eulerf(attitude_setpoint.roll_body, attitude_setpoint.pitch_body, attitude_setpoint.yaw_body); q_sp.copyTo(attitude_setpoint.q_d); - attitude_setpoint.thrust_body[2] = -throttle_curve(_manual_control_sp.z); + attitude_setpoint.thrust_body[2] = -throttle_curve(_manual_control_setpoint.z); attitude_setpoint.timestamp = hrt_absolute_time(); _vehicle_attitude_setpoint_pub.publish(attitude_setpoint); @@ -299,7 +299,7 @@ MulticopterAttitudeControl::Run() _last_run = now; /* check for updates in other topics */ - _manual_control_sp_sub.update(&_manual_control_sp); + _manual_control_setpoint_sub.update(&_manual_control_setpoint); _v_control_mode_sub.update(&_v_control_mode); _vehicle_land_detected_sub.update(&_vehicle_land_detected); _vehicle_status_sub.update(&_vehicle_status); @@ -309,8 +309,8 @@ MulticopterAttitudeControl::Run() * even bother running the attitude controllers */ if (_v_control_mode.flag_control_rattitude_enabled) { _v_control_mode.flag_control_attitude_enabled = - fabsf(_manual_control_sp.y) <= _param_mc_ratt_th.get() && - fabsf(_manual_control_sp.x) <= _param_mc_ratt_th.get(); + fabsf(_manual_control_setpoint.y) <= _param_mc_ratt_th.get() && + fabsf(_manual_control_setpoint.x) <= _param_mc_ratt_th.get(); } bool attitude_setpoint_generated = false; diff --git a/src/modules/mc_rate_control/MulticopterRateControl.cpp b/src/modules/mc_rate_control/MulticopterRateControl.cpp index b445733176..7b1c322e05 100644 --- a/src/modules/mc_rate_control/MulticopterRateControl.cpp +++ b/src/modules/mc_rate_control/MulticopterRateControl.cpp @@ -109,10 +109,10 @@ MulticopterRateControl::get_landing_gear_state() float landing_gear = landing_gear_s::GEAR_DOWN; // default to down - if (_manual_control_sp.gear_switch == manual_control_setpoint_s::SWITCH_POS_ON && _gear_state_initialized) { + if (_manual_control_setpoint.gear_switch == manual_control_setpoint_s::SWITCH_POS_ON && _gear_state_initialized) { landing_gear = landing_gear_s::GEAR_UP; - } else if (_manual_control_sp.gear_switch == manual_control_setpoint_s::SWITCH_POS_OFF) { + } else if (_manual_control_setpoint.gear_switch == manual_control_setpoint_s::SWITCH_POS_OFF) { // Switching the gear off does put it into a safe defined state _gear_state_initialized = true; } @@ -173,7 +173,7 @@ MulticopterRateControl::Run() _vehicle_status_sub.update(&_vehicle_status); - const bool manual_control_updated = _manual_control_sp_sub.update(&_manual_control_sp); + const bool manual_control_updated = _manual_control_setpoint_sub.update(&_manual_control_setpoint); // generate the rate setpoint from sticks? bool manual_rate_sp = false; @@ -199,8 +199,8 @@ MulticopterRateControl::Run() // if true then use published rate setpoint, otherwise generate from manual_control_setpoint (like acro) if (_v_control_mode.flag_control_rattitude_enabled) { manual_rate_sp = - (fabsf(_manual_control_sp.y) > _param_mc_ratt_th.get()) || - (fabsf(_manual_control_sp.x) > _param_mc_ratt_th.get()); + (fabsf(_manual_control_setpoint.y) > _param_mc_ratt_th.get()) || + (fabsf(_manual_control_setpoint.x) > _param_mc_ratt_th.get()); } } else { @@ -212,12 +212,12 @@ MulticopterRateControl::Run() // manual rates control - ACRO mode const Vector3f man_rate_sp{ - math::superexpo(_manual_control_sp.y, _param_mc_acro_expo.get(), _param_mc_acro_supexpo.get()), - math::superexpo(-_manual_control_sp.x, _param_mc_acro_expo.get(), _param_mc_acro_supexpo.get()), - math::superexpo(_manual_control_sp.r, _param_mc_acro_expo_y.get(), _param_mc_acro_supexpoy.get())}; + math::superexpo(_manual_control_setpoint.y, _param_mc_acro_expo.get(), _param_mc_acro_supexpo.get()), + math::superexpo(-_manual_control_setpoint.x, _param_mc_acro_expo.get(), _param_mc_acro_supexpo.get()), + math::superexpo(_manual_control_setpoint.r, _param_mc_acro_expo_y.get(), _param_mc_acro_supexpoy.get())}; _rates_sp = man_rate_sp.emult(_acro_rate_max); - _thrust_sp = _manual_control_sp.z; + _thrust_sp = _manual_control_setpoint.z; // publish rate setpoint vehicle_rates_setpoint_s v_rates_sp{}; diff --git a/src/modules/mc_rate_control/MulticopterRateControl.hpp b/src/modules/mc_rate_control/MulticopterRateControl.hpp index 0feaade026..b0b612e42c 100644 --- a/src/modules/mc_rate_control/MulticopterRateControl.hpp +++ b/src/modules/mc_rate_control/MulticopterRateControl.hpp @@ -95,7 +95,7 @@ private: uORB::Subscription _battery_status_sub{ORB_ID(battery_status)}; uORB::Subscription _landing_gear_sub{ORB_ID(landing_gear)}; - uORB::Subscription _manual_control_sp_sub{ORB_ID(manual_control_setpoint)}; + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; uORB::Subscription _motor_limits_sub{ORB_ID(multirotor_motor_limits)}; uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; uORB::Subscription _v_control_mode_sub{ORB_ID(vehicle_control_mode)}; @@ -112,7 +112,7 @@ private: uORB::Publication _v_rates_sp_pub{ORB_ID(vehicle_rates_setpoint)}; /**< rate setpoint publication */ landing_gear_s _landing_gear{}; - manual_control_setpoint_s _manual_control_sp{}; + manual_control_setpoint_s _manual_control_setpoint{}; vehicle_control_mode_s _v_control_mode{}; vehicle_status_s _vehicle_status{}; diff --git a/src/modules/rc_update/rc_update.cpp b/src/modules/rc_update/rc_update.cpp index 56b3141576..e4fd5c8daa 100644 --- a/src/modules/rc_update/rc_update.cpp +++ b/src/modules/rc_update/rc_update.cpp @@ -438,25 +438,25 @@ RCUpdate::Run() if (!signal_lost && rc_input.timestamp_last_signal > 0) { /* initialize manual setpoint */ - manual_control_setpoint_s manual{}; + manual_control_setpoint_s manual_control_setpoint{}; /* set mode slot to unassigned */ - manual.mode_slot = manual_control_setpoint_s::MODE_SLOT_NONE; + manual_control_setpoint.mode_slot = manual_control_setpoint_s::MODE_SLOT_NONE; /* set the timestamp to the last signal time */ - manual.timestamp = rc_input.timestamp_last_signal; - manual.data_source = manual_control_setpoint_s::SOURCE_RC; + manual_control_setpoint.timestamp = rc_input.timestamp_last_signal; + manual_control_setpoint.data_source = manual_control_setpoint_s::SOURCE_RC; /* limit controls */ - manual.y = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_ROLL, -1.0, 1.0); - manual.x = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_PITCH, -1.0, 1.0); - manual.r = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_YAW, -1.0, 1.0); - manual.z = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_THROTTLE, 0.0, 1.0); - manual.flaps = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_FLAPS, -1.0, 1.0); - manual.aux1 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_1, -1.0, 1.0); - manual.aux2 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_2, -1.0, 1.0); - manual.aux3 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_3, -1.0, 1.0); - manual.aux4 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_4, -1.0, 1.0); - manual.aux5 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_5, -1.0, 1.0); - manual.aux6 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_6, -1.0, 1.0); + manual_control_setpoint.y = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_ROLL, -1.0, 1.0); + manual_control_setpoint.x = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_PITCH, -1.0, 1.0); + manual_control_setpoint.r = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_YAW, -1.0, 1.0); + manual_control_setpoint.z = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_THROTTLE, 0.0, 1.0); + manual_control_setpoint.flaps = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_FLAPS, -1.0, 1.0); + manual_control_setpoint.aux1 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_1, -1.0, 1.0); + manual_control_setpoint.aux2 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_2, -1.0, 1.0); + manual_control_setpoint.aux3 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_3, -1.0, 1.0); + manual_control_setpoint.aux4 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_4, -1.0, 1.0); + manual_control_setpoint.aux5 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_5, -1.0, 1.0); + manual_control_setpoint.aux6 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_6, -1.0, 1.0); if (_param_rc_map_fltmode.get() > 0) { /* number of valid slots */ @@ -476,60 +476,61 @@ RCUpdate::Run() * slots. And finally we add half a slot width to ensure that integer rounding * will take us to the correct final index. */ - manual.mode_slot = (((((_rc.channels[_param_rc_map_fltmode.get() - 1] - slot_min) * num_slots) + slot_width_half) / - (slot_max - slot_min)) + (1.0f / num_slots)) + 1; + manual_control_setpoint.mode_slot = (((((_rc.channels[_param_rc_map_fltmode.get() - 1] - slot_min) * num_slots) + + slot_width_half) + / (slot_max - slot_min)) + (1.0f / num_slots)) + 1; - if (manual.mode_slot > num_slots) { - manual.mode_slot = num_slots; + if (manual_control_setpoint.mode_slot > num_slots) { + manual_control_setpoint.mode_slot = num_slots; } } /* mode switches */ - manual.mode_switch = get_rc_sw3pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_MODE, - _param_rc_auto_th.get(), _param_rc_auto_th.get() < 0.f, - _param_rc_assist_th.get(), _param_rc_assist_th.get() < 0.f); - - manual.rattitude_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_RATTITUDE, - _param_rc_ratt_th.get(), _param_rc_ratt_th.get() < 0.f); - manual.posctl_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_POSCTL, - _param_rc_posctl_th.get(), _param_rc_posctl_th.get() < 0.f); - manual.return_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_RETURN, - _param_rc_return_th.get(), _param_rc_return_th.get() < 0.f); - manual.loiter_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_LOITER, - _param_rc_loiter_th.get(), _param_rc_loiter_th.get() < 0.f); - manual.acro_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_ACRO, - _param_rc_acro_th.get(), _param_rc_acro_th.get() < 0.f); - manual.offboard_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_OFFBOARD, - _param_rc_offb_th.get(), _param_rc_offb_th.get() < 0.f); - manual.kill_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_KILLSWITCH, - _param_rc_killswitch_th.get(), _param_rc_killswitch_th.get() < 0.f); - manual.arm_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_ARMSWITCH, - _param_rc_armswitch_th.get(), _param_rc_armswitch_th.get() < 0.f); - manual.transition_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_TRANSITION, - _param_rc_trans_th.get(), _param_rc_trans_th.get() < 0.f); - manual.gear_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_GEAR, - _param_rc_gear_th.get(), _param_rc_gear_th.get() < 0.f); - manual.stab_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_STAB, - _param_rc_stab_th.get(), _param_rc_stab_th.get() < 0.f); - manual.man_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_MAN, - _param_rc_man_th.get(), _param_rc_man_th.get() < 0.f); + manual_control_setpoint.mode_switch = get_rc_sw3pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_MODE, + _param_rc_auto_th.get(), _param_rc_auto_th.get() < 0.f, + _param_rc_assist_th.get(), _param_rc_assist_th.get() < 0.f); + + manual_control_setpoint.rattitude_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_RATTITUDE, + _param_rc_ratt_th.get(), _param_rc_ratt_th.get() < 0.f); + manual_control_setpoint.posctl_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_POSCTL, + _param_rc_posctl_th.get(), _param_rc_posctl_th.get() < 0.f); + manual_control_setpoint.return_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_RETURN, + _param_rc_return_th.get(), _param_rc_return_th.get() < 0.f); + manual_control_setpoint.loiter_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_LOITER, + _param_rc_loiter_th.get(), _param_rc_loiter_th.get() < 0.f); + manual_control_setpoint.acro_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_ACRO, + _param_rc_acro_th.get(), _param_rc_acro_th.get() < 0.f); + manual_control_setpoint.offboard_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_OFFBOARD, + _param_rc_offb_th.get(), _param_rc_offb_th.get() < 0.f); + manual_control_setpoint.kill_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_KILLSWITCH, + _param_rc_killswitch_th.get(), _param_rc_killswitch_th.get() < 0.f); + manual_control_setpoint.arm_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_ARMSWITCH, + _param_rc_armswitch_th.get(), _param_rc_armswitch_th.get() < 0.f); + manual_control_setpoint.transition_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_TRANSITION, + _param_rc_trans_th.get(), _param_rc_trans_th.get() < 0.f); + manual_control_setpoint.gear_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_GEAR, + _param_rc_gear_th.get(), _param_rc_gear_th.get() < 0.f); + manual_control_setpoint.stab_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_STAB, + _param_rc_stab_th.get(), _param_rc_stab_th.get() < 0.f); + manual_control_setpoint.man_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_MAN, + _param_rc_man_th.get(), _param_rc_man_th.get() < 0.f); /* publish manual_control_setpoint topic */ - _manual_control_pub.publish(manual); + _manual_control_setpoint_pub.publish(manual_control_setpoint); - /* copy from mapped manual control to control group 3 */ + /* copy from mapped manual_control_setpoint control to control group 3 */ actuator_controls_s actuator_group_3{}; actuator_group_3.timestamp = rc_input.timestamp_last_signal; - actuator_group_3.control[0] = manual.y; - actuator_group_3.control[1] = manual.x; - actuator_group_3.control[2] = manual.r; - actuator_group_3.control[3] = manual.z; - actuator_group_3.control[4] = manual.flaps; - actuator_group_3.control[5] = manual.aux1; - actuator_group_3.control[6] = manual.aux2; - actuator_group_3.control[7] = manual.aux3; + actuator_group_3.control[0] = manual_control_setpoint.y; + actuator_group_3.control[1] = manual_control_setpoint.x; + actuator_group_3.control[2] = manual_control_setpoint.r; + actuator_group_3.control[3] = manual_control_setpoint.z; + actuator_group_3.control[4] = manual_control_setpoint.flaps; + actuator_group_3.control[5] = manual_control_setpoint.aux1; + actuator_group_3.control[6] = manual_control_setpoint.aux2; + actuator_group_3.control[7] = manual_control_setpoint.aux3; /* publish actuator_controls_3 topic */ _actuator_group_3_pub.publish(actuator_group_3); diff --git a/src/modules/rc_update/rc_update.h b/src/modules/rc_update/rc_update.h index 091e029c65..39b6b91e87 100644 --- a/src/modules/rc_update/rc_update.h +++ b/src/modules/rc_update/rc_update.h @@ -157,7 +157,7 @@ private: uORB::Publication _rc_pub{ORB_ID(rc_channels)}; /**< raw r/c control topic */ uORB::Publication _actuator_group_3_pub{ORB_ID(actuator_controls_3)}; /**< manual control as actuator topic */ - uORB::PublicationMulti _manual_control_pub{ORB_ID(manual_control_setpoint), ORB_PRIO_HIGH}; /**< manual control signal topic */ + uORB::PublicationMulti _manual_control_setpoint_pub{ORB_ID(manual_control_setpoint), ORB_PRIO_HIGH}; /**< manual control signal topic */ rc_channels_s _rc {}; /**< r/c channel data */ diff --git a/src/modules/rover_pos_control/RoverPositionControl.cpp b/src/modules/rover_pos_control/RoverPositionControl.cpp index efd638d3fd..a19d9b1989 100644 --- a/src/modules/rover_pos_control/RoverPositionControl.cpp +++ b/src/modules/rover_pos_control/RoverPositionControl.cpp @@ -108,10 +108,10 @@ void RoverPositionControl::manual_control_setpoint_poll() { bool manual_updated; - orb_check(_manual_control_sub, &manual_updated); + orb_check(_manual_control_setpoint_sub, &manual_updated); if (manual_updated) { - orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sub, &_manual); + orb_copy(ORB_ID(manual_control_setpoint), _manual_control_setpoint_sub, &_manual_control_setpoint); } } @@ -352,7 +352,7 @@ RoverPositionControl::run() _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); _local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); - _manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + _manual_control_setpoint_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); _pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); _att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); @@ -374,7 +374,7 @@ RoverPositionControl::run() /* Setup of loop */ fds[0].fd = _global_pos_sub; fds[0].events = POLLIN; - fds[1].fd = _manual_control_sub; + fds[1].fd = _manual_control_setpoint_sub; fds[1].events = POLLIN; fds[2].fd = _sensor_combined_sub; fds[2].events = POLLIN; @@ -494,15 +494,15 @@ RoverPositionControl::run() // This should be copied even if not in manual mode. Otherwise, the poll(...) call will keep // returning immediately and this loop will eat up resources. - orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sub, &_manual); + orb_copy(ORB_ID(manual_control_setpoint), _manual_control_setpoint_sub, &_manual_control_setpoint); if (manual_mode) { /* manual/direct control */ //PX4_INFO("Manual mode!"); - _act_controls.control[actuator_controls_s::INDEX_ROLL] = _manual.y; - _act_controls.control[actuator_controls_s::INDEX_PITCH] = -_manual.x; - _act_controls.control[actuator_controls_s::INDEX_YAW] = _manual.r; //TODO: Readd yaw scale param - _act_controls.control[actuator_controls_s::INDEX_THROTTLE] = _manual.z; + _act_controls.control[actuator_controls_s::INDEX_ROLL] = _manual_control_setpoint.y; + _act_controls.control[actuator_controls_s::INDEX_PITCH] = -_manual_control_setpoint.x; + _act_controls.control[actuator_controls_s::INDEX_YAW] = _manual_control_setpoint.r; //TODO: Readd yaw scale param + _act_controls.control[actuator_controls_s::INDEX_THROTTLE] = _manual_control_setpoint.z; } } @@ -528,7 +528,7 @@ RoverPositionControl::run() orb_unsubscribe(_control_mode_sub); orb_unsubscribe(_global_pos_sub); orb_unsubscribe(_local_pos_sub); - orb_unsubscribe(_manual_control_sub); + orb_unsubscribe(_manual_control_setpoint_sub); orb_unsubscribe(_pos_sp_triplet_sub); orb_unsubscribe(_vehicle_attitude_sub); orb_unsubscribe(_sensor_combined_sub); diff --git a/src/modules/rover_pos_control/RoverPositionControl.hpp b/src/modules/rover_pos_control/RoverPositionControl.hpp index 62622ef15d..09b128791d 100644 --- a/src/modules/rover_pos_control/RoverPositionControl.hpp +++ b/src/modules/rover_pos_control/RoverPositionControl.hpp @@ -106,7 +106,7 @@ private: int _control_mode_sub{-1}; /**< control mode subscription */ int _global_pos_sub{-1}; int _local_pos_sub{-1}; - int _manual_control_sub{-1}; /**< notification of manual control updates */ + int _manual_control_setpoint_sub{-1}; /**< notification of manual control updates */ int _pos_sp_triplet_sub{-1}; int _att_sp_sub{-1}; int _vehicle_attitude_sub{-1}; @@ -114,7 +114,7 @@ private: uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; - manual_control_setpoint_s _manual{}; /**< r/c channel data */ + manual_control_setpoint_s _manual_control_setpoint{}; /**< r/c channel data */ position_setpoint_triplet_s _pos_sp_triplet{}; /**< triplet of mission items */ vehicle_attitude_setpoint_s _att_sp{}; /**< attitude setpoint > */ vehicle_control_mode_s _control_mode{}; /**< control mode */ diff --git a/src/modules/uuv_att_control/uuv_att_control.cpp b/src/modules/uuv_att_control/uuv_att_control.cpp index 43169237b2..6f85d0e83e 100644 --- a/src/modules/uuv_att_control/uuv_att_control.cpp +++ b/src/modules/uuv_att_control/uuv_att_control.cpp @@ -95,10 +95,10 @@ void UUVAttitudeControl::vehicle_control_mode_poll() void UUVAttitudeControl::manual_control_setpoint_poll() { bool updated = false; - orb_check(_manual_control_sub, &updated); + orb_check(_manual_control_setpoint_sub, &updated); if (updated) { - orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sub, &_manual); + orb_copy(ORB_ID(manual_control_setpoint), _manual_control_setpoint_sub, &_manual_control_setpoint); } } @@ -250,7 +250,7 @@ void UUVAttitudeControl::run() _local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); _vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); - _manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + _manual_control_setpoint_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); @@ -266,7 +266,7 @@ void UUVAttitudeControl::run() /* Setup of loop */ fds[0].fd = _vehicle_attitude_sub; fds[0].events = POLLIN; - fds[1].fd = _manual_control_sub; + fds[1].fd = _manual_control_setpoint_sub; fds[1].events = POLLIN; fds[2].fd = _sensor_combined_sub; fds[2].events = POLLIN; @@ -337,11 +337,12 @@ void UUVAttitudeControl::run() if (fds[1].revents & POLLIN) { // This should be copied even if not in manual mode. Otherwise, the poll(...) call will keep // returning immediately and this loop will eat up resources. - orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sub, &_manual); + orb_copy(ORB_ID(manual_control_setpoint), _manual_control_setpoint_sub, &_manual_control_setpoint); if (_vcontrol_mode.flag_control_manual_enabled && !_vcontrol_mode.flag_control_rates_enabled) { /* manual/direct control */ - constrain_actuator_commands(_manual.y, -_manual.x, _manual.r, _manual.z); + constrain_actuator_commands(_manual_control_setpoint.y, -_manual_control_setpoint.x, + _manual_control_setpoint.r, _manual_control_setpoint.z); } } @@ -363,7 +364,7 @@ void UUVAttitudeControl::run() } orb_unsubscribe(_vcontrol_mode_sub); - orb_unsubscribe(_manual_control_sub); + orb_unsubscribe(_manual_control_setpoint_sub); orb_unsubscribe(_vehicle_attitude_sub); orb_unsubscribe(_local_pos_sub); orb_unsubscribe(_sensor_combined_sub); diff --git a/src/modules/uuv_att_control/uuv_att_control.hpp b/src/modules/uuv_att_control/uuv_att_control.hpp index 0df6c129b7..05c7c1f9b6 100644 --- a/src/modules/uuv_att_control/uuv_att_control.hpp +++ b/src/modules/uuv_att_control/uuv_att_control.hpp @@ -126,12 +126,12 @@ private: int _vehicle_attitude_sub{-1}; /**< control state subscription */ int _angular_velocity_sub{-1}; /**< vehicle angular velocity subscription */ int _local_pos_sub{-1}; /**< local position subscription */ - int _manual_control_sub{-1}; /**< notification of manual control updates */ + int _manual_control_setpoint_sub{-1}; /**< notification of manual control updates */ int _vcontrol_mode_sub{-1}; /**< vehicle status subscription */ int _sensor_combined_sub{-1}; /**< sensor combined subscription */ actuator_controls_s _actuators {}; /**< actuator control inputs */ - manual_control_setpoint_s _manual {}; /**< r/c channel data */ + manual_control_setpoint_s _manual_control_setpoint {}; /**< r/c channel data */ vehicle_attitude_s _vehicle_attitude {}; /**< control state */ vehicle_angular_velocity_s _angular_velocity{}; /**< angular velocity */ vehicle_attitude_setpoint_s _vehicle_attitude_sp {};/**< vehicle attitude setpoint */ diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index e30cdbb22b..a4aa01a26d 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -197,9 +197,9 @@ VtolAttitudeControl::is_fixed_wing_requested() { bool to_fw = false; - if (_manual_control_sp.transition_switch != manual_control_setpoint_s::SWITCH_POS_NONE && + if (_manual_control_setpoint.transition_switch != manual_control_setpoint_s::SWITCH_POS_NONE && _v_control_mode.flag_control_manual_enabled) { - to_fw = (_manual_control_sp.transition_switch == manual_control_setpoint_s::SWITCH_POS_ON); + to_fw = (_manual_control_setpoint.transition_switch == manual_control_setpoint_s::SWITCH_POS_ON); } else { // listen to transition commands if not in manual or mode switch is not mapped @@ -387,7 +387,7 @@ VtolAttitudeControl::Run() } _v_control_mode_sub.update(&_v_control_mode); - _manual_control_sp_sub.update(&_manual_control_sp); + _manual_control_setpoint_sub.update(&_manual_control_setpoint); _v_att_sub.update(&_v_att); _local_pos_sub.update(&_local_pos); _local_pos_sp_sub.update(&_local_pos_sp); diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h index 093bc4cca8..83a6f4a034 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -138,7 +138,7 @@ private: uORB::Subscription _land_detected_sub{ORB_ID(vehicle_land_detected)}; uORB::Subscription _local_pos_sp_sub{ORB_ID(vehicle_local_position_setpoint)}; // setpoint subscription uORB::Subscription _local_pos_sub{ORB_ID(vehicle_local_position)}; // sensor subscription - uORB::Subscription _manual_control_sp_sub{ORB_ID(manual_control_setpoint)}; //manual control setpoint subscription + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; //manual control setpoint subscription uORB::Subscription _mc_virtual_att_sp_sub{ORB_ID(mc_virtual_attitude_setpoint)}; uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; uORB::Subscription _pos_sp_triplet_sub{ORB_ID(position_setpoint_triplet)}; // local position setpoint subscription @@ -165,7 +165,7 @@ private: actuator_controls_s _actuators_out_1{}; //actuator controls going to the fw mixer (used for elevons) airspeed_validated_s _airspeed_validated{}; // airspeed - manual_control_setpoint_s _manual_control_sp{}; //manual control setpoint + manual_control_setpoint_s _manual_control_setpoint{}; //manual control setpoint position_setpoint_triplet_s _pos_sp_triplet{}; tecs_status_s _tecs_status{}; vehicle_attitude_s _v_att{}; //vehicle attitude