Refactor: Name manual_control_setpoint the same way everywhere

This commit is contained in:
Matthias Grob
2020-06-22 12:06:27 -04:00
committed by Daniel Agar
parent c6dd8bfcd6
commit e9eae1bd76
27 changed files with 276 additions and 263 deletions
+10 -10
View File
@@ -284,8 +284,8 @@ int fixedwing_control_thread_main(int argc, char *argv[])
memset(&rates_sp, 0, sizeof(rates_sp)); memset(&rates_sp, 0, sizeof(rates_sp));
struct vehicle_global_position_s global_pos; struct vehicle_global_position_s global_pos;
memset(&global_pos, 0, sizeof(global_pos)); memset(&global_pos, 0, sizeof(global_pos));
struct manual_control_setpoint_s manual_sp; struct manual_control_setpoint_s manual_control_setpoint;
memset(&manual_sp, 0, sizeof(manual_sp)); memset(&manual_control_setpoint, 0, sizeof(manual_control_setpoint));
struct vehicle_status_s vstatus; struct vehicle_status_s vstatus;
memset(&vstatus, 0, sizeof(vstatus)); memset(&vstatus, 0, sizeof(vstatus));
struct position_setpoint_s global_sp; struct position_setpoint_s global_sp;
@@ -311,7 +311,7 @@ int fixedwing_control_thread_main(int argc, char *argv[])
/* subscribe to topics. */ /* subscribe to topics. */
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); 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 vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
int global_sp_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); 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); orb_check(global_pos_sub, &pos_updated);
bool global_sp_updated; bool global_sp_updated;
orb_check(global_sp_sub, &global_sp_updated); orb_check(global_sp_sub, &global_sp_updated);
bool manual_sp_updated; bool manual_control_setpoint_updated;
orb_check(manual_sp_sub, &manual_sp_updated); orb_check(manual_control_setpoint_sub, &manual_control_setpoint_updated);
/* get a local copy of attitude */ /* get a local copy of attitude */
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); 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)); 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 */ /* 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 */ /* check if the throttle was ever more than 50% - go later only to failsafe if yes */
if (PX4_ISFINITE(manual_sp.z) && if (PX4_ISFINITE(manual_control_setpoint.z) &&
(manual_sp.z >= 0.6f) && (manual_control_setpoint.z >= 0.6f) &&
(manual_sp.z <= 1.0f)) { (manual_control_setpoint.z <= 1.0f)) {
} }
/* get the system status and the flight mode we're in */ /* get the system status and the flight mode we're in */
+7 -7
View File
@@ -235,8 +235,8 @@ int rover_steering_control_thread_main(int argc, char *argv[])
memset(&att_sp, 0, sizeof(att_sp)); memset(&att_sp, 0, sizeof(att_sp));
struct vehicle_global_position_s global_pos; struct vehicle_global_position_s global_pos;
memset(&global_pos, 0, sizeof(global_pos)); memset(&global_pos, 0, sizeof(global_pos));
struct manual_control_setpoint_s manual_sp; struct manual_control_setpoint_s manual_control_setpoint;
memset(&manual_sp, 0, sizeof(manual_sp)); memset(&manual_control_setpoint, 0, sizeof(manual_control_setpoint));
struct vehicle_status_s vstatus; struct vehicle_status_s vstatus;
memset(&vstatus, 0, sizeof(vstatus)); memset(&vstatus, 0, sizeof(vstatus));
struct position_setpoint_s global_sp; 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 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 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); orb_check(global_pos_sub, &pos_updated);
bool att_sp_updated; bool att_sp_updated;
orb_check(att_sp_sub, &att_sp_updated); orb_check(att_sp_sub, &att_sp_updated);
bool manual_sp_updated; bool manual_control_setpoint_updated;
orb_check(manual_sp_sub, &manual_sp_updated); orb_check(manual_control_setpoint_sub, &manual_control_setpoint_updated);
/* get a local copy of attitude */ /* get a local copy of attitude */
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); 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 / heading */
control_attitude(&_att_sp, &att, &actuators); control_attitude(&_att_sp, &att, &actuators);
if (manual_sp_updated) if (manual_control_setpoint_updated)
/* get the RC (or otherwise user based) input */ /* 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 // XXX copy from manual depending on flight / usage mode to override
@@ -45,12 +45,12 @@ bool PreFlightCheck::manualControlCheck(orb_advert_t *mavlink_log_pub, const boo
uORB::SubscriptionData<manual_control_setpoint_s> manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; uORB::SubscriptionData<manual_control_setpoint_s> manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
manual_control_setpoint_sub.update(); 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 //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; success = false;
if (report_fail) { 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; success = false;
if (report_fail) { 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; success = false;
if (report_fail) { 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; success = false;
if (report_fail) { if (report_fail) {
+69 -64
View File
@@ -713,8 +713,8 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
break; break;
} }
const bool throttle_above_low = (_sp_man.z > 0.1f); const bool throttle_above_low = (_manual_control_setpoint.z > 0.1f);
const bool throttle_above_center = (_sp_man.z > 0.6f); const bool throttle_above_center = (_manual_control_setpoint.z > 0.6f);
if (cmd_arms && throttle_above_center && if (cmd_arms && throttle_above_center &&
(status_local->nav_state == vehicle_status_s::NAVIGATION_STATE_POSCTL || (status_local->nav_state == vehicle_status_s::NAVIGATION_STATE_POSCTL ||
@@ -1428,7 +1428,7 @@ Commander::run()
#endif // BOARD_HAS_POWER_CONTROL #endif // BOARD_HAS_POWER_CONTROL
_sp_man_sub.update(&_sp_man); _manual_control_setpoint_sub.update(&_manual_control_setpoint);
offboard_control_update(); offboard_control_update();
@@ -1773,7 +1773,7 @@ Commander::run()
// reset if no longer in LOITER or if manually switched to LOITER // 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 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) { if (!in_loiter_mode || manual_loiter_switch_on) {
_geofence_loiter_on = false; _geofence_loiter_on = false;
@@ -1782,7 +1782,7 @@ Commander::run()
// reset if no longer in RTL or if manually switched to RTL // 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 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) { if (!in_rtl_mode || manual_return_switch_on) {
_geofence_rtl_on = false; _geofence_rtl_on = false;
@@ -1819,11 +1819,11 @@ Commander::run()
if ((override_auto_mode || override_offboard_mode) && is_rotary_wing if ((override_auto_mode || override_offboard_mode) && is_rotary_wing
&& !in_low_battery_failsafe && !_geofence_warning_action_on) { && !in_low_battery_failsafe && !_geofence_warning_action_on) {
// transition to previous state if sticks are touched // transition to previous state if sticks are touched
if ((_last_sp_man.timestamp != _sp_man.timestamp) && if ((_last_manual_control_setpoint.timestamp != _manual_control_setpoint.timestamp) &&
((fabsf(_sp_man.x - _last_sp_man.x) > _min_stick_change) || ((fabsf(_manual_control_setpoint.x - _last_manual_control_setpoint.x) > _min_stick_change) ||
(fabsf(_sp_man.y - _last_sp_man.y) > _min_stick_change) || (fabsf(_manual_control_setpoint.y - _last_manual_control_setpoint.y) > _min_stick_change) ||
(fabsf(_sp_man.z - _last_sp_man.z) > _min_stick_change) || (fabsf(_manual_control_setpoint.z - _last_manual_control_setpoint.z) > _min_stick_change) ||
(fabsf(_sp_man.r - _last_sp_man.r) > _min_stick_change))) { (fabsf(_manual_control_setpoint.r - _last_manual_control_setpoint.r) > _min_stick_change))) {
// revert to position control in any case // revert to position control in any case
main_state_transition(status, commander_state_s::MAIN_STATE_POSCTL, status_flags, &_internal_state); main_state_transition(status, commander_state_s::MAIN_STATE_POSCTL, status_flags, &_internal_state);
@@ -1849,8 +1849,8 @@ Commander::run()
} }
/* RC input check */ /* RC input check */
if (!status_flags.rc_input_blocked && _sp_man.timestamp != 0 && if (!status_flags.rc_input_blocked && _manual_control_setpoint.timestamp != 0 &&
(hrt_elapsed_time(&_sp_man.timestamp) < (_param_com_rc_loss_t.get() * 1_s))) { (hrt_elapsed_time(&_manual_control_setpoint.timestamp) < (_param_com_rc_loss_t.get() * 1_s))) {
/* handle the case where RC signal was regained */ /* handle the case where RC signal was regained */
if (!status_flags.rc_signal_found_once) { if (!status_flags.rc_signal_found_once) {
@@ -1873,19 +1873,22 @@ Commander::run()
status.rc_signal_lost = false; status.rc_signal_lost = false;
const bool in_armed_state = (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); 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() 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 /* DISARM
* check if left stick is in lower left position or arm button is pushed or arm switch has transition from arm to 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) * 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. * do it only for rotary wings in manual mode or fixed wing if landed.
* Disable stick-disarming if arming switch or button is mapped */ * 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() && 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) && (_last_manual_control_setpoint_arm_switch == manual_control_setpoint_s::SWITCH_POS_ON) &&
(_sp_man.arm_switch == manual_control_setpoint_s::SWITCH_POS_OFF); (_manual_control_setpoint.arm_switch == manual_control_setpoint_s::SWITCH_POS_OFF);
if (in_armed_state && if (in_armed_state &&
(status.rc_input_mode != vehicle_status_s::RC_IN_MODE_OFF) && (status.rc_input_mode != vehicle_status_s::RC_IN_MODE_OFF) &&
@@ -1907,7 +1910,8 @@ Commander::run()
_stick_off_counter++; _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 */ /* do not reset the counter when holding the arm button longer than needed */
_stick_off_counter = 0; _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 * 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. * and we're in MANUAL mode.
* Disable stick-arming if arming switch or button is mapped */ * 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; && !arm_switch_or_button_mapped;
/* allow a grace period for re-arming: preflight checks don't need to pass during that time, /* allow a grace period for re-arming: preflight checks don't need to pass during that time,
* for example for accidential in-air disarming */ * for example for accidential in-air disarming */
@@ -1924,9 +1928,9 @@ Commander::run()
&& (hrt_elapsed_time(&_last_disarmed_timestamp) < 5_s); && (hrt_elapsed_time(&_last_disarmed_timestamp) < 5_s);
const bool arm_switch_to_arm_transition = !_param_arm_switch_is_button.get() && 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) && (_last_manual_control_setpoint_arm_switch == manual_control_setpoint_s::SWITCH_POS_OFF) &&
(_sp_man.arm_switch == manual_control_setpoint_s::SWITCH_POS_ON) && (_manual_control_setpoint.arm_switch == manual_control_setpoint_s::SWITCH_POS_ON) &&
(_sp_man.z < 0.1f || in_arming_grace_period); (_manual_control_setpoint.z < 0.1f || in_arming_grace_period);
if (!in_armed_state && if (!in_armed_state &&
(status.rc_input_mode != vehicle_status_s::RC_IN_MODE_OFF) && (status.rc_input_mode != vehicle_status_s::RC_IN_MODE_OFF) &&
@@ -1966,12 +1970,13 @@ Commander::run()
_stick_on_counter++; _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 */ /* do not reset the counter when holding the arm button longer than needed */
_stick_on_counter = 0; _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) { if (arming_ret == TRANSITION_DENIED) {
/* /*
@@ -1984,7 +1989,7 @@ Commander::run()
} }
/* evaluate the main state machine according to mode switches */ /* 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); transition_result_t main_res = set_main_state(status, &_status_changed);
/* store last position lock state */ /* store last position lock state */
@@ -2003,7 +2008,7 @@ Commander::run()
} }
/* check throttle kill switch */ /* 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 */ /* set lockdown flag */
if (!armed.manual_lockdown) { if (!armed.manual_lockdown) {
mavlink_log_emergency(&mavlink_log_pub, "Manual kill-switch engaged"); mavlink_log_emergency(&mavlink_log_pub, "Manual kill-switch engaged");
@@ -2011,7 +2016,7 @@ Commander::run()
armed.manual_lockdown = true; 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) { if (armed.manual_lockdown) {
mavlink_log_emergency(&mavlink_log_pub, "Manual kill-switch disengaged"); mavlink_log_emergency(&mavlink_log_pub, "Manual kill-switch disengaged");
_status_changed = true; _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) { 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"); mavlink_log_critical(&mavlink_log_pub, "Manual control lost");
status.rc_signal_lost = true; 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); set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, true, false, status);
_status_changed = true; _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 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 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 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 first_time_rc = (_last_manual_control_setpoint.timestamp == 0);
const bool rc_values_updated = (_last_sp_man.timestamp != _sp_man.timestamp); const bool rc_values_updated = (_last_manual_control_setpoint.timestamp != _manual_control_setpoint.timestamp);
const bool some_switch_changed = const bool some_switch_changed =
(_last_sp_man.offboard_switch != _sp_man.offboard_switch) (_last_manual_control_setpoint.offboard_switch != _manual_control_setpoint.offboard_switch)
|| (_last_sp_man.return_switch != _sp_man.return_switch) || (_last_manual_control_setpoint.return_switch != _manual_control_setpoint.return_switch)
|| (_last_sp_man.mode_switch != _sp_man.mode_switch) || (_last_manual_control_setpoint.mode_switch != _manual_control_setpoint.mode_switch)
|| (_last_sp_man.acro_switch != _sp_man.acro_switch) || (_last_manual_control_setpoint.acro_switch != _manual_control_setpoint.acro_switch)
|| (_last_sp_man.rattitude_switch != _sp_man.rattitude_switch) || (_last_manual_control_setpoint.rattitude_switch != _manual_control_setpoint.rattitude_switch)
|| (_last_sp_man.posctl_switch != _sp_man.posctl_switch) || (_last_manual_control_setpoint.posctl_switch != _manual_control_setpoint.posctl_switch)
|| (_last_sp_man.loiter_switch != _sp_man.loiter_switch) || (_last_manual_control_setpoint.loiter_switch != _manual_control_setpoint.loiter_switch)
|| (_last_sp_man.mode_slot != _sp_man.mode_slot) || (_last_manual_control_setpoint.mode_slot != _manual_control_setpoint.mode_slot)
|| (_last_sp_man.stab_switch != _sp_man.stab_switch) || (_last_manual_control_setpoint.stab_switch != _manual_control_setpoint.stab_switch)
|| (_last_sp_man.man_switch != _sp_man.man_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 // 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 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_RATTITUDE ||
_internal_state.main_state == commander_state_s::MAIN_STATE_STAB)) { _internal_state.main_state == commander_state_s::MAIN_STATE_STAB)) {
_last_sp_man.timestamp = _sp_man.timestamp; _last_manual_control_setpoint.timestamp = _manual_control_setpoint.timestamp;
_last_sp_man.x = _sp_man.x; _last_manual_control_setpoint.x = _manual_control_setpoint.x;
_last_sp_man.y = _sp_man.y; _last_manual_control_setpoint.y = _manual_control_setpoint.y;
_last_sp_man.z = _sp_man.z; _last_manual_control_setpoint.z = _manual_control_setpoint.z;
_last_sp_man.r = _sp_man.r; _last_manual_control_setpoint.r = _manual_control_setpoint.r;
} }
/* no timestamp change or no switch change -> nothing changed */ /* no timestamp change or no switch change -> nothing changed */
return TRANSITION_NOT_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 // reset the position and velocity validity calculation to give the best change of being able to select
// the desired mode // the desired mode
reset_posvel_validity(changed); reset_posvel_validity(changed);
/* offboard switch overrides main switch */ /* 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); res = main_state_transition(status_local, commander_state_s::MAIN_STATE_OFFBOARD, status_flags, &_internal_state);
if (res == TRANSITION_DENIED) { 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 */ /* 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); res = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_RTL, status_flags, &_internal_state);
if (res == TRANSITION_DENIED) { 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 */ /* 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); res = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_LOITER, status_flags, &_internal_state);
if (res == TRANSITION_DENIED) { 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 */ /* 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"); PX4_WARN("m slot overflow");
return TRANSITION_DENIED; 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) { if (new_mode < 0) {
/* slot is unused */ /* 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 */ /* 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: case manual_control_setpoint_s::SWITCH_POS_NONE:
res = TRANSITION_NOT_CHANGED; res = TRANSITION_NOT_CHANGED;
break; break;
case manual_control_setpoint_s::SWITCH_POS_OFF: // MANUAL case manual_control_setpoint_s::SWITCH_POS_OFF: // MANUAL
if (_sp_man.stab_switch == manual_control_setpoint_s::SWITCH_POS_NONE && if (_manual_control_setpoint.stab_switch == manual_control_setpoint_s::SWITCH_POS_NONE &&
_sp_man.man_switch == manual_control_setpoint_s::SWITCH_POS_NONE) { _manual_control_setpoint.man_switch == manual_control_setpoint_s::SWITCH_POS_NONE) {
/* /*
* Legacy mode: * Legacy mode:
* Acro switch being used as stabilized switch in FW. * 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 /* manual mode is stabilized already for multirotors, so switch to acro
* for any non-manual mode * 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); 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 /* Similar to acro transitions for multirotors. FW aircraft don't need a
* rattitude mode.*/ * rattitude mode.*/
if (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { 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 * - Acro is Acro
* - Manual is not default anymore when the manaul switch is assigned * - 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); 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); 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); 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); 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 // 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); 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; break;
case manual_control_setpoint_s::SWITCH_POS_MIDDLE: // ASSIST 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); res = main_state_transition(status_local, commander_state_s::MAIN_STATE_POSCTL, status_flags, &_internal_state);
if (res != TRANSITION_DENIED) { 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 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"); print_reject_mode("ALTITUDE CONTROL");
} }
+4 -4
View File
@@ -343,11 +343,11 @@ private:
unsigned int _leds_counter{0}; unsigned int _leds_counter{0};
manual_control_setpoint_s _sp_man{}; ///< the current manual control setpoint manual_control_setpoint_s _manual_control_setpoint{}; ///< 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 _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 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] {}; 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{}; float _min_stick_change{};
uint32_t _stick_off_counter{0}; uint32_t _stick_off_counter{0};
uint32_t _stick_on_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 _land_detector_sub{ORB_ID(vehicle_land_detected)};
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
uORB::Subscription _safety_sub{ORB_ID(safety)}; 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 _subsys_sub{ORB_ID(subsystem_info)};
uORB::Subscription _system_power_sub{ORB_ID(system_power)}; uORB::Subscription _system_power_sub{ORB_ID(system_power)};
uORB::Subscription _telemetry_status_sub{ORB_ID(telemetry_status)}; uORB::Subscription _telemetry_status_sub{ORB_ID(telemetry_status)};
+7 -7
View File
@@ -52,17 +52,17 @@
int do_trim_calibration(orb_advert_t *mavlink_log_pub) 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); px4_usleep(400000);
manual_control_setpoint_s sp{}; manual_control_setpoint_s manual_control_setpoint{};
bool changed = sub_man.updated(); bool changed = manual_control_setpoint_sub.updated();
if (!changed) { if (!changed) {
mavlink_log_critical(mavlink_log_pub, "no inputs, aborting"); mavlink_log_critical(mavlink_log_pub, "no inputs, aborting");
return PX4_ERROR; return PX4_ERROR;
} }
sub_man.copy(&sp); manual_control_setpoint_sub.copy(&manual_control_setpoint);
/* load trim values which are active */ /* load trim values which are active */
float roll_trim_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 /* set parameters: the new trim values are the combination of active trim values
and the values coming from the remote control of the user 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); int p1r = param_set(param_find("TRIM_ROLL"), &p);
/* /*
we explicitly swap sign here because the trim is added to the actuator controls 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 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); 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); int p3r = param_set(param_find("TRIM_YAW"), &p);
if (p1r != 0 || p2r != 0 || p3r != 0) { if (p1r != 0 || p2r != 0 || p3r != 0) {
@@ -141,11 +141,12 @@ FixedwingAttitudeControl::vehicle_manual_poll()
if (_vcontrol_mode.flag_control_manual_enabled && (!is_tailsitter_transition || is_fixed_wing)) { 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 // 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 // Check if we are in rattitude mode and the pilot is above the threshold on pitch
if (_vcontrol_mode.flag_control_rattitude_enabled) { 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; _vcontrol_mode.flag_control_attitude_enabled = false;
} }
} }
@@ -156,16 +157,17 @@ FixedwingAttitudeControl::vehicle_manual_poll()
if (_vcontrol_mode.flag_control_attitude_enabled) { if (_vcontrol_mode.flag_control_attitude_enabled) {
// STABILIZED mode generate the attitude setpoint from manual user inputs // 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, _att_sp.roll_body = constrain(_att_sp.roll_body,
-radians(_param_fw_man_r_max.get()), radians(_param_fw_man_r_max.get())); -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, _att_sp.pitch_body = constrain(_att_sp.pitch_body,
-radians(_param_fw_man_p_max.get()), radians(_param_fw_man_p_max.get())); -radians(_param_fw_man_p_max.get()), radians(_param_fw_man_p_max.get()));
_att_sp.yaw_body = 0.0f; _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)); Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body));
q.copyTo(_att_sp.q_d); 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 // RATE mode we need to generate the rate setpoint from manual user inputs
_rates_sp.timestamp = hrt_absolute_time(); _rates_sp.timestamp = hrt_absolute_time();
_rates_sp.roll = _manual.y * radians(_param_fw_acro_x_max.get()); _rates_sp.roll = _manual_control_setpoint.y * radians(_param_fw_acro_x_max.get());
_rates_sp.pitch = -_manual.x * radians(_param_fw_acro_y_max.get()); _rates_sp.pitch = -_manual_control_setpoint.x * radians(_param_fw_acro_y_max.get());
_rates_sp.yaw = _manual.r * radians(_param_fw_acro_z_max.get()); _rates_sp.yaw = _manual_control_setpoint.r * radians(_param_fw_acro_z_max.get());
_rates_sp.thrust_body[0] = _manual.z; _rates_sp.thrust_body[0] = _manual_control_setpoint.z;
_rate_sp_pub.publish(_rates_sp); _rate_sp_pub.publish(_rates_sp);
} else { } else {
/* manual/direct control */ /* 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_ROLL] =
_actuators.control[actuator_controls_s::INDEX_PITCH] = -_manual.x * _param_fw_man_p_sc.get() + _param_trim_pitch.get(); _manual_control_setpoint.y * _param_fw_man_r_sc.get() + _param_trim_roll.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_PITCH] =
_actuators.control[actuator_controls_s::INDEX_THROTTLE] = _manual.z; -_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 */ /* add in manual rudder control in manual modes */
if (_vcontrol_mode.flag_control_manual_enabled) { 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)) { if (!PX4_ISFINITE(yaw_u)) {
@@ -626,10 +631,10 @@ void FixedwingAttitudeControl::Run()
* constrain(_actuators.control[actuator_controls_s::INDEX_ROLL], -1.0f, 1.0f); * constrain(_actuators.control[actuator_controls_s::INDEX_ROLL], -1.0f, 1.0f);
_actuators.control[actuator_controls_s::INDEX_FLAPS] = _flaps_applied; _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; _actuators.control[actuator_controls_s::INDEX_AIRBRAKES] = _flaperons_applied;
// FIXME: this should use _vcontrol_mode.landing_gear_pos in the future // 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 */ /* lazily publish the setpoint only once available */
_actuators.timestamp = hrt_absolute_time(); _actuators.timestamp = hrt_absolute_time();
@@ -652,9 +657,9 @@ void FixedwingAttitudeControl::control_flaps(const float dt)
float flap_control = 0.0f; float flap_control = 0.0f;
/* map flaps by default to manual if valid */ /* 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) { && 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 } else if (_vcontrol_mode.flag_control_auto_enabled
&& fabsf(_param_fw_flaps_scl.get()) > 0.01f) { && fabsf(_param_fw_flaps_scl.get()) > 0.01f) {
@@ -686,10 +691,10 @@ void FixedwingAttitudeControl::control_flaps(const float dt)
float flaperon_control = 0.0f; float flaperon_control = 0.0f;
/* map flaperons by default to manual if valid */ /* 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) { && 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 } else if (_vcontrol_mode.flag_control_auto_enabled
&& fabsf(_param_fw_flaperon_scl.get()) > 0.01f) { && fabsf(_param_fw_flaperon_scl.get()) > 0.01f) {
@@ -98,7 +98,7 @@ private:
uORB::Subscription _att_sp_sub{ORB_ID(vehicle_attitude_setpoint)}; /**< vehicle attitude setpoint */ 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 _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 _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 _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 _rates_sp_sub{ORB_ID(vehicle_rates_setpoint)}; /**< vehicle rates setpoint */
uORB::Subscription _vcontrol_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle status subscription */ uORB::Subscription _vcontrol_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle status subscription */
@@ -114,7 +114,7 @@ private:
uORB::PublicationMulti<rate_ctrl_status_s> _rate_ctrl_status_pub{ORB_ID(rate_ctrl_status)}; uORB::PublicationMulti<rate_ctrl_status_s> _rate_ctrl_status_pub{ORB_ID(rate_ctrl_status)};
actuator_controls_s _actuators {}; /**< actuator control inputs */ 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_s _att {}; /**< vehicle attitude setpoint */
vehicle_attitude_setpoint_s _att_sp {}; /**< vehicle attitude setpoint */ vehicle_attitude_setpoint_s _att_sp {}; /**< vehicle attitude setpoint */
vehicle_control_mode_s _vcontrol_mode {}; /**< vehicle control mode */ vehicle_control_mode_s _vcontrol_mode {}; /**< vehicle control mode */
@@ -229,17 +229,17 @@ FixedwingPositionControl::get_demanded_airspeed()
float altctrl_airspeed = 0; float altctrl_airspeed = 0;
// neutral throttle corresponds to trim airspeed // 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 // lower half of throttle is min to trim airspeed
altctrl_airspeed = _param_fw_airspd_min.get() + altctrl_airspeed = _param_fw_airspd_min.get() +
(_param_fw_airspd_trim.get() - _param_fw_airspd_min.get()) * (_param_fw_airspd_trim.get() - _param_fw_airspd_min.get()) *
_manual.z * 2; _manual_control_setpoint.z * 2;
} else { } else {
// upper half of throttle is trim to max airspeed // upper half of throttle is trim to max airspeed
altctrl_airspeed = _param_fw_airspd_trim.get() + altctrl_airspeed = _param_fw_airspd_trim.get() +
(_param_fw_airspd_max.get() - _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; return altctrl_airspeed;
@@ -481,15 +481,15 @@ FixedwingPositionControl::update_desired_altitude(float dt)
* an axis. Positive X means to rotate positively around * an axis. Positive X means to rotate positively around
* the X axis in NED frame, which is pitching down * the X axis in NED frame, which is pitching down
*/ */
if (_manual.x > deadBand) { if (_manual_control_setpoint.x > deadBand) {
/* pitching down */ /* 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; _hold_alt += (_param_fw_t_sink_max.get() * dt) * pitch;
_was_in_deadband = false; _was_in_deadband = false;
} else if (_manual.x < - deadBand) { } else if (_manual_control_setpoint.x < - deadBand) {
/* pitching up */ /* 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; _hold_alt += (_param_fw_t_clmb_max.get() * dt) * pitch;
_was_in_deadband = false; _was_in_deadband = false;
climbout_mode = (pitch > MANUAL_THROTTLE_CLIMBOUT_THRESH); 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 /* reset setpoints from other modes (auto) otherwise we won't
* level out without new manual input */ * 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; _att_sp.yaw_body = 0;
} }
@@ -811,7 +811,7 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto
/* throttle limiting */ /* throttle limiting */
throttle_max = _param_fw_thr_max.get(); 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; throttle_max = 0.0f;
} }
@@ -827,8 +827,8 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto
tecs_status_s::TECS_MODE_NORMAL); tecs_status_s::TECS_MODE_NORMAL);
/* heading control */ /* heading control */
if (fabsf(_manual.y) < HDG_HOLD_MAN_INPUT_THRESH && if (fabsf(_manual_control_setpoint.y) < HDG_HOLD_MAN_INPUT_THRESH &&
fabsf(_manual.r) < HDG_HOLD_MAN_INPUT_THRESH) { fabsf(_manual_control_setpoint.r) < HDG_HOLD_MAN_INPUT_THRESH) {
/* heading / roll is zero, lock onto current heading */ /* heading / roll is zero, lock onto current heading */
if (fabsf(_vehicle_rates_sub.get().xyz[2]) < HDG_HOLD_YAWRATE_THRESH && !_yaw_lock_engaged) { 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 || if (!_yaw_lock_engaged || fabsf(_manual_control_setpoint.y) >= HDG_HOLD_MAN_INPUT_THRESH ||
fabsf(_manual.r) >= HDG_HOLD_MAN_INPUT_THRESH) { fabsf(_manual_control_setpoint.r) >= HDG_HOLD_MAN_INPUT_THRESH) {
_hdg_hold_enabled = false; _hdg_hold_enabled = false;
_yaw_lock_engaged = 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; _att_sp.yaw_body = 0;
} }
@@ -913,7 +913,7 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto
/* throttle limiting */ /* throttle limiting */
throttle_max = _param_fw_thr_max.get(); 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; 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, climbout_requested ? radians(10.0f) : pitch_limit_min,
tecs_status_s::TECS_MODE_NORMAL); 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; _att_sp.yaw_body = 0;
} else { } else {
@@ -1544,7 +1544,7 @@ FixedwingPositionControl::Run()
_pos_reset_counter = _local_pos.vxy_reset_counter; _pos_reset_counter = _local_pos.vxy_reset_counter;
airspeed_poll(); airspeed_poll();
_manual_control_sub.update(&_manual); _manual_control_setpoint_sub.update(&_manual_control_setpoint);
_pos_sp_triplet_sub.update(&_pos_sp_triplet); _pos_sp_triplet_sub.update(&_pos_sp_triplet);
vehicle_attitude_poll(); vehicle_attitude_poll();
vehicle_command_poll(); vehicle_command_poll();
@@ -152,7 +152,7 @@ private:
uORB::Subscription _control_mode_sub{ORB_ID(vehicle_control_mode)}; ///< control mode subscription 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 _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 _parameter_update_sub{ORB_ID(parameter_update)}; ///< notification of parameter updates
uORB::Subscription _pos_sp_triplet_sub{ORB_ID(position_setpoint_triplet)}; uORB::Subscription _pos_sp_triplet_sub{ORB_ID(position_setpoint_triplet)};
uORB::Subscription _sensor_baro_sub{ORB_ID(sensor_baro)}; uORB::Subscription _sensor_baro_sub{ORB_ID(sensor_baro)};
@@ -167,7 +167,7 @@ private:
uORB::Publication<position_controller_landing_status_s> _pos_ctrl_landing_status_pub{ORB_ID(position_controller_landing_status)}; ///< landing status publication uORB::Publication<position_controller_landing_status_s> _pos_ctrl_landing_status_pub{ORB_ID(position_controller_landing_status)}; ///< landing status publication
uORB::Publication<tecs_status_s> _tecs_status_pub{ORB_ID(tecs_status)}; ///< TECS status publication uORB::Publication<tecs_status_s> _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 position_setpoint_triplet_s _pos_sp_triplet {}; ///< triplet of mission items
vehicle_attitude_s _att {}; ///< vehicle attitude setpoint vehicle_attitude_s _att {}; ///< vehicle attitude setpoint
vehicle_attitude_setpoint_s _att_sp {}; ///< vehicle attitude setpoint vehicle_attitude_setpoint_s _att_sp {}; ///< vehicle attitude setpoint
+3 -3
View File
@@ -965,11 +965,11 @@ bool Logger::start_stop_logging()
if (_log_mode == LogMode::rc_aux1) { if (_log_mode == LogMode::rc_aux1) {
// aux1-based logging // 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; updated = true;
} }
+1 -1
View File
@@ -347,7 +347,7 @@ private:
hrt_abstime _logger_status_last {0}; hrt_abstime _logger_status_last {0};
#endif #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_command_sub{ORB_ID(vehicle_command)};
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
uORB::SubscriptionInterval _log_message_sub{ORB_ID(log_message), 20}; uORB::SubscriptionInterval _log_message_sub{ORB_ID(log_message), 20};
+15 -14
View File
@@ -3776,11 +3776,12 @@ public:
unsigned get_size() override 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: 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 */ /* do not allow top copying this class */
MavlinkStreamManualControl(MavlinkStreamManualControl &) = delete; MavlinkStreamManualControl(MavlinkStreamManualControl &) = delete;
@@ -3792,24 +3793,24 @@ protected:
bool send(const hrt_abstime t) override 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{}; mavlink_manual_control_t msg{};
msg.target = mavlink_system.sysid; msg.target = mavlink_system.sysid;
msg.x = manual.x * 1000; msg.x = manual_control_setpoint.x * 1000;
msg.y = manual.y * 1000; msg.y = manual_control_setpoint.y * 1000;
msg.z = manual.z * 1000; msg.z = manual_control_setpoint.z * 1000;
msg.r = manual.r * 1000; msg.r = manual_control_setpoint.r * 1000;
unsigned shift = 2; unsigned shift = 2;
msg.buttons = 0; msg.buttons = 0;
msg.buttons |= (manual.mode_switch << (shift * 0)); msg.buttons |= (manual_control_setpoint.mode_switch << (shift * 0));
msg.buttons |= (manual.return_switch << (shift * 1)); msg.buttons |= (manual_control_setpoint.return_switch << (shift * 1));
msg.buttons |= (manual.posctl_switch << (shift * 2)); msg.buttons |= (manual_control_setpoint.posctl_switch << (shift * 2));
msg.buttons |= (manual.loiter_switch << (shift * 3)); msg.buttons |= (manual_control_setpoint.loiter_switch << (shift * 3));
msg.buttons |= (manual.acro_switch << (shift * 4)); msg.buttons |= (manual_control_setpoint.acro_switch << (shift * 4));
msg.buttons |= (manual.offboard_switch << (shift * 5)); msg.buttons |= (manual_control_setpoint.offboard_switch << (shift * 5));
mavlink_msg_manual_control_send_struct(_mavlink->get_channel(), &msg); mavlink_msg_manual_control_send_struct(_mavlink->get_channel(), &msg);
+1 -1
View File
@@ -2036,7 +2036,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
manual.z = man.z / 1000.0f; manual.z = man.z / 1000.0f;
manual.data_source = manual_control_setpoint_s::SOURCE_MAVLINK_0 + _mavlink->get_instance_id(); manual.data_source = manual_control_setpoint_s::SOURCE_MAVLINK_0 + _mavlink->get_instance_id();
_manual_pub.publish(manual); _manual_control_setpoint_pub.publish(manual);
} }
} }
+1 -1
View File
@@ -265,7 +265,7 @@ private:
uORB::PublicationMulti<distance_sensor_s> _distance_sensor_pub{ORB_ID(distance_sensor), ORB_PRIO_LOW}; uORB::PublicationMulti<distance_sensor_s> _distance_sensor_pub{ORB_ID(distance_sensor), ORB_PRIO_LOW};
uORB::PublicationMulti<distance_sensor_s> _flow_distance_sensor_pub{ORB_ID(distance_sensor), ORB_PRIO_LOW}; uORB::PublicationMulti<distance_sensor_s> _flow_distance_sensor_pub{ORB_ID(distance_sensor), ORB_PRIO_LOW};
uORB::PublicationMulti<input_rc_s> _rc_pub{ORB_ID(input_rc), ORB_PRIO_LOW}; uORB::PublicationMulti<input_rc_s> _rc_pub{ORB_ID(input_rc), ORB_PRIO_LOW};
uORB::PublicationMulti<manual_control_setpoint_s> _manual_pub{ORB_ID(manual_control_setpoint), ORB_PRIO_LOW}; uORB::PublicationMulti<manual_control_setpoint_s> _manual_control_setpoint_pub{ORB_ID(manual_control_setpoint), ORB_PRIO_LOW};
uORB::PublicationMulti<ping_s> _ping_pub{ORB_ID(ping), ORB_PRIO_LOW}; uORB::PublicationMulti<ping_s> _ping_pub{ORB_ID(ping), ORB_PRIO_LOW};
uORB::PublicationMulti<radio_status_s> _radio_status_pub{ORB_ID(radio_status), ORB_PRIO_LOW}; uORB::PublicationMulti<radio_status_s> _radio_status_pub{ORB_ID(radio_status), ORB_PRIO_LOW};
@@ -109,7 +109,7 @@ private:
uORB::Subscription _v_rates_sp_sub{ORB_ID(vehicle_rates_setpoint)}; /**< vehicle rates setpoint subscription */ 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 _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 _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_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected 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_attitude_s _v_att {}; /**< vehicle attitude */
struct vehicle_rates_setpoint_s _v_rates_sp {}; /**< vehicle rates setpoint */ 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_control_mode_s _v_control_mode {}; /**< vehicle control mode */
struct vehicle_status_s _vehicle_status {}; /**< vehicle status */ struct vehicle_status_s _vehicle_status {}; /**< vehicle status */
struct vehicle_land_detected_s _vehicle_land_detected {}; struct vehicle_land_detected_s _vehicle_land_detected {};
@@ -136,10 +136,10 @@ MulticopterAttitudeControl::generate_attitude_setpoint(float dt, bool reset_yaw_
if (reset_yaw_sp) { if (reset_yaw_sp) {
_man_yaw_sp = yaw; _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()); 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); _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_x_input_filter.setParameters(dt, _param_mc_man_tilt_tau.get());
_man_y_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_x_input_filter.update(_manual_control_setpoint.x * _man_tilt_max);
_man_y_input_filter.update(_manual_control_sp.y * _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 x = _man_x_input_filter.getState();
const float y = _man_y_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); Quatf q_sp = Eulerf(attitude_setpoint.roll_body, attitude_setpoint.pitch_body, attitude_setpoint.yaw_body);
q_sp.copyTo(attitude_setpoint.q_d); 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(); attitude_setpoint.timestamp = hrt_absolute_time();
_vehicle_attitude_setpoint_pub.publish(attitude_setpoint); _vehicle_attitude_setpoint_pub.publish(attitude_setpoint);
@@ -299,7 +299,7 @@ MulticopterAttitudeControl::Run()
_last_run = now; _last_run = now;
/* check for updates in other topics */ /* 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); _v_control_mode_sub.update(&_v_control_mode);
_vehicle_land_detected_sub.update(&_vehicle_land_detected); _vehicle_land_detected_sub.update(&_vehicle_land_detected);
_vehicle_status_sub.update(&_vehicle_status); _vehicle_status_sub.update(&_vehicle_status);
@@ -309,8 +309,8 @@ MulticopterAttitudeControl::Run()
* even bother running the attitude controllers */ * even bother running the attitude controllers */
if (_v_control_mode.flag_control_rattitude_enabled) { if (_v_control_mode.flag_control_rattitude_enabled) {
_v_control_mode.flag_control_attitude_enabled = _v_control_mode.flag_control_attitude_enabled =
fabsf(_manual_control_sp.y) <= _param_mc_ratt_th.get() && fabsf(_manual_control_setpoint.y) <= _param_mc_ratt_th.get() &&
fabsf(_manual_control_sp.x) <= _param_mc_ratt_th.get(); fabsf(_manual_control_setpoint.x) <= _param_mc_ratt_th.get();
} }
bool attitude_setpoint_generated = false; bool attitude_setpoint_generated = false;
@@ -109,10 +109,10 @@ MulticopterRateControl::get_landing_gear_state()
float landing_gear = landing_gear_s::GEAR_DOWN; // default to down 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; 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 // Switching the gear off does put it into a safe defined state
_gear_state_initialized = true; _gear_state_initialized = true;
} }
@@ -173,7 +173,7 @@ MulticopterRateControl::Run()
_vehicle_status_sub.update(&_vehicle_status); _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? // generate the rate setpoint from sticks?
bool manual_rate_sp = false; 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 true then use published rate setpoint, otherwise generate from manual_control_setpoint (like acro)
if (_v_control_mode.flag_control_rattitude_enabled) { if (_v_control_mode.flag_control_rattitude_enabled) {
manual_rate_sp = manual_rate_sp =
(fabsf(_manual_control_sp.y) > _param_mc_ratt_th.get()) || (fabsf(_manual_control_setpoint.y) > _param_mc_ratt_th.get()) ||
(fabsf(_manual_control_sp.x) > _param_mc_ratt_th.get()); (fabsf(_manual_control_setpoint.x) > _param_mc_ratt_th.get());
} }
} else { } else {
@@ -212,12 +212,12 @@ MulticopterRateControl::Run()
// manual rates control - ACRO mode // manual rates control - ACRO mode
const Vector3f man_rate_sp{ 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_setpoint.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_setpoint.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.r, _param_mc_acro_expo_y.get(), _param_mc_acro_supexpoy.get())};
_rates_sp = man_rate_sp.emult(_acro_rate_max); _rates_sp = man_rate_sp.emult(_acro_rate_max);
_thrust_sp = _manual_control_sp.z; _thrust_sp = _manual_control_setpoint.z;
// publish rate setpoint // publish rate setpoint
vehicle_rates_setpoint_s v_rates_sp{}; vehicle_rates_setpoint_s v_rates_sp{};
@@ -95,7 +95,7 @@ private:
uORB::Subscription _battery_status_sub{ORB_ID(battery_status)}; uORB::Subscription _battery_status_sub{ORB_ID(battery_status)};
uORB::Subscription _landing_gear_sub{ORB_ID(landing_gear)}; 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 _motor_limits_sub{ORB_ID(multirotor_motor_limits)};
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
uORB::Subscription _v_control_mode_sub{ORB_ID(vehicle_control_mode)}; uORB::Subscription _v_control_mode_sub{ORB_ID(vehicle_control_mode)};
@@ -112,7 +112,7 @@ private:
uORB::Publication<vehicle_rates_setpoint_s> _v_rates_sp_pub{ORB_ID(vehicle_rates_setpoint)}; /**< rate setpoint publication */ uORB::Publication<vehicle_rates_setpoint_s> _v_rates_sp_pub{ORB_ID(vehicle_rates_setpoint)}; /**< rate setpoint publication */
landing_gear_s _landing_gear{}; 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_control_mode_s _v_control_mode{};
vehicle_status_s _vehicle_status{}; vehicle_status_s _vehicle_status{};
+57 -56
View File
@@ -438,25 +438,25 @@ RCUpdate::Run()
if (!signal_lost && rc_input.timestamp_last_signal > 0) { if (!signal_lost && rc_input.timestamp_last_signal > 0) {
/* initialize manual setpoint */ /* initialize manual setpoint */
manual_control_setpoint_s manual{}; manual_control_setpoint_s manual_control_setpoint{};
/* set mode slot to unassigned */ /* 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 */ /* set the timestamp to the last signal time */
manual.timestamp = rc_input.timestamp_last_signal; manual_control_setpoint.timestamp = rc_input.timestamp_last_signal;
manual.data_source = manual_control_setpoint_s::SOURCE_RC; manual_control_setpoint.data_source = manual_control_setpoint_s::SOURCE_RC;
/* limit controls */ /* limit controls */
manual.y = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_ROLL, -1.0, 1.0); manual_control_setpoint.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_control_setpoint.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_control_setpoint.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_control_setpoint.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_control_setpoint.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_control_setpoint.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_control_setpoint.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_control_setpoint.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_control_setpoint.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_control_setpoint.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.aux6 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_6, -1.0, 1.0);
if (_param_rc_map_fltmode.get() > 0) { if (_param_rc_map_fltmode.get() > 0) {
/* number of valid slots */ /* number of valid slots */
@@ -476,60 +476,61 @@ RCUpdate::Run()
* slots. And finally we add half a slot width to ensure that integer rounding * slots. And finally we add half a slot width to ensure that integer rounding
* will take us to the correct final index. * 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) / manual_control_setpoint.mode_slot = (((((_rc.channels[_param_rc_map_fltmode.get() - 1] - slot_min) * num_slots) +
(slot_max - slot_min)) + (1.0f / num_slots)) + 1; slot_width_half)
/ (slot_max - slot_min)) + (1.0f / num_slots)) + 1;
if (manual.mode_slot > num_slots) { if (manual_control_setpoint.mode_slot > num_slots) {
manual.mode_slot = num_slots; manual_control_setpoint.mode_slot = num_slots;
} }
} }
/* mode switches */ /* mode switches */
manual.mode_switch = get_rc_sw3pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_MODE, 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_auto_th.get(), _param_rc_auto_th.get() < 0.f,
_param_rc_assist_th.get(), _param_rc_assist_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, 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); _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, 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); _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, 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); _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, 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); _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, 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); _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, 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); _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, 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); _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, 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); _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, 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); _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, 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); _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, 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); _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, 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); _param_rc_man_th.get(), _param_rc_man_th.get() < 0.f);
/* publish manual_control_setpoint topic */ /* 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_controls_s actuator_group_3{};
actuator_group_3.timestamp = rc_input.timestamp_last_signal; actuator_group_3.timestamp = rc_input.timestamp_last_signal;
actuator_group_3.control[0] = manual.y; actuator_group_3.control[0] = manual_control_setpoint.y;
actuator_group_3.control[1] = manual.x; actuator_group_3.control[1] = manual_control_setpoint.x;
actuator_group_3.control[2] = manual.r; actuator_group_3.control[2] = manual_control_setpoint.r;
actuator_group_3.control[3] = manual.z; actuator_group_3.control[3] = manual_control_setpoint.z;
actuator_group_3.control[4] = manual.flaps; actuator_group_3.control[4] = manual_control_setpoint.flaps;
actuator_group_3.control[5] = manual.aux1; actuator_group_3.control[5] = manual_control_setpoint.aux1;
actuator_group_3.control[6] = manual.aux2; actuator_group_3.control[6] = manual_control_setpoint.aux2;
actuator_group_3.control[7] = manual.aux3; actuator_group_3.control[7] = manual_control_setpoint.aux3;
/* publish actuator_controls_3 topic */ /* publish actuator_controls_3 topic */
_actuator_group_3_pub.publish(actuator_group_3); _actuator_group_3_pub.publish(actuator_group_3);
+1 -1
View File
@@ -157,7 +157,7 @@ private:
uORB::Publication<rc_channels_s> _rc_pub{ORB_ID(rc_channels)}; /**< raw r/c control topic */ uORB::Publication<rc_channels_s> _rc_pub{ORB_ID(rc_channels)}; /**< raw r/c control topic */
uORB::Publication<actuator_controls_s> _actuator_group_3_pub{ORB_ID(actuator_controls_3)}; /**< manual control as actuator topic */ uORB::Publication<actuator_controls_s> _actuator_group_3_pub{ORB_ID(actuator_controls_3)}; /**< manual control as actuator topic */
uORB::PublicationMulti<manual_control_setpoint_s> _manual_control_pub{ORB_ID(manual_control_setpoint), ORB_PRIO_HIGH}; /**< manual control signal topic */ uORB::PublicationMulti<manual_control_setpoint_s> _manual_control_setpoint_pub{ORB_ID(manual_control_setpoint), ORB_PRIO_HIGH}; /**< manual control signal topic */
rc_channels_s _rc {}; /**< r/c channel data */ rc_channels_s _rc {}; /**< r/c channel data */
@@ -108,10 +108,10 @@ void
RoverPositionControl::manual_control_setpoint_poll() RoverPositionControl::manual_control_setpoint_poll()
{ {
bool manual_updated; bool manual_updated;
orb_check(_manual_control_sub, &manual_updated); orb_check(_manual_control_setpoint_sub, &manual_updated);
if (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)); _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
_local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_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)); _pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); _att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
@@ -374,7 +374,7 @@ RoverPositionControl::run()
/* Setup of loop */ /* Setup of loop */
fds[0].fd = _global_pos_sub; fds[0].fd = _global_pos_sub;
fds[0].events = POLLIN; fds[0].events = POLLIN;
fds[1].fd = _manual_control_sub; fds[1].fd = _manual_control_setpoint_sub;
fds[1].events = POLLIN; fds[1].events = POLLIN;
fds[2].fd = _sensor_combined_sub; fds[2].fd = _sensor_combined_sub;
fds[2].events = POLLIN; 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 // 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. // 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) { if (manual_mode) {
/* manual/direct control */ /* manual/direct control */
//PX4_INFO("Manual mode!"); //PX4_INFO("Manual mode!");
_act_controls.control[actuator_controls_s::INDEX_ROLL] = _manual.y; _act_controls.control[actuator_controls_s::INDEX_ROLL] = _manual_control_setpoint.y;
_act_controls.control[actuator_controls_s::INDEX_PITCH] = -_manual.x; _act_controls.control[actuator_controls_s::INDEX_PITCH] = -_manual_control_setpoint.x;
_act_controls.control[actuator_controls_s::INDEX_YAW] = _manual.r; //TODO: Readd yaw scale param _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.z; _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(_control_mode_sub);
orb_unsubscribe(_global_pos_sub); orb_unsubscribe(_global_pos_sub);
orb_unsubscribe(_local_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(_pos_sp_triplet_sub);
orb_unsubscribe(_vehicle_attitude_sub); orb_unsubscribe(_vehicle_attitude_sub);
orb_unsubscribe(_sensor_combined_sub); orb_unsubscribe(_sensor_combined_sub);
@@ -106,7 +106,7 @@ private:
int _control_mode_sub{-1}; /**< control mode subscription */ int _control_mode_sub{-1}; /**< control mode subscription */
int _global_pos_sub{-1}; int _global_pos_sub{-1};
int _local_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 _pos_sp_triplet_sub{-1};
int _att_sp_sub{-1}; int _att_sp_sub{-1};
int _vehicle_attitude_sub{-1}; int _vehicle_attitude_sub{-1};
@@ -114,7 +114,7 @@ private:
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; 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 */ position_setpoint_triplet_s _pos_sp_triplet{}; /**< triplet of mission items */
vehicle_attitude_setpoint_s _att_sp{}; /**< attitude setpoint > */ vehicle_attitude_setpoint_s _att_sp{}; /**< attitude setpoint > */
vehicle_control_mode_s _control_mode{}; /**< control mode */ vehicle_control_mode_s _control_mode{}; /**< control mode */
@@ -95,10 +95,10 @@ void UUVAttitudeControl::vehicle_control_mode_poll()
void UUVAttitudeControl::manual_control_setpoint_poll() void UUVAttitudeControl::manual_control_setpoint_poll()
{ {
bool updated = false; bool updated = false;
orb_check(_manual_control_sub, &updated); orb_check(_manual_control_setpoint_sub, &updated);
if (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)); _local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
_vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _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)); _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
@@ -266,7 +266,7 @@ void UUVAttitudeControl::run()
/* Setup of loop */ /* Setup of loop */
fds[0].fd = _vehicle_attitude_sub; fds[0].fd = _vehicle_attitude_sub;
fds[0].events = POLLIN; fds[0].events = POLLIN;
fds[1].fd = _manual_control_sub; fds[1].fd = _manual_control_setpoint_sub;
fds[1].events = POLLIN; fds[1].events = POLLIN;
fds[2].fd = _sensor_combined_sub; fds[2].fd = _sensor_combined_sub;
fds[2].events = POLLIN; fds[2].events = POLLIN;
@@ -337,11 +337,12 @@ void UUVAttitudeControl::run()
if (fds[1].revents & POLLIN) { if (fds[1].revents & POLLIN) {
// This should be copied even if not in manual mode. Otherwise, the poll(...) call will keep // 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. // 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) { if (_vcontrol_mode.flag_control_manual_enabled && !_vcontrol_mode.flag_control_rates_enabled) {
/* manual/direct control */ /* 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(_vcontrol_mode_sub);
orb_unsubscribe(_manual_control_sub); orb_unsubscribe(_manual_control_setpoint_sub);
orb_unsubscribe(_vehicle_attitude_sub); orb_unsubscribe(_vehicle_attitude_sub);
orb_unsubscribe(_local_pos_sub); orb_unsubscribe(_local_pos_sub);
orb_unsubscribe(_sensor_combined_sub); orb_unsubscribe(_sensor_combined_sub);
@@ -126,12 +126,12 @@ private:
int _vehicle_attitude_sub{-1}; /**< control state subscription */ int _vehicle_attitude_sub{-1}; /**< control state subscription */
int _angular_velocity_sub{-1}; /**< vehicle angular velocity subscription */ int _angular_velocity_sub{-1}; /**< vehicle angular velocity subscription */
int _local_pos_sub{-1}; /**< local position 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 _vcontrol_mode_sub{-1}; /**< vehicle status subscription */
int _sensor_combined_sub{-1}; /**< sensor combined subscription */ int _sensor_combined_sub{-1}; /**< sensor combined subscription */
actuator_controls_s _actuators {}; /**< actuator control inputs */ 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_attitude_s _vehicle_attitude {}; /**< control state */
vehicle_angular_velocity_s _angular_velocity{}; /**< angular velocity */ vehicle_angular_velocity_s _angular_velocity{}; /**< angular velocity */
vehicle_attitude_setpoint_s _vehicle_attitude_sp {};/**< vehicle attitude setpoint */ vehicle_attitude_setpoint_s _vehicle_attitude_sp {};/**< vehicle attitude setpoint */
@@ -197,9 +197,9 @@ VtolAttitudeControl::is_fixed_wing_requested()
{ {
bool to_fw = false; 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) { _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 { } else {
// listen to transition commands if not in manual or mode switch is not mapped // 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); _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); _v_att_sub.update(&_v_att);
_local_pos_sub.update(&_local_pos); _local_pos_sub.update(&_local_pos);
_local_pos_sp_sub.update(&_local_pos_sp); _local_pos_sp_sub.update(&_local_pos_sp);
@@ -138,7 +138,7 @@ private:
uORB::Subscription _land_detected_sub{ORB_ID(vehicle_land_detected)}; 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_sp_sub{ORB_ID(vehicle_local_position_setpoint)}; // setpoint subscription
uORB::Subscription _local_pos_sub{ORB_ID(vehicle_local_position)}; // sensor 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 _mc_virtual_att_sp_sub{ORB_ID(mc_virtual_attitude_setpoint)};
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
uORB::Subscription _pos_sp_triplet_sub{ORB_ID(position_setpoint_triplet)}; // local position setpoint subscription 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) actuator_controls_s _actuators_out_1{}; //actuator controls going to the fw mixer (used for elevons)
airspeed_validated_s _airspeed_validated{}; // airspeed 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{}; position_setpoint_triplet_s _pos_sp_triplet{};
tecs_status_s _tecs_status{}; tecs_status_s _tecs_status{};
vehicle_attitude_s _v_att{}; //vehicle attitude vehicle_attitude_s _v_att{}; //vehicle attitude