fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-30 02:10:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

new manual_control_switches msg (split out of manual_control_setpoint) (#16270)

Browse Source 
- split out switches from manual_control_setpoint into new message manual_control_switches
 - manual_control_switches published at minimal rate (~ 1 Hz) or immediately on change
 - simple switch debounce in rc_update (2 consecutive identical decodes required)
 - manual_control_switches logged at full rate rather than sampled at (5-10% of messages logged)
 - manual_control_setpoint publish at minimal rate unless changing
 - commander handle landing gear switch for manual modes
 - processing of mode_slot and mode_switch is now split so we only do one or the other (not both)
     - a future step will be to finally drop mode_switch and accompanying switches entirely

Co-authored-by: Matthias Grob <maetugr@gmail.com>
This commit is contained in:
Daniel Agar
2020-12-11 12:11:35 -05:00
committed by GitHub
parent 25ef76b3b8
commit ef6209ba03
25 changed files with 670 additions and 587 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_rate_control/MulticopterRateControl.cpp
+11 -36
View File
@@ -96,30 +96,6 @@ MulticopterRateControl::parameters_updated()
_actuators_0_circuit_breaker_enabled = circuit_breaker_enabled_by_val(_param_cbrk_rate_ctrl.get(), CBRK_RATE_CTRL_KEY);
}
float
MulticopterRateControl::get_landing_gear_state()
{
// Only switch the landing gear up if we are not landed and if
// the user switched from gear down to gear up.
// If the user had the switch in the gear up position and took off ignore it
// until he toggles the switch to avoid retracting the gear immediately on takeoff.
if (_landed) {
_gear_state_initialized = false;
}
float landing_gear = landing_gear_s::GEAR_DOWN; // default to down
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_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;
}
return landing_gear;
}
void
MulticopterRateControl::Run()
{
@@ -173,6 +149,16 @@ MulticopterRateControl::Run()
_vehicle_status_sub.update(&_vehicle_status);
if (_landing_gear_sub.updated()) {
landing_gear_s landing_gear;
if (_landing_gear_sub.copy(&landing_gear)) {
if (landing_gear.landing_gear != landing_gear_s::GEAR_KEEP) {
_landing_gear = landing_gear.landing_gear;
}
}
}
const bool manual_control_updated = _manual_control_setpoint_sub.update(&_manual_control_setpoint);
// generate the rate setpoint from sticks?
@@ -183,14 +169,6 @@ MulticopterRateControl::Run()
!_v_control_mode.flag_control_velocity_enabled &&
!_v_control_mode.flag_control_position_enabled) {
// landing gear controlled from stick inputs if we are in Manual/Stabilized mode
// limit landing gear update rate to 10 Hz
if ((now - _landing_gear.timestamp) > 100_ms) {
_landing_gear.landing_gear = get_landing_gear_state();
_landing_gear.timestamp = hrt_absolute_time();
_landing_gear_pub.publish(_landing_gear);
}
if (!_v_control_mode.flag_control_attitude_enabled) {
manual_rate_sp = true;
}
@@ -202,9 +180,6 @@ MulticopterRateControl::Run()
(fabsf(_manual_control_setpoint.y) > _param_mc_ratt_th.get()) ||
(fabsf(_manual_control_setpoint.x) > _param_mc_ratt_th.get());
}
} else {
_landing_gear_sub.update(&_landing_gear);
}
if (manual_rate_sp) {
@@ -279,7 +254,7 @@ MulticopterRateControl::Run()
actuators.control[actuator_controls_s::INDEX_PITCH] = PX4_ISFINITE(att_control(1)) ? att_control(1) : 0.0f;
actuators.control[actuator_controls_s::INDEX_YAW] = PX4_ISFINITE(att_control(2)) ? att_control(2) : 0.0f;
actuators.control[actuator_controls_s::INDEX_THROTTLE] = PX4_ISFINITE(_thrust_sp) ? _thrust_sp : 0.0f;
actuators.control[actuator_controls_s::INDEX_LANDING_GEAR] = (float)_landing_gear.landing_gear;
actuators.control[actuator_controls_s::INDEX_LANDING_GEAR] = _landing_gear;
actuators.timestamp_sample = angular_velocity.timestamp_sample;
// scale effort by battery status if enabled