fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-22 07:27:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

Refactor: Name manual_control_setpoint the same way everywhere

Browse Source
This commit is contained in:
Matthias Grob
2020-06-22 15:06:47 +02:00
committed by Daniel Agar
parent c6dd8bfcd6
commit e9eae1bd76
27 changed files with 276 additions and 263 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/examples/fixedwing_control/main.cpp
+10 -10
View File
@@ -284,8 +284,8 @@ int fixedwing_control_thread_main(int argc, char *argv[])
memset(&rates_sp, 0, sizeof(rates_sp));
struct vehicle_global_position_s global_pos;
memset(&global_pos, 0, sizeof(global_pos));
struct manual_control_setpoint_s manual_sp;
memset(&manual_sp, 0, sizeof(manual_sp));
struct manual_control_setpoint_s manual_control_setpoint;
memset(&manual_control_setpoint, 0, sizeof(manual_control_setpoint));
struct vehicle_status_s vstatus;
memset(&vstatus, 0, sizeof(vstatus));
struct position_setpoint_s global_sp;
@@ -311,7 +311,7 @@ int fixedwing_control_thread_main(int argc, char *argv[])
/* subscribe to topics. */
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
int manual_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
int manual_control_setpoint_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
int global_sp_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
@@ -367,8 +367,8 @@ int fixedwing_control_thread_main(int argc, char *argv[])
orb_check(global_pos_sub, &pos_updated);
bool global_sp_updated;
orb_check(global_sp_sub, &global_sp_updated);
bool manual_sp_updated;
orb_check(manual_sp_sub, &manual_sp_updated);
bool manual_control_setpoint_updated;
orb_check(manual_control_setpoint_sub, &manual_control_setpoint_updated);
/* get a local copy of attitude */
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
@@ -379,16 +379,16 @@ int fixedwing_control_thread_main(int argc, char *argv[])
memcpy(&global_sp, &triplet.current, sizeof(global_sp));
}
if (manual_sp_updated)
if (manual_control_setpoint_updated)
/* get the RC (or otherwise user based) input */
{
orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp);
orb_copy(ORB_ID(manual_control_setpoint), manual_control_setpoint_sub, &manual_control_setpoint);
}
/* check if the throttle was ever more than 50% - go later only to failsafe if yes */
if (PX4_ISFINITE(manual_sp.z) &&
(manual_sp.z >= 0.6f) &&
(manual_sp.z <= 1.0f)) {
if (PX4_ISFINITE(manual_control_setpoint.z) &&
(manual_control_setpoint.z >= 0.6f) &&
(manual_control_setpoint.z <= 1.0f)) {
}
/* get the system status and the flight mode we're in */
src/examples/rover_steering_control/main.cpp
+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));
struct vehicle_global_position_s global_pos;
memset(&global_pos, 0, sizeof(global_pos));
struct manual_control_setpoint_s manual_sp;
memset(&manual_sp, 0, sizeof(manual_sp));
struct manual_control_setpoint_s manual_control_setpoint;
memset(&manual_control_setpoint, 0, sizeof(manual_control_setpoint));
struct vehicle_status_s vstatus;
memset(&vstatus, 0, sizeof(vstatus));
struct position_setpoint_s global_sp;
@@ -265,7 +265,7 @@ int rover_steering_control_thread_main(int argc, char *argv[])
int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
int manual_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
int manual_control_setpoint_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
@@ -325,8 +325,8 @@ int rover_steering_control_thread_main(int argc, char *argv[])
orb_check(global_pos_sub, &pos_updated);
bool att_sp_updated;
orb_check(att_sp_sub, &att_sp_updated);
bool manual_sp_updated;
orb_check(manual_sp_sub, &manual_sp_updated);
bool manual_control_setpoint_updated;
orb_check(manual_control_setpoint_sub, &manual_control_setpoint_updated);
/* get a local copy of attitude */
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
@@ -338,10 +338,10 @@ int rover_steering_control_thread_main(int argc, char *argv[])
/* control attitude / heading */
control_attitude(&_att_sp, &att, &actuators);
if (manual_sp_updated)
if (manual_control_setpoint_updated)
/* get the RC (or otherwise user based) input */
{
orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp);
orb_copy(ORB_ID(manual_control_setpoint), manual_control_setpoint_sub, &manual_control_setpoint);
}
// XXX copy from manual depending on flight / usage mode to override
src/modules/commander/Arming/PreFlightCheck/checks/manualControlCheck.cpp
+6 -6
View File
@@ -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)};
manual_control_setpoint_sub.update();
const manual_control_setpoint_s &manual_control = manual_control_setpoint_sub.get();
const manual_control_setpoint_s &manual_control_setpoint = manual_control_setpoint_sub.get();
if (hrt_elapsed_time(&manual_control.timestamp) < 1_s) {
if (hrt_elapsed_time(&manual_control_setpoint.timestamp) < 1_s) {
//check action switches
if (manual_control.return_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
if (manual_control_setpoint.return_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
success = false;
if (report_fail) {
@@ -58,7 +58,7 @@ bool PreFlightCheck::manualControlCheck(orb_advert_t *mavlink_log_pub, const boo
}
}
if (manual_control.kill_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
if (manual_control_setpoint.kill_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
success = false;
if (report_fail) {
@@ -66,7 +66,7 @@ bool PreFlightCheck::manualControlCheck(orb_advert_t *mavlink_log_pub, const boo
}
}
if (manual_control.gear_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
if (manual_control_setpoint.gear_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
success = false;
if (report_fail) {
@@ -74,7 +74,7 @@ bool PreFlightCheck::manualControlCheck(orb_advert_t *mavlink_log_pub, const boo
}
}
if (manual_control.transition_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
if (manual_control_setpoint.transition_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
success = false;
if (report_fail) {
src/modules/commander/Commander.cpp
+69 -64
View File
@@ -713,8 +713,8 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
break;
}
const bool throttle_above_low = (_sp_man.z > 0.1f);
const bool throttle_above_center = (_sp_man.z > 0.6f);
const bool throttle_above_low = (_manual_control_setpoint.z > 0.1f);
const bool throttle_above_center = (_manual_control_setpoint.z > 0.6f);
if (cmd_arms && throttle_above_center &&
(status_local->nav_state == vehicle_status_s::NAVIGATION_STATE_POSCTL ||
@@ -1428,7 +1428,7 @@ Commander::run()
#endif // BOARD_HAS_POWER_CONTROL
_sp_man_sub.update(&_sp_man);
_manual_control_setpoint_sub.update(&_manual_control_setpoint);
offboard_control_update();
@@ -1773,7 +1773,7 @@ Commander::run()
// reset if no longer in LOITER or if manually switched to LOITER
const bool in_loiter_mode = _internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LOITER;
const bool manual_loiter_switch_on = _sp_man.loiter_switch == manual_control_setpoint_s::SWITCH_POS_ON;
const bool manual_loiter_switch_on = _manual_control_setpoint.loiter_switch == manual_control_setpoint_s::SWITCH_POS_ON;
if (!in_loiter_mode || manual_loiter_switch_on) {
_geofence_loiter_on = false;
@@ -1782,7 +1782,7 @@ Commander::run()
// reset if no longer in RTL or if manually switched to RTL
const bool in_rtl_mode = _internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_RTL;
const bool manual_return_switch_on = _sp_man.return_switch == manual_control_setpoint_s::SWITCH_POS_ON;
const bool manual_return_switch_on = _manual_control_setpoint.return_switch == manual_control_setpoint_s::SWITCH_POS_ON;
if (!in_rtl_mode || manual_return_switch_on) {
_geofence_rtl_on = false;
@@ -1819,11 +1819,11 @@ Commander::run()
if ((override_auto_mode || override_offboard_mode) && is_rotary_wing
&& !in_low_battery_failsafe && !_geofence_warning_action_on) {
// transition to previous state if sticks are touched
if ((_last_sp_man.timestamp != _sp_man.timestamp) &&
((fabsf(_sp_man.x - _last_sp_man.x) > _min_stick_change) ||
(fabsf(_sp_man.y - _last_sp_man.y) > _min_stick_change) ||
(fabsf(_sp_man.z - _last_sp_man.z) > _min_stick_change) ||
(fabsf(_sp_man.r - _last_sp_man.r) > _min_stick_change))) {
if ((_last_manual_control_setpoint.timestamp != _manual_control_setpoint.timestamp) &&
((fabsf(_manual_control_setpoint.x - _last_manual_control_setpoint.x) > _min_stick_change) ||
(fabsf(_manual_control_setpoint.y - _last_manual_control_setpoint.y) > _min_stick_change) ||
(fabsf(_manual_control_setpoint.z - _last_manual_control_setpoint.z) > _min_stick_change) ||
(fabsf(_manual_control_setpoint.r - _last_manual_control_setpoint.r) > _min_stick_change))) {
// revert to position control in any case
main_state_transition(status, commander_state_s::MAIN_STATE_POSCTL, status_flags, &_internal_state);
@@ -1849,8 +1849,8 @@ Commander::run()
}
/* RC input check */
if (!status_flags.rc_input_blocked && _sp_man.timestamp != 0 &&
(hrt_elapsed_time(&_sp_man.timestamp) < (_param_com_rc_loss_t.get() * 1_s))) {
if (!status_flags.rc_input_blocked && _manual_control_setpoint.timestamp != 0 &&
(hrt_elapsed_time(&_manual_control_setpoint.timestamp) < (_param_com_rc_loss_t.get() * 1_s))) {
/* handle the case where RC signal was regained */
if (!status_flags.rc_signal_found_once) {
@@ -1873,19 +1873,22 @@ Commander::run()
status.rc_signal_lost = false;
const bool in_armed_state = (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
const bool arm_switch_or_button_mapped = _sp_man.arm_switch != manual_control_setpoint_s::SWITCH_POS_NONE;
const bool arm_switch_or_button_mapped =
_manual_control_setpoint.arm_switch != manual_control_setpoint_s::SWITCH_POS_NONE;
const bool arm_button_pressed = _param_arm_switch_is_button.get()
&& (_sp_man.arm_switch == manual_control_setpoint_s::SWITCH_POS_ON);
&& (_manual_control_setpoint.arm_switch == manual_control_setpoint_s::SWITCH_POS_ON);
/* DISARM
* check if left stick is in lower left position or arm button is pushed or arm switch has transition from arm to disarm
* and we are in MANUAL, Rattitude, or AUTO_READY mode or (ASSIST mode and landed)
* do it only for rotary wings in manual mode or fixed wing if landed.
* Disable stick-disarming if arming switch or button is mapped */
const bool stick_in_lower_left = _sp_man.r < -STICK_ON_OFF_LIMIT && (_sp_man.z < 0.1f) && !arm_switch_or_button_mapped;
const bool stick_in_lower_left = _manual_control_setpoint.r < -STICK_ON_OFF_LIMIT
&& (_manual_control_setpoint.z < 0.1f)
&& !arm_switch_or_button_mapped;
const bool arm_switch_to_disarm_transition = !_param_arm_switch_is_button.get() &&
(_last_sp_man_arm_switch == manual_control_setpoint_s::SWITCH_POS_ON) &&
(_sp_man.arm_switch == manual_control_setpoint_s::SWITCH_POS_OFF);
(_last_manual_control_setpoint_arm_switch == manual_control_setpoint_s::SWITCH_POS_ON) &&
(_manual_control_setpoint.arm_switch == manual_control_setpoint_s::SWITCH_POS_OFF);
if (in_armed_state &&
(status.rc_input_mode != vehicle_status_s::RC_IN_MODE_OFF) &&
@@ -1907,7 +1910,8 @@ Commander::run()
_stick_off_counter++;
} else if (!(_param_arm_switch_is_button.get() && _sp_man.arm_switch == manual_control_setpoint_s::SWITCH_POS_ON)) {
} else if (!(_param_arm_switch_is_button.get()
&& _manual_control_setpoint.arm_switch == manual_control_setpoint_s::SWITCH_POS_ON)) {
/* do not reset the counter when holding the arm button longer than needed */
_stick_off_counter = 0;
}
@@ -1916,7 +1920,7 @@ Commander::run()
* check if left stick is in lower right position or arm button is pushed or arm switch has transition from disarm to arm
* and we're in MANUAL mode.
* Disable stick-arming if arming switch or button is mapped */
const bool stick_in_lower_right = _sp_man.r > STICK_ON_OFF_LIMIT && _sp_man.z < 0.1f
const bool stick_in_lower_right = _manual_control_setpoint.r > STICK_ON_OFF_LIMIT && _manual_control_setpoint.z < 0.1f
&& !arm_switch_or_button_mapped;
/* allow a grace period for re-arming: preflight checks don't need to pass during that time,
* for example for accidential in-air disarming */
@@ -1924,9 +1928,9 @@ Commander::run()
&& (hrt_elapsed_time(&_last_disarmed_timestamp) < 5_s);
const bool arm_switch_to_arm_transition = !_param_arm_switch_is_button.get() &&
(_last_sp_man_arm_switch == manual_control_setpoint_s::SWITCH_POS_OFF) &&
(_sp_man.arm_switch == manual_control_setpoint_s::SWITCH_POS_ON) &&
(_sp_man.z < 0.1f || in_arming_grace_period);
(_last_manual_control_setpoint_arm_switch == manual_control_setpoint_s::SWITCH_POS_OFF) &&
(_manual_control_setpoint.arm_switch == manual_control_setpoint_s::SWITCH_POS_ON) &&
(_manual_control_setpoint.z < 0.1f || in_arming_grace_period);
if (!in_armed_state &&
(status.rc_input_mode != vehicle_status_s::RC_IN_MODE_OFF) &&
@@ -1966,12 +1970,13 @@ Commander::run()
_stick_on_counter++;
} else if (!(_param_arm_switch_is_button.get() && _sp_man.arm_switch == manual_control_setpoint_s::SWITCH_POS_ON)) {
} else if (!(_param_arm_switch_is_button.get()
&& _manual_control_setpoint.arm_switch == manual_control_setpoint_s::SWITCH_POS_ON)) {
/* do not reset the counter when holding the arm button longer than needed */
_stick_on_counter = 0;
}
_last_sp_man_arm_switch = _sp_man.arm_switch;
_last_manual_control_setpoint_arm_switch = _manual_control_setpoint.arm_switch;
if (arming_ret == TRANSITION_DENIED) {
/*
@@ -1984,7 +1989,7 @@ Commander::run()
}
/* evaluate the main state machine according to mode switches */
bool first_rc_eval = (_last_sp_man.timestamp == 0) && (_sp_man.timestamp > 0);
bool first_rc_eval = (_last_manual_control_setpoint.timestamp == 0) && (_manual_control_setpoint.timestamp > 0);
transition_result_t main_res = set_main_state(status, &_status_changed);
/* store last position lock state */
@@ -2003,7 +2008,7 @@ Commander::run()
}
/* check throttle kill switch */
if (_sp_man.kill_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
if (_manual_control_setpoint.kill_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
/* set lockdown flag */
if (!armed.manual_lockdown) {
mavlink_log_emergency(&mavlink_log_pub, "Manual kill-switch engaged");
@@ -2011,7 +2016,7 @@ Commander::run()
armed.manual_lockdown = true;
}
} else if (_sp_man.kill_switch == manual_control_setpoint_s::SWITCH_POS_OFF) {
} else if (_manual_control_setpoint.kill_switch == manual_control_setpoint_s::SWITCH_POS_OFF) {
if (armed.manual_lockdown) {
mavlink_log_emergency(&mavlink_log_pub, "Manual kill-switch disengaged");
_status_changed = true;
@@ -2025,7 +2030,7 @@ Commander::run()
if (!status_flags.rc_input_blocked && !status.rc_signal_lost && status_flags.rc_signal_found_once) {
mavlink_log_critical(&mavlink_log_pub, "Manual control lost");
status.rc_signal_lost = true;
_rc_signal_lost_timestamp = _sp_man.timestamp;
_rc_signal_lost_timestamp = _manual_control_setpoint.timestamp;
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, true, false, status);
_status_changed = true;
}
@@ -2618,19 +2623,19 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed
const bool altitude_got_valid = (!_last_condition_local_altitude_valid && status_flags.condition_local_altitude_valid);
const bool lpos_got_valid = (!_last_condition_local_position_valid && status_flags.condition_local_position_valid);
const bool gpos_got_valid = (!_last_condition_global_position_valid && status_flags.condition_global_position_valid);
const bool first_time_rc = (_last_sp_man.timestamp == 0);
const bool rc_values_updated = (_last_sp_man.timestamp != _sp_man.timestamp);
const bool first_time_rc = (_last_manual_control_setpoint.timestamp == 0);
const bool rc_values_updated = (_last_manual_control_setpoint.timestamp != _manual_control_setpoint.timestamp);
const bool some_switch_changed =
(_last_sp_man.offboard_switch != _sp_man.offboard_switch)
|| (_last_sp_man.return_switch != _sp_man.return_switch)
|| (_last_sp_man.mode_switch != _sp_man.mode_switch)
|| (_last_sp_man.acro_switch != _sp_man.acro_switch)
|| (_last_sp_man.rattitude_switch != _sp_man.rattitude_switch)
|| (_last_sp_man.posctl_switch != _sp_man.posctl_switch)
|| (_last_sp_man.loiter_switch != _sp_man.loiter_switch)
|| (_last_sp_man.mode_slot != _sp_man.mode_slot)
|| (_last_sp_man.stab_switch != _sp_man.stab_switch)
|| (_last_sp_man.man_switch != _sp_man.man_switch);
(_last_manual_control_setpoint.offboard_switch != _manual_control_setpoint.offboard_switch)
|| (_last_manual_control_setpoint.return_switch != _manual_control_setpoint.return_switch)
|| (_last_manual_control_setpoint.mode_switch != _manual_control_setpoint.mode_switch)
|| (_last_manual_control_setpoint.acro_switch != _manual_control_setpoint.acro_switch)
|| (_last_manual_control_setpoint.rattitude_switch != _manual_control_setpoint.rattitude_switch)
|| (_last_manual_control_setpoint.posctl_switch != _manual_control_setpoint.posctl_switch)
|| (_last_manual_control_setpoint.loiter_switch != _manual_control_setpoint.loiter_switch)
|| (_last_manual_control_setpoint.mode_slot != _manual_control_setpoint.mode_slot)
|| (_last_manual_control_setpoint.stab_switch != _manual_control_setpoint.stab_switch)
|| (_last_manual_control_setpoint.man_switch != _manual_control_setpoint.man_switch);
// only switch mode based on RC switch if necessary to also allow mode switching via MAVLink
const bool should_evaluate_rc_mode_switch = first_time_rc
@@ -2653,25 +2658,25 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed
_internal_state.main_state == commander_state_s::MAIN_STATE_RATTITUDE ||
_internal_state.main_state == commander_state_s::MAIN_STATE_STAB)) {
_last_sp_man.timestamp = _sp_man.timestamp;
_last_sp_man.x = _sp_man.x;
_last_sp_man.y = _sp_man.y;
_last_sp_man.z = _sp_man.z;
_last_sp_man.r = _sp_man.r;
_last_manual_control_setpoint.timestamp = _manual_control_setpoint.timestamp;
_last_manual_control_setpoint.x = _manual_control_setpoint.x;
_last_manual_control_setpoint.y = _manual_control_setpoint.y;
_last_manual_control_setpoint.z = _manual_control_setpoint.z;
_last_manual_control_setpoint.r = _manual_control_setpoint.r;
}
/* no timestamp change or no switch change -> nothing changed */
return TRANSITION_NOT_CHANGED;
}
_last_sp_man = _sp_man;
_last_manual_control_setpoint = _manual_control_setpoint;
// reset the position and velocity validity calculation to give the best change of being able to select
// the desired mode
reset_posvel_validity(changed);
/* offboard switch overrides main switch */
if (_sp_man.offboard_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
if (_manual_control_setpoint.offboard_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
res = main_state_transition(status_local, commander_state_s::MAIN_STATE_OFFBOARD, status_flags, &_internal_state);
if (res == TRANSITION_DENIED) {
@@ -2685,7 +2690,7 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed
}
/* RTL switch overrides main switch */
if (_sp_man.return_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
if (_manual_control_setpoint.return_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
res = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_RTL, status_flags, &_internal_state);
if (res == TRANSITION_DENIED) {
@@ -2704,7 +2709,7 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed
}
/* Loiter switch overrides main switch */
if (_sp_man.loiter_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
if (_manual_control_setpoint.loiter_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
res = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_LOITER, status_flags, &_internal_state);
if (res == TRANSITION_DENIED) {
@@ -2716,14 +2721,14 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed
}
/* we know something has changed - check if we are in mode slot operation */
if (_sp_man.mode_slot != manual_control_setpoint_s::MODE_SLOT_NONE) {
if (_manual_control_setpoint.mode_slot != manual_control_setpoint_s::MODE_SLOT_NONE) {
if (_sp_man.mode_slot > manual_control_setpoint_s::MODE_SLOT_NUM) {
if (_manual_control_setpoint.mode_slot > manual_control_setpoint_s::MODE_SLOT_NUM) {
PX4_WARN("m slot overflow");
return TRANSITION_DENIED;
}
int new_mode = _flight_mode_slots[_sp_man.mode_slot - 1];
int new_mode = _flight_mode_slots[_manual_control_setpoint.mode_slot - 1];
if (new_mode < 0) {
/* slot is unused */
@@ -2855,19 +2860,19 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed
}
/* offboard and RTL switches off or denied, check main mode switch */
switch (_sp_man.mode_switch) {
switch (_manual_control_setpoint.mode_switch) {
case manual_control_setpoint_s::SWITCH_POS_NONE:
res = TRANSITION_NOT_CHANGED;
break;
case manual_control_setpoint_s::SWITCH_POS_OFF: // MANUAL
if (_sp_man.stab_switch == manual_control_setpoint_s::SWITCH_POS_NONE &&
_sp_man.man_switch == manual_control_setpoint_s::SWITCH_POS_NONE) {
if (_manual_control_setpoint.stab_switch == manual_control_setpoint_s::SWITCH_POS_NONE &&
_manual_control_setpoint.man_switch == manual_control_setpoint_s::SWITCH_POS_NONE) {
/*
* Legacy mode:
* Acro switch being used as stabilized switch in FW.
*/
if (_sp_man.acro_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
if (_manual_control_setpoint.acro_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
/* manual mode is stabilized already for multirotors, so switch to acro
* for any non-manual mode
*/
@@ -2881,7 +2886,7 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed
res = main_state_transition(status_local, commander_state_s::MAIN_STATE_MANUAL, status_flags, &_internal_state);
}
} else if (_sp_man.rattitude_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
} else if (_manual_control_setpoint.rattitude_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
/* Similar to acro transitions for multirotors. FW aircraft don't need a
* rattitude mode.*/
if (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
@@ -2900,19 +2905,19 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed
* - Acro is Acro
* - Manual is not default anymore when the manaul switch is assigned
*/
if (_sp_man.man_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
if (_manual_control_setpoint.man_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
res = main_state_transition(status_local, commander_state_s::MAIN_STATE_MANUAL, status_flags, &_internal_state);
} else if (_sp_man.acro_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
} else if (_manual_control_setpoint.acro_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
res = main_state_transition(status_local, commander_state_s::MAIN_STATE_ACRO, status_flags, &_internal_state);
} else if (_sp_man.rattitude_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
} else if (_manual_control_setpoint.rattitude_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
res = main_state_transition(status_local, commander_state_s::MAIN_STATE_RATTITUDE, status_flags, &_internal_state);
} else if (_sp_man.stab_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
} else if (_manual_control_setpoint.stab_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
res = main_state_transition(status_local, commander_state_s::MAIN_STATE_STAB, status_flags, &_internal_state);
} else if (_sp_man.man_switch == manual_control_setpoint_s::SWITCH_POS_NONE) {
} else if (_manual_control_setpoint.man_switch == manual_control_setpoint_s::SWITCH_POS_NONE) {
// default to MANUAL when no manual switch is set
res = main_state_transition(status_local, commander_state_s::MAIN_STATE_MANUAL, status_flags, &_internal_state);
@@ -2926,7 +2931,7 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed
break;
case manual_control_setpoint_s::SWITCH_POS_MIDDLE: // ASSIST
if (_sp_man.posctl_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
if (_manual_control_setpoint.posctl_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
res = main_state_transition(status_local, commander_state_s::MAIN_STATE_POSCTL, status_flags, &_internal_state);
if (res != TRANSITION_DENIED) {
@@ -2943,7 +2948,7 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed
break; // changed successfully or already in this mode
}
if (_sp_man.posctl_switch != manual_control_setpoint_s::SWITCH_POS_ON) {
if (_manual_control_setpoint.posctl_switch != manual_control_setpoint_s::SWITCH_POS_ON) {
print_reject_mode("ALTITUDE CONTROL");
}
src/modules/commander/Commander.hpp
+4 -4
View File
@@ -343,11 +343,11 @@ private:
unsigned int _leds_counter{0};
manual_control_setpoint_s _sp_man{}; ///< the current manual control setpoint
manual_control_setpoint_s _last_sp_man{}; ///< the manual control setpoint valid at the last mode switch
manual_control_setpoint_s _manual_control_setpoint{}; ///< the current manual control setpoint
manual_control_setpoint_s _last_manual_control_setpoint{}; ///< the manual control setpoint valid at the last mode switch
hrt_abstime _rc_signal_lost_timestamp{0}; ///< Time at which the RC reception was lost
int32_t _flight_mode_slots[manual_control_setpoint_s::MODE_SLOT_NUM] {};
uint8_t _last_sp_man_arm_switch{0};
uint8_t _last_manual_control_setpoint_arm_switch{0};
float _min_stick_change{};
uint32_t _stick_off_counter{0};
uint32_t _stick_on_counter{0};
@@ -400,7 +400,7 @@ private:
uORB::Subscription _land_detector_sub{ORB_ID(vehicle_land_detected)};
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
uORB::Subscription _safety_sub{ORB_ID(safety)};
uORB::Subscription _sp_man_sub{ORB_ID(manual_control_setpoint)};
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
uORB::Subscription _subsys_sub{ORB_ID(subsystem_info)};
uORB::Subscription _system_power_sub{ORB_ID(system_power)};
uORB::Subscription _telemetry_status_sub{ORB_ID(telemetry_status)};
src/modules/commander/rc_calibration.cpp
+7 -7
View File
@@ -52,17 +52,17 @@
int do_trim_calibration(orb_advert_t *mavlink_log_pub)
{
uORB::Subscription sub_man{ORB_ID(manual_control_setpoint)};
uORB::Subscription manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
px4_usleep(400000);
manual_control_setpoint_s sp{};
bool changed = sub_man.updated();
manual_control_setpoint_s manual_control_setpoint{};
bool changed = manual_control_setpoint_sub.updated();
if (!changed) {
mavlink_log_critical(mavlink_log_pub, "no inputs, aborting");
return PX4_ERROR;
}
sub_man.copy(&sp);
manual_control_setpoint_sub.copy(&manual_control_setpoint);
/* load trim values which are active */
float roll_trim_active;
@@ -83,15 +83,15 @@ int do_trim_calibration(orb_advert_t *mavlink_log_pub)
/* set parameters: the new trim values are the combination of active trim values
and the values coming from the remote control of the user
*/
float p = sp.y * roll_scale + roll_trim_active;
float p = manual_control_setpoint.y * roll_scale + roll_trim_active;
int p1r = param_set(param_find("TRIM_ROLL"), &p);
/*
we explicitly swap sign here because the trim is added to the actuator controls
which are moving in an inverse sense to manual pitch inputs
*/
p = -sp.x * pitch_scale + pitch_trim_active;
p = -manual_control_setpoint.x * pitch_scale + pitch_trim_active;
int p2r = param_set(param_find("TRIM_PITCH"), &p);
p = sp.r * yaw_scale + yaw_trim_active;
p = manual_control_setpoint.r * yaw_scale + yaw_trim_active;
int p3r = param_set(param_find("TRIM_YAW"), &p);
if (p1r != 0 || p2r != 0 || p3r != 0) {
src/modules/fw_att_control/FixedwingAttitudeControl.cpp
+25 -20
View File
@@ -141,11 +141,12 @@ FixedwingAttitudeControl::vehicle_manual_poll()
if (_vcontrol_mode.flag_control_manual_enabled && (!is_tailsitter_transition || is_fixed_wing)) {
// Always copy the new manual setpoint, even if it wasn't updated, to fill the _actuators with valid values
if (_manual_sub.copy(&_manual)) {
if (_manual_control_setpoint_sub.copy(&_manual_control_setpoint)) {
// Check if we are in rattitude mode and the pilot is above the threshold on pitch
if (_vcontrol_mode.flag_control_rattitude_enabled) {
if (fabsf(_manual.y) > _param_fw_ratt_th.get() || fabsf(_manual.x) > _param_fw_ratt_th.get()) {
if (fabsf(_manual_control_setpoint.y) > _param_fw_ratt_th.get()
|| fabsf(_manual_control_setpoint.x) > _param_fw_ratt_th.get()) {
_vcontrol_mode.flag_control_attitude_enabled = false;
}
}
@@ -156,16 +157,17 @@ FixedwingAttitudeControl::vehicle_manual_poll()
if (_vcontrol_mode.flag_control_attitude_enabled) {
// STABILIZED mode generate the attitude setpoint from manual user inputs
_att_sp.roll_body = _manual.y * radians(_param_fw_man_r_max.get()) + radians(_param_fw_rsp_off.get());
_att_sp.roll_body = _manual_control_setpoint.y * radians(_param_fw_man_r_max.get()) + radians(_param_fw_rsp_off.get());
_att_sp.roll_body = constrain(_att_sp.roll_body,
-radians(_param_fw_man_r_max.get()), radians(_param_fw_man_r_max.get()));
_att_sp.pitch_body = -_manual.x * radians(_param_fw_man_p_max.get()) + radians(_param_fw_psp_off.get());
_att_sp.pitch_body = -_manual_control_setpoint.x * radians(_param_fw_man_p_max.get())
+ radians(_param_fw_psp_off.get());
_att_sp.pitch_body = constrain(_att_sp.pitch_body,
-radians(_param_fw_man_p_max.get()), radians(_param_fw_man_p_max.get()));
_att_sp.yaw_body = 0.0f;
_att_sp.thrust_body[0] = _manual.z;
_att_sp.thrust_body[0] = _manual_control_setpoint.z;
Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body));
q.copyTo(_att_sp.q_d);
@@ -179,19 +181,22 @@ FixedwingAttitudeControl::vehicle_manual_poll()
// RATE mode we need to generate the rate setpoint from manual user inputs
_rates_sp.timestamp = hrt_absolute_time();
_rates_sp.roll = _manual.y * radians(_param_fw_acro_x_max.get());
_rates_sp.pitch = -_manual.x * radians(_param_fw_acro_y_max.get());
_rates_sp.yaw = _manual.r * radians(_param_fw_acro_z_max.get());
_rates_sp.thrust_body[0] = _manual.z;
_rates_sp.roll = _manual_control_setpoint.y * radians(_param_fw_acro_x_max.get());
_rates_sp.pitch = -_manual_control_setpoint.x * radians(_param_fw_acro_y_max.get());
_rates_sp.yaw = _manual_control_setpoint.r * radians(_param_fw_acro_z_max.get());
_rates_sp.thrust_body[0] = _manual_control_setpoint.z;
_rate_sp_pub.publish(_rates_sp);
} else {
/* manual/direct control */
_actuators.control[actuator_controls_s::INDEX_ROLL] = _manual.y * _param_fw_man_r_sc.get() + _param_trim_roll.get();
_actuators.control[actuator_controls_s::INDEX_PITCH] = -_manual.x * _param_fw_man_p_sc.get() + _param_trim_pitch.get();
_actuators.control[actuator_controls_s::INDEX_YAW] = _manual.r * _param_fw_man_y_sc.get() + _param_trim_yaw.get();
_actuators.control[actuator_controls_s::INDEX_THROTTLE] = _manual.z;
_actuators.control[actuator_controls_s::INDEX_ROLL] =
_manual_control_setpoint.y * _param_fw_man_r_sc.get() + _param_trim_roll.get();
_actuators.control[actuator_controls_s::INDEX_PITCH] =
-_manual_control_setpoint.x * _param_fw_man_p_sc.get() + _param_trim_pitch.get();
_actuators.control[actuator_controls_s::INDEX_YAW] =
_manual_control_setpoint.r * _param_fw_man_y_sc.get() + _param_trim_yaw.get();
_actuators.control[actuator_controls_s::INDEX_THROTTLE] = _manual_control_setpoint.z;
}
}
}
@@ -543,7 +548,7 @@ void FixedwingAttitudeControl::Run()
/* add in manual rudder control in manual modes */
if (_vcontrol_mode.flag_control_manual_enabled) {
_actuators.control[actuator_controls_s::INDEX_YAW] += _manual.r;
_actuators.control[actuator_controls_s::INDEX_YAW] += _manual_control_setpoint.r;
}
if (!PX4_ISFINITE(yaw_u)) {
@@ -626,10 +631,10 @@ void FixedwingAttitudeControl::Run()
* constrain(_actuators.control[actuator_controls_s::INDEX_ROLL], -1.0f, 1.0f);
_actuators.control[actuator_controls_s::INDEX_FLAPS] = _flaps_applied;
_actuators.control[5] = _manual.aux1;
_actuators.control[5] = _manual_control_setpoint.aux1;
_actuators.control[actuator_controls_s::INDEX_AIRBRAKES] = _flaperons_applied;
// FIXME: this should use _vcontrol_mode.landing_gear_pos in the future
_actuators.control[7] = _manual.aux3;
_actuators.control[7] = _manual_control_setpoint.aux3;
/* lazily publish the setpoint only once available */
_actuators.timestamp = hrt_absolute_time();
@@ -652,9 +657,9 @@ void FixedwingAttitudeControl::control_flaps(const float dt)
float flap_control = 0.0f;
/* map flaps by default to manual if valid */
if (PX4_ISFINITE(_manual.flaps) && _vcontrol_mode.flag_control_manual_enabled
if (PX4_ISFINITE(_manual_control_setpoint.flaps) && _vcontrol_mode.flag_control_manual_enabled
&& fabsf(_param_fw_flaps_scl.get()) > 0.01f) {
flap_control = 0.5f * (_manual.flaps + 1.0f) * _param_fw_flaps_scl.get();
flap_control = 0.5f * (_manual_control_setpoint.flaps + 1.0f) * _param_fw_flaps_scl.get();
} else if (_vcontrol_mode.flag_control_auto_enabled
&& fabsf(_param_fw_flaps_scl.get()) > 0.01f) {
@@ -686,10 +691,10 @@ void FixedwingAttitudeControl::control_flaps(const float dt)
float flaperon_control = 0.0f;
/* map flaperons by default to manual if valid */
if (PX4_ISFINITE(_manual.aux2) && _vcontrol_mode.flag_control_manual_enabled
if (PX4_ISFINITE(_manual_control_setpoint.aux2) && _vcontrol_mode.flag_control_manual_enabled
&& fabsf(_param_fw_flaperon_scl.get()) > 0.01f) {
flaperon_control = 0.5f * (_manual.aux2 + 1.0f) * _param_fw_flaperon_scl.get();
flaperon_control = 0.5f * (_manual_control_setpoint.aux2 + 1.0f) * _param_fw_flaperon_scl.get();
} else if (_vcontrol_mode.flag_control_auto_enabled
&& fabsf(_param_fw_flaperon_scl.get()) > 0.01f) {
src/modules/fw_att_control/FixedwingAttitudeControl.hpp
+2 -2
View File
@@ -98,7 +98,7 @@ private:
uORB::Subscription _att_sp_sub{ORB_ID(vehicle_attitude_setpoint)}; /**< vehicle attitude setpoint */
uORB::Subscription _battery_status_sub{ORB_ID(battery_status)}; /**< battery status subscription */
uORB::Subscription _local_pos_sub{ORB_ID(vehicle_local_position)}; /**< local position subscription */
uORB::Subscription _manual_sub{ORB_ID(manual_control_setpoint)}; /**< notification of manual control updates */
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; /**< notification of manual control updates */
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; /**< notification of parameter updates */
uORB::Subscription _rates_sp_sub{ORB_ID(vehicle_rates_setpoint)}; /**< vehicle rates setpoint */
uORB::Subscription _vcontrol_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle status subscription */
@@ -114,7 +114,7 @@ private:
uORB::PublicationMulti<rate_ctrl_status_s> _rate_ctrl_status_pub{ORB_ID(rate_ctrl_status)};
actuator_controls_s _actuators {}; /**< actuator control inputs */
manual_control_setpoint_s _manual {}; /**< r/c channel data */
manual_control_setpoint_s _manual_control_setpoint {}; /**< r/c channel data */
vehicle_attitude_s _att {}; /**< vehicle attitude setpoint */
vehicle_attitude_setpoint_s _att_sp {}; /**< vehicle attitude setpoint */
vehicle_control_mode_s _vcontrol_mode {}; /**< vehicle control mode */
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+17 -17
View File
@@ -229,17 +229,17 @@ FixedwingPositionControl::get_demanded_airspeed()
float altctrl_airspeed = 0;
// neutral throttle corresponds to trim airspeed
if (_manual.z < 0.5f) {
if (_manual_control_setpoint.z < 0.5f) {
// lower half of throttle is min to trim airspeed
altctrl_airspeed = _param_fw_airspd_min.get() +
(_param_fw_airspd_trim.get() - _param_fw_airspd_min.get()) *
_manual.z * 2;
_manual_control_setpoint.z * 2;
} else {
// upper half of throttle is trim to max airspeed
altctrl_airspeed = _param_fw_airspd_trim.get() +
(_param_fw_airspd_max.get() - _param_fw_airspd_trim.get()) *
(_manual.z * 2 - 1);
(_manual_control_setpoint.z * 2 - 1);
}
return altctrl_airspeed;
@@ -481,15 +481,15 @@ FixedwingPositionControl::update_desired_altitude(float dt)
* an axis. Positive X means to rotate positively around
* the X axis in NED frame, which is pitching down
*/
if (_manual.x > deadBand) {
if (_manual_control_setpoint.x > deadBand) {
/* pitching down */
float pitch = -(_manual.x - deadBand) / factor;
float pitch = -(_manual_control_setpoint.x - deadBand) / factor;
_hold_alt += (_param_fw_t_sink_max.get() * dt) * pitch;
_was_in_deadband = false;
} else if (_manual.x < - deadBand) {
} else if (_manual_control_setpoint.x < - deadBand) {
/* pitching up */
float pitch = -(_manual.x + deadBand) / factor;
float pitch = -(_manual_control_setpoint.x + deadBand) / factor;
_hold_alt += (_param_fw_t_clmb_max.get() * dt) * pitch;
_was_in_deadband = false;
climbout_mode = (pitch > MANUAL_THROTTLE_CLIMBOUT_THRESH);
@@ -791,7 +791,7 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto
/* reset setpoints from other modes (auto) otherwise we won't
* level out without new manual input */
_att_sp.roll_body = _manual.y * radians(_param_fw_man_r_max.get());
_att_sp.roll_body = _manual_control_setpoint.y * radians(_param_fw_man_r_max.get());
_att_sp.yaw_body = 0;
}
@@ -811,7 +811,7 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto
/* throttle limiting */
throttle_max = _param_fw_thr_max.get();
if (_vehicle_land_detected.landed && (fabsf(_manual.z) < THROTTLE_THRESH)) {
if (_vehicle_land_detected.landed && (fabsf(_manual_control_setpoint.z) < THROTTLE_THRESH)) {
throttle_max = 0.0f;
}
@@ -827,8 +827,8 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto
tecs_status_s::TECS_MODE_NORMAL);
/* heading control */
if (fabsf(_manual.y) < HDG_HOLD_MAN_INPUT_THRESH &&
fabsf(_manual.r) < HDG_HOLD_MAN_INPUT_THRESH) {
if (fabsf(_manual_control_setpoint.y) < HDG_HOLD_MAN_INPUT_THRESH &&
fabsf(_manual_control_setpoint.r) < HDG_HOLD_MAN_INPUT_THRESH) {
/* heading / roll is zero, lock onto current heading */
if (fabsf(_vehicle_rates_sub.get().xyz[2]) < HDG_HOLD_YAWRATE_THRESH && !_yaw_lock_engaged) {
@@ -879,12 +879,12 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto
}
}
if (!_yaw_lock_engaged || fabsf(_manual.y) >= HDG_HOLD_MAN_INPUT_THRESH ||
fabsf(_manual.r) >= HDG_HOLD_MAN_INPUT_THRESH) {
if (!_yaw_lock_engaged || fabsf(_manual_control_setpoint.y) >= HDG_HOLD_MAN_INPUT_THRESH ||
fabsf(_manual_control_setpoint.r) >= HDG_HOLD_MAN_INPUT_THRESH) {
_hdg_hold_enabled = false;
_yaw_lock_engaged = false;
_att_sp.roll_body = _manual.y * radians(_param_fw_man_r_max.get());
_att_sp.roll_body = _manual_control_setpoint.y * radians(_param_fw_man_r_max.get());
_att_sp.yaw_body = 0;
}
@@ -913,7 +913,7 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto
/* throttle limiting */
throttle_max = _param_fw_thr_max.get();
if (_vehicle_land_detected.landed && (fabsf(_manual.z) < THROTTLE_THRESH)) {
if (_vehicle_land_detected.landed && (fabsf(_manual_control_setpoint.z) < THROTTLE_THRESH)) {
throttle_max = 0.0f;
}
@@ -928,7 +928,7 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto
climbout_requested ? radians(10.0f) : pitch_limit_min,
tecs_status_s::TECS_MODE_NORMAL);
_att_sp.roll_body = _manual.y * radians(_param_fw_man_r_max.get());
_att_sp.roll_body = _manual_control_setpoint.y * radians(_param_fw_man_r_max.get());
_att_sp.yaw_body = 0;
} else {
@@ -1544,7 +1544,7 @@ FixedwingPositionControl::Run()
_pos_reset_counter = _local_pos.vxy_reset_counter;
airspeed_poll();
_manual_control_sub.update(&_manual);
_manual_control_setpoint_sub.update(&_manual_control_setpoint);
_pos_sp_triplet_sub.update(&_pos_sp_triplet);
vehicle_attitude_poll();
vehicle_command_poll();
src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
+2 -2
View File
@@ -152,7 +152,7 @@ private:
uORB::Subscription _control_mode_sub{ORB_ID(vehicle_control_mode)}; ///< control mode subscription
uORB::Subscription _global_pos_sub{ORB_ID(vehicle_global_position)};
uORB::Subscription _manual_control_sub{ORB_ID(manual_control_setpoint)}; ///< notification of manual control updates
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; ///< notification of manual control updates
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; ///< notification of parameter updates
uORB::Subscription _pos_sp_triplet_sub{ORB_ID(position_setpoint_triplet)};
uORB::Subscription _sensor_baro_sub{ORB_ID(sensor_baro)};
@@ -167,7 +167,7 @@ private:
uORB::Publication<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
manual_control_setpoint_s _manual {}; ///< r/c channel data
manual_control_setpoint_s _manual_control_setpoint {}; ///< r/c channel data
position_setpoint_triplet_s _pos_sp_triplet {}; ///< triplet of mission items
vehicle_attitude_s _att {}; ///< vehicle attitude setpoint
vehicle_attitude_setpoint_s _att_sp {}; ///< vehicle attitude setpoint
src/modules/logger/logger.cpp
+3 -3
View File
@@ -965,11 +965,11 @@ bool Logger::start_stop_logging()
if (_log_mode == LogMode::rc_aux1) {
// aux1-based logging
manual_control_setpoint_s manual_sp;
manual_control_setpoint_s manual_control_setpoint;
if (_manual_control_sp_sub.update(&manual_sp)) {
if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
desired_state = (manual_sp.aux1 > 0.3f);
desired_state = (manual_control_setpoint.aux1 > 0.3f);
updated = true;
}
src/modules/logger/logger.h
+1 -1
View File
@@ -347,7 +347,7 @@ private:
hrt_abstime _logger_status_last {0};
#endif
uORB::Subscription _manual_control_sp_sub{ORB_ID(manual_control_setpoint)};
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
uORB::SubscriptionInterval _log_message_sub{ORB_ID(log_message), 20};
src/modules/mavlink/mavlink_messages.cpp
+15 -14
View File
@@ -3776,11 +3776,12 @@ public:
unsigned get_size() override
{
return _manual_sub.advertised() ? (MAVLINK_MSG_ID_MANUAL_CONTROL_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
return _manual_control_setpoint_sub.advertised() ?
(MAVLINK_MSG_ID_MANUAL_CONTROL_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
}
private:
uORB::Subscription _manual_sub{ORB_ID(manual_control_setpoint)};
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
/* do not allow top copying this class */
MavlinkStreamManualControl(MavlinkStreamManualControl &) = delete;
@@ -3792,24 +3793,24 @@ protected:
bool send(const hrt_abstime t) override
{
manual_control_setpoint_s manual;
manual_control_setpoint_s manual_control_setpoint;
if (_manual_sub.update(&manual)) {
if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
mavlink_manual_control_t msg{};
msg.target = mavlink_system.sysid;
msg.x = manual.x * 1000;
msg.y = manual.y * 1000;
msg.z = manual.z * 1000;
msg.r = manual.r * 1000;
msg.x = manual_control_setpoint.x * 1000;
msg.y = manual_control_setpoint.y * 1000;
msg.z = manual_control_setpoint.z * 1000;
msg.r = manual_control_setpoint.r * 1000;
unsigned shift = 2;
msg.buttons = 0;
msg.buttons |= (manual.mode_switch << (shift * 0));
msg.buttons |= (manual.return_switch << (shift * 1));
msg.buttons |= (manual.posctl_switch << (shift * 2));
msg.buttons |= (manual.loiter_switch << (shift * 3));
msg.buttons |= (manual.acro_switch << (shift * 4));
msg.buttons |= (manual.offboard_switch << (shift * 5));
msg.buttons |= (manual_control_setpoint.mode_switch << (shift * 0));
msg.buttons |= (manual_control_setpoint.return_switch << (shift * 1));
msg.buttons |= (manual_control_setpoint.posctl_switch << (shift * 2));
msg.buttons |= (manual_control_setpoint.loiter_switch << (shift * 3));
msg.buttons |= (manual_control_setpoint.acro_switch << (shift * 4));
msg.buttons |= (manual_control_setpoint.offboard_switch << (shift * 5));
mavlink_msg_manual_control_send_struct(_mavlink->get_channel(), &msg);
src/modules/mavlink/mavlink_receiver.cpp
+1 -1
View File
@@ -2036,7 +2036,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
manual.z = man.z / 1000.0f;
manual.data_source = manual_control_setpoint_s::SOURCE_MAVLINK_0 + _mavlink->get_instance_id();
_manual_pub.publish(manual);
_manual_control_setpoint_pub.publish(manual);
}
}
src/modules/mavlink/mavlink_receiver.h
+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> _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<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<radio_status_s> _radio_status_pub{ORB_ID(radio_status), ORB_PRIO_LOW};
src/modules/mc_att_control/mc_att_control.hpp
+2 -2
View File
@@ -109,7 +109,7 @@ private:
uORB::Subscription _v_rates_sp_sub{ORB_ID(vehicle_rates_setpoint)}; /**< vehicle rates setpoint subscription */
uORB::Subscription _v_control_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle control mode subscription */
uORB::Subscription _params_sub{ORB_ID(parameter_update)}; /**< parameter updates subscription */
uORB::Subscription _manual_control_sp_sub{ORB_ID(manual_control_setpoint)}; /**< manual control setpoint subscription */
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; /**< manual control setpoint subscription */
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */
@@ -120,7 +120,7 @@ private:
struct vehicle_attitude_s _v_att {}; /**< vehicle attitude */
struct vehicle_rates_setpoint_s _v_rates_sp {}; /**< vehicle rates setpoint */
struct manual_control_setpoint_s _manual_control_sp {}; /**< manual control setpoint */
struct manual_control_setpoint_s _manual_control_setpoint {}; /**< manual control setpoint */
struct vehicle_control_mode_s _v_control_mode {}; /**< vehicle control mode */
struct vehicle_status_s _vehicle_status {}; /**< vehicle status */
struct vehicle_land_detected_s _vehicle_land_detected {};
src/modules/mc_att_control/mc_att_control_main.cpp
+8 -8
View File
@@ -136,10 +136,10 @@ MulticopterAttitudeControl::generate_attitude_setpoint(float dt, bool reset_yaw_
if (reset_yaw_sp) {
_man_yaw_sp = yaw;
} else if (_manual_control_sp.z > 0.05f || _param_mc_airmode.get() == (int32_t)Mixer::Airmode::roll_pitch_yaw) {
} else if (_manual_control_setpoint.z > 0.05f || _param_mc_airmode.get() == (int32_t)Mixer::Airmode::roll_pitch_yaw) {
const float yaw_rate = math::radians(_param_mpc_man_y_max.get());
attitude_setpoint.yaw_sp_move_rate = _manual_control_sp.r * yaw_rate;
attitude_setpoint.yaw_sp_move_rate = _manual_control_setpoint.r * yaw_rate;
_man_yaw_sp = wrap_pi(_man_yaw_sp + attitude_setpoint.yaw_sp_move_rate * dt);
}
@@ -155,8 +155,8 @@ MulticopterAttitudeControl::generate_attitude_setpoint(float dt, bool reset_yaw_
*/
_man_x_input_filter.setParameters(dt, _param_mc_man_tilt_tau.get());
_man_y_input_filter.setParameters(dt, _param_mc_man_tilt_tau.get());
_man_x_input_filter.update(_manual_control_sp.x * _man_tilt_max);
_man_y_input_filter.update(_manual_control_sp.y * _man_tilt_max);
_man_x_input_filter.update(_manual_control_setpoint.x * _man_tilt_max);
_man_y_input_filter.update(_manual_control_setpoint.y * _man_tilt_max);
const float x = _man_x_input_filter.getState();
const float y = _man_y_input_filter.getState();
@@ -219,7 +219,7 @@ MulticopterAttitudeControl::generate_attitude_setpoint(float dt, bool reset_yaw_
Quatf q_sp = Eulerf(attitude_setpoint.roll_body, attitude_setpoint.pitch_body, attitude_setpoint.yaw_body);
q_sp.copyTo(attitude_setpoint.q_d);
attitude_setpoint.thrust_body[2] = -throttle_curve(_manual_control_sp.z);
attitude_setpoint.thrust_body[2] = -throttle_curve(_manual_control_setpoint.z);
attitude_setpoint.timestamp = hrt_absolute_time();
_vehicle_attitude_setpoint_pub.publish(attitude_setpoint);
@@ -299,7 +299,7 @@ MulticopterAttitudeControl::Run()
_last_run = now;
/* check for updates in other topics */
_manual_control_sp_sub.update(&_manual_control_sp);
_manual_control_setpoint_sub.update(&_manual_control_setpoint);
_v_control_mode_sub.update(&_v_control_mode);
_vehicle_land_detected_sub.update(&_vehicle_land_detected);
_vehicle_status_sub.update(&_vehicle_status);
@@ -309,8 +309,8 @@ MulticopterAttitudeControl::Run()
* even bother running the attitude controllers */
if (_v_control_mode.flag_control_rattitude_enabled) {
_v_control_mode.flag_control_attitude_enabled =
fabsf(_manual_control_sp.y) <= _param_mc_ratt_th.get() &&
fabsf(_manual_control_sp.x) <= _param_mc_ratt_th.get();
fabsf(_manual_control_setpoint.y) <= _param_mc_ratt_th.get() &&
fabsf(_manual_control_setpoint.x) <= _param_mc_ratt_th.get();
}
bool attitude_setpoint_generated = false;
src/modules/mc_rate_control/MulticopterRateControl.cpp
+9 -9
View File
@@ -109,10 +109,10 @@ MulticopterRateControl::get_landing_gear_state()
float landing_gear = landing_gear_s::GEAR_DOWN; // default to down
if (_manual_control_sp.gear_switch == manual_control_setpoint_s::SWITCH_POS_ON && _gear_state_initialized) {
if (_manual_control_setpoint.gear_switch == manual_control_setpoint_s::SWITCH_POS_ON && _gear_state_initialized) {
landing_gear = landing_gear_s::GEAR_UP;
} else if (_manual_control_sp.gear_switch == manual_control_setpoint_s::SWITCH_POS_OFF) {
} else if (_manual_control_setpoint.gear_switch == manual_control_setpoint_s::SWITCH_POS_OFF) {
// Switching the gear off does put it into a safe defined state
_gear_state_initialized = true;
}
@@ -173,7 +173,7 @@ MulticopterRateControl::Run()
_vehicle_status_sub.update(&_vehicle_status);
const bool manual_control_updated = _manual_control_sp_sub.update(&_manual_control_sp);
const bool manual_control_updated = _manual_control_setpoint_sub.update(&_manual_control_setpoint);
// generate the rate setpoint from sticks?
bool manual_rate_sp = false;
@@ -199,8 +199,8 @@ MulticopterRateControl::Run()
// if true then use published rate setpoint, otherwise generate from manual_control_setpoint (like acro)
if (_v_control_mode.flag_control_rattitude_enabled) {
manual_rate_sp =
(fabsf(_manual_control_sp.y) > _param_mc_ratt_th.get()) ||
(fabsf(_manual_control_sp.x) > _param_mc_ratt_th.get());
(fabsf(_manual_control_setpoint.y) > _param_mc_ratt_th.get()) ||
(fabsf(_manual_control_setpoint.x) > _param_mc_ratt_th.get());
}
} else {
@@ -212,12 +212,12 @@ MulticopterRateControl::Run()
// manual rates control - ACRO mode
const Vector3f man_rate_sp{
math::superexpo(_manual_control_sp.y, _param_mc_acro_expo.get(), _param_mc_acro_supexpo.get()),
math::superexpo(-_manual_control_sp.x, _param_mc_acro_expo.get(), _param_mc_acro_supexpo.get()),
math::superexpo(_manual_control_sp.r, _param_mc_acro_expo_y.get(), _param_mc_acro_supexpoy.get())};
math::superexpo(_manual_control_setpoint.y, _param_mc_acro_expo.get(), _param_mc_acro_supexpo.get()),
math::superexpo(-_manual_control_setpoint.x, _param_mc_acro_expo.get(), _param_mc_acro_supexpo.get()),
math::superexpo(_manual_control_setpoint.r, _param_mc_acro_expo_y.get(), _param_mc_acro_supexpoy.get())};
_rates_sp = man_rate_sp.emult(_acro_rate_max);
_thrust_sp = _manual_control_sp.z;
_thrust_sp = _manual_control_setpoint.z;
// publish rate setpoint
vehicle_rates_setpoint_s v_rates_sp{};
src/modules/mc_rate_control/MulticopterRateControl.hpp
+2 -2
View File
@@ -95,7 +95,7 @@ private:
uORB::Subscription _battery_status_sub{ORB_ID(battery_status)};
uORB::Subscription _landing_gear_sub{ORB_ID(landing_gear)};
uORB::Subscription _manual_control_sp_sub{ORB_ID(manual_control_setpoint)};
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
uORB::Subscription _motor_limits_sub{ORB_ID(multirotor_motor_limits)};
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
uORB::Subscription _v_control_mode_sub{ORB_ID(vehicle_control_mode)};
@@ -112,7 +112,7 @@ private:
uORB::Publication<vehicle_rates_setpoint_s> _v_rates_sp_pub{ORB_ID(vehicle_rates_setpoint)}; /**< rate setpoint publication */
landing_gear_s _landing_gear{};
manual_control_setpoint_s _manual_control_sp{};
manual_control_setpoint_s _manual_control_setpoint{};
vehicle_control_mode_s _v_control_mode{};
vehicle_status_s _vehicle_status{};
src/modules/rc_update/rc_update.cpp
+57 -56
View File
@@ -438,25 +438,25 @@ RCUpdate::Run()
if (!signal_lost && rc_input.timestamp_last_signal > 0) {
/* initialize manual setpoint */
manual_control_setpoint_s manual{};
manual_control_setpoint_s manual_control_setpoint{};
/* set mode slot to unassigned */
manual.mode_slot = manual_control_setpoint_s::MODE_SLOT_NONE;
manual_control_setpoint.mode_slot = manual_control_setpoint_s::MODE_SLOT_NONE;
/* set the timestamp to the last signal time */
manual.timestamp = rc_input.timestamp_last_signal;
manual.data_source = manual_control_setpoint_s::SOURCE_RC;
manual_control_setpoint.timestamp = rc_input.timestamp_last_signal;
manual_control_setpoint.data_source = manual_control_setpoint_s::SOURCE_RC;
/* limit controls */
manual.y = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_ROLL, -1.0, 1.0);
manual.x = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_PITCH, -1.0, 1.0);
manual.r = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_YAW, -1.0, 1.0);
manual.z = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_THROTTLE, 0.0, 1.0);
manual.flaps = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_FLAPS, -1.0, 1.0);
manual.aux1 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_1, -1.0, 1.0);
manual.aux2 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_2, -1.0, 1.0);
manual.aux3 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_3, -1.0, 1.0);
manual.aux4 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_4, -1.0, 1.0);
manual.aux5 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_5, -1.0, 1.0);
manual.aux6 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_6, -1.0, 1.0);
manual_control_setpoint.y = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_ROLL, -1.0, 1.0);
manual_control_setpoint.x = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_PITCH, -1.0, 1.0);
manual_control_setpoint.r = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_YAW, -1.0, 1.0);
manual_control_setpoint.z = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_THROTTLE, 0.0, 1.0);
manual_control_setpoint.flaps = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_FLAPS, -1.0, 1.0);
manual_control_setpoint.aux1 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_1, -1.0, 1.0);
manual_control_setpoint.aux2 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_2, -1.0, 1.0);
manual_control_setpoint.aux3 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_3, -1.0, 1.0);
manual_control_setpoint.aux4 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_4, -1.0, 1.0);
manual_control_setpoint.aux5 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_5, -1.0, 1.0);
manual_control_setpoint.aux6 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_6, -1.0, 1.0);
if (_param_rc_map_fltmode.get() > 0) {
/* number of valid slots */
@@ -476,60 +476,61 @@ RCUpdate::Run()
* slots. And finally we add half a slot width to ensure that integer rounding
* will take us to the correct final index.
*/
manual.mode_slot = (((((_rc.channels[_param_rc_map_fltmode.get() - 1] - slot_min) * num_slots) + slot_width_half) /
(slot_max - slot_min)) + (1.0f / num_slots)) + 1;
manual_control_setpoint.mode_slot = (((((_rc.channels[_param_rc_map_fltmode.get() - 1] - slot_min) * num_slots) +
slot_width_half)
/ (slot_max - slot_min)) + (1.0f / num_slots)) + 1;
if (manual.mode_slot > num_slots) {
manual.mode_slot = num_slots;
if (manual_control_setpoint.mode_slot > num_slots) {
manual_control_setpoint.mode_slot = num_slots;
}
}
/* mode switches */
manual.mode_switch = get_rc_sw3pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_MODE,
_param_rc_auto_th.get(), _param_rc_auto_th.get() < 0.f,
_param_rc_assist_th.get(), _param_rc_assist_th.get() < 0.f);
manual_control_setpoint.mode_switch = get_rc_sw3pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_MODE,
_param_rc_auto_th.get(), _param_rc_auto_th.get() < 0.f,
_param_rc_assist_th.get(), _param_rc_assist_th.get() < 0.f);
manual.rattitude_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_RATTITUDE,
_param_rc_ratt_th.get(), _param_rc_ratt_th.get() < 0.f);
manual.posctl_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_POSCTL,
_param_rc_posctl_th.get(), _param_rc_posctl_th.get() < 0.f);
manual.return_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_RETURN,
_param_rc_return_th.get(), _param_rc_return_th.get() < 0.f);
manual.loiter_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_LOITER,
_param_rc_loiter_th.get(), _param_rc_loiter_th.get() < 0.f);
manual.acro_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_ACRO,
_param_rc_acro_th.get(), _param_rc_acro_th.get() < 0.f);
manual.offboard_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_OFFBOARD,
_param_rc_offb_th.get(), _param_rc_offb_th.get() < 0.f);
manual.kill_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_KILLSWITCH,
_param_rc_killswitch_th.get(), _param_rc_killswitch_th.get() < 0.f);
manual.arm_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_ARMSWITCH,
_param_rc_armswitch_th.get(), _param_rc_armswitch_th.get() < 0.f);
manual.transition_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_TRANSITION,
_param_rc_trans_th.get(), _param_rc_trans_th.get() < 0.f);
manual.gear_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_GEAR,
_param_rc_gear_th.get(), _param_rc_gear_th.get() < 0.f);
manual.stab_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_STAB,
_param_rc_stab_th.get(), _param_rc_stab_th.get() < 0.f);
manual.man_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_MAN,
_param_rc_man_th.get(), _param_rc_man_th.get() < 0.f);
manual_control_setpoint.rattitude_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_RATTITUDE,
_param_rc_ratt_th.get(), _param_rc_ratt_th.get() < 0.f);
manual_control_setpoint.posctl_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_POSCTL,
_param_rc_posctl_th.get(), _param_rc_posctl_th.get() < 0.f);
manual_control_setpoint.return_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_RETURN,
_param_rc_return_th.get(), _param_rc_return_th.get() < 0.f);
manual_control_setpoint.loiter_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_LOITER,
_param_rc_loiter_th.get(), _param_rc_loiter_th.get() < 0.f);
manual_control_setpoint.acro_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_ACRO,
_param_rc_acro_th.get(), _param_rc_acro_th.get() < 0.f);
manual_control_setpoint.offboard_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_OFFBOARD,
_param_rc_offb_th.get(), _param_rc_offb_th.get() < 0.f);
manual_control_setpoint.kill_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_KILLSWITCH,
_param_rc_killswitch_th.get(), _param_rc_killswitch_th.get() < 0.f);
manual_control_setpoint.arm_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_ARMSWITCH,
_param_rc_armswitch_th.get(), _param_rc_armswitch_th.get() < 0.f);
manual_control_setpoint.transition_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_TRANSITION,
_param_rc_trans_th.get(), _param_rc_trans_th.get() < 0.f);
manual_control_setpoint.gear_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_GEAR,
_param_rc_gear_th.get(), _param_rc_gear_th.get() < 0.f);
manual_control_setpoint.stab_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_STAB,
_param_rc_stab_th.get(), _param_rc_stab_th.get() < 0.f);
manual_control_setpoint.man_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_MAN,
_param_rc_man_th.get(), _param_rc_man_th.get() < 0.f);
/* publish manual_control_setpoint topic */
_manual_control_pub.publish(manual);
_manual_control_setpoint_pub.publish(manual_control_setpoint);
/* copy from mapped manual control to control group 3 */
/* copy from mapped manual_control_setpoint control to control group 3 */
actuator_controls_s actuator_group_3{};
actuator_group_3.timestamp = rc_input.timestamp_last_signal;
actuator_group_3.control[0] = manual.y;
actuator_group_3.control[1] = manual.x;
actuator_group_3.control[2] = manual.r;
actuator_group_3.control[3] = manual.z;
actuator_group_3.control[4] = manual.flaps;
actuator_group_3.control[5] = manual.aux1;
actuator_group_3.control[6] = manual.aux2;
actuator_group_3.control[7] = manual.aux3;
actuator_group_3.control[0] = manual_control_setpoint.y;
actuator_group_3.control[1] = manual_control_setpoint.x;
actuator_group_3.control[2] = manual_control_setpoint.r;
actuator_group_3.control[3] = manual_control_setpoint.z;
actuator_group_3.control[4] = manual_control_setpoint.flaps;
actuator_group_3.control[5] = manual_control_setpoint.aux1;
actuator_group_3.control[6] = manual_control_setpoint.aux2;
actuator_group_3.control[7] = manual_control_setpoint.aux3;
/* publish actuator_controls_3 topic */
_actuator_group_3_pub.publish(actuator_group_3);
src/modules/rc_update/rc_update.h
+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<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 */
src/modules/rover_pos_control/RoverPositionControl.cpp
+10 -10
View File
@@ -108,10 +108,10 @@ void
RoverPositionControl::manual_control_setpoint_poll()
{
bool manual_updated;
orb_check(_manual_control_sub, &manual_updated);
orb_check(_manual_control_setpoint_sub, &manual_updated);
if (manual_updated) {
orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sub, &_manual);
orb_copy(ORB_ID(manual_control_setpoint), _manual_control_setpoint_sub, &_manual_control_setpoint);
}
}
@@ -352,7 +352,7 @@ RoverPositionControl::run()
_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
_local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
_manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
_manual_control_setpoint_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
_pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
@@ -374,7 +374,7 @@ RoverPositionControl::run()
/* Setup of loop */
fds[0].fd = _global_pos_sub;
fds[0].events = POLLIN;
fds[1].fd = _manual_control_sub;
fds[1].fd = _manual_control_setpoint_sub;
fds[1].events = POLLIN;
fds[2].fd = _sensor_combined_sub;
fds[2].events = POLLIN;
@@ -494,15 +494,15 @@ RoverPositionControl::run()
// This should be copied even if not in manual mode. Otherwise, the poll(...) call will keep
// returning immediately and this loop will eat up resources.
orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sub, &_manual);
orb_copy(ORB_ID(manual_control_setpoint), _manual_control_setpoint_sub, &_manual_control_setpoint);
if (manual_mode) {
/* manual/direct control */
//PX4_INFO("Manual mode!");
_act_controls.control[actuator_controls_s::INDEX_ROLL] = _manual.y;
_act_controls.control[actuator_controls_s::INDEX_PITCH] = -_manual.x;
_act_controls.control[actuator_controls_s::INDEX_YAW] = _manual.r; //TODO: Readd yaw scale param
_act_controls.control[actuator_controls_s::INDEX_THROTTLE] = _manual.z;
_act_controls.control[actuator_controls_s::INDEX_ROLL] = _manual_control_setpoint.y;
_act_controls.control[actuator_controls_s::INDEX_PITCH] = -_manual_control_setpoint.x;
_act_controls.control[actuator_controls_s::INDEX_YAW] = _manual_control_setpoint.r; //TODO: Readd yaw scale param
_act_controls.control[actuator_controls_s::INDEX_THROTTLE] = _manual_control_setpoint.z;
}
}
@@ -528,7 +528,7 @@ RoverPositionControl::run()
orb_unsubscribe(_control_mode_sub);
orb_unsubscribe(_global_pos_sub);
orb_unsubscribe(_local_pos_sub);
orb_unsubscribe(_manual_control_sub);
orb_unsubscribe(_manual_control_setpoint_sub);
orb_unsubscribe(_pos_sp_triplet_sub);
orb_unsubscribe(_vehicle_attitude_sub);
orb_unsubscribe(_sensor_combined_sub);
src/modules/rover_pos_control/RoverPositionControl.hpp
+2 -2
View File
@@ -106,7 +106,7 @@ private:
int _control_mode_sub{-1}; /**< control mode subscription */
int _global_pos_sub{-1};
int _local_pos_sub{-1};
int _manual_control_sub{-1}; /**< notification of manual control updates */
int _manual_control_setpoint_sub{-1}; /**< notification of manual control updates */
int _pos_sp_triplet_sub{-1};
int _att_sp_sub{-1};
int _vehicle_attitude_sub{-1};
@@ -114,7 +114,7 @@ private:
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
manual_control_setpoint_s _manual{}; /**< r/c channel data */
manual_control_setpoint_s _manual_control_setpoint{}; /**< r/c channel data */
position_setpoint_triplet_s _pos_sp_triplet{}; /**< triplet of mission items */
vehicle_attitude_setpoint_s _att_sp{}; /**< attitude setpoint > */
vehicle_control_mode_s _control_mode{}; /**< control mode */
src/modules/uuv_att_control/uuv_att_control.cpp
+8 -7
View File
@@ -95,10 +95,10 @@ void UUVAttitudeControl::vehicle_control_mode_poll()
void UUVAttitudeControl::manual_control_setpoint_poll()
{
bool updated = false;
orb_check(_manual_control_sub, &updated);
orb_check(_manual_control_setpoint_sub, &updated);
if (updated) {
orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sub, &_manual);
orb_copy(ORB_ID(manual_control_setpoint), _manual_control_setpoint_sub, &_manual_control_setpoint);
}
}
@@ -250,7 +250,7 @@ void UUVAttitudeControl::run()
_local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
_vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
_manual_control_setpoint_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
@@ -266,7 +266,7 @@ void UUVAttitudeControl::run()
/* Setup of loop */
fds[0].fd = _vehicle_attitude_sub;
fds[0].events = POLLIN;
fds[1].fd = _manual_control_sub;
fds[1].fd = _manual_control_setpoint_sub;
fds[1].events = POLLIN;
fds[2].fd = _sensor_combined_sub;
fds[2].events = POLLIN;
@@ -337,11 +337,12 @@ void UUVAttitudeControl::run()
if (fds[1].revents & POLLIN) {
// This should be copied even if not in manual mode. Otherwise, the poll(...) call will keep
// returning immediately and this loop will eat up resources.
orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sub, &_manual);
orb_copy(ORB_ID(manual_control_setpoint), _manual_control_setpoint_sub, &_manual_control_setpoint);
if (_vcontrol_mode.flag_control_manual_enabled && !_vcontrol_mode.flag_control_rates_enabled) {
/* manual/direct control */
constrain_actuator_commands(_manual.y, -_manual.x, _manual.r, _manual.z);
constrain_actuator_commands(_manual_control_setpoint.y, -_manual_control_setpoint.x,
_manual_control_setpoint.r, _manual_control_setpoint.z);
}
}
@@ -363,7 +364,7 @@ void UUVAttitudeControl::run()
}
orb_unsubscribe(_vcontrol_mode_sub);
orb_unsubscribe(_manual_control_sub);
orb_unsubscribe(_manual_control_setpoint_sub);
orb_unsubscribe(_vehicle_attitude_sub);
orb_unsubscribe(_local_pos_sub);
orb_unsubscribe(_sensor_combined_sub);
src/modules/uuv_att_control/uuv_att_control.hpp
+2 -2
View File
@@ -126,12 +126,12 @@ private:
int _vehicle_attitude_sub{-1}; /**< control state subscription */
int _angular_velocity_sub{-1}; /**< vehicle angular velocity subscription */
int _local_pos_sub{-1}; /**< local position subscription */
int _manual_control_sub{-1}; /**< notification of manual control updates */
int _manual_control_setpoint_sub{-1}; /**< notification of manual control updates */
int _vcontrol_mode_sub{-1}; /**< vehicle status subscription */
int _sensor_combined_sub{-1}; /**< sensor combined subscription */
actuator_controls_s _actuators {}; /**< actuator control inputs */
manual_control_setpoint_s _manual {}; /**< r/c channel data */
manual_control_setpoint_s _manual_control_setpoint {}; /**< r/c channel data */
vehicle_attitude_s _vehicle_attitude {}; /**< control state */
vehicle_angular_velocity_s _angular_velocity{}; /**< angular velocity */
vehicle_attitude_setpoint_s _vehicle_attitude_sp {};/**< vehicle attitude setpoint */
src/modules/vtol_att_control/vtol_att_control_main.cpp
+3 -3
View File
@@ -197,9 +197,9 @@ VtolAttitudeControl::is_fixed_wing_requested()
{
bool to_fw = false;
if (_manual_control_sp.transition_switch != manual_control_setpoint_s::SWITCH_POS_NONE &&
if (_manual_control_setpoint.transition_switch != manual_control_setpoint_s::SWITCH_POS_NONE &&
_v_control_mode.flag_control_manual_enabled) {
to_fw = (_manual_control_sp.transition_switch == manual_control_setpoint_s::SWITCH_POS_ON);
to_fw = (_manual_control_setpoint.transition_switch == manual_control_setpoint_s::SWITCH_POS_ON);
} else {
// listen to transition commands if not in manual or mode switch is not mapped
@@ -387,7 +387,7 @@ VtolAttitudeControl::Run()
}
_v_control_mode_sub.update(&_v_control_mode);
_manual_control_sp_sub.update(&_manual_control_sp);
_manual_control_setpoint_sub.update(&_manual_control_setpoint);
_v_att_sub.update(&_v_att);
_local_pos_sub.update(&_local_pos);
_local_pos_sp_sub.update(&_local_pos_sp);
src/modules/vtol_att_control/vtol_att_control_main.h
+2 -2
View File
@@ -138,7 +138,7 @@ private:
uORB::Subscription _land_detected_sub{ORB_ID(vehicle_land_detected)};
uORB::Subscription _local_pos_sp_sub{ORB_ID(vehicle_local_position_setpoint)}; // setpoint subscription
uORB::Subscription _local_pos_sub{ORB_ID(vehicle_local_position)}; // sensor subscription
uORB::Subscription _manual_control_sp_sub{ORB_ID(manual_control_setpoint)}; //manual control setpoint subscription
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; //manual control setpoint subscription
uORB::Subscription _mc_virtual_att_sp_sub{ORB_ID(mc_virtual_attitude_setpoint)};
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
uORB::Subscription _pos_sp_triplet_sub{ORB_ID(position_setpoint_triplet)}; // local position setpoint subscription
@@ -165,7 +165,7 @@ private:
actuator_controls_s _actuators_out_1{}; //actuator controls going to the fw mixer (used for elevons)
airspeed_validated_s _airspeed_validated{}; // airspeed
manual_control_setpoint_s _manual_control_sp{}; //manual control setpoint
manual_control_setpoint_s _manual_control_setpoint{}; //manual control setpoint
position_setpoint_triplet_s _pos_sp_triplet{};
tecs_status_s _tecs_status{};
vehicle_attitude_s _v_att{}; //vehicle attitude