mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-11-22 14:19:57 +08:00
* test(mavsdk): fail test cases on sanitizer reports The runner only looked at the mavsdk_tests exit code, so ASan, TSan and UBSan reports from px4 ended up in the logs without failing anything. Scan the output of all processes for sanitizer report markers and fail the test case if any appear, including leak reports printed at exit. Assisted-by: Claude:claude-opus-5-5 Signed-off-by: Julian Oes <julian@oes.ch> * fix(mavlink): allow more boot time when sim speed factor is set In lockstep SITL the boot timeout is measured in sim time, but the remainder of rcS after mavlink start runs in real time. At high speed factors (e.g. 30x in mavsdk_tests) the 20 s sim-time timeout expires before rcS reaches "mavlink boot_complete", triggering a spurious error. Use 40 s when PX4_SIM_SPEED_FACTOR is set. Assisted-by: Claude:claude-opus-5-5 Signed-off-by: Julian Oes <julian@oes.ch> * fix(mavlink): don't share vehicle_status subscription across threads send_autopilot_capabilities() (also called from the receiver thread on REQUEST_MESSAGE) and the SETUP_SIGNING armed check (receiver thread) used the same _vehicle_status_sub as handleStatus() on the main thread. Besides being a data race flagged by TSan, advancing the subscription's generation from another thread can make handleStatus() miss a vehicle_status update. Use local subscriptions for those two callers. Assisted-by: Claude:claude-opus-5-5 Signed-off-by: Julian Oes <julian@oes.ch> * fix(gyro_fft): do initial sensor selection on the work queue init() ran SensorSelectionUpdate() on the shell thread, which registers the gyro callback before setting _selected_sensor_device_id, _gyro_fifo and the gap perf counter. A gyro sample can then start Run() on the work queue while those are still being written, which TSan flagged. Schedule Run() instead and let it do the first selection. Assisted-by: Claude:claude-opus-5-5 Signed-off-by: Julian Oes <julian@oes.ch> * fix(logger): copy events into an aligned struct before access The event was copied into _msg_buffer at an offset that isn't aligned for event_s and then accessed through an event_s pointer, which is undefined behaviour (flagged by UBSan) even for single-byte fields. Copy into a local event_s, adjust it there and memcpy it into the buffer. The bytes written to the log are unchanged. Assisted-by: Claude:claude-opus-5-5 Signed-off-by: Julian Oes <julian@oes.ch> * fix(manual_control): don't convert negative kill gesture time to unsigned MAN_KILL_GEST_T is negative by default to disable the gesture, which made the hysteresis time a negative float converted to hrt_abstime, undefined behaviour flagged by UBSan. Clamp to 0; the gesture is only evaluated when the parameter is positive. Assisted-by: Claude:claude-opus-5-5 Signed-off-by: Julian Oes <julian@oes.ch> * fix(mavlink): define mavlink_channel_t with all 6 channels The library's mavlink_channel_t ends at MAVLINK_COMM_3, and channels 4 and 5 were created by casting out-of-range values to the enum, which UBSan flags. Use the library's HAVE_MAVLINK_CHANNEL_T hook to define the enum with all channels when MAVLINK_COMM_NUM_BUFFERS is 6. Assisted-by: Claude:claude-opus-5-5 Signed-off-by: Julian Oes <julian@oes.ch> * fix(mavlink): clamp DISTANCE_SENSOR fields to their integer ranges A slightly negative range reading (seen with SIH near the ground) was converted to the uint16_t distance field, which is undefined behaviour flagged by UBSan. Clamp distances and covariance to their field ranges. Assisted-by: Claude:claude-opus-5-5 Signed-off-by: Julian Oes <julian@oes.ch> * fix(sensors): skip baro GNSS offset without baro samples With no baro samples accumulated, the average pressure was 0/0 = NaN. A NaN delta altitude poisons the offset low-pass filter, so the baro GNSS autocal could silently never complete. Skip the run instead, and leave the GNSS sample for the next one. Flagged by UBSan. Assisted-by: Claude:claude-opus-5-5 Signed-off-by: Julian Oes <julian@oes.ch> * fix(mavlink): don't divide by zero rate without variable rate streams Without variable rate streams the bandwidth multiplier was a division by zero, and the resulting inf was only clamped away later. There is nothing to scale in that case, so use 1. Flagged by UBSan. Assisted-by: Claude:claude-opus-5-5 Signed-off-by: Julian Oes <julian@oes.ch> * fix(gyro_fft): skip peak interpolation for a zero or flat spectrum Quinn's estimator divides by the peak bin magnitude and by 1 - ap and 1 - am, which are zero for a zero or flat spectrum (e.g. noise-free SIH gyro data). Return NAN, the existing no-estimate value, instead. Flagged by UBSan. Assisted-by: Claude:claude-opus-5-5 Signed-off-by: Julian Oes <julian@oes.ch> * fix(logger): don't compute write rate before time has passed Right after a log file is opened the elapsed time is zero, so the write rate was a division by zero. Report 0 until time has passed. Flagged by UBSan. Assisted-by: Claude:claude-opus-5-5 Signed-off-by: Julian Oes <julian@oes.ch> * fix(battery): check cell voltage before computing scale Check the filtered cell voltage before dividing by it rather than relying on the NaN/inf result of the division. Flagged by UBSan. Assisted-by: Claude:claude-opus-5-5 Signed-off-by: Julian Oes <julian@oes.ch> * fix(perf): report 0 rms for a single event With one event the rms was computed as 0/0. Flagged by UBSan. Assisted-by: Claude:claude-opus-5-5 Signed-off-by: Julian Oes <julian@oes.ch> * fix(matrix): only normalize axis for non-zero axis-angle rotations The axis was normalized before checking for a zero angle, dividing by a zero norm (e.g. SIH with no rotation). The result was already unused in that case. Flagged by UBSan. Assisted-by: Claude:claude-opus-5-5 Signed-off-by: Julian Oes <julian@oes.ch> * fix(mathlib): return infinite cutoff for unconfigured AlphaFilter A time constant of zero means no filtering; return an infinite cutoff frequency explicitly instead of dividing by zero. Flagged by UBSan. Assisted-by: Claude:claude-opus-5-5 Signed-off-by: Julian Oes <julian@oes.ch> --------- Signed-off-by: Julian Oes <julian@oes.ch>
665 lines
23 KiB
C++
665 lines
23 KiB
C++
/****************************************************************************
|
|
*
|
|
* Copyright (c) 2021 PX4 Development Team. All rights reserved.
|
|
*
|
|
* Redistribution and use in source and binary forms, with or without
|
|
* modification, are permitted provided that the following conditions
|
|
* are met:
|
|
*
|
|
* 1. Redistributions of source code must retain the above copyright
|
|
* notice, this list of conditions and the following disclaimer.
|
|
* 2. Redistributions in binary form must reproduce the above copyright
|
|
* notice, this list of conditions and the following disclaimer in
|
|
* the documentation and/or other materials provided with the
|
|
* distribution.
|
|
* 3. Neither the name PX4 nor the names of its contributors may be
|
|
* used to endorse or promote products derived from this software
|
|
* without specific prior written permission.
|
|
*
|
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
|
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
|
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
|
* POSSIBILITY OF SUCH DAMAGE.
|
|
*
|
|
****************************************************************************/
|
|
|
|
#include "ManualControl.hpp"
|
|
|
|
#include <px4_platform_common/events.h>
|
|
#include <lib/mathlib/mathlib.h>
|
|
#include <lib/systemlib/mavlink_log.h>
|
|
#include <uORB/topics/vehicle_command.h>
|
|
|
|
ModuleBase::Descriptor ManualControl::desc{task_spawn, custom_command, print_usage};
|
|
|
|
ManualControl::ManualControl() :
|
|
ModuleParams(nullptr),
|
|
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default)
|
|
{
|
|
updateParams();
|
|
}
|
|
|
|
ManualControl::~ManualControl()
|
|
{
|
|
perf_free(_loop_perf);
|
|
perf_free(_loop_interval_perf);
|
|
}
|
|
|
|
bool ManualControl::init()
|
|
{
|
|
#if defined(PAYLOAD_POWER_EN)
|
|
|
|
// If the payload power switch is mapped, default to power off until the RC switch explicitly commands it on.
|
|
if (_param_rc_map_pay_sw.get()) {
|
|
PAYLOAD_POWER_EN(false);
|
|
}
|
|
|
|
#endif // PAYLOAD_POWER_EN
|
|
|
|
ScheduleNow();
|
|
return true;
|
|
}
|
|
|
|
void ManualControl::Run()
|
|
{
|
|
if (should_exit()) {
|
|
ScheduleClear();
|
|
exit_and_cleanup(desc);
|
|
return;
|
|
}
|
|
|
|
perf_begin(_loop_perf);
|
|
perf_count(_loop_interval_perf);
|
|
|
|
processInput(hrt_absolute_time());
|
|
|
|
// reschedule to detect timeouts
|
|
ScheduleDelayed(200_ms);
|
|
|
|
perf_end(_loop_perf);
|
|
}
|
|
|
|
void ManualControl::processInput(hrt_abstime now)
|
|
{
|
|
if (_vehicle_status_sub.updated()) {
|
|
vehicle_status_s vehicle_status;
|
|
|
|
if (_vehicle_status_sub.copy(&vehicle_status)) {
|
|
_armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
|
|
_system_id = vehicle_status.system_id;
|
|
_rotary_wing = (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING);
|
|
_vtol = vehicle_status.is_vtol;
|
|
}
|
|
}
|
|
|
|
// Check if parameters have changed
|
|
if (_parameter_update_sub.updated()) {
|
|
// clear update
|
|
parameter_update_s param_update;
|
|
_parameter_update_sub.copy(¶m_update);
|
|
|
|
updateParams();
|
|
}
|
|
|
|
_selector.updateValidityOfChosenInput(now);
|
|
|
|
for (int i = 0; i < MAX_MANUAL_INPUT_COUNT; i++) {
|
|
manual_control_setpoint_s manual_control_input;
|
|
|
|
if (_manual_control_input_subs[i].update(&manual_control_input)) {
|
|
_selector.updateWithNewInputSample(now, manual_control_input, i);
|
|
}
|
|
}
|
|
|
|
if (_selector.setpoint().valid) {
|
|
_published_invalid_once = false;
|
|
|
|
processStickArming(_selector.setpoint());
|
|
bool kill_gesture_active = processStickKillGesture(_selector.setpoint());
|
|
|
|
if (kill_gesture_active) {
|
|
// Zero sticks and set min throttle to prevent extreme setpoints
|
|
// while waiting for the kill hysteresis to trigger.
|
|
|
|
// Note: Arm/Disarm gestures dont require stick centering because
|
|
// they aren't active during flight
|
|
|
|
_selector.setpoint().roll = 0.f;
|
|
_selector.setpoint().pitch = 0.f;
|
|
_selector.setpoint().throttle = -1.0f;
|
|
_selector.setpoint().yaw = 0.f;
|
|
}
|
|
|
|
// User override by stick
|
|
const float dt_s = (now - _timestamp_last_loop) / 1e6f;
|
|
const float velocity_threshold = _param_man_override_spd.get();
|
|
|
|
_selector.setpoint().sticks_moving =
|
|
(velocity_threshold >= 0.f) &&
|
|
((fabsf(_roll_diff.update(_selector.setpoint().roll, dt_s)) > velocity_threshold
|
|
&& _roll_diff.consecutiveSameSign() >= MIN_SIGN_CONSECUTIVE)
|
|
|| (fabsf(_pitch_diff.update(_selector.setpoint().pitch, dt_s)) > velocity_threshold
|
|
&& _pitch_diff.consecutiveSameSign() >= MIN_SIGN_CONSECUTIVE)
|
|
|| (fabsf(_yaw_diff.update(_selector.setpoint().yaw, dt_s)) > velocity_threshold
|
|
&& _yaw_diff.consecutiveSameSign() >= MIN_SIGN_CONSECUTIVE)
|
|
|| (fabsf(_throttle_diff.update(_selector.setpoint().throttle, dt_s)) > velocity_threshold
|
|
&& _throttle_diff.consecutiveSameSign() >= MIN_SIGN_CONSECUTIVE));
|
|
|
|
_selector.setpoint().timestamp = now;
|
|
_manual_control_setpoint_pub.publish(_selector.setpoint());
|
|
|
|
// Attach scheduling to new samples of the chosen input
|
|
const int instance = _selector.instance();
|
|
|
|
if (instance != _previous_manual_control_input_instance) {
|
|
if ((0 <= _previous_manual_control_input_instance)
|
|
&& (_previous_manual_control_input_instance < MAX_MANUAL_INPUT_COUNT)) {
|
|
_manual_control_input_subs[_previous_manual_control_input_instance].unregisterCallback();
|
|
}
|
|
|
|
if ((0 <= instance) && (instance < MAX_MANUAL_INPUT_COUNT)) {
|
|
_manual_control_input_subs[instance].registerCallback();
|
|
}
|
|
|
|
_previous_manual_control_input_instance = instance;
|
|
}
|
|
|
|
_manual_control_switches_sub.registerCallback();
|
|
|
|
} else {
|
|
if (!_published_invalid_once) {
|
|
_published_invalid_once = true;
|
|
_manual_control_setpoint_pub.publish(_selector.setpoint());
|
|
}
|
|
|
|
_roll_diff.reset();
|
|
_pitch_diff.reset();
|
|
_yaw_diff.reset();
|
|
_throttle_diff.reset();
|
|
_stick_arm_hysteresis.set_state_and_update(false, now);
|
|
_stick_disarm_hysteresis.set_state_and_update(false, now);
|
|
_stick_kill_hysteresis.set_state_and_update(false, now);
|
|
_button_arm_hysteresis.set_state_and_update(false, now);
|
|
}
|
|
|
|
processSwitches(now);
|
|
|
|
_timestamp_last_loop = now;
|
|
}
|
|
|
|
void ManualControl::processSwitches(hrt_abstime &now)
|
|
{
|
|
manual_control_switches_s switches;
|
|
const bool switches_updated = _manual_control_switches_sub.update(&switches);
|
|
|
|
// Only use switches if the currently valid source is RC as well
|
|
if (_selector.setpoint().valid
|
|
&& _selector.setpoint().data_source == manual_control_setpoint_s::SOURCE_RC) {
|
|
if (switches_updated) {
|
|
if (_previous_switches_initialized) {
|
|
if (switches.mode_slot != _previous_switches.mode_slot) {
|
|
evaluateModeSlot(switches.mode_slot);
|
|
}
|
|
|
|
if (_param_com_arm_swisbtn.get()) {
|
|
// Arming button
|
|
const bool previous_button_arm_hysteresis = _button_arm_hysteresis.get_state();
|
|
_button_arm_hysteresis.set_state_and_update(switches.arm_switch == manual_control_switches_s::SWITCH_POS_ON, now);
|
|
|
|
if (!previous_button_arm_hysteresis && _button_arm_hysteresis.get_state()) {
|
|
sendActionRequest(action_request_s::ACTION_TOGGLE_ARMING, action_request_s::SOURCE_RC_BUTTON);
|
|
}
|
|
|
|
} else {
|
|
// Arming switch
|
|
if (switches.arm_switch != _previous_switches.arm_switch) {
|
|
if (switches.arm_switch == manual_control_switches_s::SWITCH_POS_ON) {
|
|
sendActionRequest(action_request_s::ACTION_ARM, action_request_s::SOURCE_RC_SWITCH);
|
|
|
|
} else if (switches.arm_switch == manual_control_switches_s::SWITCH_POS_OFF) {
|
|
sendActionRequest(action_request_s::ACTION_DISARM, action_request_s::SOURCE_RC_SWITCH);
|
|
}
|
|
}
|
|
}
|
|
|
|
if (switches.return_switch != _previous_switches.return_switch) {
|
|
if (switches.return_switch == manual_control_switches_s::SWITCH_POS_ON) {
|
|
sendActionRequest(action_request_s::ACTION_SWITCH_MODE, action_request_s::SOURCE_RC_SWITCH,
|
|
vehicle_status_s::NAVIGATION_STATE_AUTO_RTL);
|
|
|
|
} else if (switches.return_switch == manual_control_switches_s::SWITCH_POS_OFF) {
|
|
evaluateModeSlot(switches.mode_slot);
|
|
}
|
|
}
|
|
|
|
if (switches.loiter_switch != _previous_switches.loiter_switch) {
|
|
if (switches.loiter_switch == manual_control_switches_s::SWITCH_POS_ON) {
|
|
sendActionRequest(action_request_s::ACTION_SWITCH_MODE, action_request_s::SOURCE_RC_SWITCH,
|
|
vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER);
|
|
|
|
} else if (switches.loiter_switch == manual_control_switches_s::SWITCH_POS_OFF) {
|
|
evaluateModeSlot(switches.mode_slot);
|
|
}
|
|
}
|
|
|
|
if (switches.offboard_switch != _previous_switches.offboard_switch) {
|
|
if (switches.offboard_switch == manual_control_switches_s::SWITCH_POS_ON) {
|
|
sendActionRequest(action_request_s::ACTION_SWITCH_MODE, action_request_s::SOURCE_RC_SWITCH,
|
|
vehicle_status_s::NAVIGATION_STATE_OFFBOARD);
|
|
|
|
} else if (switches.offboard_switch == manual_control_switches_s::SWITCH_POS_OFF) {
|
|
evaluateModeSlot(switches.mode_slot);
|
|
}
|
|
}
|
|
|
|
if (switches.kill_switch != _previous_switches.kill_switch) {
|
|
if (switches.kill_switch == manual_control_switches_s::SWITCH_POS_ON) {
|
|
sendActionRequest(action_request_s::ACTION_KILL, action_request_s::SOURCE_RC_SWITCH);
|
|
|
|
} else if (switches.kill_switch == manual_control_switches_s::SWITCH_POS_OFF) {
|
|
sendActionRequest(action_request_s::ACTION_UNKILL, action_request_s::SOURCE_RC_SWITCH);
|
|
}
|
|
}
|
|
|
|
if (switches.termination_switch != _previous_switches.termination_switch
|
|
&& switches.termination_switch == manual_control_switches_s::SWITCH_POS_ON) {
|
|
sendActionRequest(action_request_s::ACTION_TERMINATION, action_request_s::SOURCE_RC_SWITCH);
|
|
}
|
|
|
|
if (switches.gear_switch != _previous_switches.gear_switch
|
|
&& _previous_switches.gear_switch != manual_control_switches_s::SWITCH_POS_NONE) {
|
|
|
|
if (switches.gear_switch == manual_control_switches_s::SWITCH_POS_ON) {
|
|
publishLandingGear(landing_gear_s::GEAR_UP);
|
|
|
|
} else if (switches.gear_switch == manual_control_switches_s::SWITCH_POS_OFF) {
|
|
publishLandingGear(landing_gear_s::GEAR_DOWN);
|
|
}
|
|
}
|
|
|
|
if (switches.transition_switch != _previous_switches.transition_switch) {
|
|
if (switches.transition_switch == manual_control_switches_s::SWITCH_POS_ON) {
|
|
sendActionRequest(action_request_s::ACTION_VTOL_TRANSITION_TO_FIXEDWING, action_request_s::SOURCE_RC_SWITCH);
|
|
|
|
} else if (switches.transition_switch == manual_control_switches_s::SWITCH_POS_OFF) {
|
|
sendActionRequest(action_request_s::ACTION_VTOL_TRANSITION_TO_MULTICOPTER, action_request_s::SOURCE_RC_SWITCH);
|
|
}
|
|
}
|
|
|
|
if (switches.photo_switch != _previous_switches.photo_switch) {
|
|
if (switches.photo_switch == manual_control_switches_s::SWITCH_POS_ON) {
|
|
send_camera_mode_command(CameraMode::Image);
|
|
send_photo_command();
|
|
}
|
|
}
|
|
|
|
if (switches.video_switch != _previous_switches.video_switch) {
|
|
if (switches.video_switch == manual_control_switches_s::SWITCH_POS_ON) {
|
|
send_camera_mode_command(CameraMode::Video);
|
|
send_video_command();
|
|
}
|
|
}
|
|
|
|
#if defined(PAYLOAD_POWER_EN)
|
|
|
|
if (switches.payload_power_switch != _previous_switches.payload_power_switch) {
|
|
if (switches.payload_power_switch == manual_control_switches_s::SWITCH_POS_ON) {
|
|
PAYLOAD_POWER_EN(true);
|
|
|
|
} else if (switches.payload_power_switch == manual_control_switches_s::SWITCH_POS_OFF
|
|
|| switches.payload_power_switch == manual_control_switches_s::SWITCH_POS_MIDDLE) {
|
|
PAYLOAD_POWER_EN(false);
|
|
}
|
|
}
|
|
|
|
#endif // PAYLOAD_POWER_EN
|
|
|
|
} else if (!_armed) {
|
|
// Directly initialize mode using RC switch but only before arming
|
|
evaluateModeSlot(switches.mode_slot);
|
|
#if defined(PAYLOAD_POWER_EN)
|
|
|
|
// Apply payload power state on first switch receipt if not armed
|
|
if (switches.payload_power_switch == manual_control_switches_s::SWITCH_POS_ON) {
|
|
PAYLOAD_POWER_EN(true);
|
|
|
|
} else if (switches.payload_power_switch == manual_control_switches_s::SWITCH_POS_OFF
|
|
|| switches.payload_power_switch == manual_control_switches_s::SWITCH_POS_MIDDLE) {
|
|
PAYLOAD_POWER_EN(false);
|
|
}
|
|
|
|
#endif // PAYLOAD_POWER_EN
|
|
}
|
|
|
|
_previous_switches = switches;
|
|
_previous_switches_initialized = true;
|
|
}
|
|
|
|
} else {
|
|
// Don't react on switch changes while RC was not in use
|
|
_previous_switches_initialized = false;
|
|
}
|
|
}
|
|
|
|
void ManualControl::updateParams()
|
|
{
|
|
ModuleParams::updateParams();
|
|
|
|
_stick_arm_hysteresis.set_hysteresis_time_from(false, 1_s);
|
|
_stick_disarm_hysteresis.set_hysteresis_time_from(false, 1_s);
|
|
_button_arm_hysteresis.set_hysteresis_time_from(false, 1_s);
|
|
// A negative MAN_KILL_GEST_T disables the gesture, don't convert it to an unsigned time
|
|
_stick_kill_hysteresis.set_hysteresis_time_from(false, math::max(_param_man_kill_gest_t.get(), 0.f) * 1_s);
|
|
|
|
_selector.setRcInMode(_param_com_rc_in_mode.get());
|
|
_selector.setTimeout(_param_com_rc_loss_t.get() * 1_s);
|
|
|
|
// MAN_ARM_GESTURE
|
|
if (_param_man_arm_gesture.get() == 1) {
|
|
// RC_MAP_ARM_SW & MAN_ARM_GESTURE: disable arm gesture if an arm switch is configured
|
|
param_t param_rc_map_arm_sw = param_find("RC_MAP_ARM_SW");
|
|
|
|
if (param_rc_map_arm_sw != PARAM_INVALID) {
|
|
int32_t rc_map_arm_sw = 0;
|
|
param_get(param_rc_map_arm_sw, &rc_map_arm_sw);
|
|
|
|
if (rc_map_arm_sw > 0) {
|
|
_param_man_arm_gesture.set(0); // disable arm gesture
|
|
_param_man_arm_gesture.commit();
|
|
|
|
orb_advert_t mavlink_log_pub = nullptr;
|
|
mavlink_log_critical(&mavlink_log_pub, "Arm stick gesture disabled if arm switch in use\t")
|
|
/* EVENT
|
|
* @description <param>MAN_ARM_GESTURE</param> is now set to disable arm/disarm stick gesture.
|
|
*/
|
|
events::send(events::ID("stick_gesture_disabled_by_arm_switch"), {events::Log::Info, events::LogInternal::Disabled},
|
|
"Arm stick gesture disabled if arm switch in use");
|
|
}
|
|
}
|
|
|
|
// MC_AIRMODE & MAN_ARM_GESTURE: check for unsafe Airmode settings: yaw airmode requires disabling the stick arm gesture
|
|
if ((_param_man_arm_gesture.get() == 1) && (_rotary_wing || _vtol)) {
|
|
param_t param_mc_airmode = param_find("MC_AIRMODE");
|
|
|
|
if (param_mc_airmode != PARAM_INVALID) {
|
|
int32_t airmode = 0;
|
|
param_get(param_mc_airmode, &airmode);
|
|
|
|
if (airmode == 2) {
|
|
airmode = 1; // change to roll/pitch airmode
|
|
param_set(param_mc_airmode, &airmode);
|
|
|
|
orb_advert_t mavlink_log_pub = nullptr;
|
|
mavlink_log_critical(&mavlink_log_pub, "Yaw Airmode requires disabling the stick arm gesture\t")
|
|
/* EVENT
|
|
* @description <param>MC_AIRMODE</param> is now set to roll/pitch airmode.
|
|
*/
|
|
events::send(events::ID("commander_airmode_requires_no_arm_gesture"), {events::Log::Error, events::LogInternal::Disabled},
|
|
"Yaw Airmode requires disabling the stick arm gesture");
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
void ManualControl::processStickArming(const manual_control_setpoint_s &input)
|
|
{
|
|
// Arm gesture
|
|
const bool right_stick_centered = (fabsf(input.pitch) < 0.1f) && (fabsf(input.roll) < 0.1f);
|
|
const bool left_stick_lower_right = (input.throttle < -0.8f) && (input.yaw > 0.9f);
|
|
|
|
const bool previous_stick_arm_hysteresis = _stick_arm_hysteresis.get_state();
|
|
_stick_arm_hysteresis.set_state_and_update(left_stick_lower_right && right_stick_centered, input.timestamp);
|
|
|
|
if (_param_man_arm_gesture.get() && !previous_stick_arm_hysteresis && _stick_arm_hysteresis.get_state()) {
|
|
sendActionRequest(action_request_s::ACTION_ARM, action_request_s::SOURCE_STICK_GESTURE);
|
|
}
|
|
|
|
// Disarm gesture
|
|
const bool left_stick_lower_left = (input.throttle < -0.8f) && (input.yaw < -0.9f);
|
|
|
|
const bool previous_stick_disarm_hysteresis = _stick_disarm_hysteresis.get_state();
|
|
_stick_disarm_hysteresis.set_state_and_update(left_stick_lower_left && right_stick_centered, input.timestamp);
|
|
|
|
if (_param_man_arm_gesture.get() && !previous_stick_disarm_hysteresis && _stick_disarm_hysteresis.get_state()) {
|
|
sendActionRequest(action_request_s::ACTION_DISARM, action_request_s::SOURCE_STICK_GESTURE);
|
|
}
|
|
}
|
|
|
|
bool ManualControl::processStickKillGesture(const manual_control_setpoint_s &input)
|
|
{
|
|
if (_param_man_kill_gest_t.get() > 0.f) {
|
|
const bool right_stick_lower_right = (input.pitch < -0.9f) && (input.roll > 0.9f);
|
|
const bool left_stick_lower_left = (input.throttle < -0.8f) && (input.yaw < -0.9f);
|
|
const bool kill_gesture = right_stick_lower_right && left_stick_lower_left;
|
|
|
|
const bool previous_stick_kill_hysteresis = _stick_kill_hysteresis.get_state();
|
|
_stick_kill_hysteresis.set_state_and_update(kill_gesture, input.timestamp);
|
|
|
|
if (!previous_stick_kill_hysteresis && _stick_kill_hysteresis.get_state()) {
|
|
sendActionRequest(action_request_s::ACTION_KILL, action_request_s::SOURCE_STICK_GESTURE);
|
|
}
|
|
|
|
return kill_gesture;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
void ManualControl::evaluateModeSlot(uint8_t mode_slot)
|
|
{
|
|
switch (mode_slot) {
|
|
case manual_control_switches_s::MODE_SLOT_NONE:
|
|
break;
|
|
|
|
case manual_control_switches_s::MODE_SLOT_1:
|
|
sendActionRequest(action_request_s::ACTION_SWITCH_MODE, action_request_s::SOURCE_RC_MODE_SLOT,
|
|
navStateFromParam(_param_fltmode_1.get()));
|
|
break;
|
|
|
|
case manual_control_switches_s::MODE_SLOT_2:
|
|
sendActionRequest(action_request_s::ACTION_SWITCH_MODE, action_request_s::SOURCE_RC_MODE_SLOT,
|
|
navStateFromParam(_param_fltmode_2.get()));
|
|
break;
|
|
|
|
case manual_control_switches_s::MODE_SLOT_3:
|
|
sendActionRequest(action_request_s::ACTION_SWITCH_MODE, action_request_s::SOURCE_RC_MODE_SLOT,
|
|
navStateFromParam(_param_fltmode_3.get()));
|
|
break;
|
|
|
|
case manual_control_switches_s::MODE_SLOT_4:
|
|
sendActionRequest(action_request_s::ACTION_SWITCH_MODE, action_request_s::SOURCE_RC_MODE_SLOT,
|
|
navStateFromParam(_param_fltmode_4.get()));
|
|
break;
|
|
|
|
case manual_control_switches_s::MODE_SLOT_5:
|
|
sendActionRequest(action_request_s::ACTION_SWITCH_MODE, action_request_s::SOURCE_RC_MODE_SLOT,
|
|
navStateFromParam(_param_fltmode_5.get()));
|
|
break;
|
|
|
|
case manual_control_switches_s::MODE_SLOT_6:
|
|
sendActionRequest(action_request_s::ACTION_SWITCH_MODE, action_request_s::SOURCE_RC_MODE_SLOT,
|
|
navStateFromParam(_param_fltmode_6.get()));
|
|
break;
|
|
|
|
default:
|
|
PX4_WARN("mode slot overflow");
|
|
break;
|
|
}
|
|
}
|
|
|
|
void ManualControl::sendActionRequest(int8_t action, int8_t source, int8_t mode)
|
|
{
|
|
// We catch default unassigned mode slots which have value -1
|
|
if (action == action_request_s::ACTION_SWITCH_MODE && mode < 0) {
|
|
return;
|
|
}
|
|
|
|
action_request_s action_request{};
|
|
action_request.action = action;
|
|
action_request.source = source;
|
|
action_request.mode = mode;
|
|
action_request.timestamp = hrt_absolute_time();
|
|
_action_request_pub.publish(action_request);
|
|
}
|
|
|
|
void ManualControl::publishLandingGear(int8_t action)
|
|
{
|
|
landing_gear_s landing_gear{};
|
|
landing_gear.landing_gear = action;
|
|
landing_gear.timestamp = hrt_absolute_time();
|
|
_landing_gear_pub.publish(landing_gear);
|
|
}
|
|
|
|
void ManualControl::send_camera_mode_command(CameraMode camera_mode)
|
|
{
|
|
vehicle_command_s command{};
|
|
command.command = vehicle_command_s::VEHICLE_CMD_SET_CAMERA_MODE;
|
|
command.param2 = static_cast<float>(camera_mode);
|
|
command.target_system = _system_id;
|
|
command.target_component = 100; // MAV_COMP_ID_CAMERA
|
|
|
|
uORB::Publication<vehicle_command_s> command_pub{ORB_ID(vehicle_command)};
|
|
command.timestamp = hrt_absolute_time();
|
|
command_pub.publish(command);
|
|
}
|
|
|
|
void ManualControl::send_photo_command()
|
|
{
|
|
vehicle_command_s command{};
|
|
command.command = vehicle_command_s::VEHICLE_CMD_IMAGE_START_CAPTURE;
|
|
command.param3 = 1; // one picture
|
|
command.param4 = _image_sequence++;
|
|
command.target_system = _system_id;
|
|
command.target_component = 100; // MAV_COMP_ID_CAMERA
|
|
|
|
uORB::Publication<vehicle_command_s> command_pub{ORB_ID(vehicle_command)};
|
|
command.timestamp = hrt_absolute_time();
|
|
command_pub.publish(command);
|
|
}
|
|
|
|
void ManualControl::send_video_command()
|
|
{
|
|
vehicle_command_s command{};
|
|
|
|
if (_video_recording) {
|
|
command.command = vehicle_command_s::VEHICLE_CMD_VIDEO_STOP_CAPTURE;
|
|
command.param2 = 1; // status at 1 Hz
|
|
|
|
} else {
|
|
command.command = vehicle_command_s::VEHICLE_CMD_VIDEO_START_CAPTURE;
|
|
}
|
|
|
|
command.target_system = _system_id;
|
|
command.target_component = 100; // any camera
|
|
|
|
uORB::Publication<vehicle_command_s> command_pub{ORB_ID(vehicle_command)};
|
|
command.timestamp = hrt_absolute_time();
|
|
command_pub.publish(command);
|
|
|
|
_video_recording = !_video_recording;
|
|
}
|
|
|
|
int ManualControl::task_spawn(int argc, char *argv[])
|
|
{
|
|
ManualControl *instance = new ManualControl();
|
|
|
|
if (instance) {
|
|
desc.object.store(instance);
|
|
desc.task_id = task_id_is_work_queue;
|
|
|
|
if (instance->init()) {
|
|
return PX4_OK;
|
|
}
|
|
|
|
} else {
|
|
PX4_ERR("alloc failed");
|
|
}
|
|
|
|
delete instance;
|
|
desc.object.store(nullptr);
|
|
desc.task_id = -1;
|
|
|
|
return PX4_ERROR;
|
|
}
|
|
|
|
int ManualControl::print_status()
|
|
{
|
|
perf_print_counter(_loop_perf);
|
|
perf_print_counter(_loop_interval_perf);
|
|
return 0;
|
|
}
|
|
|
|
int ManualControl::custom_command(int argc, char *argv[])
|
|
{
|
|
return print_usage("unknown command");
|
|
}
|
|
|
|
int ManualControl::print_usage(const char *reason)
|
|
{
|
|
if (reason) {
|
|
PX4_WARN("%s\n", reason);
|
|
}
|
|
|
|
PRINT_MODULE_DESCRIPTION(
|
|
R"DESCR_STR(
|
|
### Description
|
|
Module consuming manual_control_inputs publishing one manual_control_setpoint.
|
|
|
|
)DESCR_STR");
|
|
|
|
PRINT_MODULE_USAGE_NAME("manual_control", "system");
|
|
PRINT_MODULE_USAGE_COMMAND("start");
|
|
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
|
|
|
|
return 0;
|
|
}
|
|
|
|
int8_t ManualControl::navStateFromParam(int32_t param_value)
|
|
{
|
|
// See src/modules/commander/module.yaml COM_FLTMODE${i}
|
|
switch(param_value) {
|
|
case 0: return vehicle_status_s::NAVIGATION_STATE_MANUAL;
|
|
case 1: return vehicle_status_s::NAVIGATION_STATE_ALTCTL;
|
|
case 2: return vehicle_status_s::NAVIGATION_STATE_POSCTL;
|
|
case 3: return vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION;
|
|
case 4: return vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
|
|
case 5: return vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
|
|
case 6: return vehicle_status_s::NAVIGATION_STATE_ACRO;
|
|
case 7: return vehicle_status_s::NAVIGATION_STATE_OFFBOARD;
|
|
case 8: return vehicle_status_s::NAVIGATION_STATE_STAB;
|
|
case 9: return vehicle_status_s::NAVIGATION_STATE_POSITION_SLOW;
|
|
case 10: return vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF;
|
|
case 11: return vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
|
|
case 12: return vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET;
|
|
case 13: return vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND;
|
|
case 14: return vehicle_status_s::NAVIGATION_STATE_ORBIT;
|
|
case 15: return vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF;
|
|
case 16: return vehicle_status_s::NAVIGATION_STATE_ALTITUDE_CRUISE;
|
|
case 17: return vehicle_status_s::NAVIGATION_STATE_MANUAL_PARKING;
|
|
|
|
case 100: return vehicle_status_s::NAVIGATION_STATE_EXTERNAL1;
|
|
case 101: return vehicle_status_s::NAVIGATION_STATE_EXTERNAL2;
|
|
case 102: return vehicle_status_s::NAVIGATION_STATE_EXTERNAL3;
|
|
case 103: return vehicle_status_s::NAVIGATION_STATE_EXTERNAL4;
|
|
case 104: return vehicle_status_s::NAVIGATION_STATE_EXTERNAL5;
|
|
case 105: return vehicle_status_s::NAVIGATION_STATE_EXTERNAL6;
|
|
case 106: return vehicle_status_s::NAVIGATION_STATE_EXTERNAL7;
|
|
case 107: return vehicle_status_s::NAVIGATION_STATE_EXTERNAL8;
|
|
}
|
|
return -1;
|
|
}
|
|
|
|
extern "C" __EXPORT int manual_control_main(int argc, char *argv[])
|
|
{
|
|
return ModuleBase::main(ManualControl::desc, argc, argv);
|
|
}
|